WebSVN - FlightCtrl - Diff - Rev 1870 and 1872 - /branches/dongfang_FC

  * 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;
-uint8_t gyroPFactor, gyroIFactor;       // the PD factors for the attitude control
+uint8_t gyroPFactor, gyroIFactor; // the PD factors for the attitude control
 uint8_t yawPFactor, yawIFactor; // the PD factors for the yaw control
 // Some integral weight constant...
 uint16_t Ki = 10300 / 33;
 uint8_t RequiredMotors = 0;
 /************************************************************************/
 /*  Filter for motor value smoothing (necessary???)                     */
 /************************************************************************/
 int16_t motorFilter(int16_t newvalue, int16_t oldvalue) {
-  switch(dynamicParams.UserParams[5]) {
+  switch (dynamicParams.UserParams[5]) {
   case 0:
     return newvalue;
   case 1:
     return (oldvalue + newvalue) / 2;
   case 2:
-    if(newvalue > oldvalue)
+    if (newvalue > oldvalue)
-      return (1 * (int16_t)oldvalue + newvalue) / 2;  //mean of old and new
+      return (1 * (int16_t) oldvalue + newvalue) / 2; //mean of old and new
     else
       return newvalue - (oldvalue - newvalue) * 1; // 2 * new - old
   case 3:
-    if(newvalue < oldvalue)
+    if (newvalue < oldvalue)
+      return (1 * (int16_t) oldvalue + newvalue) / 2; //mean of old and new
-      return (1 * (int16_t)oldvalue + newvalue) / 2;  //mean of old and new
+    else
-    else
+      return newvalue - (oldvalue - newvalue) * 1; // 2 * new - old
-      return newvalue - (oldvalue - newvalue) * 1; // 2 * new - old
+  default:
-  default: return newvalue;
+    return newvalue;
+  }
   dynamicParams.KalmanMaxDrift = 0;
   dynamicParams.KalmanMaxFusion = 32;
   controlMixer_initVariables();
+}
+void setFlightParameters(uint8_t _Ki, uint8_t _gyroPFactor,
-void setFlightParameters(uint8_t _Ki, uint8_t _gyroPFactor, uint8_t _gyroIFactor, uint8_t _yawPFactor, uint8_t _yawIFactor) {
+    uint8_t _gyroIFactor, uint8_t _yawPFactor, uint8_t _yawIFactor) {
   Ki = 10300 / _Ki;
   gyroPFactor = _gyroPFactor;
   gyroIFactor = _gyroIFactor;
   yawPFactor = _yawPFactor;
   yawIFactor = _yawIFactor;
+}
-void setNormalFlightParameters(void) {
-  setFlightParameters(dynamicParams.IFactor + 1,
+void setNormalFlightParameters(void) {
-                      dynamicParams.GyroP + 10,
-                      staticParams.GlobalConfig & CFG_HEADING_HOLD ? 0 : dynamicParams.GyroI,
+  setFlightParameters(dynamicParams.IFactor + 1, dynamicParams.GyroP + 10,
-                      dynamicParams.GyroP + 10,
-                      dynamicParams.UserParams[6]
+      staticParams.GlobalConfig & CFG_HEADING_HOLD ? 0 : dynamicParams.GyroI,
-                      );
+      dynamicParams.GyroP + 10, dynamicParams.UserParams[6]);
+}
-void setStableFlightParameters(void) {
-  setFlightParameters(33, 90, 120, 90, 120);
+void setStableFlightParameters(void) {
   setFlightParameters(33, 90, 120, 90, 120);
+}
 /************************************************************************/
 /*  Main Flight Control                                                 */
 /************************************************************************/
 void flight_control(void) {
   int16_t tmp_int;
-    // Mixer Fractions that are combined for Motor Control
+  // Mixer Fractions that are combined for Motor Control
   int16_t yawTerm, throttleTerm, term[2];
   // PID controller variables
   int16_t PDPart[2], PDPartYaw, PPart[2];
   uint8_t i, axis;
   // Fire the main flight attitude calculation, including integration of angles.
   // We want that to kick as early as possible, not to delay new AD sampling further.
-  calculateFlightAttitude();
   calculateFlightAttitude();
-  controlMixer_update();
   controlMixer_update();
+  throttleTerm = controlThrottle;
   throttleTerm = controlThrottle;
   // This check removed. Is done on a per-motor basis, after output matrix multiplication.
+  if (throttleTerm < staticParams.MinThrottle + 10)
-  if(throttleTerm < staticParams.MinThrottle + 10) throttleTerm = staticParams.MinThrottle + 10;
+    throttleTerm = staticParams.MinThrottle + 10;
+  else if (throttleTerm > staticParams.MaxThrottle - 20)
-  else if(throttleTerm > staticParams.MaxThrottle - 20) throttleTerm = (staticParams.MaxThrottle - 20);
+    throttleTerm = (staticParams.MaxThrottle - 20);
   /************************************************************************/
   /* RC-signal is bad                                                     */
   /* This part could be abstracted, as having yet another control input   */
   /* to the control mixer: An emergency autopilot control.                */
   /************************************************************************/
-  if(controlMixer_getSignalQuality() <= SIGNAL_BAD) {           // the rc-frame signal is not reveived or noisy
+  if (controlMixer_getSignalQuality() <= SIGNAL_BAD) { // the rc-frame signal is not reveived or noisy
     RED_ON;
     beepRCAlarm();
-    if(emergencyFlightTime) {
+    if (emergencyFlightTime) {
       // continue emergency flight
       emergencyFlightTime--;
-      if(isFlying > 256) {
+      if (isFlying > 256) {
         // We're probably still flying. Descend slowly.
-        throttleTerm = staticParams.EmergencyGas;  // Set emergency throttle
+        throttleTerm = staticParams.EmergencyGas; // Set emergency throttle
-        MKFlags |= (MKFLAG_EMERGENCY_LANDING);     // Set flag for emergency landing
+        MKFlags |= (MKFLAG_EMERGENCY_LANDING); // Set flag for emergency landing
         setStableFlightParameters();
       } else {
-        MKFlags &= ~(MKFLAG_MOTOR_RUN);            // Probably not flying, and bad R/C signal. Kill motors.
+        MKFlags &= ~(MKFLAG_MOTOR_RUN); // Probably not flying, and bad R/C signal. Kill motors.
+      }
     } else {
       // end emergency flight (just cut the motors???)
       MKFlags &= ~(MKFLAG_MOTOR_RUN | MKFLAG_EMERGENCY_LANDING);
     }
   } else {
     // signal is acceptable
-    if(controlMixer_getSignalQuality() > SIGNAL_BAD) {
+    if (controlMixer_getSignalQuality() > SIGNAL_BAD) {
       // Reset emergency landing control variables.
-      MKFlags &= ~(MKFLAG_EMERGENCY_LANDING);  // clear flag for emergency landing
+      MKFlags &= ~(MKFLAG_EMERGENCY_LANDING); // clear flag for emergency landing
       // The time is in whole seconds.
-      emergencyFlightTime = (uint16_t)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 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)) {
-    if(throttleTerm > 40 && (MKFlags & MKFLAG_MOTOR_RUN)) {
+      // increment flight-time counter until overflow.
-      // increment flight-time counter until overflow.
+      if (isFlying != 0xFFFF)
-      if(isFlying != 0xFFFF) isFlying++;
+        isFlying++;
     } else
-      /*
+    /*
-       * When standing on the ground, do not apply I controls and zero the yaw stick.
+     * 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
+     * Probably to avoid integration effects that will cause the copter to spin
-       * or flip when taking off.
+     * or flip when taking off.
-       */
+     */
-      if(isFlying < 256) {
+    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.
+      PDPartYaw = 0; // instead.
-        if(isFlying == 250) {
-          // HC_setGround();
-          updateCompassCourse = 1;
-          yawAngleDiff = 0;
-        }
-      } else {
+      if (isFlying == 250) {
+        // HC_setGround();
+        updateCompassCourse = 1;
+        yawAngleDiff = 0;
+      }
+    } else {
-            // Set fly flag. TODO: Hmmm what can we trust - the isFlying counter or the flag?
+      // 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.
+      // Answer: The counter. The flag is not read from anywhere anyway... except the NC maybe.
-            MKFlags |= (MKFLAG_FLY);
+      MKFlags |= (MKFLAG_FLY);
+    }
   /*
    * 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.
    * This is the throttle part.
    */
-  if(looping) {
+  if (looping) {
-    if(throttleTerm > staticParams.LoopGasLimit) throttleTerm = staticParams.LoopGasLimit;
+    if (throttleTerm > staticParams.LoopGasLimit)
+      throttleTerm = staticParams.LoopGasLimit;
+  }
   /************************************************************************/
   /*  Yawing                                                              */
   /************************************************************************/
-  if(abs(controlYaw) > 4 * staticParams.StickYawP) { // yaw stick is activated
+  if (abs(controlYaw) > 4 * staticParams.StickYawP) { // yaw stick is activated
     ignoreCompassTimer = 1000;
-    if(!(staticParams.GlobalConfig & CFG_COMPASS_FIX)) {
+    if (!(staticParams.GlobalConfig & CFG_COMPASS_FIX)) {
       updateCompassCourse = 1;
+    }
   }
   //  yawControlRate = controlYaw;
   // Trim drift of yawAngleDiff with controlYaw.
   // TODO: We want NO feedback of control related stuff to the attitude related stuff.
   // This seems to be used as: Difference desired <--> real heading.
   yawAngleDiff -= controlYaw;
   // limit the effect
   CHECK_MIN_MAX(yawAngleDiff, -50000, 50000);
   /************************************************************************/
   /* Compass is currently not supported.                                  */
   /************************************************************************/
-  if(staticParams.GlobalConfig & (CFG_COMPASS_ACTIVE|CFG_GPS_ACTIVE)) {
+  if (staticParams.GlobalConfig & (CFG_COMPASS_ACTIVE | CFG_GPS_ACTIVE)) {
     updateCompass();
+  }
 #if defined (USE_NAVICTRL)
   /************************************************************************/
   /* GPS is currently not supported.                                      */
+  }
 #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)                */
+  /* Calculate control feedback from angle (gyro integral)                */
-    /* and angular velocity (gyro signal)                                   */
+  /* and angular velocity (gyro signal)                                   */
-    /************************************************************************/
+  /************************************************************************/
-    // The P-part is the P of the PID controller. That's the angle integrals (not rates).
+  // The P-part is the P of the PID controller. That's the angle integrals (not rates).
-  for (axis=PITCH; axis<=ROLL; axis++) {
+  for (axis = PITCH; axis <= ROLL; axis++) {
-    if(looping & ((1<<4)<<axis)) {
+    if (looping & ((1 << 4) << axis)) {
       PPart[axis] = 0;
     } else { // TODO: Where do the 44000 come from???
       PPart[axis] = angle[axis] * gyroIFactor / (44000 / CONTROL_SCALING); // P-Part - Proportional to Integral
+    }
     /*
      * Now blend in the D-part - proportional to the Differential of the integral = the rate.
      * Read this as: PDPart = PPart + rate_PID * pfactor * CONTROL_SCALING
+     * where pfactor is in [0..1].
-     * where pfactor is in [0..1].
+     */
-     */
+    PDPart[axis] = PPart[axis] + (int32_t) ((int32_t) rate_PID[axis]
-    PDPart[axis] = PPart[axis] + (int32_t)((int32_t)rate_PID[axis] * gyroPFactor / (256L / CONTROL_SCALING))
+        * gyroPFactor / (256L / CONTROL_SCALING)) + (differential[axis]
-      + (differential[axis] * (int16_t)dynamicParams.GyroD) / 16;
+        * (int16_t) dynamicParams.GyroD) / 16;
-    CHECK_MIN_MAX(PDPart[axis], -SENSOR_LIMIT, SENSOR_LIMIT);
+    CHECK_MIN_MAX(PDPart[axis], -SENSOR_LIMIT, SENSOR_LIMIT);
   }
-  PDPartYaw =
+  PDPartYaw = (int32_t) (yawRate * 2 * (int32_t) yawPFactor) / (256L
-    (int32_t)(yawRate * 2 * (int32_t)yawPFactor) / (256L / CONTROL_SCALING)
+      / CONTROL_SCALING) + (int32_t) (yawAngleDiff * yawIFactor) / (2 * (44000
-  + (int32_t)(yawAngleDiff * yawIFactor) / (2 * (44000 / CONTROL_SCALING));
+      / CONTROL_SCALING));
   // limit control feedback
   CHECK_MIN_MAX(PDPartYaw, -SENSOR_LIMIT, SENSOR_LIMIT);
   /*
    * Compose throttle term.
    * If a Bl-Ctrl is missing, prevent takeoff.
    */
-  if(missingMotor) {
+  if (missingMotor) {
     // if we are in the lift off condition. Hmmmmmm when is throttleTerm == 0 anyway???
    */
 #define MIN_YAWGAS (40 * CONTROL_SCALING)  // yaw also below this gas value
   yawTerm = PDPartYaw - controlYaw * CONTROL_SCALING;
   // Limit yawTerm
   DebugOut.Digital[0] &= ~DEBUG_CLIP;
-  if(throttleTerm > MIN_YAWGAS) {
+  if (throttleTerm > MIN_YAWGAS) {
-    if (yawTerm < -throttleTerm/2) {
+    if (yawTerm < -throttleTerm / 2) {
       DebugOut.Digital[0] |= DEBUG_CLIP;
-      yawTerm = -throttleTerm/2;
+      yawTerm = -throttleTerm / 2;
-    } else if (yawTerm > throttleTerm/2) {
+    } else if (yawTerm > throttleTerm / 2) {
       DebugOut.Digital[0] |= DEBUG_CLIP;
-      yawTerm = throttleTerm/2;
+      yawTerm = throttleTerm / 2;
+    }
     //CHECK_MIN_MAX(yawTerm, - (throttleTerm / 2), (throttleTerm / 2));
   } else {
-    if (yawTerm < -MIN_YAWGAS/2) {
+    if (yawTerm < -MIN_YAWGAS / 2) {
       DebugOut.Digital[0] |= DEBUG_CLIP;
-      yawTerm = -MIN_YAWGAS/2;
+      yawTerm = -MIN_YAWGAS / 2;
-    } else if (yawTerm > MIN_YAWGAS/2) {
+    } else if (yawTerm > MIN_YAWGAS / 2) {
       DebugOut.Digital[0] |= DEBUG_CLIP;
-      yawTerm = MIN_YAWGAS/2;
+      yawTerm = MIN_YAWGAS / 2;
+    }
     //CHECK_MIN_MAX(yawTerm, - (MIN_YAWGAS / 2), (MIN_YAWGAS / 2));
+  }
     DebugOut.Digital[0] |= DEBUG_CLIP;
+  }
   // CHECK_MIN_MAX(yawTerm, -(tmp_int - throttleTerm), (tmp_int - throttleTerm));
   DebugOut.Digital[1] &= ~DEBUG_CLIP;
-  for (axis=PITCH; axis<=ROLL; axis++) {
+  for (axis = PITCH; axis <= ROLL; axis++) {
     /*
      * Compose pitch and roll terms. This is finally where the sticks come into play.
      */
-    if(gyroIFactor) {
+    if (gyroIFactor) {
       // Integration mode: Integrate (angle - stick) = the difference between angle and stick pos.
       // That means: Holding the stick a little forward will, at constant flight attitude, cause this to grow (decline??) over time.
       // TODO: Find out why this seems to be proportional to stick position - not integrating it at all.
       IPart[axis] += PPart[axis] - control[axis]; // Integrate difference between P part (the angle) and the stick pos.
     } 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.
     }
     tmp_int = (int32_t) ((int32_t) dynamicParams.DynamicStability
-    tmp_int = (int32_t)((int32_t) dynamicParams.DynamicStability * (int32_t) (throttleTerm + abs(yawTerm) / 2)) / 64;
+        * (int32_t) (throttleTerm + abs(yawTerm) / 2)) / 64;
     // 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
+    term[axis] = PDPart[axis] - control[axis] + IPart[axis] / Ki; // PID-controller for pitch
     } else if (term[axis] > tmp_int) {
       DebugOut.Digital[1] |= DEBUG_CLIP;
+    }
     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++) {
+  for (i = 0; i < MAX_MOTORS; i++) {
     int16_t tmp;
     if (MKFlags & MKFLAG_MOTOR_RUN && Mixer.Motor[i][MIX_THROTTLE] > 0) {
-      tmp = ((int32_t) throttleTerm * Mixer.Motor[i][MIX_THROTTLE]) / 64L;
+      tmp = throttleTerm * Mixer.Motor[i][MIX_THROTTLE];
-      tmp += ((int32_t) term[PITCH] * Mixer.Motor[i][MIX_PITCH]) / 64L;
+      tmp += term[PITCH] * Mixer.Motor[i][MIX_PITCH];
-      tmp += ((int32_t) term[ROLL] * Mixer.Motor[i][MIX_ROLL]) / 64L;
+      tmp += term[ROLL] * Mixer.Motor[i][MIX_ROLL];
+      tmp += yawTerm * Mixer.Motor[i][MIX_YAW];
-      tmp += ((int32_t) yawTerm * Mixer.Motor[i][MIX_YAW]) / 64L;
+      tmp = tmp >> 6;
       motorFilters[i] = motorFilter(tmp, motorFilters[i]);
       // Now we scale back down to a 0..255 range.
       tmp = motorFilters[i] / MOTOR_SCALING;
       // 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.
       // Apply to individual signals instead.
-      CHECK_MIN_MAX(tmp, 8, 255);
+      CHECK_MIN_MAX(tmp, 1, 255);
       motor[i].SetPoint = tmp;
-    }
-    else if (motorTestActive) {
+    } else if (motorTestActive) {
       motor[i].SetPoint = motorTest[i];
     } else {
       motor[i].SetPoint = 0;
+    }
     if (i < 4)
-      DebugOut.Analog[22+i] = motor[i].SetPoint;
+      DebugOut.Analog[22 + i] = motor[i].SetPoint;
+  }
   I2C_Start(TWI_STATE_MOTOR_TX);
   // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
   // Debugging
   // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
-  if(!(--debugDataTimer)) {
+  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[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[1] = (10 * angle[ROLL]) / GYRO_DEG_FACTOR_PITCHROLL; // in 0.1 deg
-    DebugOut.Analog[2]  = yawGyroHeading / GYRO_DEG_FACTOR_YAW;
+    DebugOut.Analog[2] = yawGyroHeading / GYRO_DEG_FACTOR_YAW;
     /*
-    DebugOut.Analog[23] = (yawRate * 2 * (int32_t)yawPFactor) / (256L / CONTROL_SCALING);
+     DebugOut.Analog[23] = (yawRate * 2 * (int32_t)yawPFactor) / (256L / CONTROL_SCALING);
-    DebugOut.Analog[24] = controlYaw;
+     DebugOut.Analog[24] = controlYaw;
-    DebugOut.Analog[25] = yawAngleDiff / 100L;
+     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];
+     DebugOut.Analog[30] = gyroNoisePeak[PITCH];
-    DebugOut.Analog[31] = gyroNoisePeak[ROLL];
+     DebugOut.Analog[31] = gyroNoisePeak[ROLL];
-    */
+     */
+  }

Subversion Repositories FlightCtrl

(root)/branches/dongfang_FC_rewrite/flight.c @ 2165 - Rev 1870 → 1872