Diff: main.cpp
- Revision:
- 12:67a06c9b69d5
- Parent:
- 11:9bf69bc6df45
- Child:
- 13:4737ee9ebfee
--- a/main.cpp Thu Oct 18 20:04:16 2012 +0000
+++ b/main.cpp Sat Oct 20 17:28:28 2012 +0000
@@ -13,6 +13,8 @@
#define Rad2Deg 57.295779513082320876798154814105
#define RATE 0.02 // speed of Ticker/PID
+//#define COMPASSCALIBRATE
+
Timer GlobalTimer; // global time to calculate processing speed
Ticker Datagetter; // timecontrolled interrupt to get data form IMU and RC
PID controller(1.0, 0.0, 0.0, RATE); // test PID controller for throttle
@@ -29,10 +31,11 @@
Servo Motor[] = {(p15), (p16), (p17), (p18)};
// variables for loop
-unsigned long dt = 0;
-unsigned long time_loop = 0;
+unsigned long dt_get_data = 0;
+unsigned long time_get_data = 0;
+unsigned long dt_read_sensors = 0;
+unsigned long time_read_sensors = 0;
float angle[3] = {0,0,0}; // angle 0: x,roll / 1: y,pitch / 2: z,yaw
-float Comp_angle = 0;
float tempangle = 0;
int Motorvalue[3];
@@ -40,74 +43,92 @@
void get_Data()
{
+ time_read_sensors = GlobalTimer.read_us();
+
// read data from sensors
Gyro.read();
Acc.read();
Comp.read();
- Alt.Update();
-
- //calculate angle for yaw from compass
- //Comp_angle = Comp.getAngle(Comp.Mag[0], Comp.Mag[1]);
+ //Alt.Update();
+
+ dt_read_sensors = GlobalTimer.read_us() - time_read_sensors;
// meassure dt
- dt = GlobalTimer.read_us() - time_loop; // time in us since last loop
- time_loop = GlobalTimer.read_us(); // set new time for next measurement
+ dt_get_data = GlobalTimer.read_us() - time_get_data; // time in us since last loop
+ time_get_data = GlobalTimer.read_us(); // set new time for next measurement
// calculate angles for roll, pitch an yaw
- angle[0] += (Acc.angle[0] - angle[0])/50 + Gyro.data[0] *dt/15000000.0;
- angle[1] += (Acc.angle[1]+3 - angle[1])/50 + Gyro.data[1] *dt/15000000.0;// TODO Offset accelerometer einstellen
- tempangle += (Comp_angle - tempangle)/50 - Gyro.data[2] *dt/15000000.0;
- angle[2] += Gyro.data[2] *dt/15000000.0; // gyro only here
+ angle[0] += (Acc.angle[0] - angle[0])/50 + Gyro.data[0] *dt_get_data/15000000.0;
+ angle[1] += (Acc.angle[1]+3 - angle[1])/50 + Gyro.data[1] *dt_get_data/15000000.0;// TODO Offset accelerometer einstellen
+ tempangle += (Comp.get_angle() - tempangle)/50 + Gyro.data[2] *dt_get_data/15000000.0;
+ angle[2] += Gyro.data[2] *dt_get_data/15000000.0; // gyro only here
+
+ // TODO Read RC data
- // Read RC data
- controller.setProcessValue(RC[3 -1].read());
- for (int j = 0; j < 4; j++)
- Motorvalue[j] = controller.compute(); // throttle
-
- for (int j = 0; j < 4; j++)
- Motor[j] = 1000 + 5*abs(angle[1]);//Motorvalue[j]; // set new motorspeeds
- pid.setProcessValue(angle[0]);
- pidtester = pid.compute();
+ // calculate new motorspeeds
+ /*
+ Motor[0] = 1000 + (100 + (angle[0] * 500/90)) * (RC[1].read() - 1000) / 1000;
+ Motor[1] = 1000 + (100 + (angle[1] * 500/90)) * (RC[1].read() - 1000) / 1000;
+ Motor[2] = 1000 + (100 - (angle[0] * 500/90)) * (RC[1].read() - 1000) / 1000;
+ Motor[3] = 1000 + (100 - (angle[1] * 500/90)) * (RC[1].read() - 1000) / 1000;
+ */
}
int main() {
+ //NVIC_SetPriority(TIMER3_IRQn, 2); // set priorty of tickers below hardware interrupts
+
+ #ifdef COMPASSCALIBRATE
+ pc.locate(10,5);
+ pc.printf("CALIBRATING");
+ Comp.calibrate(60);
+ #endif
+
// init screen
pc.locate(10,5);
pc.printf("Flybed v0.2");
LEDs.roll(2);
- controller.setInputLimits(1000.0, 2000.0);
- controller.setOutputLimits(1000.0, 2000.0);
- controller.setMode(AUTO_MODE);
-
- pid.setInputLimits(-90.0, 90.0);
- pid.setOutputLimits(-90.0, 90.0);
- pid.setMode(AUTO_MODE);
- pid.setSetPoint(0.0);
-
// Start!
GlobalTimer.start();
- Datagetter.attach(&get_Data, RATE); // start to get data all 10ms
- while(1) {
+ Datagetter.attach(&get_Data, RATE); // start to get data all RATEms
+
+ while(1) {
pc.locate(10,5); // PC output
- pc.printf("dt:%dms %6.1fm ", dt/1000, Alt.CalcAltitude(Alt.Pressure));
+ pc.printf("dt:%dms dt_sensors:%dus Altitude:%6.1fm ", dt_get_data/1000, dt_read_sensors, Alt.CalcAltitude(Alt.Pressure));
pc.locate(10,8);
pc.printf("Roll:%6.1f Pitch:%6.1f Yaw:%6.1f ", angle[0], angle[1], angle[2]);
pc.locate(10,9);
pc.printf("ACC: Roll:%6.1f Pitch:%6.1f Yaw:%6.1f ", Acc.angle[0], Acc.angle[1], Acc.angle[2]);
pc.locate(10,10);
- pc.printf("Debug_Yaw: Comp:%6.1f tempangle:%6.1f ", Comp_angle, tempangle);
+ pc.printf("Debug_Yaw: Comp:%6.1f tempangle:%6.1f ", Comp.get_angle(), tempangle);
pc.locate(10,12);
- pc.printf("Comp_data: %6.1f %6.1f %6.1f |||| %6.1f ", Comp.data[0], Comp.data[1], Comp.data[2], Comp.heading);
+ pc.printf("Comp_data: %6.1f %6.1f %6.1f |||| %6.1f ", Comp.data[0], Comp.data[1], Comp.data[2], Comp.get_angle());
pc.locate(10,13);
pc.printf("Comp_scale: %6.4f %6.4f %6.4f ", Comp.scale[0], Comp.scale[1], Comp.scale[2]);
pc.locate(10,15);
pc.printf("pidtester: %6.1f RC: %d %d %d %d ", pidtester, RC[0].read(), RC[1].read(), RC[2].read(), RC[3].read());
- pc.locate(10,18);
- pc.printf("Max: %d %d %d ", Comp.Max[0], Comp.Max[1], Comp.Max[2]);
+
pc.locate(10,19);
- pc.printf("Min: %d %d %d ", Comp.Min[0], Comp.Min[1], Comp.Min[2]);
+ pc.printf("RC0: %d :[", RC[0].read());
+ for (int i = 0; i < (RC[0].read() - 1000)/17; i++)
+ pc.printf("=");
+ pc.printf(" ");
+ pc.locate(10,20);
+ pc.printf("RC1: %d :[", RC[1].read());
+ for (int i = 0; i < (RC[1].read() - 1000)/17; i++)
+ pc.printf("=");
+ pc.printf(" ");
+ pc.locate(10,21);
+ pc.printf("RC2: %d :[", RC[2].read());
+ for (int i = 0; i < (RC[2].read() - 1000)/17; i++)
+ pc.printf("=");
+ pc.printf(" ");
+ pc.locate(10,22);
+ pc.printf("RC3: %d :[", RC[3].read());
+ for (int i = 0; i < (RC[3].read() - 1000)/17; i++)
+ pc.printf("=");
+ pc.printf(" ");
LEDs.rollnext();
}
}
Matthias Grob
