WebSVN - FlightCtrl - Diff - Rev 1960 and 1961 - /branches/dongfang_FC

 /*
  * 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;
+  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;
+  return lastCommand;
+}
 uint8_t controlMixer_isCommandRepeated(void) {
-        return isCommandRepeated;
+  return isCommandRepeated;
+}
+void controlMixer_setNeutral() {
-}
+  for (uint8_t i=0; i<VARIABLE_COUNT; i++) {
+    variables[i] = 0;
 void controlMixer_setNeutral() {
-        EC_setNeutral();
+  EC_setNeutral();
-        HC_setGround();
+  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;
+  uint8_t i;
-        for (i = 0; i < 8; i++) {
+  for (i=0; i < VARIABLE_COUNT; i++) {
-                variables[i] = RC_getVariable(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;
+  uint8_t i;
-        int16_t targetvalue;
+  int16_t targetvalue;
-        for (i = 0; i < 8; i++) {
+  for (i=0; i < VARIABLE_COUNT; i++) {
-                targetvalue = RC_getVariable(i);
+    targetvalue = RC_getVariable(i);
-                if (targetvalue < 0)
+    if (targetvalue < 0)
-                        targetvalue = 0;
+      targetvalue = 0;
-                if (variables[i] < targetvalue && variables[i] < 255)
+    if (variables[i] < targetvalue && variables[i] < 255)
-                        variables[i]++;
+      variables[i]++;
-                else if (variables[i] > 0 && variables[i] > targetvalue)
+    else if (variables[i] > 0 && variables[i] > targetvalue)
-                        variables[i]--;
+      variables[i]--;
-        }
+  }
 }
 uint8_t controlMixer_getSignalQuality(void) {
-        uint8_t rcQ = RC_getSignalQuality();
+  uint8_t rcQ = RC_getSignalQuality();
-        uint8_t ecQ = EC_getSignalQuality();
+  uint8_t ecQ = EC_getSignalQuality();
-        // This needs not be the only correct solution...
+  // This needs not be the only correct solution...
-        return rcQ > ecQ ? rcQ : ecQ;
+  return rcQ > ecQ ? rcQ : ecQ;
 }
 void updateControlAndMeasureControlActivity(uint8_t index, int16_t newValue) {
-    int16_t tmp = controls[index];
+  int16_t tmp = controls[index];
-    controls[index] = newValue;
+  controls[index] = newValue;
-    tmp -= newValue;
+  tmp -= newValue;
-    tmp /= 2;
+  tmp /= 2;
-    tmp = tmp * tmp;
+  tmp = tmp * tmp;
-    // tmp += (newValue >= 0) ? newValue : -newValue;
+  // tmp += (newValue >= 0) ? newValue : -newValue;
-    if (controlActivity + (uint16_t)tmp >= controlActivity)
+  if (controlActivity + (uint16_t)tmp >= controlActivity)
-      controlActivity += tmp;
+    controlActivity += tmp;
-    else controlActivity = 0xffff;
+  else controlActivity = 0xffff;
+}
 #define CADAMPING 10
 void dampenControlActivity(void) {
-    uint32_t tmp = controlActivity;
+  uint32_t tmp = controlActivity;
-    tmp *= ((1<<CADAMPING)-1);
+  tmp *= ((1<<CADAMPING)-1);
-    tmp >>= CADAMPING;
+  tmp >>= CADAMPING;
-    controlActivity = tmp;
+  controlActivity = tmp;
 }
 /*
  * Update the variables indicating stick position from the sum of R/C, GPS and external control.
  */
 void controlMixer_update(void) {
-        // calculate Stick inputs by rc channels (P) and changing of rc channels (D)
+  // calculate Stick inputs by rc channels (P) and changing of rc channels (D)
-        // TODO: If no signal --> zero.
+  // TODO: If no signal --> zero.
-        int16_t tempThrottle;
+  int16_t tempThrottle;
-        // takes almost no time...
+  // takes almost no time...
-        RC_update();
+  RC_update();
-        // takes almost no time...
+  // takes almost no time...
-        EC_update();
+  EC_update();
-        // takes about 80 usec.
+  // takes about 80 usec.
-        HC_update();
+  HC_update();
-    int16_t* RC_PRTY = RC_getPRTY();
+  int16_t* RC_PRTY = RC_getPRTY();
-    int16_t* EC_PRTY = EC_getPRTY();
+  int16_t* EC_PRTY = EC_getPRTY();
-    updateControlAndMeasureControlActivity(CONTROL_PITCH, RC_PRTY[CONTROL_PITCH] + EC_PRTY[CONTROL_PITCH]);
+  updateControlAndMeasureControlActivity(CONTROL_PITCH, RC_PRTY[CONTROL_PITCH] + EC_PRTY[CONTROL_PITCH]);
-    updateControlAndMeasureControlActivity(CONTROL_ROLL, RC_PRTY[CONTROL_ROLL] + EC_PRTY[CONTROL_ROLL]);
+  updateControlAndMeasureControlActivity(CONTROL_ROLL, RC_PRTY[CONTROL_ROLL] + EC_PRTY[CONTROL_ROLL]);
-    updateControlAndMeasureControlActivity(CONTROL_YAW, RC_PRTY[CONTROL_YAW] + EC_PRTY[CONTROL_YAW]);
+  updateControlAndMeasureControlActivity(CONTROL_YAW, RC_PRTY[CONTROL_YAW] + EC_PRTY[CONTROL_YAW]);
-    dampenControlActivity();
+  dampenControlActivity();
-    //debugOut.analog[14] = controlActivity/10;
+  //debugOut.analog[14] = controlActivity/10;
-        tempThrottle = HC_getThrottle(RC_PRTY[CONTROL_THROTTLE] + EC_PRTY[CONTROL_THROTTLE]);
+  tempThrottle = HC_getThrottle(RC_PRTY[CONTROL_THROTTLE] + EC_PRTY[CONTROL_THROTTLE]);
-        controls[CONTROL_THROTTLE] = AC_getThrottle(tempThrottle);
+  controls[CONTROL_THROTTLE] = AC_getThrottle(tempThrottle);
-        // controlYaw = RC_PRTY[CONTROL_YAW] + EC_PRTY[CONTROL_YAW];
+  // controlYaw = RC_PRTY[CONTROL_YAW] + EC_PRTY[CONTROL_YAW];
-        if (controlMixer_getSignalQuality() >= SIGNAL_GOOD) {
+  if (controlMixer_getSignalQuality() >= SIGNAL_GOOD) {
-          controlMixer_updateVariables();
+    controlMixer_updateVariables();
-          configuration_applyVariablesToParams();
+    configuration_applyVariablesToParams();
-          //looping = RC_getLooping(looping);
+    //looping = RC_getLooping(looping);
-        } else { // Signal is not OK
+  } else { // Signal is not OK
-                // Could handle switch to emergency flight here.
+    // Could handle switch to emergency flight here.
-                // throttle is handled elsewhere.
+    // throttle is handled elsewhere.
-                // looping = 0;
+    // looping = 0;
-        }
+  }
-        // part1a end.
+  // part1a end.
-        /* This is not really necessary with the dead-band feature on all sticks (see rc.c)
+  /* 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(staticParams.GlobalConfig & (CFG_COMPASS_ACTIVE | CFG_GPS_ACTIVE)) {
-         if (controlYaw > 2) controlYaw-= 2;
+     if (controlYaw > 2) controlYaw-= 2;
-         else if (controlYaw< -2) controlYaw += 2;
+     else if (controlYaw< -2) controlYaw += 2;
-         else controlYaw = 0;
+     else controlYaw = 0;
-         }
+     }
-         */
+  */
-        /*
+  /*
-         * Record maxima
+   * Record maxima
-        for (axis = PITCH; axis <= ROLL; axis++) {
+   for (axis = PITCH; axis <= ROLL; axis++) {
-                if (abs(control[axis] / CONTROL_SCALING) > maxControl[axis]) {
+   if (abs(control[axis] / CONTROL_SCALING) > maxControl[axis]) {
-                        maxControl[axis] = abs(control[axis]) / CONTROL_SCALING;
+   maxControl[axis] = abs(control[axis]) / CONTROL_SCALING;
-                        if (maxControl[axis] > 100)
+   if (maxControl[axis] > 100)
-                                maxControl[axis] = 100;
+   maxControl[axis] = 100;
-                } else if (maxControl[axis])
+   } else if (maxControl[axis])
-                        maxControl[axis]--;
+   maxControl[axis]--;
-        }
+   }
-    */
+  */
-        uint8_t rcCommand = (RC_getSignalQuality() >= SIGNAL_OK) ? RC_getCommand()
+  uint8_t rcCommand = (RC_getSignalQuality() >= SIGNAL_OK) ? RC_getCommand()
-                        : COMMAND_NONE;
+    : COMMAND_NONE;
-        uint8_t ecCommand = (EC_getSignalQuality() >= SIGNAL_OK) ? EC_getCommand()
+  uint8_t ecCommand = (EC_getSignalQuality() >= SIGNAL_OK) ? EC_getCommand()
-                        : COMMAND_NONE;
+    : COMMAND_NONE;
-        if (rcCommand != COMMAND_NONE) {
+  if (rcCommand != COMMAND_NONE) {
-                isCommandRepeated = (lastCommand == rcCommand);
+    isCommandRepeated = (lastCommand == rcCommand);
-                lastCommand = rcCommand;
+    lastCommand = rcCommand;
-                lastArgument = RC_getArgument();
+    lastArgument = RC_getArgument();
-        } else if (ecCommand != COMMAND_NONE) {
+  } else if (ecCommand != COMMAND_NONE) {
-                isCommandRepeated = (lastCommand == ecCommand);
+    isCommandRepeated = (lastCommand == ecCommand);
-                lastCommand = ecCommand;
+    lastCommand = ecCommand;
-                lastArgument = EC_getArgument();
+    lastArgument = EC_getArgument();
-        } else {
+  } else {
-                // Both sources have no command, or one or both are out.
+    // 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.
+    // Just set to false. There is no reason to check if the none-command was repeated anyway.
-                isCommandRepeated = 0;
+    isCommandRepeated = 0;
-                lastCommand = COMMAND_NONE;
+    lastCommand = COMMAND_NONE;
-        }
+  }
-        /*
+  /*
-        if (isCommandRepeated)
+    if (isCommandRepeated)
-                DebugOut.Digital[0] |= DEBUG_COMMANDREPEATED;
+    DebugOut.Digital[0] |= DEBUG_COMMANDREPEATED;
-        else
+    else
-                DebugOut.Digital[0] &= ~DEBUG_COMMANDREPEATED;
+    DebugOut.Digital[0] &= ~DEBUG_COMMANDREPEATED;
-        if (rcCommand)
+    if (rcCommand)

Subversion Repositories FlightCtrl

(root)/branches/dongfang_FC_rewrite/controlMixer.c @ 1887 - Rev 1960 → 1961