Rev 2075 |
Rev 2089 |
Go to most recent revision |
Blame |
Compare with Previous |
Last modification |
View Log
| RSS feed
#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.
// This is to set the home pos in navi.
// MKFlags |= MKFLAG_CALIBRATE;
}
/*
* 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
if (staticParams.bitConfig & (CFG_NAVI_ENABLED))
navigation_periodicTaskAndPRTY(tempPRTY);
#endif
// Add compass control (could also have been before navi, they are independent)
CC_periodicTaskAndPRTY(tempPRTY);
FC_periodicTaskAndPRTY(tempPRTY);
// This is temporary. There might be some emergency height control also.
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();
// 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]--;
}
*/
}