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();

}