Subversion Repositories FlightCtrl

#include <stdlib.h>

#include "controlMixer.h"

#include "rc.h"

#include "heightControl.h"

#include "attitudeControl.h"

#include "externalControl.h"

#include "compassControl.h"

#include "failsafeControl.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] = RC_getVariable(i);

  }

  EC_setNeutral();

  HC_setGround();

  FC_setNeutral();  // FC is FailsafeControl, not FlightCtrl.

}

/*

 * 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 < 0x8000)

    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.

 */

void controlMixer_periodicTask(void) {

  int16_t tempPRTY[4] = { 0, 0, 0, 0 };

  // Decode commands.

  uint8_t rcCommand = (RC_getSignalQuality() >= SIGNAL_OK) ? RC_getCommand()

    : COMMAND_NONE;

  uint8_t ecCommand = (EC_getSignalQuality() >= SIGNAL_OK) ? EC_getCommand()

    : COMMAND_NONE;

  // 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.

  }

  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;

  }

  // This will init the values (not just add to them).

  RC_periodicTaskAndPRTY(tempPRTY);

  // Add external control to RC

  EC_periodicTaskAndPRTY(tempPRTY);

#ifdef USE_DIRECT_GPS

  // Add navigations control to RC and external control.

  navigation_periodicTaskAndPRTY(tempPRTY);

#endif

  // Add compass control (could also have been before navi, they are independent)

  CC_periodicTaskAndPRTY(tempPRTY);

  FC_periodicTaskAndPRTY(tempPRTY);

  if (!MKFlags & MKFLAG_EMERGENCY_FLIGHT) {

    // 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.

  controls[CONTROL_THROTTLE] = tempPRTY[CONTROL_THROTTLE];

  updateControlAndMeasureControlActivity(CONTROL_PITCH, tempPRTY[CONTROL_PITCH]);

  updateControlAndMeasureControlActivity(CONTROL_ROLL, tempPRTY[CONTROL_ROLL]);

  updateControlAndMeasureControlActivity(CONTROL_YAW, tempPRTY[CONTROL_YAW]);

  dampenControlActivity();

  //debugOut.analog[17] = controlActivity/10;

  // 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]--;

   }

  */

 }