WebSVN - FlightCtrl - Diff - Rev 2103 and 2104 - /branches/dongfang_FC

 #include "flight.h"
 #include "output.h"
 // Necessary for external control and motor test
-#include "uart0.h"
 #include "uart0.h"
 #include "timer2.h"
 #include "analog.h"
 #include "attitude.h"
+#include "controlMixer.h"
-#include "controlMixer.h"
+#include "configuration.h"
 #define CHECK_MIN_MAX(value, min, max) {if(value < min) value = min; else if(value > max) value = max;}
 /*
  * Error integrals.
  */
 int32_t error[3];
-uint8_t pFactor[3];
-uint8_t dFactor[3];
 uint8_t iFactor[3];
+uint8_t reverse[3];
-uint8_t reverse[3];
+int32_t maxError[3];
+int32_t IPart[3] = { 0, 0, 0 };
-int32_t IPart[3] = { 0, 0, 0 };
+PID_t airspeedPID[3];
-int16_t controlServos[MAX_CONTROL_SERVOS];
+int16_t controlServos[NUM_CONTROL_SERVOS];
 /************************************************************************/
         target[PITCH] = attitude[PITCH];
         target[ROLL] = attitude[ROLL];
         target[YAW] = attitude[YAW];
+}
-// this should be followed by a call to switchToFlightMode!!
-void flight_updateFlightParametersToFlightMode(uint8_t flightMode) {
+void flight_updateFlightParametersToFlightMode(void) {
-        debugOut.analog[16] = flightMode;
+        debugOut.analog[16] = currentFlightMode;
-        reverse[PITCH] = staticParams.controlServosReverse
-                        & CONTROL_SERVO_REVERSE_ELEVATOR;
+        reverse[PITCH] = staticParams.controlServosReverse & CONTROL_SERVO_REVERSE_ELEVATOR;
-        reverse[ROLL] = staticParams.controlServosReverse
-                        & CONTROL_SERVO_REVERSE_AILERONS;
+        reverse[ROLL] = staticParams.controlServosReverse & CONTROL_SERVO_REVERSE_AILERONS;
-        reverse[YAW] = staticParams.controlServosReverse
-                        & CONTROL_SERVO_REVERSE_RUDDER;
+        reverse[YAW] = staticParams.controlServosReverse & CONTROL_SERVO_REVERSE_RUDDER;
+        // At a switch to angles, we want to kill errors first.
-        for (uint8_t i = 0; i < 3; i++) {
+        // This should be triggered only once per mode change!
-                if (flightMode == FLIGHT_MODE_MANUAL) {
-                        pFactor[i] = 0;
+        if (currentFlightMode == FLIGHT_MODE_ANGLES) {
-                        dFactor[i] = 0;
-                } else if(flightMode == FLIGHT_MODE_RATE || flightMode == FLIGHT_MODE_ANGLES) {
+                target[PITCH] = attitude[PITCH];
-                        pFactor[i] = staticParams.gyroPID[i].P;
+                target[ROLL] = attitude[ROLL];
-                        dFactor[i] = staticParams.gyroPID[i].D;
+                target[YAW] = attitude[YAW];
-                }
+        }
+        for (uint8_t axis=0; axis<3; axis++) {
+                maxError[axis] = (int32_t)staticParams.gyroPID[axis].iMax * GYRO_DEG_FACTOR;
+        }
+}
-                if (flightMode == FLIGHT_MODE_ANGLES) {
+// Normal at airspeed = 10.
+uint8_t calcAirspeedPID(uint8_t pid) {
+        if (staticParams.bitConfig & CFG_USE_AIRSPEED_PID) {
+                return pid;
+        }
+        uint16_t result = (pid * 10) / airspeed;
-                        iFactor[i] = staticParams.gyroPID[i].I;
-                } else if(flightMode == FLIGHT_MODE_RATE || flightMode == FLIGHT_MODE_MANUAL) {
+        if (result > 240 || airspeed == 0) {
+                result = 240;
+        }
+        return result;
+}
+void setAirspeedPIDs(void) {
+        for (uint8_t axis = 0; axis<3; axis++) {
+                airspeedPID[axis].P = calcAirspeedPID(dynamicParams.gyroPID[axis].P);
-                        iFactor[i] = 0;
+                airspeedPID[axis].I = calcAirspeedPID(dynamicParams.gyroPID[axis].I); // Should this be???
                 airspeedPID[axis].D = dynamicParams.gyroPID[axis].D;
+        }
+}
         // High resolution motor values for smoothing of PID motor outputs
         // static int16_t outputFilters[MAX_OUTPUTS];
-        uint8_t axis;
-        // TODO: Check modern version.
-        // calculateFlightAttitude();
+        uint8_t axis;
-        // TODO: Check modern version.
+        setAirspeedPIDs();
         // controlMixer_update();
         term[CONTROL_THROTTLE] = controls[CONTROL_THROTTLE];
         for (axis = PITCH; axis <= YAW; axis++) {
                 if (target[axis] > OVER180) {
                         target[axis] -= OVER360;
                 } else if (target[axis] <= -OVER180) {
-                  target[axis] += OVER360;
+                        target[axis] += OVER360;
+                }
                 if (reverse[axis])
                         error[axis] = attitude[axis] + target[axis];
+                else
-                else
+                        error[axis] = attitude[axis] - target[axis];
-                        error[axis] = attitude[axis] - target[axis];
-                if (error[axis] > OVER180) {
+                if (error[axis] > maxError[axis]) {
-                        error[axis] -= OVER360;
+                        error[axis] = maxError[axis];
-                } else if (error[axis] <= -OVER180) {
+                } else if (error[axis] < -maxError[axis]) {
+                        error[axis] =- maxError[axis];
+                }
+#define LOG_P_SCALE 6
-                        error[axis] += OVER360;
+#define LOG_I_SCALE 6
 #define LOG_D_SCALE 4
                 /************************************************************************/
                 /* Calculate control feedback from angle (gyro integral)                */
+                /* and angular velocity (gyro signal)                                   */
-                /* and angular velocity (gyro signal)                                   */
+                /************************************************************************/
-                /************************************************************************/
+                if (currentFlightMode == FLIGHT_MODE_ANGLES || currentFlightMode
-                PDPart[axis] = (((int32_t) gyro_PID[axis] * pFactor[axis]) >> 6)
+                                == FLIGHT_MODE_RATE) {
-                                + ((gyroD[axis] * (int16_t) dFactor[axis]) >> 4);
-                if (reverse[axis])
+                        PDPart[axis] = (((int32_t) gyro_PID[axis]
-                        PDPart[axis] = -PDPart[axis];
+                                        * (int16_t) airspeedPID[axis].P) >> LOG_P_SCALE)
+                                        + ((gyroD[axis] * (int16_t) airspeedPID[axis].D)
-                int16_t anglePart = (error[axis] * iFactor[axis]) >> 10;
+                                                        >> LOG_D_SCALE);
-                if (reverse[axis])
+                        if (reverse[axis])
+                                PDPart[axis] = -PDPart[axis];
+                } else {
+                        PDPart[axis] = 0;
+                }
+                if (currentFlightMode == FLIGHT_MODE_ANGLES) {
+                        int16_t anglePart = (int32_t)(
+                                        error[axis] * (int32_t) airspeedPID[axis].I)
+                                        >> LOG_I_SCALE;
+                        if (reverse[axis])
-                        PDPart[axis] += anglePart;
+                                PDPart[axis] += anglePart;
-                else
+                        else
-                        PDPart[axis] -= anglePart;
+                                PDPart[axis] -= anglePart;
+                }
         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 < MAX_CONTROL_SERVOS; i++) {
+        for (uint8_t i = 0; i < NUM_CONTROL_SERVOS; i++) {
                 int16_t tmp;
                 if (servoTestActive) {
                         controlServos[i] = ((int16_t) servoTest[i] - 128) * 4;
                 } else {

Subversion Repositories FlightCtrl

(root)/branches/dongfang_FC_fixedwing/flight.c - Rev 2103 → 2104