Revision 37:34917f7c10ae, committed 2013-06-12
- Comitter:
- maetugr
- Date:
- Wed Jun 12 16:42:32 2013 +0000
- Parent:
- 36:128c55793728
- Child:
- 38:ff95fd524c9e
- Commit message:
- Solved -180 to 180 degree YAW-range-issue, not physicaly tested yet
Changed in this revision
--- a/IMU_Filter/IMU_Filter.cpp Wed Jun 12 10:26:18 2013 +0000
+++ b/IMU_Filter/IMU_Filter.cpp Wed Jun 12 16:42:32 2013 +0000
@@ -61,6 +61,21 @@
rangle[1] = asin(2*q0*q2 - 2*q3*q1);
rangle[2] = atan2(2*q0*q3 + 2*q1*q2, 1 - 2*(q2*q2 + q3*q3));
+ // TODO
+ // Pitch should have a range of +/-90 degrees.
+ // After you pitch past vertical (90 degrees) your roll and yaw value should swing 180 degrees.
+ // A pitch value of 100 degrees is measured as a pitch of 80 degrees and inverted flight (roll = 180 degrees).
+ // Another example is a pitch of 180 degrees (upside down). This is measured as a level pitch (0 degrees) and a roll of 180 degrees.
+ // And I think this solves the upside down issue...
+ // Handle roll reversal when inverted
+ /*if (Acc_data[2] < 0) {
+ if (Acc_data[0] < 0) {
+ rangle[1] = (180 - rangle[1]);
+ } else {
+ rangle[1] = (-180 - rangle[1]);
+ }
+ }*/
+
for(int i=0; i<3; i++) // angle in degree
angle[i] = rangle[i] * 180 / PI;
#endif
--- a/Mixer/Mixer.cpp Wed Jun 12 10:26:18 2013 +0000
+++ b/Mixer/Mixer.cpp Wed Jun 12 16:42:32 2013 +0000
@@ -33,6 +33,8 @@
Motor_speed[i] += mix_table[Configuration][i][j] * controller_value[j];
}
- for(int i = 0; i < 4; i++) // make sure no motor stands still
+ for(int i = 0; i < 4; i++) { // make sure no motor stands still or gets a higher speed than 1000
Motor_speed[i] = Motor_speed[i] > 150 ? Motor_speed[i] : 150;
+ Motor_speed[i] = Motor_speed[i] <= 1000 ? Motor_speed[i] : 1000;
+ }
}
\ No newline at end of file
--- a/RC/RC_Channel.cpp Wed Jun 12 10:26:18 2013 +0000
+++ b/RC/RC_Channel.cpp Wed Jun 12 16:42:32 2013 +0000
@@ -19,7 +19,8 @@
{
if(time == -100)
return time;
- return scale * (float)(time) + offset; // calibration of the readings
+ float result = scale * (float)(time) + offset; // calibration of the readings
+ return (result > 490 && result < 510) ? 500 : result; // make the middle of the stickpositions non drifting
}
void RC_Channel::rise()
--- a/main.cpp Wed Jun 12 10:26:18 2013 +0000
+++ b/main.cpp Wed Jun 12 16:42:32 2013 +0000
@@ -15,6 +15,7 @@
#define PPM_FREQU 495 // Hz Frequency of PPM Signal for ESCs (maximum <500Hz)
#define RC_SENSITIVITY 30 // maximal angle from horizontal that the PID is aming for
#define YAWSPEED 0.2 // maximal speed of yaw rotation in degree per Rate
+#define INTEGRAL_MAX 300
// RC
#define AILERON 0
@@ -30,16 +31,18 @@
// Global variables
bool armed = false; // this variable is for security (when false no motor rotates any more)
+bool RC_present = false; // this variable shows if an RC is present
float dt = 0;
float time_for_dt = 0;
float dt_read_sensors = 0;
float time_read_sensors = 0;
float controller_value[] = {0,0,0}; // The calculated answer form the Controller
float RC_angle[] = {0,0,0}; // Angle of the RC Sticks, to steer the QC
+float RC_yaw_adding; // temporary variable to take the desired yaw adjustment
float P = 4.0; // PID values
float I = 0;
-float D = 2.0;
+float D = 1.0;
Timer GlobalTimer; // global time to calculate processing speed
Ticker Dutycycler; // timecontrolled interrupt for exact timed control loop
@@ -47,7 +50,7 @@
// Initialisation of hardware (see includes for more info)
LED LEDs;
#ifdef PC_CONNECTED
- PC pc(USBTX, USBRX, 921600); // USB
+ PC pc(USBTX, USBRX, 115200); // USB
//PC pc(p9, p10, 115200); // Bluetooth
#endif
L3G4200D Gyro(p28, p27);
@@ -58,7 +61,7 @@
Servo_PWM ESC[] = {Servo_PWM(p21,PPM_FREQU), Servo_PWM(p22,PPM_FREQU), Servo_PWM(p23,PPM_FREQU), Servo_PWM(p24,PPM_FREQU)}; // p21 - p26 only because PWM needed!
IMU_Filter IMU; // (don't write () after constructor for no arguments!)
Mixer MIX(1); // 0 for +-Formation, 1 for X-Formation
-PID Controller[] = {PID(P, I, D, 1000), PID(P, I, D, 1000), PID(0.5, 0, 0, 1000)}; // 0:X:Roll 1:Y:Pitch 2:Z:Yaw
+PID Controller[] = {PID(P, I, D, INTEGRAL_MAX), PID(P, I, D, INTEGRAL_MAX), PID(1.0, 0, 0, INTEGRAL_MAX)}; // 0:X:Roll 1:Y:Pitch 2:Z:Yaw
void dutycycle() // method which is called by the Ticker Dutycycler every RATE seconds
{
@@ -70,7 +73,7 @@
//Comp.read(); // TODO: not every loop every sensor? altitude not that important
//Alt.Update(); // TODO needs very long to read because of waits
- pc.printf("%6.1f,%6.1f,%6.1f,%6.1f,%6.1f,%6.1f\r\n", Gyro.data[0], Gyro.data[1], Gyro.data[2], Acc.data[0], Acc.data[1], Acc.data[2]);
+ //pc.printf("%6.1f,%6.1f,%6.1f,%6.1f,%6.1f,%6.1f\r\n", Gyro.data[0], Gyro.data[1], Gyro.data[2], Acc.data[0], Acc.data[1], Acc.data[2]);
dt_read_sensors = GlobalTimer.read() - time_read_sensors; // stop time measure for sensors
@@ -81,34 +84,58 @@
IMU.compute(dt, Gyro.data, Acc.data);
//pc.printf("%f,%f,%f,%3.5fs,%3.5fs\r\n", IMU.angle[0], IMU.angle[1], IMU.angle[2], dt, dt_read_sensors);
+ if(RC[AILERON].read() == -100 || RC[ELEVATOR].read() == -100 || RC[RUDDER].read() == -100 || RC[THROTTLE].read() == -100)
+ RC_present = false;
+ else
+ RC_present = true;
+
// Arming / disarming
if(RC[THROTTLE].read() < 20 && RC[RUDDER].read() > 850) {
armed = true;
}
- if((RC[THROTTLE].read() < 30 && RC[RUDDER].read() < 30) || RC[RUDDER].read() < -10 || RC[THROTTLE].read() < -10 || RC[ELEVATOR].read() < -10 || RC[AILERON].read() < -10) {
+ if((RC[THROTTLE].read() < 30 && RC[RUDDER].read() < 30) || !RC_present) {
armed = false;
}
- // RC Angle
+ // RC Angle ROLL-PITCH-Part
for(int i=0;i<2;i++) { // calculate new angle we want the QC to have
- RC_angle[i] = (RC[i].read()-500)*RC_SENSITIVITY/500.0;
- if (RC_angle[i] < -RC_SENSITIVITY-2)
+ if (RC_present)
+ RC_angle[i] = (RC[i].read()-500)*RC_SENSITIVITY/500.0;
+ else
RC_angle[i] = 0;
}
- //RC_angle[2] += (RC[3].read()-500)*YAWSPEED/500; // for yaw angle is integrated
+
+ // RC Angle YAW-Part
+ if (RC_present && RC[THROTTLE].read() > 20)
+ RC_yaw_adding = (RC[RUDDER].read()-500)*YAWSPEED/500;
+ else
+ RC_yaw_adding = 0;
+
+ while(RC_angle[YAW] + RC_yaw_adding < -180 || RC_angle[YAW] + RC_yaw_adding > 180) { // make shure it's in the cycle -180 to 180
+ if(RC_angle[YAW] + RC_yaw_adding < -180)
+ RC_yaw_adding += 360;
+ if(RC_angle[YAW] + RC_yaw_adding > 180)
+ RC_yaw_adding -= 360;
+ }
+ RC_angle[YAW] += RC_yaw_adding; // for yaw angle it's integrated
// PID controlling
- for(int i=0;i<3;i++) {
+ for(int i=0;i<2;i++) {
Controller[i].setIntegrate(armed); // only integrate in controller when armed, so the value is not totally odd from not flying
- controller_value[i] = Controller[i].compute(0, IMU.angle[i]);
- //controller_value[i] = Controller[i].compute(RC_angle[i], IMU.angle[i]); // give the controller the actual angle and get his advice to correct
+ controller_value[i] = Controller[i].compute(RC_angle[i], IMU.angle[i]); // give the controller the actual angle and get his advice to correct
}
-
+ Controller[YAW].setIntegrate(armed); // same for YAW
+ if (abs(RC_angle[YAW] - IMU.angle[YAW]) > 180) // for YAW a special calculation because of range -180 to 180
+ if (RC_angle[YAW] > IMU.angle[YAW])
+ controller_value[YAW] = Controller[YAW].compute(RC_angle[YAW] - 360, IMU.angle[YAW]);
+ else
+ controller_value[YAW] = Controller[YAW].compute(RC_angle[YAW] + 360, IMU.angle[YAW]);
+ else
+ controller_value[YAW] = Controller[YAW].compute(RC_angle[YAW], IMU.angle[YAW]);
if (armed) // for SECURITY!
{
MIX.compute(RC[THROTTLE].read(), controller_value); // let the Mixer compute motorspeeds based on throttle and controller output
-
for(int i=0;i<4;i++) // Set new motorspeeds
ESC[i] = (int)MIX.Motor_speed[i];
@@ -153,7 +180,7 @@
pc.readcommand(&commandexecuter);
//pc.printf("%f %f %f %f %f %f\r\n", IMU.angle[0], IMU.angle[1], IMU.angle[2], controller_value[0], controller_value[1], controller_value[2]); // For live plot in MATLAB of IMU
//pc.printf("%f,%f,%f,%f,%f,%f\r\n", IMU.angle[0], IMU.angle[1], IMU.angle[2], controller_value[0], controller_value[1], controller_value[2]);
- #if 0 //pc.cls();
+ #if 1 //pc.cls();
pc.locate(20,0); // PC output
pc.printf("dt:%3.5fs dt_sensors:%3.5fs Altitude:%6.1fm ", dt, dt_read_sensors, Alt.CalcAltitude(Alt.Pressure));
pc.locate(5,1);
Matthias Grob
