rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-02-05 21:52:10 +01:00
Code Issues Releases Activity

OP-938 Add an "Exp" to throttle stick when in AH to make it useable

Browse Source 
Conflicts:
	flight/modules/ManualControl/manualcontrol.c
This commit is contained in:
Alessio Morale 2013-05-03 01:43:14 +02:00
parent 9f65409c5a
commit 976999eba0
1 changed files with 349 additions and 329 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

678
flight/modules/ManualControl/manualcontrol.c
View File

@ -53,26 +53,25 @@
#if defined(PIOS_INCLUDE_USB_RCTX) #if defined(PIOS_INCLUDE_USB_RCTX)
#include "pios_usb_rctx.h" #include "pios_usb_rctx.h"
#endif /* PIOS_INCLUDE_USB_RCTX */ #endif /* PIOS_INCLUDE_USB_RCTX */
// Private constants // Private constants
#if defined(PIOS_MANUAL_STACK_SIZE) #if defined(PIOS_MANUAL_STACK_SIZE)
#define STACK_SIZE_BYTES PIOS_MANUAL_STACK_SIZE #define STACK_SIZE_BYTES PIOS_MANUAL_STACK_SIZE
#else #else
#define STACK_SIZE_BYTES 1024 #define STACK_SIZE_BYTES 1024
#endif #endif
#define TASK_PRIORITY (tskIDLE_PRIORITY+4) #define TASK_PRIORITY (tskIDLE_PRIORITY + 4)
#define UPDATE_PERIOD_MS 20 #define UPDATE_PERIOD_MS 20
#define THROTTLE_FAILSAFE -0.1f #define THROTTLE_FAILSAFE -0.1f
#define ARMED_TIME_MS 1000 #define ARMED_TIME_MS 1000
#define ARMED_THRESHOLD 0.50f #define ARMED_THRESHOLD 0.50f
//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 250 #define CONNECTION_OFFSET 250
// Private types // Private types
typedef enum typedef enum {
{
ARM_STATE_DISARMED, ARM_STATE_ARMING_MANUAL, ARM_STATE_ARMED, ARM_STATE_DISARMING_MANUAL, ARM_STATE_DISARMING_TIMEOUT ARM_STATE_DISARMED, ARM_STATE_ARMING_MANUAL, ARM_STATE_ARMED, ARM_STATE_DISARMING_MANUAL, ARM_STATE_DISARMING_TIMEOUT
} ArmState_t; } ArmState_t;
@ -87,15 +86,15 @@ static float inputFiltered[MANUALCONTROLSETTINGS_RESPONSETIME_NUMELEM];
#endif #endif
// 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 updateLandDesired(ManualControlCommandData * cmd, bool changed); static void updateLandDesired(ManualControlCommandData *cmd, bool changed);
static void altitudeHoldDesired(ManualControlCommandData * cmd, bool changed); static void altitudeHoldDesired(ManualControlCommandData *cmd, bool changed);
static void updatePathDesired(ManualControlCommandData * cmd, bool changed, bool home); static void updatePathDesired(ManualControlCommandData *cmd, bool changed, bool home);
static void processFlightMode(ManualControlSettingsData * settings, float flightMode); static void processFlightMode(ManualControlSettingsData *settings, float flightMode);
static void processArm(ManualControlCommandData * cmd, ManualControlSettingsData * settings); static void processArm(ManualControlCommandData *cmd, ManualControlSettingsData *settings);
static void setArmedIfChanged(uint8_t val); static void setArmedIfChanged(uint8_t val);
static void configurationUpdatedCb(UAVObjEvent * ev); static void configurationUpdatedCb(UAVObjEvent *ev);
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); static float scaleChannel(int16_t value, int16_t max, int16_t min, int16_t neutral);
@ -109,17 +108,16 @@ static void applyLPF(float *value, ManualControlSettingsResponseTimeElem channel
#endif #endif
#define RCVR_ACTIVITY_MONITOR_CHANNELS_PER_GROUP 12 #define RCVR_ACTIVITY_MONITOR_CHANNELS_PER_GROUP 12
#define RCVR_ACTIVITY_MONITOR_MIN_RANGE 10 #define RCVR_ACTIVITY_MONITOR_MIN_RANGE 10
struct rcvr_activity_fsm struct rcvr_activity_fsm {
{
ManualControlSettingsChannelGroupsOptions group; ManualControlSettingsChannelGroupsOptions group;
uint16_t prev[RCVR_ACTIVITY_MONITOR_CHANNELS_PER_GROUP]; uint16_t prev[RCVR_ACTIVITY_MONITOR_CHANNELS_PER_GROUP];
uint8_t sample_count; uint8_t sample_count;
}; };
static struct rcvr_activity_fsm activity_fsm; static struct rcvr_activity_fsm activity_fsm;
static void resetRcvrActivity(struct rcvr_activity_fsm * fsm); static void resetRcvrActivity(struct rcvr_activity_fsm *fsm);
static bool updateRcvrActivity(struct rcvr_activity_fsm * fsm); static bool updateRcvrActivity(struct rcvr_activity_fsm *fsm);
#define assumptions (assumptions1 && assumptions3 && assumptions5 && assumptions7 && assumptions8 && assumptions_flightmode && assumptions_channelcount) #define assumptions (assumptions1 && assumptions3 && assumptions5 && assumptions7 && assumptions8 && assumptions_flightmode && assumptions_channelcount)
@ -129,7 +127,7 @@ static bool updateRcvrActivity(struct rcvr_activity_fsm * fsm);
int32_t ManualControlStart() int32_t ManualControlStart()
{ {
// Start main task // Start main task
xTaskCreate(manualControlTask, (signed char *) "ManualControl", STACK_SIZE_BYTES / 4, NULL, TASK_PRIORITY, &taskHandle); xTaskCreate(manualControlTask, (signed char *)"ManualControl", STACK_SIZE_BYTES / 4, NULL, TASK_PRIORITY, &taskHandle);
TaskMonitorAdd(TASKINFO_RUNNING_MANUALCONTROL, taskHandle); TaskMonitorAdd(TASKINFO_RUNNING_MANUALCONTROL, taskHandle);
PIOS_WDG_RegisterFlag(PIOS_WDG_MANUAL); PIOS_WDG_RegisterFlag(PIOS_WDG_MANUAL);
@ -141,10 +139,10 @@ int32_t ManualControlStart()
*/ */
int32_t ManualControlInitialize() int32_t ManualControlInitialize()
{ {
/* Check the assumptions about uavobject enum's are correct */ /* Check the assumptions about uavobject enum's are correct */
if (!assumptions) if (!assumptions) {
return -1; return -1;
}
AccessoryDesiredInitialize(); AccessoryDesiredInitialize();
ManualControlCommandInitialize(); ManualControlCommandInitialize();
@ -155,7 +153,7 @@ int32_t ManualControlInitialize()
return 0; return 0;
} }
MODULE_INITCALL( ManualControlInitialize, ManualControlStart) MODULE_INITCALL(ManualControlInitialize, ManualControlStart)
/** /**
* Module task * Module task
@ -168,7 +166,7 @@ static void manualControlTask(void *parameters)
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;
// For now manual instantiate extra instances of Accessory Desired. In future should be done dynamically // For now manual instantiate extra instances of Accessory Desired. In future should be done dynamically
// this includes not even registering it if not used // this includes not even registering it if not used
@ -231,7 +229,6 @@ static void manualControlTask(void *parameters)
} }
if (!ManualControlCommandReadOnly()) { if (!ManualControlCommandReadOnly()) {
bool valid_input_detected = true; bool valid_input_detected = true;
// Read channel values in us // Read channel values in us
@ -246,37 +243,37 @@ static void manualControlTask(void *parameters)
// If a channel has timed out this is not valid data and we shouldn't update anything // If a channel has timed out this is not valid data and we shouldn't update anything
// until we decide to go to failsafe // until we decide to go to failsafe
if (cmd.Channel[n] == PIOS_RCVR_TIMEOUT) if (cmd.Channel[n] == PIOS_RCVR_TIMEOUT) {
valid_input_detected = false; valid_input_detected = false;
else } else {
scaledChannel[n] = scaleChannel(cmd.Channel[n], settings.ChannelMax[n], settings.ChannelMin[n], settings.ChannelNeutral[n]); 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
if (settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL] >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE if (settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL] >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE
|| settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH] >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE || settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH] >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE
|| settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW] >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE || settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW] >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE
|| settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE] >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE || settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE] >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE
|| ||
// Check all channel mappings are valid // Check all channel mappings are valid
cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL] == (uint16_t) PIOS_RCVR_INVALID cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL] == (uint16_t)PIOS_RCVR_INVALID
|| cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH] == (uint16_t) PIOS_RCVR_INVALID || cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH] == (uint16_t)PIOS_RCVR_INVALID
|| cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW] == (uint16_t) PIOS_RCVR_INVALID || cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW] == (uint16_t)PIOS_RCVR_INVALID
|| cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE] == (uint16_t) PIOS_RCVR_INVALID || cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE] == (uint16_t)PIOS_RCVR_INVALID
|| ||
// Check the driver exists // Check the driver exists
cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL] == (uint16_t) PIOS_RCVR_NODRIVER cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL] == (uint16_t)PIOS_RCVR_NODRIVER
|| cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH] == (uint16_t) PIOS_RCVR_NODRIVER || cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH] == (uint16_t)PIOS_RCVR_NODRIVER
|| cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW] == (uint16_t) PIOS_RCVR_NODRIVER || cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW] == (uint16_t)PIOS_RCVR_NODRIVER
|| cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE] == (uint16_t) PIOS_RCVR_NODRIVER || || cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE] == (uint16_t)PIOS_RCVR_NODRIVER ||
// Check the FlightModeNumber is valid // Check the FlightModeNumber is valid
settings.FlightModeNumber < 1 || settings.FlightModeNumber > MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_NUMELEM || settings.FlightModeNumber < 1 || settings.FlightModeNumber > MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_NUMELEM ||
// Similar checks for FlightMode channel but only if more than one flight mode has been set. Otherwise don't care // Similar checks for FlightMode channel but only if more than one flight mode has been set. Otherwise don't care
((settings.FlightModeNumber > 1) ((settings.FlightModeNumber > 1)
&& (settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_FLIGHTMODE] >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE && (settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_FLIGHTMODE] >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE
|| cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_FLIGHTMODE] == (uint16_t) PIOS_RCVR_INVALID || cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_FLIGHTMODE] == (uint16_t)PIOS_RCVR_INVALID
|| cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_FLIGHTMODE] == (uint16_t) PIOS_RCVR_NODRIVER))) { || cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_FLIGHTMODE] == (uint16_t)PIOS_RCVR_NODRIVER))) {
AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_CRITICAL); AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_CRITICAL);
cmd.Connected = MANUALCONTROLCOMMAND_CONNECTED_FALSE; cmd.Connected = MANUALCONTROLCOMMAND_CONNECTED_FALSE;
ManualControlCommandSet(&cmd); ManualControlCommandSet(&cmd);
@ -290,32 +287,32 @@ static void manualControlTask(void *parameters)
// decide if we have valid manual input or not // decide if we have valid manual input or not
valid_input_detected &= validInputRange(settings.ChannelMin[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE], valid_input_detected &= validInputRange(settings.ChannelMin[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE],
settings.ChannelMax[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE], cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE]) settings.ChannelMax[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE], cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE])
&& validInputRange(settings.ChannelMin[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL], && validInputRange(settings.ChannelMin[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL],
settings.ChannelMax[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL], cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL]) settings.ChannelMax[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL], cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL])
&& validInputRange(settings.ChannelMin[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW], && validInputRange(settings.ChannelMin[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW],
settings.ChannelMax[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW], cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW]) settings.ChannelMax[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW], cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW])
&& validInputRange(settings.ChannelMin[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH], && validInputRange(settings.ChannelMin[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH],
settings.ChannelMax[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH], cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH]); settings.ChannelMax[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH], cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH]);
// Implement hysteresis loop on connection status // Implement hysteresis loop on connection status
if (valid_input_detected && (++connected_count > 10)) { if (valid_input_detected && (++connected_count > 10)) {
cmd.Connected = MANUALCONTROLCOMMAND_CONNECTED_TRUE; cmd.Connected = MANUALCONTROLCOMMAND_CONNECTED_TRUE;
connected_count = 0; connected_count = 0;
disconnected_count = 0; disconnected_count = 0;
} else if (!valid_input_detected && (++disconnected_count > 10)) { } else if (!valid_input_detected && (++disconnected_count > 10)) {
cmd.Connected = MANUALCONTROLCOMMAND_CONNECTED_FALSE; cmd.Connected = MANUALCONTROLCOMMAND_CONNECTED_FALSE;
connected_count = 0; connected_count = 0;
disconnected_count = 0; disconnected_count = 0;
} }
if (cmd.Connected == MANUALCONTROLCOMMAND_CONNECTED_FALSE) { if (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;
cmd.Pitch = 0; cmd.Pitch = 0;
cmd.Collective = 0; cmd.Collective = 0;
//cmd.FlightMode = MANUALCONTROLCOMMAND_FLIGHTMODE_AUTO; // don't do until AUTO implemented and functioning // cmd.FlightMode = MANUALCONTROLCOMMAND_FLIGHTMODE_AUTO; // don't do until AUTO implemented and functioning
// Important: Throttle < 0 will reset Stabilization coefficients among other things. Either change this, // Important: Throttle < 0 will reset Stabilization coefficients among other things. Either change this,
// or leave throttle at IDLE speed or above when going into AUTO-failsafe. // or leave throttle at IDLE speed or above when going into AUTO-failsafe.
AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING); AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING);
@ -324,31 +321,33 @@ static void manualControlTask(void *parameters)
// Set Accessory 0 // Set Accessory 0
if (settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY0] != MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) { if (settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY0] != MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) {
accessory.AccessoryVal = 0; accessory.AccessoryVal = 0;
if (AccessoryDesiredInstSet(0, &accessory) != 0) if (AccessoryDesiredInstSet(0, &accessory) != 0) {
AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING); AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING);
}
} }
// Set Accessory 1 // Set Accessory 1
if (settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY1] != MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) { if (settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY1] != MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) {
accessory.AccessoryVal = 0; accessory.AccessoryVal = 0;
if (AccessoryDesiredInstSet(1, &accessory) != 0) if (AccessoryDesiredInstSet(1, &accessory) != 0) {
AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING); AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING);
}
} }
// Set Accessory 2 // Set Accessory 2
if (settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY2] != MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) { if (settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY2] != MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) {
accessory.AccessoryVal = 0; accessory.AccessoryVal = 0;
if (AccessoryDesiredInstSet(2, &accessory) != 0) if (AccessoryDesiredInstSet(2, &accessory) != 0) {
AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING); AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING);
}
} }
} else if (valid_input_detected) { } else if (valid_input_detected) {
AlarmsClear(SYSTEMALARMS_ALARM_MANUALCONTROL); AlarmsClear(SYSTEMALARMS_ALARM_MANUALCONTROL);
// Scale channels to -1 -> +1 range // Scale channels to -1 -> +1 range
cmd.Roll = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL]; cmd.Roll = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL];
cmd.Pitch = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH]; cmd.Pitch = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH];
cmd.Yaw = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW]; cmd.Yaw = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW];
cmd.Throttle = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE]; cmd.Throttle = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE];
flightMode = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_FLIGHTMODE]; flightMode = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_FLIGHTMODE];
// Apply deadband for Roll/Pitch/Yaw stick inputs // Apply deadband for Roll/Pitch/Yaw stick inputs
if (settings.Deadband) { if (settings.Deadband) {
@ -360,18 +359,19 @@ static void manualControlTask(void *parameters)
// Apply Low Pass Filter to input channels, time delta between calls in ms // Apply Low Pass Filter to input channels, time delta between calls in ms
portTickType thisSysTime = xTaskGetTickCount(); portTickType thisSysTime = xTaskGetTickCount();
float dT = (thisSysTime > lastSysTimeLPF) ? float dT = (thisSysTime > lastSysTimeLPF) ?
(float)((thisSysTime - lastSysTimeLPF) * portTICK_RATE_MS) : (float)((thisSysTime - lastSysTimeLPF) * portTICK_RATE_MS) :
(float)UPDATE_PERIOD_MS; (float)UPDATE_PERIOD_MS;
lastSysTimeLPF = thisSysTime; lastSysTimeLPF = thisSysTime;
applyLPF(&cmd.Roll, MANUALCONTROLSETTINGS_RESPONSETIME_ROLL, &settings, dT); applyLPF(&cmd.Roll, MANUALCONTROLSETTINGS_RESPONSETIME_ROLL, &settings, dT);
applyLPF(&cmd.Pitch, MANUALCONTROLSETTINGS_RESPONSETIME_PITCH, &settings, dT); applyLPF(&cmd.Pitch, MANUALCONTROLSETTINGS_RESPONSETIME_PITCH, &settings, dT);
applyLPF(&cmd.Yaw, MANUALCONTROLSETTINGS_RESPONSETIME_YAW, &settings, dT); applyLPF(&cmd.Yaw, MANUALCONTROLSETTINGS_RESPONSETIME_YAW, &settings, dT);
#endif // USE_INPUT_LPF #endif // USE_INPUT_LPF
if (cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_COLLECTIVE] != (uint16_t) PIOS_RCVR_INVALID if (cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_COLLECTIVE] != (uint16_t)PIOS_RCVR_INVALID
&& cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_COLLECTIVE] != (uint16_t) PIOS_RCVR_NODRIVER && cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_COLLECTIVE] != (uint16_t)PIOS_RCVR_NODRIVER
&& cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_COLLECTIVE] != (uint16_t) PIOS_RCVR_TIMEOUT) && cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_COLLECTIVE] != (uint16_t)PIOS_RCVR_TIMEOUT) {
cmd.Collective = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_COLLECTIVE]; cmd.Collective = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_COLLECTIVE];
}
AccessoryDesiredData accessory; AccessoryDesiredData accessory;
// Set Accessory 0 // Set Accessory 0
@ -380,8 +380,9 @@ static void manualControlTask(void *parameters)
#ifdef USE_INPUT_LPF #ifdef USE_INPUT_LPF
applyLPF(&accessory.AccessoryVal, MANUALCONTROLSETTINGS_RESPONSETIME_ACCESSORY0, &settings, dT); applyLPF(&accessory.AccessoryVal, MANUALCONTROLSETTINGS_RESPONSETIME_ACCESSORY0, &settings, dT);
#endif #endif
if (AccessoryDesiredInstSet(0, &accessory) != 0) if (AccessoryDesiredInstSet(0, &accessory) != 0) {
AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING); AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING);
}
} }
// Set Accessory 1 // Set Accessory 1
if (settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY1] != MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) { if (settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY1] != MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) {
@ -389,8 +390,9 @@ static void manualControlTask(void *parameters)
#ifdef USE_INPUT_LPF #ifdef USE_INPUT_LPF
applyLPF(&accessory.AccessoryVal, MANUALCONTROLSETTINGS_RESPONSETIME_ACCESSORY1, &settings, dT); applyLPF(&accessory.AccessoryVal, MANUALCONTROLSETTINGS_RESPONSETIME_ACCESSORY1, &settings, dT);
#endif #endif
if (AccessoryDesiredInstSet(1, &accessory) != 0) if (AccessoryDesiredInstSet(1, &accessory) != 0) {
AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING); AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING);
}
} }
// Set Accessory 2 // Set Accessory 2
if (settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY2] != MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) { if (settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY2] != MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) {
@ -398,12 +400,12 @@ static void manualControlTask(void *parameters)
#ifdef USE_INPUT_LPF #ifdef USE_INPUT_LPF
applyLPF(&accessory.AccessoryVal, MANUALCONTROLSETTINGS_RESPONSETIME_ACCESSORY2, &settings, dT); applyLPF(&accessory.AccessoryVal, MANUALCONTROLSETTINGS_RESPONSETIME_ACCESSORY2, &settings, dT);
#endif #endif
if (AccessoryDesiredInstSet(2, &accessory) != 0) if (AccessoryDesiredInstSet(2, &accessory) != 0) {
AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING); AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING);
}
} }
processFlightMode(&settings, flightMode); processFlightMode(&settings, flightMode);
} }
// Process arming outside conditional so system will disarm when disconnected // Process arming outside conditional so system will disarm when disconnected
@ -414,13 +416,12 @@ static void manualControlTask(void *parameters)
#if defined(PIOS_INCLUDE_USB_RCTX) #if defined(PIOS_INCLUDE_USB_RCTX)
if (pios_usb_rctx_id) { if (pios_usb_rctx_id) {
PIOS_USB_RCTX_Update(pios_usb_rctx_id, PIOS_USB_RCTX_Update(pios_usb_rctx_id,
cmd.Channel, cmd.Channel,
settings.ChannelMin, settings.ChannelMin,
settings.ChannelMax, settings.ChannelMax,
NELEMENTS(cmd.Channel)); NELEMENTS(cmd.Channel));
} }
#endif /* PIOS_INCLUDE_USB_RCTX */ #endif /* PIOS_INCLUDE_USB_RCTX */
} else { } else {
ManualControlCommandGet(&cmd); /* Under GCS control */ ManualControlCommandGet(&cmd); /* Under GCS control */
} }
@ -430,48 +431,48 @@ static void manualControlTask(void *parameters)
// Depending on the mode update the Stabilization or Actuator objects // Depending on the mode update the Stabilization or Actuator objects
static uint8_t lastFlightMode = FLIGHTSTATUS_FLIGHTMODE_MANUAL; static uint8_t lastFlightMode = FLIGHTSTATUS_FLIGHTMODE_MANUAL;
switch (PARSE_FLIGHT_MODE(flightStatus.FlightMode)) { switch (PARSE_FLIGHT_MODE(flightStatus.FlightMode)) {
case FLIGHTMODE_UNDEFINED: case FLIGHTMODE_UNDEFINED:
// This reflects a bug in the code architecture! // This reflects a bug in the code architecture!
AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_CRITICAL);
break;
case FLIGHTMODE_MANUAL:
updateActuatorDesired(&cmd);
break;
case FLIGHTMODE_STABILIZED:
updateStabilizationDesired(&cmd, &settings);
break;
case FLIGHTMODE_TUNING:
// Tuning takes settings directly from manualcontrolcommand. No need to
// call anything else. This just avoids errors.
break;
case FLIGHTMODE_GUIDANCE:
switch (flightStatus.FlightMode) {
case FLIGHTSTATUS_FLIGHTMODE_ALTITUDEHOLD:
altitudeHoldDesired(&cmd, lastFlightMode != flightStatus.FlightMode);
break;
case FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD:
case FLIGHTSTATUS_FLIGHTMODE_POI:
updatePathDesired(&cmd, lastFlightMode != flightStatus.FlightMode, false);
break;
case FLIGHTSTATUS_FLIGHTMODE_RETURNTOBASE:
updatePathDesired(&cmd, lastFlightMode != flightStatus.FlightMode, true);
break;
case FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER:
// No need to call anything. This just avoids errors.
break;
case FLIGHTSTATUS_FLIGHTMODE_LAND:
updateLandDesired(&cmd, lastFlightMode != flightStatus.FlightMode);
break;
default:
AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_CRITICAL); AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_CRITICAL);
break; }
case FLIGHTMODE_MANUAL: break;
updateActuatorDesired(&cmd);
break;
case FLIGHTMODE_STABILIZED:
updateStabilizationDesired(&cmd, &settings);
break;
case FLIGHTMODE_TUNING:
// Tuning takes settings directly from manualcontrolcommand. No need to
// call anything else. This just avoids errors.
break;
case FLIGHTMODE_GUIDANCE:
switch (flightStatus.FlightMode) {
case FLIGHTSTATUS_FLIGHTMODE_ALTITUDEHOLD:
altitudeHoldDesired(&cmd, lastFlightMode != flightStatus.FlightMode);
break;
case FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD:
case FLIGHTSTATUS_FLIGHTMODE_POI:
updatePathDesired(&cmd, lastFlightMode != flightStatus.FlightMode, false);
break;
case FLIGHTSTATUS_FLIGHTMODE_RETURNTOBASE:
updatePathDesired(&cmd, lastFlightMode != flightStatus.FlightMode, true);
break;
case FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER:
// No need to call anything. This just avoids errors.
break;
case FLIGHTSTATUS_FLIGHTMODE_LAND:
updateLandDesired(&cmd, lastFlightMode != flightStatus.FlightMode);
break;
default:
AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_CRITICAL);
}
break;
} }
lastFlightMode = flightStatus.FlightMode; lastFlightMode = flightStatus.FlightMode;
} }
} }
static void resetRcvrActivity(struct rcvr_activity_fsm * fsm) static void resetRcvrActivity(struct rcvr_activity_fsm *fsm)
{ {
ReceiverActivityData data; ReceiverActivityData data;
bool updated = false; bool updated = false;
@ -479,7 +480,7 @@ static void resetRcvrActivity(struct rcvr_activity_fsm * fsm)
/* Clear all channel activity flags */ /* Clear all channel activity flags */
ReceiverActivityGet(&data); ReceiverActivityGet(&data);
if (data.ActiveGroup != RECEIVERACTIVITY_ACTIVEGROUP_NONE && data.ActiveChannel != 255) { if (data.ActiveGroup != RECEIVERACTIVITY_ACTIVEGROUP_NONE && data.ActiveChannel != 255) {
data.ActiveGroup = RECEIVERACTIVITY_ACTIVEGROUP_NONE; data.ActiveGroup = RECEIVERACTIVITY_ACTIVEGROUP_NONE;
data.ActiveChannel = 255; data.ActiveChannel = 255;
updated = true; updated = true;
} }
@ -500,14 +501,14 @@ static void updateRcvrActivitySample(uint32_t rcvr_id, uint16_t samples[], uint8
} }
} }
static bool updateRcvrActivityCompare(uint32_t rcvr_id, struct rcvr_activity_fsm * fsm) static bool updateRcvrActivityCompare(uint32_t rcvr_id, struct rcvr_activity_fsm *fsm)
{ {
bool activity_updated = false; bool activity_updated = false;
/* Compare the current value to the previous sampled value */ /* Compare the current value to the previous sampled value */
for (uint8_t channel = 1; channel <= RCVR_ACTIVITY_MONITOR_CHANNELS_PER_GROUP; channel++) { for (uint8_t channel = 1; channel <= RCVR_ACTIVITY_MONITOR_CHANNELS_PER_GROUP; channel++) {
uint16_t delta; uint16_t delta;
uint16_t prev = fsm->prev[channel - 1]; // Subtract 1 because channels are 1 indexed uint16_t prev = fsm->prev[channel - 1]; // Subtract 1 because channels are 1 indexed
uint16_t curr = PIOS_RCVR_Read(rcvr_id, channel); uint16_t curr = PIOS_RCVR_Read(rcvr_id, channel);
if (curr > prev) { if (curr > prev) {
delta = curr - prev; delta = curr - prev;
@ -521,41 +522,41 @@ static bool updateRcvrActivityCompare(uint32_t rcvr_id, struct rcvr_activity_fsm
/* Don't assume manualcontrolsettings and receiveractivity are in the same order. */ /* Don't assume manualcontrolsettings and receiveractivity are in the same order. */
switch (fsm->group) { switch (fsm->group) {
case MANUALCONTROLSETTINGS_CHANNELGROUPS_PWM: case MANUALCONTROLSETTINGS_CHANNELGROUPS_PWM:
group = RECEIVERACTIVITY_ACTIVEGROUP_PWM; group = RECEIVERACTIVITY_ACTIVEGROUP_PWM;
break; break;
case MANUALCONTROLSETTINGS_CHANNELGROUPS_PPM: case MANUALCONTROLSETTINGS_CHANNELGROUPS_PPM:
group = RECEIVERACTIVITY_ACTIVEGROUP_PPM; group = RECEIVERACTIVITY_ACTIVEGROUP_PPM;
break; break;
case MANUALCONTROLSETTINGS_CHANNELGROUPS_DSMMAINPORT: case MANUALCONTROLSETTINGS_CHANNELGROUPS_DSMMAINPORT:
group = RECEIVERACTIVITY_ACTIVEGROUP_DSMMAINPORT; group = RECEIVERACTIVITY_ACTIVEGROUP_DSMMAINPORT;
break; break;
case MANUALCONTROLSETTINGS_CHANNELGROUPS_DSMFLEXIPORT: case MANUALCONTROLSETTINGS_CHANNELGROUPS_DSMFLEXIPORT:
group = RECEIVERACTIVITY_ACTIVEGROUP_DSMFLEXIPORT; group = RECEIVERACTIVITY_ACTIVEGROUP_DSMFLEXIPORT;
break; break;
case MANUALCONTROLSETTINGS_CHANNELGROUPS_SBUS: case MANUALCONTROLSETTINGS_CHANNELGROUPS_SBUS:
group = RECEIVERACTIVITY_ACTIVEGROUP_SBUS; group = RECEIVERACTIVITY_ACTIVEGROUP_SBUS;
break; break;
case MANUALCONTROLSETTINGS_CHANNELGROUPS_GCS: case MANUALCONTROLSETTINGS_CHANNELGROUPS_GCS:
group = RECEIVERACTIVITY_ACTIVEGROUP_GCS; group = RECEIVERACTIVITY_ACTIVEGROUP_GCS;
break; break;
case MANUALCONTROLSETTINGS_CHANNELGROUPS_OPLINK: case MANUALCONTROLSETTINGS_CHANNELGROUPS_OPLINK:
group = RECEIVERACTIVITY_ACTIVEGROUP_OPLINK; group = RECEIVERACTIVITY_ACTIVEGROUP_OPLINK;
break; break;
default: default:
PIOS_Assert(0); PIOS_Assert(0);
break; break;
} }
ReceiverActivityActiveGroupSet((uint8_t*) &group); ReceiverActivityActiveGroupSet((uint8_t *)&group);
ReceiverActivityActiveChannelSet(&channel); ReceiverActivityActiveChannelSet(&channel);
activity_updated = true; activity_updated = true;
} }
} }
return (activity_updated); return activity_updated;
} }
static bool updateRcvrActivity(struct rcvr_activity_fsm * fsm) static bool updateRcvrActivity(struct rcvr_activity_fsm *fsm)
{ {
bool activity_updated = false; bool activity_updated = false;
@ -574,13 +575,13 @@ static bool updateRcvrActivity(struct rcvr_activity_fsm * fsm)
/* Take a sample of each channel in this group */ /* Take a sample of each channel in this group */
updateRcvrActivitySample(pios_rcvr_group_map[fsm->group], fsm->prev, NELEMENTS(fsm->prev)); updateRcvrActivitySample(pios_rcvr_group_map[fsm->group], fsm->prev, NELEMENTS(fsm->prev));
fsm->sample_count++; fsm->sample_count++;
return (false); return false;
} }
/* Compare with previous sample */ /* Compare with previous sample */
activity_updated = updateRcvrActivityCompare(pios_rcvr_group_map[fsm->group], fsm); activity_updated = updateRcvrActivityCompare(pios_rcvr_group_map[fsm->group], fsm);
group_completed: group_completed:
/* Reset the sample counter */ /* Reset the sample counter */
fsm->sample_count = 0; fsm->sample_count = 0;
@ -604,87 +605,89 @@ static bool updateRcvrActivity(struct rcvr_activity_fsm * fsm)
} }
} }
return (activity_updated); return activity_updated;
} }
static void updateActuatorDesired(ManualControlCommandData * cmd) static void updateActuatorDesired(ManualControlCommandData *cmd)
{ {
ActuatorDesiredData actuator; ActuatorDesiredData actuator;
ActuatorDesiredGet(&actuator); ActuatorDesiredGet(&actuator);
actuator.Roll = cmd->Roll; actuator.Roll = cmd->Roll;
actuator.Pitch = cmd->Pitch; actuator.Pitch = cmd->Pitch;
actuator.Yaw = cmd->Yaw; actuator.Yaw = cmd->Yaw;
actuator.Throttle = (cmd->Throttle < 0) ? -1 : cmd->Throttle; actuator.Throttle = (cmd->Throttle < 0) ? -1 : cmd->Throttle;
ActuatorDesiredSet(&actuator); ActuatorDesiredSet(&actuator);
} }
static void updateStabilizationDesired(ManualControlCommandData * cmd, ManualControlSettingsData * settings) static void updateStabilizationDesired(ManualControlCommandData *cmd, ManualControlSettingsData *settings)
{ {
StabilizationDesiredData stabilization; StabilizationDesiredData stabilization;
StabilizationDesiredGet(&stabilization); StabilizationDesiredGet(&stabilization);
StabilizationSettingsData stabSettings; StabilizationSettingsData stabSettings;
StabilizationSettingsGet(&stabSettings); StabilizationSettingsGet(&stabSettings);
uint8_t * stab_settings; uint8_t *stab_settings;
FlightStatusData flightStatus; FlightStatusData flightStatus;
FlightStatusGet(&flightStatus); FlightStatusGet(&flightStatus);
switch (flightStatus.FlightMode) { switch (flightStatus.FlightMode) {
case FLIGHTSTATUS_FLIGHTMODE_STABILIZED1: case FLIGHTSTATUS_FLIGHTMODE_STABILIZED1:
stab_settings = settings->Stabilization1Settings; stab_settings = settings->Stabilization1Settings;
break; break;
case FLIGHTSTATUS_FLIGHTMODE_STABILIZED2: case FLIGHTSTATUS_FLIGHTMODE_STABILIZED2:
stab_settings = settings->Stabilization2Settings; stab_settings = settings->Stabilization2Settings;
break; break;
case FLIGHTSTATUS_FLIGHTMODE_STABILIZED3: case FLIGHTSTATUS_FLIGHTMODE_STABILIZED3:
stab_settings = settings->Stabilization3Settings; stab_settings = settings->Stabilization3Settings;
break; break;
default: default:
// Major error, this should not occur because only enter this block when one of these is true // Major error, this should not occur because only enter this block when one of these is true
AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_CRITICAL); AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_CRITICAL);
return; return;
} }
// TOOD: Add assumption about order of stabilization desired and manual control stabilization mode fields having same order // TOOD: Add assumption about order of stabilization desired and manual control stabilization mode fields having same order
stabilization.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_ROLL] = stab_settings[0]; stabilization.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_ROLL] = stab_settings[0];
stabilization.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_PITCH] = stab_settings[1]; stabilization.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_PITCH] = stab_settings[1];
stabilization.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = stab_settings[2]; stabilization.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = stab_settings[2];
stabilization.Roll = stabilization.Roll =
(stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) ? cmd->Roll : (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) ? cmd->Roll :
(stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) ? cmd->Roll * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_ROLL] : (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) ? cmd->Roll * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_ROLL] :
(stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) ? (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) ?
cmd->Roll * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_ROLL] : cmd->Roll * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_ROLL] :
(stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) ? cmd->Roll * stabSettings.RollMax : (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) ? cmd->Roll * stabSettings.RollMax :
(stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) ? cmd->Roll * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_ROLL] : (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) ? cmd->Roll * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_ROLL] :
(stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR) ? cmd->Roll : (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR) ? cmd->Roll :
(stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE) ? (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE) ?
cmd->Roll * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_ROLL] : cmd->Roll * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_ROLL] :
(stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYATTITUDE) ? cmd->Roll * stabSettings.RollMax : 0; // this is an invalid mode (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYATTITUDE) ? cmd->Roll * stabSettings.RollMax : 0; // this is an invalid mode
; ;
stabilization.Pitch = stabilization.Pitch =
(stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) ? cmd->Pitch : (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) ? cmd->Pitch :
(stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) ? cmd->Pitch * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_PITCH] : (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) ? cmd->Pitch * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_PITCH] :
(stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) ? (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) ?
cmd->Pitch * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_PITCH] : cmd->Pitch * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_PITCH] :
(stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) ? cmd->Pitch * stabSettings.PitchMax : (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) ? cmd->Pitch * stabSettings.PitchMax :
(stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) ? (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) ?
cmd->Pitch * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_PITCH] : cmd->Pitch * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_PITCH] :
(stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR) ? cmd->Pitch : (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR) ? cmd->Pitch :
(stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE) ? (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE) ?
cmd->Pitch * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_PITCH] : cmd->Pitch * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_PITCH] :
(stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYATTITUDE) ? cmd->Pitch * stabSettings.PitchMax : 0; // this is an invalid mode (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYATTITUDE) ? cmd->Pitch * stabSettings.PitchMax : 0; // this is an invalid mode
stabilization.Yaw = stabilization.Yaw =
(stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) ? cmd->Yaw : (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) ? cmd->Yaw :
(stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) ? cmd->Yaw * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_YAW] : (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) ? cmd->Yaw * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_YAW] :
(stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) ? (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) ?
cmd->Yaw * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_YAW] : cmd->Yaw * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_YAW] :
(stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) ? cmd->Yaw * stabSettings.YawMax : (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) ? cmd->Yaw * stabSettings.YawMax :
(stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) ? cmd->Yaw * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_YAW] : (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) ? cmd->Yaw * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_YAW] :
(stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR) ? cmd->Yaw : (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR) ? cmd->Yaw :
(stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE) ? cmd->Yaw * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_YAW] : (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE) ? cmd->Yaw * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_YAW] :
(stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYATTITUDE) ? cmd->Yaw * stabSettings.YawMax : 0; // this is an invalid mode (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYATTITUDE) ? cmd->Yaw * stabSettings.YawMax : 0; // this is an invalid mode
stabilization.Throttle = (cmd->Throttle < 0) ? -1 : cmd->Throttle; stabilization.Throttle = (cmd->Throttle < 0) ? -1 : cmd->Throttle;
StabilizationDesiredSet(&stabilization); StabilizationDesiredSet(&stabilization);
@ -696,10 +699,11 @@ static void updateStabilizationDesired(ManualControlCommandData * cmd, ManualCon
* @brief Update the position desired to current location when * @brief Update the position desired to current location when
* enabled and allow the waypoint to be moved by transmitter * enabled and allow the waypoint to be moved by transmitter
*/ */
static void updatePathDesired(ManualControlCommandData * cmd, bool changed,bool home) static void updatePathDesired(ManualControlCommandData *cmd, bool changed, bool home)
{ {
static portTickType lastSysTime; static portTickType lastSysTime;
portTickType thisSysTime = xTaskGetTickCount(); portTickType thisSysTime = xTaskGetTickCount();
/* float dT = (thisSysTime - lastSysTime) / portTICK_RATE_MS / 1000.0f; */ /* float dT = (thisSysTime - lastSysTime) / portTICK_RATE_MS / 1000.0f; */
lastSysTime = thisSysTime; lastSysTime = thisSysTime;
@ -711,16 +715,16 @@ static void updatePathDesired(ManualControlCommandData * cmd, bool changed,bool
PathDesiredData pathDesired; PathDesiredData pathDesired;
PathDesiredGet(&pathDesired); PathDesiredGet(&pathDesired);
pathDesired.Start[PATHDESIRED_START_NORTH] = 0; pathDesired.Start[PATHDESIRED_START_NORTH] = 0;
pathDesired.Start[PATHDESIRED_START_EAST] = 0; pathDesired.Start[PATHDESIRED_START_EAST] = 0;
pathDesired.Start[PATHDESIRED_START_DOWN] = positionActual.Down - 10; pathDesired.Start[PATHDESIRED_START_DOWN] = positionActual.Down - 10;
pathDesired.End[PATHDESIRED_END_NORTH] = 0; pathDesired.End[PATHDESIRED_END_NORTH] = 0;
pathDesired.End[PATHDESIRED_END_EAST] = 0; pathDesired.End[PATHDESIRED_END_EAST] = 0;
pathDesired.End[PATHDESIRED_END_DOWN] = positionActual.Down - 10; pathDesired.End[PATHDESIRED_END_DOWN] = positionActual.Down - 10;
pathDesired.StartingVelocity=1; pathDesired.StartingVelocity = 1;
pathDesired.EndingVelocity=0; pathDesired.EndingVelocity = 0;
pathDesired.Mode = PATHDESIRED_MODE_FLYENDPOINT; pathDesired.Mode = PATHDESIRED_MODE_FLYENDPOINT;
PathDesiredSet(&pathDesired); PathDesiredSet(&pathDesired);
} else if(changed) { } else if (changed) {
// After not being in this mode for a while init at current height // After not being in this mode for a while init at current height
PositionActualData positionActual; PositionActualData positionActual;
PositionActualGet(&positionActual); PositionActualGet(&positionActual);
@ -728,28 +732,28 @@ static void updatePathDesired(ManualControlCommandData * cmd, bool changed,bool
PathDesiredData pathDesired; PathDesiredData pathDesired;
PathDesiredGet(&pathDesired); PathDesiredGet(&pathDesired);
pathDesired.Start[PATHDESIRED_START_NORTH] = positionActual.North; pathDesired.Start[PATHDESIRED_START_NORTH] = positionActual.North;
pathDesired.Start[PATHDESIRED_START_EAST] = positionActual.East; pathDesired.Start[PATHDESIRED_START_EAST] = positionActual.East;
pathDesired.Start[PATHDESIRED_START_DOWN] = positionActual.Down; pathDesired.Start[PATHDESIRED_START_DOWN] = positionActual.Down;
pathDesired.End[PATHDESIRED_END_NORTH] = positionActual.North; pathDesired.End[PATHDESIRED_END_NORTH] = positionActual.North;
pathDesired.End[PATHDESIRED_END_EAST] = positionActual.East; pathDesired.End[PATHDESIRED_END_EAST] = positionActual.East;
pathDesired.End[PATHDESIRED_END_DOWN] = positionActual.Down; pathDesired.End[PATHDESIRED_END_DOWN] = positionActual.Down;
pathDesired.StartingVelocity=1; pathDesired.StartingVelocity = 1;
pathDesired.EndingVelocity=0; pathDesired.EndingVelocity = 0;
pathDesired.Mode = PATHDESIRED_MODE_FLYENDPOINT; pathDesired.Mode = PATHDESIRED_MODE_FLYENDPOINT;
PathDesiredSet(&pathDesired); PathDesiredSet(&pathDesired);
/* Disable this section, until such time as proper discussion can be had about how to implement it for all types of crafts. /* Disable this section, until such time as proper discussion can be had about how to implement it for all types of crafts.
} else { } else {
PathDesiredData pathDesired; PathDesiredData pathDesired;
PathDesiredGet(&pathDesired); PathDesiredGet(&pathDesired);
pathDesired.End[PATHDESIRED_END_NORTH] += dT * -cmd->Pitch; pathDesired.End[PATHDESIRED_END_NORTH] += dT * -cmd->Pitch;
pathDesired.End[PATHDESIRED_END_EAST] += dT * cmd->Roll; pathDesired.End[PATHDESIRED_END_EAST] += dT * cmd->Roll;
pathDesired.Mode = PATHDESIRED_MODE_FLYENDPOINT; pathDesired.Mode = PATHDESIRED_MODE_FLYENDPOINT;
PathDesiredSet(&pathDesired); PathDesiredSet(&pathDesired);
*/ */
} }
} }
static void updateLandDesired(ManualControlCommandData * cmd, bool changed) static void updateLandDesired(ManualControlCommandData *cmd, bool changed)
{ {
static portTickType lastSysTime; static portTickType lastSysTime;
portTickType thisSysTime; portTickType thisSysTime;
@ -764,19 +768,19 @@ static void updateLandDesired(ManualControlCommandData * cmd, bool changed)
PathDesiredData pathDesired; PathDesiredData pathDesired;
PathDesiredGet(&pathDesired); PathDesiredGet(&pathDesired);
if(changed) { if (changed) {
// After not being in this mode for a while init at current height // After not being in this mode for a while init at current height
pathDesired.Start[PATHDESIRED_START_NORTH] = positionActual.North; pathDesired.Start[PATHDESIRED_START_NORTH] = positionActual.North;
pathDesired.Start[PATHDESIRED_START_EAST] = positionActual.East; pathDesired.Start[PATHDESIRED_START_EAST] = positionActual.East;
pathDesired.Start[PATHDESIRED_START_DOWN] = positionActual.Down; pathDesired.Start[PATHDESIRED_START_DOWN] = positionActual.Down;
pathDesired.End[PATHDESIRED_END_NORTH] = positionActual.North; pathDesired.End[PATHDESIRED_END_NORTH] = positionActual.North;
pathDesired.End[PATHDESIRED_END_EAST] = positionActual.East; pathDesired.End[PATHDESIRED_END_EAST] = positionActual.East;
pathDesired.End[PATHDESIRED_END_DOWN] = positionActual.Down; pathDesired.End[PATHDESIRED_END_DOWN] = positionActual.Down;
pathDesired.StartingVelocity=1; pathDesired.StartingVelocity = 1;
pathDesired.EndingVelocity=0; pathDesired.EndingVelocity = 0;
pathDesired.Mode = PATHDESIRED_MODE_FLYENDPOINT; pathDesired.Mode = PATHDESIRED_MODE_FLYENDPOINT;
} }
pathDesired.End[PATHDESIRED_END_DOWN] = positionActual.Down+5; pathDesired.End[PATHDESIRED_END_DOWN] = positionActual.Down + 5;
PathDesiredSet(&pathDesired); PathDesiredSet(&pathDesired);
} }
@ -785,16 +789,20 @@ static void updateLandDesired(ManualControlCommandData * cmd, bool changed)
* enabled and enable altitude mode for stabilization * enabled and enable altitude mode for stabilization
* @todo: Need compile flag to exclude this from copter control * @todo: Need compile flag to exclude this from copter control
*/ */
static void altitudeHoldDesired(ManualControlCommandData * cmd, bool changed) static void altitudeHoldDesired(ManualControlCommandData *cmd, bool changed)
{ {
const float DEADBAND_HIGH = 0.55; const float DEADBAND = .20f;
const float DEADBAND_LOW = 0.45; const float DEADBAND_HIGH = 1.0f / 2 + DEADBAND / 2;
const float DEADBAND_LOW = 1.0f / 2 - DEADBAND / 2;
// this is the max speed in m/s at the extents of throttle
const float MAX_ALT_CHANGE_RATE = 5;
static portTickType lastSysTime; static portTickType lastSysTime;
static bool zeroed = false; static bool zeroed = false;
portTickType thisSysTime; portTickType thisSysTime;
float dT; float dT;
AltitudeHoldDesiredData altitudeHoldDesired; AltitudeHoldDesiredData altitudeHoldDesired;
AltitudeHoldDesiredGet(&altitudeHoldDesired); AltitudeHoldDesiredGet(&altitudeHoldDesired);
StabilizationSettingsData stabSettings; StabilizationSettingsData stabSettings;
@ -804,47 +812,48 @@ static void altitudeHoldDesired(ManualControlCommandData * cmd, bool changed)
dT = (thisSysTime - lastSysTime) / portTICK_RATE_MS / 1000.0f; dT = (thisSysTime - lastSysTime) / portTICK_RATE_MS / 1000.0f;
lastSysTime = thisSysTime; lastSysTime = thisSysTime;
altitudeHoldDesired.Roll = cmd->Roll * stabSettings.RollMax; altitudeHoldDesired.Roll = cmd->Roll * stabSettings.RollMax;
altitudeHoldDesired.Pitch = cmd->Pitch * stabSettings.PitchMax; altitudeHoldDesired.Pitch = cmd->Pitch * stabSettings.PitchMax;
altitudeHoldDesired.Yaw = cmd->Yaw * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_YAW]; altitudeHoldDesired.Yaw = cmd->Yaw * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_YAW];
float currentDown; float currentDown;
PositionActualDownGet(&currentDown); PositionActualDownGet(&currentDown);
if(changed) { if (changed) {
// After not being in this mode for a while init at current height // After not being in this mode for a while init at current height
altitudeHoldDesired.Altitude = 0; altitudeHoldDesired.Altitude = 0;
zeroed = false; zeroed = false;
} else if (cmd->Throttle > DEADBAND_HIGH && zeroed) { } else if (cmd->Throttle > DEADBAND_HIGH && zeroed) {
altitudeHoldDesired.Altitude += (cmd->Throttle - DEADBAND_HIGH) * dT; // being the two band simmetrical I can divide by DEADBAND_LOW to scale it to a value betweeon 0 and 1
// then apply an "exp"
altitudeHoldDesired.Altitude += powf((cmd->Throttle - DEADBAND_HIGH) / (DEADBAND_LOW), 3) * MAX_ALT_CHANGE_RATE * dT;
} else if (cmd->Throttle < DEADBAND_LOW && zeroed) { } else if (cmd->Throttle < DEADBAND_LOW && zeroed) {
altitudeHoldDesired.Altitude += (cmd->Throttle - DEADBAND_LOW) * dT; altitudeHoldDesired.Altitude += powf(((cmd->Throttle < 0 ? 0 : cmd->Throttle) - DEADBAND_LOW) / (DEADBAND_LOW), 3) * MAX_ALT_CHANGE_RATE * dT;
} else if (cmd->Throttle >= DEADBAND_LOW && cmd->Throttle <= DEADBAND_HIGH) { } else if (cmd->Throttle >= DEADBAND_LOW && cmd->Throttle <= DEADBAND_HIGH) {
// Require the stick to enter the dead band before they can move height // Require the stick to enter the dead band before they can move height
zeroed = true; zeroed = true;
} }
AltitudeHoldDesiredSet(&altitudeHoldDesired); AltitudeHoldDesiredSet(&altitudeHoldDesired);
} }
#else #else /* if defined(REVOLUTION) */
// TODO: These functions should never be accessible on CC. Any configuration that // TODO: These functions should never be accessible on CC. Any configuration that
// could allow them to be called sholud already throw an error to prevent this happening // could allow them to be called sholud already throw an error to prevent this happening
// in flight // in flight
static void updatePathDesired(ManualControlCommandData * cmd, bool changed, bool home) static void updatePathDesired(ManualControlCommandData *cmd, bool changed, bool home)
{ {
AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_ERROR); AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_ERROR);
} }
static void updateLandDesired(ManualControlCommandData * cmd, bool changed) static void updateLandDesired(ManualControlCommandData *cmd, bool changed)
{ {
AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_ERROR); AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_ERROR);
} }
static void altitudeHoldDesired(ManualControlCommandData * cmd, bool changed) static void altitudeHoldDesired(ManualControlCommandData *cmd, bool changed)
{ {
AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_ERROR); AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_ERROR);
} }
#endif #endif /* if defined(REVOLUTION) */
/** /**
* Convert channel from servo pulse duration (microseconds) to scaled -1/+1 range. * Convert channel from servo pulse duration (microseconds) to scaled -1/+1 range.
*/ */
@ -855,13 +864,13 @@ static float scaleChannel(int16_t value, int16_t max, int16_t min, int16_t neutr
// Scale // Scale
if ((max > min && value >= neutral) || (min > max && value <= neutral)) { if ((max > min && value >= neutral) || (min > max && value <= neutral)) {
if (max != neutral) { if (max != neutral) {
valueScaled = (float) (value - neutral) / (float) (max - neutral); valueScaled = (float)(value - neutral) / (float)(max - neutral);
} else { } else {
valueScaled = 0; valueScaled = 0;
} }
} else { } else {
if (min != neutral) { if (min != neutral) {
valueScaled = (float) (value - neutral) / (float) (neutral - min); valueScaled = (float)(value - neutral) / (float)(neutral - min);
} else { } else {
valueScaled = 0; valueScaled = 0;
} }
@ -893,13 +902,15 @@ static bool okToArm(void)
{ {
// read alarms // read alarms
SystemAlarmsData alarms; SystemAlarmsData alarms;
SystemAlarmsGet(&alarms); SystemAlarmsGet(&alarms);
// Check each alarm // Check each alarm
for (int i = 0; i < SYSTEMALARMS_ALARM_NUMELEM; i++) { for (int i = 0; i < SYSTEMALARMS_ALARM_NUMELEM; i++) {
if (alarms.Alarm[i] >= SYSTEMALARMS_ALARM_ERROR) { // found an alarm thats set if (alarms.Alarm[i] >= SYSTEMALARMS_ALARM_ERROR) { // found an alarm thats set
if (i == SYSTEMALARMS_ALARM_GPS || i == SYSTEMALARMS_ALARM_TELEMETRY) if (i == SYSTEMALARMS_ALARM_GPS || i == SYSTEMALARMS_ALARM_TELEMETRY) {
continue; continue;
}
return false; return false;
} }
@ -915,6 +926,7 @@ static bool forcedDisArm(void)
{ {
// read alarms // read alarms
SystemAlarmsData alarms; SystemAlarmsData alarms;
SystemAlarmsGet(&alarms); SystemAlarmsGet(&alarms);
if (alarms.Alarm[SYSTEMALARMS_ALARM_GUIDANCE] == SYSTEMALARMS_ALARM_CRITICAL) { if (alarms.Alarm[SYSTEMALARMS_ALARM_GUIDANCE] == SYSTEMALARMS_ALARM_CRITICAL) {
@ -930,6 +942,7 @@ static bool forcedDisArm(void)
static void setArmedIfChanged(uint8_t val) static void setArmedIfChanged(uint8_t val)
{ {
FlightStatusData flightStatus; FlightStatusData flightStatus;
FlightStatusGet(&flightStatus); FlightStatusGet(&flightStatus);
if (flightStatus.Armed != val) { if (flightStatus.Armed != val) {
@ -943,9 +956,8 @@ static void setArmedIfChanged(uint8_t val)
* @param[out] cmd The structure to set the armed in * @param[out] cmd The structure to set the armed in
* @param[in] settings Settings indicating the necessary position * @param[in] settings Settings indicating the necessary position
*/ */
static void processArm(ManualControlCommandData * cmd, ManualControlSettingsData * settings) static void processArm(ManualControlCommandData *cmd, ManualControlSettingsData *settings)
{ {
bool lowThrottle = cmd->Throttle <= 0; bool lowThrottle = cmd->Throttle <= 0;
if (forcedDisArm()) { if (forcedDisArm()) {
@ -959,22 +971,23 @@ static void processArm(ManualControlCommandData * cmd, ManualControlSettingsData
setArmedIfChanged(FLIGHTSTATUS_ARMED_DISARMED); setArmedIfChanged(FLIGHTSTATUS_ARMED_DISARMED);
} else { } else {
// Not really needed since this function not called when disconnected // Not really needed since this function not called when disconnected
if (cmd->Connected == MANUALCONTROLCOMMAND_CONNECTED_FALSE) if (cmd->Connected == MANUALCONTROLCOMMAND_CONNECTED_FALSE) {
lowThrottle = true; lowThrottle = true;
}
// The throttle is not low, in case we where arming or disarming, abort // The throttle is not low, in case we where arming or disarming, abort
if (!lowThrottle) { if (!lowThrottle) {
switch (armState) { switch (armState) {
case ARM_STATE_DISARMING_MANUAL: case ARM_STATE_DISARMING_MANUAL:
case ARM_STATE_DISARMING_TIMEOUT: case ARM_STATE_DISARMING_TIMEOUT:
armState = ARM_STATE_ARMED; armState = ARM_STATE_ARMED;
break; break;
case ARM_STATE_ARMING_MANUAL: case ARM_STATE_ARMING_MANUAL:
armState = ARM_STATE_DISARMED; armState = ARM_STATE_DISARMED;
break; break;
default: default:
// Nothing needs to be done in the other states // Nothing needs to be done in the other states
break; break;
} }
return; return;
} }
@ -994,72 +1007,76 @@ static void processArm(ManualControlCommandData * cmd, ManualControlSettingsData
// Calc channel see assumptions7 // Calc channel see assumptions7
int8_t sign = ((settings->Arming - MANUALCONTROLSETTINGS_ARMING_ROLLLEFT) % 2) ? -1 : 1; int8_t sign = ((settings->Arming - MANUALCONTROLSETTINGS_ARMING_ROLLLEFT) % 2) ? -1 : 1;
switch ((settings->Arming - MANUALCONTROLSETTINGS_ARMING_ROLLLEFT) / 2) { switch ((settings->Arming - MANUALCONTROLSETTINGS_ARMING_ROLLLEFT) / 2) {
case ARMING_CHANNEL_ROLL: case ARMING_CHANNEL_ROLL:
armingInputLevel = sign * cmd->Roll; armingInputLevel = sign * cmd->Roll;
break; break;
case ARMING_CHANNEL_PITCH: case ARMING_CHANNEL_PITCH:
armingInputLevel = sign * cmd->Pitch; armingInputLevel = sign * cmd->Pitch;
break; break;
case ARMING_CHANNEL_YAW: case ARMING_CHANNEL_YAW:
armingInputLevel = sign * cmd->Yaw; armingInputLevel = sign * cmd->Yaw;
break; break;
} }
bool manualArm = false; bool manualArm = false;
bool manualDisarm = false; bool manualDisarm = false;
if (armingInputLevel <= -ARMED_THRESHOLD) if (armingInputLevel <= -ARMED_THRESHOLD) {
manualArm = true; manualArm = true;
else if (armingInputLevel >= +ARMED_THRESHOLD) } else if (armingInputLevel >= +ARMED_THRESHOLD) {
manualDisarm = true; manualDisarm = true;
}
switch (armState) { switch (armState) {
case ARM_STATE_DISARMED: case ARM_STATE_DISARMED:
setArmedIfChanged(FLIGHTSTATUS_ARMED_DISARMED); setArmedIfChanged(FLIGHTSTATUS_ARMED_DISARMED);
// only allow arming if it's OK too // only allow arming if it's OK too
if (manualArm && okToArm()) { if (manualArm && okToArm()) {
armedDisarmStart = lastSysTime;
armState = ARM_STATE_ARMING_MANUAL;
}
break;
case ARM_STATE_ARMING_MANUAL:
setArmedIfChanged(FLIGHTSTATUS_ARMED_ARMING);
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; armedDisarmStart = lastSysTime;
armState = ARM_STATE_DISARMING_TIMEOUT; armState = ARM_STATE_ARMING_MANUAL;
setArmedIfChanged(FLIGHTSTATUS_ARMED_ARMED); }
break; break;
case ARM_STATE_DISARMING_TIMEOUT: case ARM_STATE_ARMING_MANUAL:
// We get here when armed while throttle low, even when the arming timeout is not enabled setArmedIfChanged(FLIGHTSTATUS_ARMED_ARMING);
if ((settings->ArmedTimeout != 0) && (timeDifferenceMs(armedDisarmStart, lastSysTime) > settings->ArmedTimeout))
armState = ARM_STATE_DISARMED;
// Switch to disarming due to manual control when needed if (manualArm && (timeDifferenceMs(armedDisarmStart, lastSysTime) > ARMED_TIME_MS)) {
if (manualDisarm) { armState = ARM_STATE_ARMED;
armedDisarmStart = lastSysTime; } else if (!manualArm) {
armState = ARM_STATE_DISARMING_MANUAL; armState = ARM_STATE_DISARMED;
} }
break; break;
case ARM_STATE_DISARMING_MANUAL: case ARM_STATE_ARMED:
if (manualDisarm && (timeDifferenceMs(armedDisarmStart, lastSysTime) > ARMED_TIME_MS)) // When we get here, the throttle is low,
armState = ARM_STATE_DISARMED; // we go immediately to disarming due to timeout, also when the disarming mechanism is not enabled
else if (!manualDisarm) armedDisarmStart = lastSysTime;
armState = ARM_STATE_ARMED; armState = ARM_STATE_DISARMING_TIMEOUT;
break; setArmedIfChanged(FLIGHTSTATUS_ARMED_ARMED);
} // End Switch 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
} }
} }
@ -1072,12 +1089,14 @@ static void processArm(ManualControlCommandData * cmd, ManualControlSettingsData
static void processFlightMode(ManualControlSettingsData *settings, float flightMode) static void processFlightMode(ManualControlSettingsData *settings, float flightMode)
{ {
FlightStatusData flightStatus; FlightStatusData flightStatus;
FlightStatusGet(&flightStatus); FlightStatusGet(&flightStatus);
// Convert flightMode value into the switch position in the range [0..N-1] // Convert flightMode value into the switch position in the range [0..N-1]
uint8_t pos = ((int16_t)(flightMode * 256.0f) + 256) * settings->FlightModeNumber >> 9; uint8_t pos = ((int16_t)(flightMode * 256.0f) + 256) * settings->FlightModeNumber >> 9;
if (pos >= settings->FlightModeNumber) if (pos >= settings->FlightModeNumber) {
pos = settings->FlightModeNumber - 1; pos = settings->FlightModeNumber - 1;
}
uint8_t newMode = settings->FlightModePosition[pos]; uint8_t newMode = settings->FlightModePosition[pos];
@ -1098,7 +1117,7 @@ bool validInputRange(int16_t min, int16_t max, uint16_t value)
min = max; min = max;
max = tmp; max = tmp;
} }
return (value >= min - CONNECTION_OFFSET && value <= max + CONNECTION_OFFSET); return value >= min - CONNECTION_OFFSET && value <= max + CONNECTION_OFFSET;
} }
/** /**
@ -1106,12 +1125,13 @@ bool validInputRange(int16_t min, int16_t max, uint16_t value)
*/ */
static void applyDeadband(float *value, float deadband) static void applyDeadband(float *value, float deadband)
{ {
if (fabs(*value) < deadband) if (fabs(*value) < deadband) {
*value = 0.0f; *value = 0.0f;
else if (*value > 0.0f) } else if (*value > 0.0f) {
*value -= deadband; *value -= deadband;
else } else {
*value += deadband; *value += deadband;
}
} }
#ifdef USE_INPUT_LPF #ifdef USE_INPUT_LPF
@ -1130,7 +1150,7 @@ static void applyLPF(float *value, ManualControlSettingsResponseTimeElem channel
/** /**
* Called whenever a critical configuration component changes * Called whenever a critical configuration component changes
*/ */
static void configurationUpdatedCb(UAVObjEvent * ev) static void configurationUpdatedCb(UAVObjEvent *ev)
{ {
configuration_check(); configuration_check();
} }