| 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; |
| return lastCommand; |
| } |
| |
| uint8_t controlMixer_isCommandRepeated(void) { |
| return commandTimer > COMMAND_TIMER ? 1 : 0; |
| return isCommandRepeated; |
| } |
| |
| void controlMixer_setNeutral(uint8_t calibrate) { |
| if (calibrate) |
| RC_calibrate(); |
| EC_setNeutral(); |
| 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 (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; |
| } |
| |
| if (commandNow != lastCommand) { |
| lastCommand = commandNow; |
| commandTimer = 0; |
| } else if (commandTimer < COMMAND_TIMER + 1) |
| commandTimer++; |
| 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. |
| } |
| |
| /* |
| 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); |
| } |
| // TODO: Integrate into command system. |
| uint8_t controlMixer_testCompassCalState(void) { |
| return RC_testCompassCalState(); |
| } |
| */ |