52,31 → 52,35 |
#include <stdlib.h> |
#include "controlMixer.h" |
#include "rc.h" |
#include "heightControl.h" |
#include "attitudeControl.h" |
#include "externalControl.h" |
#include "configuration.h" |
#include "attitude.h" |
#include "eeprom.h" |
#include "flight.h" |
#include "commands.h" |
#include "output.h" |
|
/* |
* Number of cycles a command must be repeated before commit. Maybe this really belongs in RC. |
*/ |
#define COMMAND_TIMER 200 |
|
uint16_t maxControl[2] = {0,0}; |
int16_t control[2] = {0,0}, controlYaw = 0, controlThrottle = 0; |
int16_t control[2] = {0,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 commandTimer = 0; |
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 RC_getArgument(); |
return lastArgument; |
} |
|
/* |
84,21 → 88,16 |
* 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. |
if (commandTimer >= COMMAND_TIMER) { |
return lastCommand; |
} |
return COMMAND_NONE; |
} |
|
uint8_t controlMixer_isCommandRepeated(void) { |
return commandTimer > COMMAND_TIMER ? 1 : 0; |
return isCommandRepeated; |
} |
|
void controlMixer_setNeutral(uint8_t calibrate) { |
if (calibrate) |
RC_calibrate(); |
void controlMixer_setNeutral() { |
EC_setNeutral(); |
HC_setGround(); |
} |
|
/* |
114,6 → 113,7 |
|
/* |
* 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; |
139,18 → 139,30 |
// 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[ROLL] = RC_PRTY[CONTROL_ROLL] + EC_PRTY[CONTROL_ROLL]; |
controlThrottle = RC_PRTY[CONTROL_THROTTLE] + EC_PRTY[CONTROL_THROTTLE]; |
// 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]; |
|
if (RC_getSignalQuality() >= SIGNAL_GOOD) { |
DebugOut.Analog[12] = control[PITCH]; |
DebugOut.Analog[13] = control[ROLL]; |
//DebugOut.Analog[26] = controlYaw; |
|
if (controlMixer_getSignalQuality() >= SIGNAL_GOOD) { |
controlMixer_updateVariables(); |
configuration_applyVariablesToParams(); |
looping = RC_getLooping(looping); |
160,15 → 172,15 |
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 (controlYaw > 2) controlYaw-= 2; |
else if (controlYaw< -2) controlYaw += 2; |
else controlYaw = 0; |
} |
*/ |
|
/* |
* Record maxima |
180,27 → 192,31 |
} else if (maxControl[axis]) maxControl[axis]--; |
} |
|
// Here we could blend in other sources. |
uint8_t commandNow = RC_getCommand(); |
uint8_t rcCommand = (RC_getSignalQuality() >= SIGNAL_OK) ? RC_getCommand() : COMMAND_NONE; |
uint8_t ecCommand = (EC_getSignalQuality() >= SIGNAL_OK) ? EC_getCommand() : COMMAND_NONE; |
|
if (commandNow != lastCommand) { |
lastCommand = commandNow; |
commandTimer = 0; |
} else if (commandTimer < COMMAND_TIMER + 1) |
commandTimer++; |
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; |
} |
|
/* |
void setCompassCalState(void) { |
static uint8_t stick = 1; |
// if pitch is centered or top set stick to zero |
if(RCChannel(CH_PITCH) > -20) stick = 0; |
// if pitch is down trigger to next cal state |
if((RCChannel(CH_PITCH) < -70) && !stick) { |
stick = 1; |
compassCalState++; |
if(compassCalState < 5) beepNumber(compassCalState); |
else beep(1000); |
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; |
|
// part1 end. |
} |
|
// TODO: Integrate into command system. |
uint8_t controlMixer_testCompassCalState(void) { |
return RC_testCompassCalState(); |
} |
*/ |