mirror of
https://bitbucket.org/librepilot/librepilot.git
synced 2024-12-01 09:24:10 +01:00
Restructure the ManualControlCommand code to be much more readable. Also
facilitates the new FlightMode object.
This commit is contained in:
James Cotton
parent
d1fb254a41
commit
e2819c6815
@ -54,6 +54,7 @@
|
||||
#define THROTTLE_FAILSAFE -0.1
|
||||
#define FLIGHT_MODE_LIMIT 1.0/3.0
|
||||
#define ARMED_TIME_MS 1000
|
||||
#define ARMED_THRESHOLD 0.50
|
||||
//safe band to allow a bit of calibration error or trim offset (in microseconds)
|
||||
#define CONNECTION_OFFSET 150
|
||||
|
||||
@ -70,13 +71,16 @@ typedef enum
|
||||
// Private variables
|
||||
static xTaskHandle taskHandle;
|
||||
static ArmState_t armState;
|
||||
static portTickType lastSysTime;
|
||||
|
||||
// Private functions
|
||||
static void updateActuatorDesired(ManualControlCommandData * cmd);
|
||||
static void updateStabilizationDesired(ManualControlCommandData * cmd, ManualControlSettingsData * settings);
|
||||
static void processFlightMode(ManualControlCommandData * cmd, ManualControlSettingsData * settings, float flightMode);
|
||||
static void processArm(ManualControlCommandData * cmd, ManualControlSettingsData * settings);
|
||||
|
||||
static void manualControlTask(void *parameters);
|
||||
static float scaleChannel(int16_t value, int16_t max, int16_t min, int16_t neutral, int16_t deadband_percent);
|
||||
static float scaleChannel(int16_t value, int16_t max, int16_t min, int16_t neutral);
|
||||
static uint32_t timeDifferenceMs(portTickType start_time, portTickType end_time);
|
||||
static bool okToArm(void);
|
||||
static bool validInputRange(int16_t min, int16_t max, uint16_t value);
|
||||
@ -159,13 +163,11 @@ static void manualControlTask(void *parameters)
|
||||
{
|
||||
ManualControlSettingsData settings;
|
||||
ManualControlCommandData cmd;
|
||||
portTickType lastSysTime;
|
||||
|
||||
float flightMode = 0;
|
||||
|
||||
uint8_t disconnected_count = 0;
|
||||
uint8_t connected_count = 0;
|
||||
enum { CONNECTED, DISCONNECTED } connection_state = DISCONNECTED;
|
||||
|
||||
// Make sure unarmed on power up
|
||||
ManualControlCommandGet(&cmd);
|
||||
@ -211,7 +213,7 @@ static void manualControlTask(void *parameters)
|
||||
#elif defined(PIOS_INCLUDE_SPEKTRUM)
|
||||
cmd.Channel[n] = PIOS_SPEKTRUM_Get(n);
|
||||
#endif
|
||||
scaledChannel[n] = scaleChannel(cmd.Channel[n], settings.ChannelMax[n], settings.ChannelMin[n], settings.ChannelNeutral[n], 0);
|
||||
scaledChannel[n] = scaleChannel(cmd.Channel[n], settings.ChannelMax[n], settings.ChannelMin[n], settings.ChannelNeutral[n]);
|
||||
}
|
||||
|
||||
// Check settings, if error raise alarm
|
||||
@ -227,38 +229,23 @@ static void manualControlTask(void *parameters)
|
||||
}
|
||||
|
||||
// decide if we have valid manual input or not
|
||||
bool valid_input_detected = TRUE;
|
||||
if (!validInputRange(settings.ChannelMin[settings.Throttle], settings.ChannelMax[settings.Throttle], cmd.Channel[settings.Throttle]))
|
||||
valid_input_detected = FALSE;
|
||||
if (!validInputRange(settings.ChannelMin[settings.Roll], settings.ChannelMax[settings.Roll], cmd.Channel[settings.Roll]))
|
||||
valid_input_detected = FALSE;
|
||||
if (!validInputRange(settings.ChannelMin[settings.Yaw], settings.ChannelMax[settings.Yaw], cmd.Channel[settings.Yaw]))
|
||||
valid_input_detected = FALSE;
|
||||
if (!validInputRange(settings.ChannelMin[settings.Pitch], settings.ChannelMax[settings.Pitch], cmd.Channel[settings.Pitch]))
|
||||
valid_input_detected = FALSE;
|
||||
bool valid_input_detected = validInputRange(settings.ChannelMin[settings.Throttle], settings.ChannelMax[settings.Throttle], cmd.Channel[settings.Throttle]) &&
|
||||
validInputRange(settings.ChannelMin[settings.Roll], settings.ChannelMax[settings.Roll], cmd.Channel[settings.Roll]) &&
|
||||
validInputRange(settings.ChannelMin[settings.Yaw], settings.ChannelMax[settings.Yaw], cmd.Channel[settings.Yaw]) &&
|
||||
validInputRange(settings.ChannelMin[settings.Pitch], settings.ChannelMax[settings.Pitch], cmd.Channel[settings.Pitch]);
|
||||
|
||||
// Implement hysteresis loop on connection status
|
||||
if (valid_input_detected)
|
||||
{
|
||||
if (++connected_count > 10)
|
||||
{
|
||||
connection_state = CONNECTED;
|
||||
if (valid_input_detected && (++connected_count > 10)) {
|
||||
cmd.Connected = MANUALCONTROLCOMMAND_CONNECTED_TRUE;
|
||||
connected_count = 0;
|
||||
disconnected_count = 0;
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
if (++disconnected_count > 10)
|
||||
{
|
||||
connection_state = DISCONNECTED;
|
||||
} else if (!valid_input_detected && (++disconnected_count > 10)) {
|
||||
cmd.Connected = MANUALCONTROLCOMMAND_CONNECTED_FALSE;
|
||||
connected_count = 0;
|
||||
disconnected_count = 0;
|
||||
}
|
||||
}
|
||||
|
||||
if (connection_state == DISCONNECTED) {
|
||||
cmd.Connected = MANUALCONTROLCOMMAND_CONNECTED_FALSE;
|
||||
if (cmd.Connected == MANUALCONTROLCOMMAND_CONNECTED_FALSE) {
|
||||
cmd.Throttle = -1; // Shut down engine with no control
|
||||
cmd.Roll = 0;
|
||||
cmd.Yaw = 0;
|
||||
@ -269,7 +256,6 @@ static void manualControlTask(void *parameters)
|
||||
AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING);
|
||||
ManualControlCommandSet(&cmd);
|
||||
} else {
|
||||
cmd.Connected = MANUALCONTROLCOMMAND_CONNECTED_TRUE;
|
||||
AlarmsClear(SYSTEMALARMS_ALARM_MANUALCONTROL);
|
||||
|
||||
// Scale channels to -1 -> +1 range
|
||||
@ -279,151 +265,13 @@ static void manualControlTask(void *parameters)
|
||||
cmd.Throttle = scaledChannel[settings.Throttle];
|
||||
flightMode = scaledChannel[settings.FlightMode];
|
||||
|
||||
if (settings.Accessory1 != MANUALCONTROLSETTINGS_ACCESSORY1_NONE)
|
||||
cmd.Accessory1 = scaledChannel[settings.Accessory1];
|
||||
else
|
||||
cmd.Accessory1 = 0;
|
||||
// Set accessory channels
|
||||
cmd.Accessory1 = (settings.Accessory1 != MANUALCONTROLSETTINGS_ACCESSORY1_NONE) ? scaledChannel[settings.Accessory1] : 0;
|
||||
cmd.Accessory2 = (settings.Accessory1 != MANUALCONTROLSETTINGS_ACCESSORY2_NONE) ? scaledChannel[settings.Accessory2] : 0;
|
||||
cmd.Accessory3 = (settings.Accessory1 != MANUALCONTROLSETTINGS_ACCESSORY3_NONE) ? scaledChannel[settings.Accessory3] : 0;
|
||||
|
||||
if (settings.Accessory2 != MANUALCONTROLSETTINGS_ACCESSORY2_NONE)
|
||||
cmd.Accessory2 = scaledChannel[settings.Accessory2];
|
||||
else
|
||||
cmd.Accessory2 = 0;
|
||||
|
||||
if (settings.Accessory3 != MANUALCONTROLSETTINGS_ACCESSORY3_NONE)
|
||||
cmd.Accessory3 = scaledChannel[settings.Accessory3];
|
||||
else
|
||||
cmd.Accessory3 = 0;
|
||||
|
||||
// Note here the code is ass
|
||||
if (flightMode < -FLIGHT_MODE_LIMIT)
|
||||
cmd.FlightMode = settings.FlightModePosition[0];
|
||||
else if (flightMode > FLIGHT_MODE_LIMIT)
|
||||
cmd.FlightMode = settings.FlightModePosition[2];
|
||||
else
|
||||
cmd.FlightMode = settings.FlightModePosition[1];
|
||||
|
||||
|
||||
//
|
||||
// Arming and Disarming mechanism
|
||||
//
|
||||
|
||||
if (settings.Arming == MANUALCONTROLSETTINGS_ARMING_ALWAYSDISARMED) {
|
||||
// In this configuration we always disarm
|
||||
cmd.Armed = MANUALCONTROLCOMMAND_ARMED_FALSE;
|
||||
} else {
|
||||
// In all other cases, we will not change the arm state when disconnected
|
||||
if (connection_state == CONNECTED)
|
||||
{
|
||||
if (settings.Arming == MANUALCONTROLSETTINGS_ARMING_ALWAYSARMED) {
|
||||
// In this configuration, we go into armed state as soon as the throttle is low, never disarm
|
||||
if (cmd.Throttle < 0) {
|
||||
cmd.Armed = MANUALCONTROLCOMMAND_ARMED_TRUE;
|
||||
}
|
||||
} else {
|
||||
// When the configuration is not "Always armed" and no "Always disarmed",
|
||||
// the state will not be changed when the throttle is not low
|
||||
if (cmd.Throttle < 0) {
|
||||
static portTickType armedDisarmStart;
|
||||
float armingInputLevel = 0;
|
||||
|
||||
// Calc channel see assumptions7
|
||||
switch ( (settings.Arming-MANUALCONTROLSETTINGS_ARMING_ROLLLEFT)/2 ) {
|
||||
case ARMING_CHANNEL_ROLL: armingInputLevel = cmd.Roll; break;
|
||||
case ARMING_CHANNEL_PITCH: armingInputLevel = cmd.Pitch; break;
|
||||
case ARMING_CHANNEL_YAW: armingInputLevel = cmd.Yaw; break;
|
||||
}
|
||||
|
||||
bool manualArm = false;
|
||||
bool manualDisarm = false;
|
||||
|
||||
if (connection_state == CONNECTED) {
|
||||
// Should use RC input only if RX is connected
|
||||
if (armingInputLevel <= -0.50)
|
||||
manualArm = true;
|
||||
else if (armingInputLevel >= +0.50)
|
||||
manualDisarm = true;
|
||||
}
|
||||
|
||||
// Swap arm-disarming see assumptions8
|
||||
if ((settings.Arming-MANUALCONTROLSETTINGS_ARMING_ROLLLEFT)%2) {
|
||||
bool temp = manualArm;
|
||||
manualArm = manualDisarm;
|
||||
manualDisarm = temp;
|
||||
}
|
||||
|
||||
switch(armState) {
|
||||
case ARM_STATE_DISARMED:
|
||||
cmd.Armed = MANUALCONTROLCOMMAND_ARMED_FALSE;
|
||||
|
||||
if (manualArm)
|
||||
{
|
||||
if (okToArm()) // only allow arming if it's OK too
|
||||
{
|
||||
armedDisarmStart = lastSysTime;
|
||||
armState = ARM_STATE_ARMING_MANUAL;
|
||||
}
|
||||
}
|
||||
break;
|
||||
|
||||
case ARM_STATE_ARMING_MANUAL:
|
||||
if (manualArm) {
|
||||
if (timeDifferenceMs(armedDisarmStart, lastSysTime) > ARMED_TIME_MS)
|
||||
armState = ARM_STATE_ARMED;
|
||||
}
|
||||
else
|
||||
armState = ARM_STATE_DISARMED;
|
||||
break;
|
||||
|
||||
case ARM_STATE_ARMED:
|
||||
// When we get here, the throttle is low,
|
||||
// we go immediately to disarming due to timeout, also when the disarming mechanism is not enabled
|
||||
armedDisarmStart = lastSysTime;
|
||||
armState = ARM_STATE_DISARMING_TIMEOUT;
|
||||
cmd.Armed = MANUALCONTROLCOMMAND_ARMED_TRUE;
|
||||
break;
|
||||
|
||||
case ARM_STATE_DISARMING_TIMEOUT:
|
||||
// We get here when armed while throttle low, even when the arming timeout is not enabled
|
||||
if (settings.ArmedTimeout != 0)
|
||||
if (timeDifferenceMs(armedDisarmStart, lastSysTime) > settings.ArmedTimeout)
|
||||
armState = ARM_STATE_DISARMED;
|
||||
// Switch to disarming due to manual control when needed
|
||||
if (manualDisarm) {
|
||||
armedDisarmStart = lastSysTime;
|
||||
armState = ARM_STATE_DISARMING_MANUAL;
|
||||
}
|
||||
break;
|
||||
|
||||
case ARM_STATE_DISARMING_MANUAL:
|
||||
if (manualDisarm) {
|
||||
if (timeDifferenceMs(armedDisarmStart, lastSysTime) > ARMED_TIME_MS)
|
||||
armState = ARM_STATE_DISARMED;
|
||||
}
|
||||
else
|
||||
armState = ARM_STATE_ARMED;
|
||||
break;
|
||||
} // End Switch
|
||||
} else {
|
||||
// The throttle is not low, in case we where arming or disarming, abort
|
||||
switch(armState) {
|
||||
case ARM_STATE_DISARMING_MANUAL:
|
||||
case ARM_STATE_DISARMING_TIMEOUT:
|
||||
armState = ARM_STATE_ARMED;
|
||||
break;
|
||||
case ARM_STATE_ARMING_MANUAL:
|
||||
armState = ARM_STATE_DISARMED;
|
||||
break;
|
||||
default:
|
||||
// Nothing needs to be done in the other states
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
//
|
||||
// End of arming/disarming
|
||||
//
|
||||
processFlightMode(&cmd, &settings, flightMode);
|
||||
processArm(&cmd, &settings);
|
||||
|
||||
// Update cmd object
|
||||
ManualControlCommandSet(&cmd);
|
||||
@ -517,7 +365,7 @@ static void updateStabilizationDesired(ManualControlCommandData * cmd, ManualCon
|
||||
/**
|
||||
* Convert channel from servo pulse duration (microseconds) to scaled -1/+1 range.
|
||||
*/
|
||||
static float scaleChannel(int16_t value, int16_t max, int16_t min, int16_t neutral, int16_t deadband_percent)
|
||||
static float scaleChannel(int16_t value, int16_t max, int16_t min, int16_t neutral)
|
||||
{
|
||||
float valueScaled;
|
||||
|
||||
@ -537,20 +385,6 @@ static float scaleChannel(int16_t value, int16_t max, int16_t min, int16_t neutr
|
||||
valueScaled = 0;
|
||||
}
|
||||
|
||||
// Neutral RC stick position dead band
|
||||
if (deadband_percent > 0)
|
||||
{
|
||||
if (deadband_percent > 50) deadband_percent = 50; // limit deadband to a maximum of 50%
|
||||
float deadband = (float)deadband_percent / 100;
|
||||
if (fabs(valueScaled) <= deadband)
|
||||
valueScaled = 0; // deadband the value
|
||||
else
|
||||
if (valueScaled < 0)
|
||||
valueScaled = (valueScaled + deadband) / (1.0 - deadband); // value scales 0.0 to -1.0 after deadband
|
||||
else
|
||||
valueScaled = (valueScaled - deadband) / (1.0 - deadband); // value scales 0.0 to +1.0 after deadband
|
||||
}
|
||||
|
||||
// Bound
|
||||
if (valueScaled > 1.0) valueScaled = 1.0;
|
||||
else
|
||||
@ -591,6 +425,134 @@ static bool okToArm(void)
|
||||
return true;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Process the inputs and determine whether to arm or not
|
||||
* @param[out] cmd The structure to set the armed in
|
||||
* @param[in] settings Settings indicating the necessary position
|
||||
*/
|
||||
static void processArm(ManualControlCommandData * cmd, ManualControlSettingsData * settings)
|
||||
{
|
||||
bool lowThrottle = cmd->Throttle <= 0;
|
||||
|
||||
if (settings->Arming == MANUALCONTROLSETTINGS_ARMING_ALWAYSDISARMED) {
|
||||
// In this configuration we always disarm
|
||||
cmd->Armed = MANUALCONTROLCOMMAND_ARMED_FALSE;
|
||||
} else {
|
||||
// Not really needed since this function not called when disconnected
|
||||
if (cmd->Connected == MANUALCONTROLCOMMAND_CONNECTED_FALSE)
|
||||
return;
|
||||
|
||||
// The throttle is not low, in case we where arming or disarming, abort
|
||||
if (!lowThrottle) {
|
||||
switch(armState) {
|
||||
case ARM_STATE_DISARMING_MANUAL:
|
||||
case ARM_STATE_DISARMING_TIMEOUT:
|
||||
armState = ARM_STATE_ARMED;
|
||||
break;
|
||||
case ARM_STATE_ARMING_MANUAL:
|
||||
armState = ARM_STATE_DISARMED;
|
||||
break;
|
||||
default:
|
||||
// Nothing needs to be done in the other states
|
||||
break;
|
||||
}
|
||||
return;
|
||||
}
|
||||
|
||||
// The rest of these cases throttle is low
|
||||
if (settings->Arming == MANUALCONTROLSETTINGS_ARMING_ALWAYSARMED) {
|
||||
// In this configuration, we go into armed state as soon as the throttle is low, never disarm
|
||||
cmd->Armed = MANUALCONTROLCOMMAND_ARMED_TRUE;
|
||||
return;
|
||||
}
|
||||
|
||||
|
||||
// When the configuration is not "Always armed" and no "Always disarmed",
|
||||
// the state will not be changed when the throttle is not low
|
||||
static portTickType armedDisarmStart;
|
||||
float armingInputLevel = 0;
|
||||
|
||||
// Calc channel see assumptions7
|
||||
int8_t sign = ((settings->Arming-MANUALCONTROLSETTINGS_ARMING_ROLLLEFT)%2) ? -1 : 1;
|
||||
switch ( (settings->Arming-MANUALCONTROLSETTINGS_ARMING_ROLLLEFT)/2 ) {
|
||||
case ARMING_CHANNEL_ROLL: armingInputLevel = sign * cmd->Roll; break;
|
||||
case ARMING_CHANNEL_PITCH: armingInputLevel = sign * cmd->Pitch; break;
|
||||
case ARMING_CHANNEL_YAW: armingInputLevel = sign * cmd->Yaw; break;
|
||||
}
|
||||
|
||||
bool manualArm = false;
|
||||
bool manualDisarm = false;
|
||||
|
||||
if (armingInputLevel <= -ARMED_THRESHOLD)
|
||||
manualArm = true;
|
||||
else if (armingInputLevel >= +ARMED_THRESHOLD)
|
||||
manualDisarm = true;
|
||||
|
||||
switch(armState) {
|
||||
case ARM_STATE_DISARMED:
|
||||
cmd->Armed = MANUALCONTROLCOMMAND_ARMED_FALSE;
|
||||
|
||||
// only allow arming if it's OK too
|
||||
if (manualArm && okToArm()) {
|
||||
armedDisarmStart = lastSysTime;
|
||||
armState = ARM_STATE_ARMING_MANUAL;
|
||||
}
|
||||
break;
|
||||
|
||||
case ARM_STATE_ARMING_MANUAL:
|
||||
if (manualArm && (timeDifferenceMs(armedDisarmStart, lastSysTime) > ARMED_TIME_MS))
|
||||
armState = ARM_STATE_ARMED;
|
||||
else if (!manualArm)
|
||||
armState = ARM_STATE_DISARMED;
|
||||
break;
|
||||
|
||||
case ARM_STATE_ARMED:
|
||||
// When we get here, the throttle is low,
|
||||
// we go immediately to disarming due to timeout, also when the disarming mechanism is not enabled
|
||||
armedDisarmStart = lastSysTime;
|
||||
armState = ARM_STATE_DISARMING_TIMEOUT;
|
||||
cmd->Armed = MANUALCONTROLCOMMAND_ARMED_TRUE;
|
||||
break;
|
||||
|
||||
case ARM_STATE_DISARMING_TIMEOUT:
|
||||
// We get here when armed while throttle low, even when the arming timeout is not enabled
|
||||
if ((settings->ArmedTimeout != 0) && (timeDifferenceMs(armedDisarmStart, lastSysTime) > settings->ArmedTimeout))
|
||||
armState = ARM_STATE_DISARMED;
|
||||
|
||||
// Switch to disarming due to manual control when needed
|
||||
if (manualDisarm) {
|
||||
armedDisarmStart = lastSysTime;
|
||||
armState = ARM_STATE_DISARMING_MANUAL;
|
||||
}
|
||||
break;
|
||||
|
||||
case ARM_STATE_DISARMING_MANUAL:
|
||||
if (manualDisarm &&(timeDifferenceMs(armedDisarmStart, lastSysTime) > ARMED_TIME_MS))
|
||||
armState = ARM_STATE_DISARMED;
|
||||
else if (!manualDisarm)
|
||||
armState = ARM_STATE_ARMED;
|
||||
break;
|
||||
} // End Switch
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Determine which of three positions the flight mode switch is in and set flight mode accordingly
|
||||
* @param[out] cmd Pointer to the command structure to set the flight mode in
|
||||
* @param[in] settings The settings which indicate which position is which mode
|
||||
* @param[in] flightMode the value of the switch position
|
||||
*/
|
||||
static void processFlightMode(ManualControlCommandData * cmd, ManualControlSettingsData * settings, float flightMode)
|
||||
{
|
||||
// Note here the code is ass
|
||||
if (flightMode < -FLIGHT_MODE_LIMIT)
|
||||
cmd->FlightMode = settings->FlightModePosition[0];
|
||||
else if (flightMode > FLIGHT_MODE_LIMIT)
|
||||
cmd->FlightMode = settings->FlightModePosition[2];
|
||||
else
|
||||
cmd->FlightMode = settings->FlightModePosition[1];
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Determine if the manual input value is within acceptable limits
|
||||
* @returns return TRUE if so, otherwise return FALSE
|
||||
@ -606,16 +568,6 @@ bool validInputRange(int16_t min, int16_t max, uint16_t value)
|
||||
return (value >= min - CONNECTION_OFFSET && value <= max + CONNECTION_OFFSET);
|
||||
}
|
||||
|
||||
//
|
||||
//static void armingMechanism(uint8_t* armingState, const ManualControlSettingsData* settings, const ManualControlCommandData* cmd)
|
||||
//{
|
||||
// if (settings->Arming == MANUALCONTROLSETTINGS_ARMING_ALWAYSDISARMED) {
|
||||
// *armingState = MANUALCONTROLCOMMAND_ARMED_FALSE;
|
||||
// return;
|
||||
// }
|
||||
//
|
||||
//
|
||||
//}
|
||||
/**
|
||||
* @}
|
||||
* @}
|
||||
|
||||
Loading…
Reference in New Issue
Block a user