rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2024-11-29 07:24:13 +01:00
Code Issues Releases Activity

Restructure the ManualControlCommand code to be much more readable. Also

Browse Source 
facilitates the new FlightMode object.
This commit is contained in:
James Cotton 2011-05-07 12:43:13 -05:00
parent d1fb254a41
commit e2819c6815
1 changed files with 160 additions and 208 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

368
flight/Modules/ManualControl/manualcontrol.c
View File

@ -54,6 +54,7 @@
#define THROTTLE_FAILSAFE -0.1 #define THROTTLE_FAILSAFE -0.1
#define FLIGHT_MODE_LIMIT 1.0/3.0 #define FLIGHT_MODE_LIMIT 1.0/3.0
#define ARMED_TIME_MS 1000 #define ARMED_TIME_MS 1000
#define ARMED_THRESHOLD 0.50
//safe band to allow a bit of calibration error or trim offset (in microseconds) //safe band to allow a bit of calibration error or trim offset (in microseconds)
#define CONNECTION_OFFSET 150 #define CONNECTION_OFFSET 150
@ -70,13 +71,16 @@ typedef enum
// Private variables // Private variables
static xTaskHandle taskHandle; static xTaskHandle taskHandle;
static ArmState_t armState; static ArmState_t armState;
static portTickType lastSysTime;
// Private functions // Private functions
static void updateActuatorDesired(ManualControlCommandData * cmd); static void updateActuatorDesired(ManualControlCommandData * cmd);
static void updateStabilizationDesired(ManualControlCommandData * cmd, ManualControlSettingsData * settings); 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 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 uint32_t timeDifferenceMs(portTickType start_time, portTickType end_time);
static bool okToArm(void); static bool okToArm(void);
static bool validInputRange(int16_t min, int16_t max, uint16_t value); static bool validInputRange(int16_t min, int16_t max, uint16_t value);
@ -159,13 +163,11 @@ static void manualControlTask(void *parameters)
{ {
ManualControlSettingsData settings; ManualControlSettingsData settings;
ManualControlCommandData cmd; ManualControlCommandData cmd;
portTickType lastSysTime;
float flightMode = 0; float flightMode = 0;
uint8_t disconnected_count = 0; uint8_t disconnected_count = 0;
uint8_t connected_count = 0; uint8_t connected_count = 0;
enum { CONNECTED, DISCONNECTED } connection_state = DISCONNECTED;
// Make sure unarmed on power up // Make sure unarmed on power up
ManualControlCommandGet(&cmd); ManualControlCommandGet(&cmd);
@ -211,7 +213,7 @@ static void manualControlTask(void *parameters)
#elif defined(PIOS_INCLUDE_SPEKTRUM) #elif defined(PIOS_INCLUDE_SPEKTRUM)
cmd.Channel[n] = PIOS_SPEKTRUM_Get(n); cmd.Channel[n] = PIOS_SPEKTRUM_Get(n);
#endif #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 // Check settings, if error raise alarm
@ -227,38 +229,23 @@ static void manualControlTask(void *parameters)
} }
// decide if we have valid manual input or not // decide if we have valid manual input or not
bool valid_input_detected = TRUE; bool valid_input_detected = validInputRange(settings.ChannelMin[settings.Throttle], settings.ChannelMax[settings.Throttle], cmd.Channel[settings.Throttle]) &&
if (!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]) &&
valid_input_detected = FALSE; validInputRange(settings.ChannelMin[settings.Yaw], settings.ChannelMax[settings.Yaw], cmd.Channel[settings.Yaw]) &&
if (!validInputRange(settings.ChannelMin[settings.Roll], settings.ChannelMax[settings.Roll], cmd.Channel[settings.Roll])) validInputRange(settings.ChannelMin[settings.Pitch], settings.ChannelMax[settings.Pitch], cmd.Channel[settings.Pitch]);
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;
// Implement hysteresis loop on connection status // Implement hysteresis loop on connection status
if (valid_input_detected) if (valid_input_detected && (++connected_count > 10)) {
{ cmd.Connected = MANUALCONTROLCOMMAND_CONNECTED_TRUE;
if (++connected_count > 10) connected_count = 0;
{ disconnected_count = 0;
connection_state = CONNECTED; } else if (!valid_input_detected && (++disconnected_count > 10)) {
connected_count = 0; cmd.Connected = MANUALCONTROLCOMMAND_CONNECTED_FALSE;
disconnected_count = 0; connected_count = 0;
} disconnected_count = 0;
}
else
{
if (++disconnected_count > 10)
{
connection_state = DISCONNECTED;
connected_count = 0;
disconnected_count = 0;
}
} }
if (connection_state == DISCONNECTED) { if (cmd.Connected == MANUALCONTROLCOMMAND_CONNECTED_FALSE) {
cmd.Connected = MANUALCONTROLCOMMAND_CONNECTED_FALSE;
cmd.Throttle = -1; // Shut down engine with no control cmd.Throttle = -1; // Shut down engine with no control
cmd.Roll = 0; cmd.Roll = 0;
cmd.Yaw = 0; cmd.Yaw = 0;
@ -269,161 +256,22 @@ static void manualControlTask(void *parameters)
AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING); AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING);
ManualControlCommandSet(&cmd); ManualControlCommandSet(&cmd);
} else { } else {
cmd.Connected = MANUALCONTROLCOMMAND_CONNECTED_TRUE;
AlarmsClear(SYSTEMALARMS_ALARM_MANUALCONTROL); AlarmsClear(SYSTEMALARMS_ALARM_MANUALCONTROL);
// Scale channels to -1 -> +1 range // Scale channels to -1 -> +1 range
cmd.Roll = scaledChannel[settings.Roll]; cmd.Roll = scaledChannel[settings.Roll];
cmd.Pitch = scaledChannel[settings.Pitch]; cmd.Pitch = scaledChannel[settings.Pitch];
cmd.Yaw = scaledChannel[settings.Yaw]; cmd.Yaw = scaledChannel[settings.Yaw];
cmd.Throttle = scaledChannel[settings.Throttle]; cmd.Throttle = scaledChannel[settings.Throttle];
flightMode = scaledChannel[settings.FlightMode]; flightMode = scaledChannel[settings.FlightMode];
if (settings.Accessory1 != MANUALCONTROLSETTINGS_ACCESSORY1_NONE) // Set accessory channels
cmd.Accessory1 = scaledChannel[settings.Accessory1]; cmd.Accessory1 = (settings.Accessory1 != MANUALCONTROLSETTINGS_ACCESSORY1_NONE) ? scaledChannel[settings.Accessory1] : 0;
else cmd.Accessory2 = (settings.Accessory1 != MANUALCONTROLSETTINGS_ACCESSORY2_NONE) ? scaledChannel[settings.Accessory2] : 0;
cmd.Accessory1 = 0; cmd.Accessory3 = (settings.Accessory1 != MANUALCONTROLSETTINGS_ACCESSORY3_NONE) ? scaledChannel[settings.Accessory3] : 0;
if (settings.Accessory2 != MANUALCONTROLSETTINGS_ACCESSORY2_NONE) processFlightMode(&cmd, &settings, flightMode);
cmd.Accessory2 = scaledChannel[settings.Accessory2]; processArm(&cmd, &settings);
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
//
// Update cmd object // Update cmd object
ManualControlCommandSet(&cmd); ManualControlCommandSet(&cmd);
@ -517,7 +365,7 @@ static void updateStabilizationDesired(ManualControlCommandData * cmd, ManualCon
/** /**
* Convert channel from servo pulse duration (microseconds) to scaled -1/+1 range. * 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; float valueScaled;
@ -537,20 +385,6 @@ static float scaleChannel(int16_t value, int16_t max, int16_t min, int16_t neutr
valueScaled = 0; 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 // Bound
if (valueScaled > 1.0) valueScaled = 1.0; if (valueScaled > 1.0) valueScaled = 1.0;
else else
@ -591,6 +425,134 @@ static bool okToArm(void)
return true; 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 * @brief Determine if the manual input value is within acceptable limits
* @returns return TRUE if so, otherwise return FALSE * @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); 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;
// }
//
//
//}
/** /**
* @} * @}
* @} * @}