WebSVN - FlightCtrl - Diff - Rev 2109 and 2132 - /branches/dongfang_FC_fixedwing/arduino

         target[YAW] = attitude[YAW];
+}
 void flight_updateFlightParametersToFlightMode(void) {
         debugOut.analog[16] = currentFlightMode;
-        reverse[PITCH] = staticParams.controlServosReverse & CONTROL_SERVO_REVERSE_ELEVATOR;
+        reverse[PITCH] = staticParams.servosReverse & CONTROL_SERVO_REVERSE_ELEVATOR;
-        reverse[ROLL] = staticParams.controlServosReverse & CONTROL_SERVO_REVERSE_AILERONS;
+        reverse[ROLL] = staticParams.servosReverse & CONTROL_SERVO_REVERSE_AILERONS;
-        reverse[YAW] = staticParams.controlServosReverse & CONTROL_SERVO_REVERSE_RUDDER;
+        reverse[YAW] = staticParams.servosReverse & CONTROL_SERVO_REVERSE_RUDDER;
         // At a switch to angles, we want to kill errors first.
         // This should be triggered only once per mode change!
+        }
+}
 // Normal at airspeed = 10.
 uint8_t calcAirspeedPID(uint8_t pid) {
-        if (staticParams.bitConfig & CFG_USE_AIRSPEED_PID) {
+        //if (!(staticParams.bitConfig & CFG_USE_AIRSPEED_PID)) {
                 return pid;
-        }
-        uint16_t result = (pid * 10) / airspeedVelocity;
-        if (result > 240 || airspeedVelocity == 0) {
-                result = 240;
+        //}
-        }
-        return result;
+                        }
 void setAirspeedPIDs(void) {
         for (uint8_t axis = 0; axis<3; axis++) {
                 airspeedPID[axis].P = calcAirspeedPID(dynamicParams.gyroPID[axis].P);
                 airspeedPID[axis].I = calcAirspeedPID(dynamicParams.gyroPID[axis].I); // Should this be???
                 airspeedPID[axis].D = dynamicParams.gyroPID[axis].D;
+        }
+}
+#define LOG_STICK_SCALE 8
+#define LOG_P_SCALE 6
 #define LOG_I_SCALE 10
 #define LOG_D_SCALE 6
 /************************************************************************/
 /*  Main Flight Control                                                 */
         setAirspeedPIDs();
         term[CONTROL_THROTTLE] = controls[CONTROL_THROTTLE];
+        // These params are just left the same in all modes. In MANUAL and RATE the results are ignored anyway.
+        int32_t tmp;
+        tmp = ((int32_t)controls[CONTROL_ELEVATOR] * staticParams.stickIElevator) >> LOG_STICK_SCALE;
-        // These params are just left the same in all modes. In MANUAL and RATE the results are ignored anyway.
+        if (reverse[PITCH]) target[PITCH] += tmp; else target[PITCH] -= tmp;
+        tmp = ((int32_t)controls[CONTROL_AILERONS] * staticParams.stickIAilerons) >> LOG_STICK_SCALE;
-        target[PITCH] += (controls[CONTROL_ELEVATOR] * staticParams.stickIElevator) >> 7;
+        if (reverse[ROLL]) target[ROLL] += tmp; else target[ROLL] -= tmp;
-        target[ROLL] += (controls[CONTROL_AILERONS] * staticParams.stickIAilerons) >> 7;
+        tmp = ((int32_t)controls[CONTROL_RUDDER] * staticParams.stickIRudder) >> LOG_STICK_SCALE;
-        target[YAW] += (controls[CONTROL_RUDDER] * staticParams.stickIRudder) >> 7;
+        if (reverse[YAW]) target[YAW] += tmp; else target[YAW] -= tmp;
         for (axis = PITCH; axis <= YAW; axis++) {
                 if (target[axis] > OVER180) {
                         target[axis] -= OVER360;
-                } else if (target[axis] <= -OVER180) {
-                        target[axis] += OVER360;
+                } else if (target[axis] <= -OVER180) {
-                }
+                        target[axis] += OVER360;
+                }
+                error[axis] = attitude[axis] - target[axis];
+#define ROTATETARGET 1
+// #define TRUNCATEERROR 1
+#ifdef ROTATETARGET
+                if(abs(error[axis]) > OVER180) {
+                  // The shortest way from attitude to target crosses -180.
+                  // Well there are 2 possibilities: A is >0 and T is < 0, that makes E a (too) large positive number. It should be wrapped to negative.
+                  // Or A is <0 and T is >0, that makes E a (too) large negative number. It should be wrapped to positive.
+                  if (error[axis] > 0) {
+                    if (error[axis] < OVER360 - maxError[axis]) {
+                      // too much err.
+                      error[axis] = -maxError[axis];
+                      target[axis] = attitude[axis] + maxError[axis];
+                      if (target[axis] > OVER180) target[axis] -= OVER360;
+                    } else {
+                      // Normal case, we just need to correct for the wrap. Error will be negative.
+                      error[axis] -= OVER360;
+                    }
+                  } else {
+            if (error[axis] > maxError[axis] - OVER360) {
+              // too much err.
+              error[axis] = maxError[axis];
+              target[axis] = attitude[axis] - maxError[axis];
+              if (target[axis] < -OVER180) target[axis] += OVER360;
+            } else {
+              // Normal case, we just need to correct for the wrap. Error will be negative.
+              error[axis] += OVER360;
+            }
+                  }
+                } else {
+                  // Simple case, linear range.
+                if (error[axis] > maxError[axis]) {
+                  error[axis] = maxError[axis];
+                  target[axis] = attitude[axis] - maxError[axis];
+                } else if (error[axis] < -maxError[axis]) {
+              error[axis] = -maxError[axis];
-                if (reverse[axis])
+              target[axis] = attitude[axis] + maxError[axis];
-                        error[axis] = attitude[axis] + target[axis];
+            }
                 else
-                        error[axis] = attitude[axis] - target[axis];
+#endif
+#ifdef TUNCATEERROR
+                if (error[axis] > maxError[axis]) {
                   error[axis] = maxError[axis];
-                if (error[axis] > maxError[axis]) {
-                        error[axis] = maxError[axis];
-                } else if (error[axis] < -maxError[axis]) {
-                        error[axis] =- maxError[axis];
+                } else if (error[axis] < -maxError[axis]) {
                   error[axis] = -maxError[axis];
                 } else {
-#define LOG_P_SCALE 6
+                        // update I parts here for angles mode. I parts in rate mode is something different.
 #define LOG_I_SCALE 6
                                 == FLIGHT_MODE_RATE) {
                         PDPart[axis] = (((int32_t) gyro_PID[axis]
                                         * (int16_t) airspeedPID[axis].P) >> LOG_P_SCALE)
                                         + ((gyroD[axis] * (int16_t) airspeedPID[axis].D)
                                                         >> LOG_D_SCALE);
-                        if (reverse[axis])
-                                PDPart[axis] = -PDPart[axis];
                 } else {
                         PDPart[axis] = 0;
+                }
-                if (currentFlightMode == FLIGHT_MODE_ANGLES) {
-                        int16_t anglePart = (int32_t)(
+                if (currentFlightMode == FLIGHT_MODE_ANGLES) {
-                                        error[axis] * (int32_t) airspeedPID[axis].I)
-                                        >> LOG_I_SCALE;
-                        if (reverse[axis])
+                        int16_t anglePart = (int32_t)(error[axis] * (int32_t) airspeedPID[axis].I) >> LOG_I_SCALE;
-                                PDPart[axis] += anglePart;
-                        else
-                                PDPart[axis] -= anglePart;
+                        PDPart[axis] += anglePart;
-                }
-                // Add I parts here... these are integrated errors.
+                }
+                // Add I parts here... these are integrated errors.
+                if (reverse[axis])
-                // When an error wraps, actually its I part should be negated or something...
+                  term[axis] = controls[axis] - PDPart[axis]; // + IPart[axis];
                 else
-                term[axis] = controls[axis] + PDPart[axis] + IPart[axis];
+                  term[axis] = controls[axis] + PDPart[axis]; // + IPart[axis];
+        }
         debugOut.analog[12] = term[PITCH];
         debugOut.analog[13] = term[ROLL];
         debugOut.analog[14] = term[YAW];
         debugOut.analog[15] = term[THROTTLE];
         for (uint8_t i = 0; i < NUM_CONTROL_SERVOS; i++) {
                 int16_t tmp;
                 if (servoTestActive) {
-                        controlServos[i] = ((int16_t) servoTest[i] - 128) * 4;
+                        controlServos[i] = ((int16_t) servoTest[i] - 128) * 8;
                 } else {

Subversion Repositories FlightCtrl

(root)/branches/dongfang_FC_fixedwing/arduino_atmega328/flight.c @ 2135 - Rev 2109 → 2132