WebSVN - FlightCtrl - Diff - Rev 1646 and 1775 - /branches/dongfang_FC

 //#include "analog.h"
 //#include "rc.h"
 // Necessary for external control and motor test
+#include "uart0.h"
+// for scope debugging
+// #include "rc.h"
 #include "uart0.h"
 #include "twimaster.h"
 #include "attitude.h"
+#include "controlMixer.h"
-#include "controlMixer.h"
+#include "commands.h"
 #ifdef USE_MK3MAG
 #include "gps.h"
 #endif
  * These are no longer maintained, just left at 0. The original implementation just summed the acc.
  * value to them every 2 ms. No filtering or anything. Just a case for an eventual overflow?? Hey???
  */
 // int16_t naviAccPitch = 0, naviAccRoll = 0, naviCntAcc = 0;
-// MK flags
-uint16_t isFlying = 0;
-volatile uint8_t MKFlags = 0;
 uint8_t gyroPFactor, gyroIFactor;       // the PD factors for the attitude control
 uint8_t yawPFactor, yawIFactor; // the PD factors for the yaw control
 /************************************************************************/
 /*  Neutral Readings                                                    */
 /************************************************************************/
-void flight_setNeutral() {
-  // GPSStickPitch = 0;
-  // GPSStickRoll = 0;
 void flight_setNeutral() {
   MKFlags |= MKFLAG_CALIBRATE;
   // not really used here any more.
   dynamicParams.KalmanK = -1;
   dynamicParams.KalmanMaxDrift = 0;
   dynamicParams.KalmanMaxFusion = 32;
-  controlMixer_initVariables();
-}
-/************************************************************************/
-/*  Transmit Motor Data via I2C                                         */
-/************************************************************************/
-void sendMotorData(void) {
-  uint8_t i;
-  if(!(MKFlags & MKFLAG_MOTOR_RUN)) {
-    // If pilot has not started the engines....
-    MKFlags &= ~(MKFLAG_FLY | MKFLAG_START); // clear flag FLY and START if motors are off
-    for(i = 0; i < MAX_MOTORS; i++) {
-      // and if we are not in motor test mode, cut throttle on all motors.
-      if(!motorTestActive) Motor[i].SetPoint = 0;
-      else                 Motor[i].SetPoint = motorTest[i];
-    }
-    if(motorTestActive) motorTestActive--;
-  }
-  /*
-  DebugOut.Analog[] = Motor[0].SetPoint; // Front
-  DebugOut.Analog[] = Motor[1].SetPoint; // Rear
-  DebugOut.Analog[] = Motor[3].SetPoint; // Left
-  DebugOut.Analog[] = Motor[2].SetPoint; // Right
-  */
   // Start I2C Interrupt Mode
-  I2C_Start(TWI_STATE_MOTOR_TX);
+  controlMixer_initVariables();
+}
 void setFlightParameters(uint8_t _Ki, uint8_t _gyroPFactor, uint8_t _gyroIFactor, uint8_t _yawPFactor, uint8_t _yawIFactor) {
 void setStableFlightParameters(void) {
   setFlightParameters(33, 90, 120, 90, 120);
-}
-void handleCommands(uint8_t command, uint8_t argument, uint8_t isCommandRepeated) {
-    if(!(MKFlags & MKFLAG_MOTOR_RUN)) {
-      if (command == COMMAND_GYROCAL && !isCommandRepeated) {
-        // Run gyro calibration but do not repeat it.
-        GRN_OFF;
-        // TODO: out of here. Anyway, MKFLAG_MOTOR_RUN is cleared. Not enough?
-        // isFlying = 0;
-        // check roll/pitch stick position
-        // if pitch stick is top or roll stick is left or right --> change parameter setting
-        // according to roll/pitch stick position
-        if (argument < 6) {
-          // Gyro calinbration, with or without selecting a new parameter-set.
-          if(argument > 0 && argument < 6) {
-            // A valid parameter-set (1..5) was chosen - use it.
-            setActiveParamSet(argument);
-          }
-          ParamSet_ReadFromEEProm(getActiveParamSet());
-          attitude_setNeutral();
-          flight_setNeutral();
-          // Right stick is centered; calibrate it to zero (hmm strictly does not belong here).
-          // If heading hold is active, do not do it. TODO: We also want to re-set old calibration.
-          controlMixer_setNeutral(!argument);
-          beepNumber(getActiveParamSet());
-        } else if(staticParams.GlobalConfig & (CFG_COMPASS_ACTIVE | CFG_GPS_ACTIVE) && argument == 7) {
-          // If right stick is centered and down
-          compassCalState = 1;
-          beep(1000);
-        }
-      }
-      // save the ACC neutral setting to eeprom
-      else  {
-        if(command == COMMAND_ACCCAL && !isCommandRepeated) {
-          // Run gyro and acc. meter calibration but do not repeat it.
-          GRN_OFF;
-          analog_calibrateAcc();
-          attitude_setNeutral();
-          flight_setNeutral();
-          controlMixer_setNeutral(1); // Calibrate right stick neutral position.
-          beepNumber(getActiveParamSet());
-        }
-      }
-    } // end !MOTOR_RUN condition.
-    if (command == COMMAND_START) {
-      isFlying = 1; // TODO: Really????
-      // if (!controlMixer_isCommandRepeated()) {
-      // attitude_startDynamicCalibration(); // Try sense the effect of the motors on sensors.
-      MKFlags |= (MKFLAG_MOTOR_RUN | MKFLAG_START); // set flag RUN and START. TODO: Is that START flag used at all???
-      // } else { // Pilot is holding stick, ever after motor start. Continue to sense the effect of the motors on sensors.
-      // attitude_continueDynamicCalibration();
-      // setPointYaw = 0;
-      // IPartPitch = 0;
-      // IPartRoll = 0;
-      // }
-    } else if (command == COMMAND_STOP) {
-      isFlying = 0;
-      MKFlags &= ~(MKFLAG_MOTOR_RUN);
+}
 /************************************************************************/
   int16_t yawTerm, throttleTerm, term[2];
   // PID controller variables
   int16_t PDPart[2], PDPartYaw, PPart[2];
   static int32_t IPart[2] = {0,0};
-  static int32_t setPointYaw = 0;
+  //  static int32_t yawControlRate = 0;
   // Removed. Too complicated, and apparently not necessary with MEMS gyros anyway.
   static int16_t motorFilters[MAX_MOTORS];
   uint8_t i, axis;
   // Fire the main flight attitude calculation, including integration of angles.
-  // Fire the main flight attitude calculation, including integration of angles.
   calculateFlightAttitude();
-  GRN_ON;
+  calculateFlightAttitude();
   /*
+   * TODO: update should: Set the stick variables if good signal, set them to zero if bad.
+   * Set variables also.
+   */
+  // start part 1: 750-800 usec.
-   * TODO: update should: Set the stick variables if good signal, set them to zero if bad.
+  // start part 1a: 750-800 usec.
+  // start part1b: 700 usec
-   * Set variables also.
+  // start part1c: 700 usec!!!!!!!!! WAY too slow.
+  controlMixer_update();
-   */
+  // end part1c
+  throttleTerm = controlThrottle;
-  controlMixer_update();
+  // This check removed. Is done on a per-motor basis, after output matrix multiplication.
   // if(throttleTerm < staticParams.MinThrottle + 10) throttleTerm = staticParams.MinThrottle + 10;
-  throttleTerm = controlThrottle;
+  // else if(throttleTerm > staticParams.MaxThrottle - 20) throttleTerm = (staticParams.MaxThrottle - 20);
   if(throttleTerm < staticParams.MinThrottle + 10) throttleTerm = staticParams.MinThrottle + 10;
   // end part1b: 700 usec.
+  /************************************************************************/
-  /************************************************************************/
+  /* RC-signal is bad                                                     */
-  /* RC-signal is bad                                                     */
+  /* This part could be abstracted, as having yet another control input   */
-  /* This part could be abstracted, as having yet another control input   */
+  /* to the control mixer: An emergency autopilot control.                */
-  /* to the control mixer: An emergency autopilot control.                */
+  /************************************************************************/
   /************************************************************************/
   if(controlMixer_getSignalQuality() <= SIGNAL_BAD) {           // the rc-frame signal is not reveived or noisy
     RED_ON;
     beepRCAlarm();
     if(emergencyFlightTime) {
       // continue emergency flight
       emergencyFlightTime--;
     // signal is acceptable
     if(controlMixer_getSignalQuality() > SIGNAL_BAD) {
       // Reset emergency landing control variables.
       MKFlags &= ~(MKFLAG_EMERGENCY_LANDING);  // clear flag for emergency landing
       // The time is in whole seconds.
-      emergencyFlightTime = staticParams.EmergencyGasDuration * 488;
+      emergencyFlightTime = (uint16_t)staticParams.EmergencyGasDuration * 488;
+    }
     // If some throttle is given, and the motor-run flag is on, increase the probability that we are flying.
     if(throttleTerm > 40 && (MKFlags & MKFLAG_MOTOR_RUN)) {
        * When standing on the ground, do not apply I controls and zero the yaw stick.
        * Probably to avoid integration effects that will cause the copter to spin
        * or flip when taking off.
        */
       if(isFlying < 256) {
-            IPart[PITCH] = IPart[ROLL] = 0;
+        IPart[PITCH] = IPart[ROLL] = 0;
-            // TODO: Don't stomp on other modules' variables!!!
+        // TODO: Don't stomp on other modules' variables!!!
-            controlYaw = 0;
+        // controlYaw = 0;
+        PDPartYaw = 0; // instead.
-            if(isFlying == 250) {
+        if(isFlying == 250) {
+          // HC_setGround();
-              updateCompassCourse = 1;
+          updateCompassCourse = 1;
-              yawAngle = 0;
+          yawAngleDiff = 0;
-              setPointYaw = 0;
+        }
       } else {
-            // DebugOut.Digital[1] = 0;
             // Set fly flag. TODO: Hmmm what can we trust - the isFlying counter or the flag?
             // Answer: The counter. The flag is not read from anywhere anyway... except the NC maybe.
             MKFlags |= (MKFLAG_FLY);
+      }
-    /*
-     * Get the current command (start/stop motors, calibrate), if any.
-     */
-    uint8_t command = controlMixer_getCommand();
-    uint8_t repeated = controlMixer_isCommandRepeated();
-    uint8_t argument = controlMixer_getArgument();
-    handleCommands(command, argument, repeated);
+        commands_handleCommands();
     // if(controlMixer_getSignalQuality() >= SIGNAL_GOOD) {
     setNormalFlightParameters();
-    // }
+    // }
   } // end else (not bad signal case)
   // end part1a: 750-800 usec.
   /*
    * Looping the H&I way basically is just a matter of turning off attitude angle measurement
    * by integration (because 300 deg/s gyros are too slow) and turning down the throttle.
     if(!(staticParams.GlobalConfig & CFG_COMPASS_FIX)) {
       updateCompassCourse = 1;
+    }
+  }
-  setPointYaw = controlYaw;
+  //  yawControlRate = controlYaw;
+  // Trim drift of yawAngleDiff with controlYaw.
-  // Trim drift of yawAngle with controlYaw.
+  // TODO: We want NO feedback of control related stuff to the attitude related stuff.
-  // TODO: We want NO feedback of control related stuff to the attitude related stuff.
+  // This seems to be used as: Difference desired <--> real heading.
-  yawAngle -= controlYaw;
+  yawAngleDiff -= controlYaw;
   // limit the effect
-  CHECK_MIN_MAX(yawAngle, -50000, 50000)
+  CHECK_MIN_MAX(yawAngleDiff, -50000, 50000);
-    /************************************************************************/
-    /* Compass is currently not supported.                                  */
+  /************************************************************************/
-    /************************************************************************/
+  /* Compass is currently not supported.                                  */
-    /*
+  /************************************************************************/
-      if(staticParams.GlobalConfig & (CFG_COMPASS_ACTIVE|CFG_GPS_ACTIVE)) {
-      updateCompass();
+  if(staticParams.GlobalConfig & (CFG_COMPASS_ACTIVE|CFG_GPS_ACTIVE)) {
     updateCompass();
-    */
+  }
 #if defined (USE_MK3MAG)
-    /************************************************************************/
-    /* GPS is currently not supported.                                      */
+  /************************************************************************/
-    /************************************************************************/
+  /* GPS is currently not supported.                                      */
-    /*
+  /************************************************************************/
-      if(staticParams.GlobalConfig & CFG_GPS_ACTIVE) {
+  if(staticParams.GlobalConfig & CFG_GPS_ACTIVE) {
-      GPS_Main();
+    GPS_Main();
-      MKFlags &= ~(MKFLAG_CALIBRATE | MKFLAG_START);
+    MKFlags &= ~(MKFLAG_CALIBRATE | MKFLAG_START);
-      }
+  }
-      else {
+  else {
-      // GPSStickPitch = 0;
-      // GPSStickRoll = 0;
+    // GPSStickPitch = 0;
-      }
+    // GPSStickRoll = 0;
+  }
-    */
+#endif
-#endif
   // end part 1: 750-800 usec.
+  // start part 3: 350 - 400 usec.
 #define SENSOR_LIMIT  (4096 * 4)
     /************************************************************************/
     /************************************************************************/
     /* Calculate control feedback from angle (gyro integral)                */
     /* and angular velocity (gyro signal)                                   */
     /************************************************************************/
     // The P-part is the P of the PID controller. That's the angle integrals (not rates).
   for (axis=PITCH; axis<=ROLL; axis++) {
-    if(looping & (1<<(4+axis))) {
+    if(looping & ((1<<4)<<axis)) {
       PPart[axis] = 0;
     PDPart[axis] = PPart[axis] + (int32_t)((int32_t)rate_PID[axis] * gyroPFactor / (256L / CONTROL_SCALING))
       + (differential[axis] * (int16_t)dynamicParams.GyroD) / 16;
     CHECK_MIN_MAX(PDPart[axis], -SENSOR_LIMIT, SENSOR_LIMIT);
   }
+  PDPartYaw =
-  PDPartYaw = (int32_t)(yawRate * 2 * (int32_t)yawPFactor) / (256L / CONTROL_SCALING)
+    (int32_t)(yawRate * 2 * (int32_t)yawPFactor) / (256L / CONTROL_SCALING)
-    + (int32_t)(yawAngle * yawIFactor) / (2 * (44000 / CONTROL_SCALING));
+  + (int32_t)(yawAngleDiff * yawIFactor) / (2 * (44000 / CONTROL_SCALING));
   // limit control feedback
-  CHECK_MIN_MAX(PDPartYaw,  -SENSOR_LIMIT, SENSOR_LIMIT);
+  CHECK_MIN_MAX(PDPartYaw, -SENSOR_LIMIT, SENSOR_LIMIT);
   /*
    * Compose throttle term.
    * If a Bl-Ctrl is missing, prevent takeoff.
    */
   if(missingMotor) {
     // if we are in the lift off condition. Hmmmmmm when is throttleTerm == 0 anyway???
-    if((isFlying > 1) && (isFlying < 50) && (throttleTerm > 0))
+    if(isFlying > 1 && isFlying < 50 && throttleTerm > 0)
-      isFlying = 1; // keep within lift off condition
-    throttleTerm = staticParams.MinThrottle; // reduce gas to min to avoid lift of
-  }
+      isFlying = 1; // keep within lift off condition
-  /*
+    throttleTerm = staticParams.MinThrottle; // reduce gas to min to avoid lift of
    * Height control was here.
    */
-  if(throttleTerm > staticParams.MaxThrottle - 20) throttleTerm = (staticParams.MaxThrottle - 20);
+  // Scale up to higher resolution. Hmm why is it not (from controlMixer and down) scaled already?
   throttleTerm *= CONTROL_SCALING;
   /*
    * Compose yaw term.
    * The yaw term is limited: Absolute value is max. = the throttle term / 2.
    * However, at low throttle the yaw term is limited to a fixed value,
    * and at high throttle it is limited by the throttle reserve (the difference
    * between current throttle and maximum throttle).
    */
 #define MIN_YAWGAS (40 * CONTROL_SCALING)  // yaw also below this gas value
-  yawTerm = PDPartYaw - setPointYaw * CONTROL_SCALING;
+  yawTerm = PDPartYaw - controlYaw * CONTROL_SCALING;
-  // limit yawTerm
+  // Limit yawTerm
   if(throttleTerm > MIN_YAWGAS) {
     CHECK_MIN_MAX(yawTerm, - (throttleTerm / 2), (throttleTerm / 2));
   } else {
     CHECK_MIN_MAX(yawTerm, - (MIN_YAWGAS / 2), (MIN_YAWGAS / 2));
     } else {
       // "HH" mode: Integrate (rate - stick) = the difference between rotation rate and stick pos.
       // To keep up with a full stick PDPart should be about 156...
       IPart[axis] += PDPart[axis] - control[axis]; // With gyroIFactor == 0, PDPart is really just a D-part. Integrate D-part (the rot. rate) and the stick pos.
+    }
     // TODO: From which planet comes the 16000?
     CHECK_MIN_MAX(IPart[axis], -(CONTROL_SCALING * 16000L), (CONTROL_SCALING * 16000L));
     // Add (P, D) parts minus stick pos. to the scaled-down I part.
     term[axis] = PDPart[axis] - control[axis] + IPart[axis] / Ki;    // PID-controller for pitch
     /*
      * Apply "dynamic stability" - that is: Limit pitch and roll terms to a growing function of throttle and yaw(!).
      * The higher the dynamic stability parameter, the wider the bounds. 64 seems to be a kind of unity
      * (max. pitch or roll term is the throttle value).
      * TODO: Why a growing function of yaw?
      */
     CHECK_MIN_MAX(term[axis], -tmp_int, tmp_int);
+  }
+  // end part 3: 350 - 400 usec.
   // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
   // Universal Mixer
   // Each (pitch, roll, throttle, yaw) term is in the range [0..255 * CONTROL_SCALING].
+  // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
+  DebugOut.Analog[12] = term[PITCH];
+  DebugOut.Analog[13] = term[ROLL];
+  DebugOut.Analog[14] = yawTerm;
+  DebugOut.Analog[15] = throttleTerm;
   // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
   for(i = 0; i < MAX_MOTORS; i++) {
     int16_t tmp;
-    if(Mixer.Motor[i][MIX_THROTTLE] > 0) { // If a motor has a zero throttle mix, it is not considered.
+    if (MKFlags & MKFLAG_MOTOR_RUN && Mixer.Motor[i][MIX_THROTTLE] > 0) {
       tmp =  ((int32_t)throttleTerm * Mixer.Motor[i][MIX_THROTTLE]) / 64L;
       tmp += ((int32_t)term[PITCH]  * Mixer.Motor[i][MIX_PITCH])    / 64L;
       tmp += ((int32_t)term[ROLL]   * Mixer.Motor[i][MIX_ROLL])     / 64L;
       tmp += ((int32_t)yawTerm      * Mixer.Motor[i][MIX_YAW])      / 64L;
+      motorFilters[i] = motorFilter(tmp, motorFilters[i]);
-      motorFilters[i] = motorFilter(tmp, motorFilters[i]);
+      // Now we scale back down to a 0..255 range.
+      tmp = motorFilters[i] / CONTROL_SCALING;
+      // So this was the THIRD time a throttle was limited. But should the limitation
+      // apply to the common throttle signal (the one used for setting the "power" of
+      // all motors together) or should it limit the throttle set for each motor,
+      // including mix components of pitch, roll and yaw? I think only the common
+      // throttle should be limited.
+      // --> WRONG. This caused motors to stall completely in tight maneuvers.
-      tmp = motorFilters[i] / CONTROL_SCALING;
+      // Apply to individual signals instead.
+      CHECK_MIN_MAX(tmp, staticParams.MinThrottle, staticParams.MaxThrottle);
-      CHECK_MIN_MAX(tmp, staticParams.MinThrottle, staticParams.MaxThrottle);
+      CHECK_MIN_MAX(tmp, 1, 255);
-      Motor[i].SetPoint = tmp;
+      motor[i].SetPoint = tmp;
+    }
+    else if (motorTestActive) {
+      motor[i].SetPoint = motorTest[i];
-    }
+    } else {
+      motor[i].SetPoint = 0;
+    }
+    if (i < 4)
-    else Motor[i].SetPoint = 0;
+      DebugOut.Analog[22+i] = motor[i].SetPoint;
+  }
-  }
+  I2C_Start(TWI_STATE_MOTOR_TX);
   // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
   // Debugging
   // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
   if(!(--debugDataTimer)) {
     debugDataTimer = 24; // update debug outputs at 488 / 24 = 20.3 Hz.
     DebugOut.Analog[0]  = (10 * angle[PITCH]) / GYRO_DEG_FACTOR_PITCHROLL; // in 0.1 deg
     DebugOut.Analog[1]  = (10 * angle[ROLL]) / GYRO_DEG_FACTOR_PITCHROLL; // in 0.1 deg
-    DebugOut.Analog[2]  = yawGyroHeading / GYRO_DEG_FACTOR_YAW;
-    // DebugOut.Analog[9]  = setPointYaw;
-    // DebugOut.Analog[10] = yawIFactor;
-    // DebugOut.Analog[11] = gyroIFactor;
-    // DebugOut.Analog[12] = RC_getVariable(0);
-    // DebugOut.Analog[13] = dynamicParams.UserParams[0];
-    // DebugOut.Analog[14] = RC_getVariable(4);
-    // DebugOut.Analog[15] = dynamicParams.UserParams[4];
+    DebugOut.Analog[2]  = yawGyroHeading / GYRO_DEG_FACTOR_YAW;
-    /* DebugOut.Analog[11] = yawGyroHeading / GYRO_DEG_FACTOR_YAW; */
-    // 12..15 are the controls.
-    // DebugOut.Analog[16] = pitchAxisAcc;
-    // DebugOut.Analog[17] = rollAxisAcc;
-    DebugOut.Analog[18] = HIRES_GYRO_INTEGRATION_FACTOR;
-    DebugOut.Analog[19] = throttleTerm;
+    /*
-    DebugOut.Analog[20] = term[PITCH];
+    DebugOut.Analog[23] = (yawRate * 2 * (int32_t)yawPFactor) / (256L / CONTROL_SCALING);
-    DebugOut.Analog[21] = term[ROLL];
-    DebugOut.Analog[22] = yawTerm;
-    DebugOut.Analog[23] = PPart[PITCH];     //
-    DebugOut.Analog[24] = IPart[PITCH] /Ki; // meget meget lille.
-    DebugOut.Analog[25] = PDPart[PITCH];    // omtrent lig ppart.
+    DebugOut.Analog[24] = controlYaw;
     DebugOut.Analog[25] = yawAngleDiff / 100L;
+    DebugOut.Analog[26] = accNoisePeak[PITCH];
-    DebugOut.Analog[26] = accNoisePeak[PITCH];
+    DebugOut.Analog[27] = accNoisePeak[ROLL];
-    DebugOut.Analog[27] = accNoisePeak[ROLL];
+    */
     DebugOut.Analog[30] = gyroNoisePeak[PITCH];

Subversion Repositories FlightCtrl

(root)/branches/dongfang_FC_rewrite/flight.c @ 1856 - Rev 1646 → 1775