WebSVN - FlightCtrl - Diff - Rev 2048 and 2049 - /branches/dongfang_FC

 #include <stdlib.h>
 #include "controlMixer.h"
 #include "rc.h"
 #include "heightControl.h"
 #include "attitudeControl.h"
 #include "externalControl.h"
 #include "compassControl.h"
 #include "naviControl.h"
 #include "configuration.h"
 #include "attitude.h"
 #include "commands.h"
 #include "output.h"
 // uint16_t maxControl[2] = { 0, 0 };
 uint16_t controlActivity = 0;
 int16_t controls[4] = { 0, 0, 0, 0 };
 // Internal variables for reading commands made with an R/C stick.
 uint8_t lastCommand = COMMAND_NONE;
 uint8_t lastArgument;
 uint8_t isCommandRepeated = 0;
 uint8_t controlMixer_didReceiveSignal = 0;
 /*
  * This could be expanded to take arguments from ohter sources than the RC
  * (read: Custom MK RC project)
  */
 uint8_t controlMixer_getArgument(void) {
   return lastArgument;
+}
 /*
  * This could be expanded to take calibrate / start / stop commands from ohter sources
  * than the R/C (read: Custom MK R/C project)
  */
 uint8_t controlMixer_getCommand(void) {
   return lastCommand;
+}
 uint8_t controlMixer_isCommandRepeated(void) {
   return isCommandRepeated;
+}
 void controlMixer_setNeutral() {
   for (uint8_t i=0; i<VARIABLE_COUNT; i++) {
     variables[i] = 0;
+  }
   EC_setNeutral();
   HC_setGround();
+}
 /*
  * Set the potientiometer values to the momentary values of the respective R/C channels.
  * No slew rate limitation.
  */
 void controlMixer_initVariables(void) {
   uint8_t i;
   for (i=0; i < VARIABLE_COUNT; i++) {
     variables[i] = RC_getVariable(i);
+  }
+}
 /*
  * Update potentiometer values with limited slew rate. Could be made faster if desired.
  * TODO: It assumes R/C as source. Not necessarily true.
  */
 void controlMixer_updateVariables(void) {
   uint8_t i;
   int16_t targetvalue;
   for (i=0; i < VARIABLE_COUNT; i++) {
     targetvalue = RC_getVariable(i);
     if (targetvalue < 0)
       targetvalue = 0;
     if (variables[i] < targetvalue && variables[i] < 255)
       variables[i]++;
     else if (variables[i] > 0 && variables[i] > targetvalue)
       variables[i]--;
+  }
+}
 uint8_t controlMixer_getSignalQuality(void) {
   uint8_t rcQ = RC_getSignalQuality();
   uint8_t ecQ = EC_getSignalQuality();
   // This needs not be the only correct solution...
   return rcQ > ecQ ? rcQ : ecQ;
+}
 void updateControlAndMeasureControlActivity(uint8_t index, int16_t newValue) {
   int16_t tmp = controls[index];
   controls[index] = newValue;
   tmp -= newValue;
   tmp /= 2;
   tmp = tmp * tmp;
   // tmp += (newValue >= 0) ? newValue : -newValue;
   /*
   if (controlActivity + (uint16_t)tmp >= controlActivity)
     controlActivity += tmp;
   else controlActivity = 0xffff;
   */
   if (controlActivity + (uint16_t)tmp < 32768)
     controlActivity += tmp;
+}
 #define CADAMPING 10
 void dampenControlActivity(void) {
   uint32_t tmp = controlActivity;
   tmp *= ((1<<CADAMPING)-1);
   tmp >>= CADAMPING;
   controlActivity = tmp;
+}
 /*
  * Update the variables indicating stick position from the sum of R/C, GPS and external control
  * and whatever other controls we invented in the meantime...
  * Update variables.
  * Decode commands but do not execute them.
  */
 int16_t tempPRTY[4];
 void controlMixer_periodicTask(void) {
+  // Decode commands.
+  uint8_t rcCommand = (RC_getSignalQuality() >= SIGNAL_OK) ? RC_getCommand()
+    : COMMAND_NONE;
-  // calculate Stick inputs by rc channels (P) and changing of rc channels (D)
+  uint8_t ecCommand = (EC_getSignalQuality() >= SIGNAL_OK) ? EC_getCommand()
+    : COMMAND_NONE;
+  // Update variables ("potis").
+  if (controlMixer_getSignalQuality() >= SIGNAL_GOOD) {
+    controlMixer_updateVariables();
+    controlMixer_didReceiveSignal = 1;
-  // TODO: If no signal --> zero.
+  } else { // Signal is not OK
+    // Could handle switch to emergency flight here.
+    // throttle is handled elsewhere.
+  }
+  if (rcCommand != COMMAND_NONE) {
+    isCommandRepeated = (lastCommand == rcCommand);
+    lastCommand = rcCommand;
+    lastArgument = RC_getArgument();
+  } else if (ecCommand != COMMAND_NONE) {
+    isCommandRepeated = (lastCommand == ecCommand);
+    lastCommand = ecCommand;
+    lastArgument = EC_getArgument();
+  } else {
+    // Both sources have no command, or one or both are out.
+    // Just set to false. There is no reason to check if the none-command was repeated anyway.
-  // HC_periodicTask();
+    isCommandRepeated = 0;
+    lastCommand = COMMAND_NONE;
+  }
   // This will init the values (not just add to them).
   RC_periodicTaskAndPRTY(tempPRTY);
   // Add external control to RC
   EC_periodicTaskAndPRTY(tempPRTY);
   // Add navigations control to RC and external control.
   navigation_periodicTaskAndPRTY(tempPRTY);
   // Add compass control (could also have been before navi, they are independent)
   CC_periodicTaskAndPRTY(tempPRTY);
   // Add height control (could also have been before navi and/or compass, they are independent)
   HC_periodicTaskAndPRTY(tempPRTY);
   // Add attitude control (could also have been before navi and/or compass, they are independent)
   AC_getPRTY(tempPRTY);
   // Commit results to global variable and also measure control activity.
   updateControlAndMeasureControlActivity(CONTROL_PITCH, tempPRTY[CONTROL_PITCH]);
   updateControlAndMeasureControlActivity(CONTROL_ROLL, tempPRTY[CONTROL_ROLL]);
   updateControlAndMeasureControlActivity(CONTROL_YAW, tempPRTY[CONTROL_YAW]);
   dampenControlActivity();
   debugOut.analog[17] = controlActivity/10;
-  // Update variables ("potis").
-  if (controlMixer_getSignalQuality() >= SIGNAL_GOOD) {
-    controlMixer_updateVariables();
-    controlMixer_didReceiveSignal = 1;
-  } else { // Signal is not OK
-    // Could handle switch to emergency flight here.
-    // throttle is handled elsewhere.
-  }
   // We can safely do this even with a bad signal - the variables will not have been updated then.
   configuration_applyVariablesToParams();
   // part1a end.
   /* This is not really necessary with the dead-band feature on all sticks (see rc.c)
      if(staticParams.GlobalConfig & (CFG_COMPASS_ACTIVE | CFG_GPS_ACTIVE)) {
      if (controlYaw > 2) controlYaw-= 2;
      else if (controlYaw< -2) controlYaw += 2;
      else controlYaw = 0;
+     }
   */
   /*
    * Record maxima. Predecessor of the control activity stuff.
    for (axis = PITCH; axis <= ROLL; axis++) {
    if (abs(control[axis] / CONTROL_SCALING) > maxControl[axis]) {
    maxControl[axis] = abs(control[axis]) / CONTROL_SCALING;
    if (maxControl[axis] > 100)
    maxControl[axis] = 100;
    } else if (maxControl[axis])
    maxControl[axis]--;
+   }
   */
-  // Decode commands.
-  uint8_t rcCommand = (RC_getSignalQuality() >= SIGNAL_OK) ? RC_getCommand()
-    : COMMAND_NONE;
-  uint8_t ecCommand = (EC_getSignalQuality() >= SIGNAL_OK) ? EC_getCommand()
-    : COMMAND_NONE;
-  if (rcCommand != COMMAND_NONE) {
-    isCommandRepeated = (lastCommand == rcCommand);
-    lastCommand = rcCommand;
-    lastArgument = RC_getArgument();
-  } else if (ecCommand != COMMAND_NONE) {
-    isCommandRepeated = (lastCommand == ecCommand);
-    lastCommand = ecCommand;
-    lastArgument = EC_getArgument();
-  } else {
-    // Both sources have no command, or one or both are out.
-    // Just set to false. There is no reason to check if the none-command was repeated anyway.
-    isCommandRepeated = 0;
-    lastCommand = COMMAND_NONE;
-  }
+ }

Subversion Repositories FlightCtrl

(root)/branches/dongfang_FC_rewrite/controlMixer.c @ 2510 - Rev 2048 → 2049