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

 // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 #include <stdlib.h>
 #include "controlMixer.h"
+#include "rc.h"
+#include "heightControl.h"
-#include "rc.h"
+#include "attitudeControl.h"
 #include "externalControl.h"
 #include "configuration.h"
 #include "attitude.h"
-#include "eeprom.h"
+#include "commands.h"
-#include "flight.h"
-/*
- * Number of cycles a command must be repeated before commit. Maybe this really belongs in RC.
- */
-#define COMMAND_TIMER 200
+#include "output.h"
+uint16_t maxControl[2] = {0,0};
-uint16_t maxControl[2] = {0,0};
+int16_t control[2] = {0,0};
-int16_t control[2] = {0,0}, controlYaw = 0, controlThrottle = 0;
+int16_t controlYaw = 0, controlThrottle = 0;
 uint8_t looping = 0;
+// Internal variables for reading commands made with an R/C stick.
+uint8_t lastCommand  = COMMAND_NONE;
+uint8_t lastArgument;
+uint8_t isCommandRepeated = 0;
-// Internal variables for reading commands made with an R/C stick.
+// MK flags. TODO: Replace by enum. State machine.
-uint8_t lastCommand  = COMMAND_NONE;
+uint16_t isFlying = 0;
-uint8_t commandTimer = 0;
+volatile uint8_t MKFlags = 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 RC_getArgument();
+  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) {
-  // If the same command was made COMMAND_TIMER times, it's stable.
+ * This could be expanded to take calibrate / start / stop commands from ohter sources
-  if (commandTimer >= COMMAND_TIMER) {
+ * than the R/C (read: Custom MK R/C project)
-    return lastCommand;
+ */
 uint8_t controlMixer_getCommand(void) {
-  return COMMAND_NONE;
+  return lastCommand;
-}
+}
-uint8_t controlMixer_isCommandRepeated(void) {
-  return commandTimer > COMMAND_TIMER ? 1 : 0;
+uint8_t controlMixer_isCommandRepeated(void) {
   return isCommandRepeated;
+}
 void controlMixer_setNeutral(uint8_t calibrate) {
+  }
+}
 /*
+ * Update potentiometer values with limited slew rate. Could be made faster if desired.
- * 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;
  */
 void controlMixer_update(void) {
   // calculate Stick inputs by rc channels (P) and changing of rc channels (D)
   // TODO: If no signal --> zero.
   uint8_t axis;
+  // takes almost no time...
   RC_update();
+  // takes almost no time...
   EC_update();
+  // takes about 80 usec.
+  HC_update();
   int16_t* RC_PRTY = RC_getPRTY();
   int16_t* EC_PRTY = EC_getPRTY();
+  control[PITCH]   = RC_PRTY[CONTROL_PITCH] + EC_PRTY[CONTROL_PITCH];
-  control[PITCH]   = RC_PRTY[CONTROL_PITCH] + EC_PRTY[CONTROL_PITCH];
+  control[ROLL]    = RC_PRTY[CONTROL_ROLL] + EC_PRTY[CONTROL_ROLL];
-  control[ROLL]    = RC_PRTY[CONTROL_ROLL] + EC_PRTY[CONTROL_ROLL];
+  // This can be a CPU time killer if the function implementations are inefficient.
+  controlThrottle  = HC_getThrottle(AC_getThrottle(RC_PRTY[CONTROL_THROTTLE] + EC_PRTY[CONTROL_THROTTLE]));
+  controlYaw       = RC_PRTY[CONTROL_YAW] + EC_PRTY[CONTROL_YAW];
+  DebugOut.Analog[12] = control[PITCH];
-  controlThrottle  = RC_PRTY[CONTROL_THROTTLE] + EC_PRTY[CONTROL_THROTTLE];
+  DebugOut.Analog[13] = control[ROLL];
-  controlYaw       = RC_PRTY[CONTROL_YAW] + EC_PRTY[CONTROL_YAW];
+  //DebugOut.Analog[26] = controlYaw;
-  if (RC_getSignalQuality() >= SIGNAL_GOOD) {
+  if (controlMixer_getSignalQuality() >= SIGNAL_GOOD) {
     controlMixer_updateVariables();
     configuration_applyVariablesToParams();
     looping = RC_getLooping(looping);
   } else { // Signal is not OK
     // Could handle switch to emergency flight here.
     // throttle is handled elsewhere.
-    looping = 0;
-  }
+    looping = 0;
+  }
   DebugOut.Analog[18] = (int16_t)looping;
   // part1a end.
   // (range of -2 .. 2 is set to zero, to avoid unwanted yaw trimming on compass correction)
-  // TODO: Correct, for changed range (stick gain + expo is now applied already)
+  /* This is not really necessary with the pull-towards-zero of 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;
       maxControl[axis] = abs(control[axis]) / CONTROL_SCALING;
       if(maxControl[axis] > 100) maxControl[axis] = 100;
     } else if (maxControl[axis]) maxControl[axis]--;
+  }
-  // Here we could blend in other sources.
+  uint8_t rcCommand = (RC_getSignalQuality() >= SIGNAL_OK) ? RC_getCommand() : COMMAND_NONE;
-  uint8_t commandNow = RC_getCommand();
+  uint8_t ecCommand = (EC_getSignalQuality() >= SIGNAL_OK) ? EC_getCommand() : COMMAND_NONE;
+  if (rcCommand != COMMAND_NONE) {
-  if (commandNow != lastCommand) {
+    isCommandRepeated = (lastCommand == rcCommand);
-    lastCommand = commandNow;
+    lastCommand = rcCommand;
-    commandTimer = 0;
+    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.
-  } else if (commandTimer < COMMAND_TIMER + 1)
+    // Just set to false. There is no reason to check if the none-command was repeated anyway.
+    isCommandRepeated = 0;
-    commandTimer++;
+    lastCommand = COMMAND_NONE;
-}
+  }
+  if (isCommandRepeated) DebugOut.Digital[0] |= DEBUG_COMMANDREPEATED; else DebugOut.Digital[0] &= ~DEBUG_COMMANDREPEATED;
+  if (rcCommand) DebugOut.Digital[1] |= DEBUG_COMMANDREPEATED; else DebugOut.Digital[1] &= ~DEBUG_COMMANDREPEATED;
-/*
-void setCompassCalState(void) {
-  static uint8_t stick = 1;
-  // if pitch is centered or top set stick to zero
+  // part1 end.
+}
-  if(RCChannel(CH_PITCH) > -20) stick = 0;
-  // if pitch is down trigger to next cal state
+// TODO: Integrate into command system.
-  if((RCChannel(CH_PITCH) < -70) && !stick) {
-    stick = 1;
+uint8_t controlMixer_testCompassCalState(void) {
-    compassCalState++;
-    if(compassCalState < 5) beepNumber(compassCalState);
-    else beep(1000);
   return RC_testCompassCalState();
-}

Subversion Repositories FlightCtrl

(root)/branches/dongfang_FC_rewrite/controlMixer.c - Rev 1645 → 1775