Subversion Repositories FlightCtrl

#include <stdlib.h>
#include "controlMixer.h"
#include "rc.h"
#include "attitude.h"
#include "externalControl.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 control[4] = {0, 0, 0, 0};
int32_t controlIntegrals[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;

// MK flags. TODO: Replace by enum. State machine.
uint16_t isFlying = 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 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() {
  //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 < 8; 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 < 8; 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 = control[index];

  // TODO: Scale by some factor. To be determined.
  controlIntegrals[index] += tmp * 4;
 
  if (controlIntegrals[index] > PITCHROLLOVER180) {
    controlIntegrals[index] -= PITCHROLLOVER360;
  } else if (controlIntegrals[index] <= -PITCHROLLOVER180) {
    controlIntegrals[index] += PITCHROLLOVER360;
  }

  control[index] = newValue;
  tmp -= newValue;
  tmp = tmp * tmp;
  // tmp += (newValue >= 0) ? newValue : -newValue;
  controlActivity += tmp;
}

#define CADAMPING 10
void dampenControlActivity(void) {
  int32_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.
 */
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;
 
  RC_update();
  // EC_update();
  // HC_update();
 
  int16_t* RC_EATR = RC_getEATR();
  // int16_t* EC_PRTY = EC_getPRTY();
 
  updateControlAndMeasureControlActivity(CONTROL_ELEVATOR, RC_EATR[CONTROL_ELEVATOR] /* + EC_PRTY[CONTROL_PITCH] */);
  updateControlAndMeasureControlActivity(CONTROL_AILERONS, RC_EATR[CONTROL_AILERONS] /* + EC_PRTY[CONTROL_ROLL] */);
  updateControlAndMeasureControlActivity(CONTROL_RUDDER, RC_EATR[CONTROL_RUDDER] /* + EC_PRTY[CONTROL_YAW] */);
  dampenControlActivity();
 
  // Do we also want to have activity measurement on throttle?
  control[CONTROL_THROTTLE] = RC_EATR[CONTROL_THROTTLE]; // + EC_PRTY[CONTROL_THROTTLE]);
 
  if (controlMixer_getSignalQuality() >= SIGNAL_GOOD) {
    controlMixer_updateVariables();
    configuration_staticToDynamic();
  } else { // Signal is not OK
    // Could handle switch to emergency flight here.
    // throttle is handled elsewhere.
  }
 
  // 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
   */
  for (axis = PITCH; axis <= ROLL; axis++) {
    if (abs(control[axis]) > maxControl[axis]) {
      maxControl[axis] = abs(control[axis]);
      if (maxControl[axis] > 100)
        maxControl[axis] = 100;
    } else if (maxControl[axis])
      maxControl[axis]--;
  }
 
  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;
  }
}

// TODO: Integrate into command system.
uint8_t controlMixer_testCompassCalState(void) {
        return RC_testCompassCalState();
}