From 976999eba08bac8c24e2cf17e624d8c8c4dc9793 Mon Sep 17 00:00:00 2001 From: Alessio Morale Date: Fri, 3 May 2013 01:43:14 +0200 Subject: [PATCH] OP-938 Add an "Exp" to throttle stick when in AH to make it useable Conflicts: flight/modules/ManualControl/manualcontrol.c --- flight/modules/ManualControl/manualcontrol.c | 678 ++++++++++--------- 1 file changed, 349 insertions(+), 329 deletions(-) diff --git a/flight/modules/ManualControl/manualcontrol.c b/flight/modules/ManualControl/manualcontrol.c index 479ea221c..fd069b33b 100644 --- a/flight/modules/ManualControl/manualcontrol.c +++ b/flight/modules/ManualControl/manualcontrol.c @@ -53,26 +53,25 @@ #if defined(PIOS_INCLUDE_USB_RCTX) #include "pios_usb_rctx.h" -#endif /* PIOS_INCLUDE_USB_RCTX */ +#endif /* PIOS_INCLUDE_USB_RCTX */ // Private constants #if defined(PIOS_MANUAL_STACK_SIZE) -#define STACK_SIZE_BYTES PIOS_MANUAL_STACK_SIZE +#define STACK_SIZE_BYTES PIOS_MANUAL_STACK_SIZE #else -#define STACK_SIZE_BYTES 1024 +#define STACK_SIZE_BYTES 1024 #endif -#define TASK_PRIORITY (tskIDLE_PRIORITY+4) -#define UPDATE_PERIOD_MS 20 +#define TASK_PRIORITY (tskIDLE_PRIORITY + 4) +#define UPDATE_PERIOD_MS 20 #define THROTTLE_FAILSAFE -0.1f -#define ARMED_TIME_MS 1000 -#define ARMED_THRESHOLD 0.50f -//safe band to allow a bit of calibration error or trim offset (in microseconds) +#define ARMED_TIME_MS 1000 +#define ARMED_THRESHOLD 0.50f +// safe band to allow a bit of calibration error or trim offset (in microseconds) #define CONNECTION_OFFSET 250 // Private types -typedef enum -{ +typedef enum { ARM_STATE_DISARMED, ARM_STATE_ARMING_MANUAL, ARM_STATE_ARMED, ARM_STATE_DISARMING_MANUAL, ARM_STATE_DISARMING_TIMEOUT } ArmState_t; @@ -87,15 +86,15 @@ static float inputFiltered[MANUALCONTROLSETTINGS_RESPONSETIME_NUMELEM]; #endif // Private functions -static void updateActuatorDesired(ManualControlCommandData * cmd); -static void updateStabilizationDesired(ManualControlCommandData * cmd, ManualControlSettingsData * settings); -static void updateLandDesired(ManualControlCommandData * cmd, bool changed); -static void altitudeHoldDesired(ManualControlCommandData * cmd, bool changed); -static void updatePathDesired(ManualControlCommandData * cmd, bool changed, bool home); -static void processFlightMode(ManualControlSettingsData * settings, float flightMode); -static void processArm(ManualControlCommandData * cmd, ManualControlSettingsData * settings); +static void updateActuatorDesired(ManualControlCommandData *cmd); +static void updateStabilizationDesired(ManualControlCommandData *cmd, ManualControlSettingsData *settings); +static void updateLandDesired(ManualControlCommandData *cmd, bool changed); +static void altitudeHoldDesired(ManualControlCommandData *cmd, bool changed); +static void updatePathDesired(ManualControlCommandData *cmd, bool changed, bool home); +static void processFlightMode(ManualControlSettingsData *settings, float flightMode); +static void processArm(ManualControlCommandData *cmd, ManualControlSettingsData *settings); static void setArmedIfChanged(uint8_t val); -static void configurationUpdatedCb(UAVObjEvent * ev); +static void configurationUpdatedCb(UAVObjEvent *ev); static void manualControlTask(void *parameters); 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 #define RCVR_ACTIVITY_MONITOR_CHANNELS_PER_GROUP 12 -#define RCVR_ACTIVITY_MONITOR_MIN_RANGE 10 -struct rcvr_activity_fsm -{ +#define RCVR_ACTIVITY_MONITOR_MIN_RANGE 10 +struct rcvr_activity_fsm { ManualControlSettingsChannelGroupsOptions group; uint16_t prev[RCVR_ACTIVITY_MONITOR_CHANNELS_PER_GROUP]; uint8_t sample_count; }; static struct rcvr_activity_fsm activity_fsm; -static void resetRcvrActivity(struct rcvr_activity_fsm * fsm); -static bool updateRcvrActivity(struct rcvr_activity_fsm * fsm); +static void resetRcvrActivity(struct rcvr_activity_fsm *fsm); +static bool updateRcvrActivity(struct rcvr_activity_fsm *fsm); #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() { // 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); PIOS_WDG_RegisterFlag(PIOS_WDG_MANUAL); @@ -141,10 +139,10 @@ int32_t ManualControlStart() */ int32_t ManualControlInitialize() { - /* Check the assumptions about uavobject enum's are correct */ - if (!assumptions) + if (!assumptions) { return -1; + } AccessoryDesiredInitialize(); ManualControlCommandInitialize(); @@ -155,7 +153,7 @@ int32_t ManualControlInitialize() return 0; } -MODULE_INITCALL( ManualControlInitialize, ManualControlStart) +MODULE_INITCALL(ManualControlInitialize, ManualControlStart) /** * Module task @@ -168,7 +166,7 @@ static void manualControlTask(void *parameters) float flightMode = 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 // this includes not even registering it if not used @@ -231,7 +229,6 @@ static void manualControlTask(void *parameters) } if (!ManualControlCommandReadOnly()) { - bool valid_input_detected = true; // 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 // 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; - else + } else { scaledChannel[n] = scaleChannel(cmd.Channel[n], settings.ChannelMax[n], settings.ChannelMin[n], settings.ChannelNeutral[n]); + } } // Check settings, if error raise alarm if (settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL] >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE - || settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH] >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE - || settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW] >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE - || settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE] >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE - || - // Check all channel mappings are valid - cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL] == (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_THROTTLE] == (uint16_t) PIOS_RCVR_INVALID - || - // Check the driver exists - cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL] == (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_THROTTLE] == (uint16_t) PIOS_RCVR_NODRIVER || - // Check the FlightModeNumber is valid - 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 - ((settings.FlightModeNumber > 1) - && (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_NODRIVER))) { - + || settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH] >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE + || settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW] >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE + || settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE] >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE + || + // Check all channel mappings are valid + cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL] == (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_THROTTLE] == (uint16_t)PIOS_RCVR_INVALID + || + // Check the driver exists + cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL] == (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_THROTTLE] == (uint16_t)PIOS_RCVR_NODRIVER || + // Check the FlightModeNumber is valid + 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 + ((settings.FlightModeNumber > 1) + && (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_NODRIVER))) { AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_CRITICAL); cmd.Connected = MANUALCONTROLCOMMAND_CONNECTED_FALSE; ManualControlCommandSet(&cmd); @@ -290,32 +287,32 @@ static void manualControlTask(void *parameters) // decide if we have valid manual input or not valid_input_detected &= validInputRange(settings.ChannelMin[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE], - settings.ChannelMax[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE], cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE]) - && validInputRange(settings.ChannelMin[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL], - settings.ChannelMax[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL], cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL]) - && validInputRange(settings.ChannelMin[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW], - settings.ChannelMax[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW], cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW]) - && validInputRange(settings.ChannelMin[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH], - settings.ChannelMax[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH], cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH]); + settings.ChannelMax[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE], cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE]) + && validInputRange(settings.ChannelMin[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL], + settings.ChannelMax[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL], cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL]) + && validInputRange(settings.ChannelMin[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW], + settings.ChannelMax[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW], cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW]) + && validInputRange(settings.ChannelMin[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH], + settings.ChannelMax[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH], cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH]); // Implement hysteresis loop on connection status if (valid_input_detected && (++connected_count > 10)) { - cmd.Connected = MANUALCONTROLCOMMAND_CONNECTED_TRUE; - connected_count = 0; + cmd.Connected = MANUALCONTROLCOMMAND_CONNECTED_TRUE; + connected_count = 0; disconnected_count = 0; } else if (!valid_input_detected && (++disconnected_count > 10)) { - cmd.Connected = MANUALCONTROLCOMMAND_CONNECTED_FALSE; - connected_count = 0; + cmd.Connected = MANUALCONTROLCOMMAND_CONNECTED_FALSE; + connected_count = 0; disconnected_count = 0; } if (cmd.Connected == MANUALCONTROLCOMMAND_CONNECTED_FALSE) { - cmd.Throttle = -1; // Shut down engine with no control - cmd.Roll = 0; + cmd.Throttle = -1; // Shut down engine with no control + cmd.Roll = 0; cmd.Yaw = 0; - cmd.Pitch = 0; + cmd.Pitch = 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, // or leave throttle at IDLE speed or above when going into AUTO-failsafe. AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING); @@ -324,31 +321,33 @@ static void manualControlTask(void *parameters) // Set Accessory 0 if (settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY0] != MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) { accessory.AccessoryVal = 0; - if (AccessoryDesiredInstSet(0, &accessory) != 0) + if (AccessoryDesiredInstSet(0, &accessory) != 0) { AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING); + } } // Set Accessory 1 if (settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY1] != MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) { accessory.AccessoryVal = 0; - if (AccessoryDesiredInstSet(1, &accessory) != 0) + if (AccessoryDesiredInstSet(1, &accessory) != 0) { AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING); + } } // Set Accessory 2 if (settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY2] != MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) { accessory.AccessoryVal = 0; - if (AccessoryDesiredInstSet(2, &accessory) != 0) + if (AccessoryDesiredInstSet(2, &accessory) != 0) { AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING); + } } - } else if (valid_input_detected) { AlarmsClear(SYSTEMALARMS_ALARM_MANUALCONTROL); // Scale channels to -1 -> +1 range - cmd.Roll = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL]; - cmd.Pitch = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH]; - cmd.Yaw = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW]; + cmd.Roll = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL]; + cmd.Pitch = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH]; + cmd.Yaw = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW]; cmd.Throttle = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE]; - flightMode = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_FLIGHTMODE]; + flightMode = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_FLIGHTMODE]; // Apply deadband for Roll/Pitch/Yaw stick inputs 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 portTickType thisSysTime = xTaskGetTickCount(); float dT = (thisSysTime > lastSysTimeLPF) ? - (float)((thisSysTime - lastSysTimeLPF) * portTICK_RATE_MS) : - (float)UPDATE_PERIOD_MS; + (float)((thisSysTime - lastSysTimeLPF) * portTICK_RATE_MS) : + (float)UPDATE_PERIOD_MS; lastSysTimeLPF = thisSysTime; applyLPF(&cmd.Roll, MANUALCONTROLSETTINGS_RESPONSETIME_ROLL, &settings, dT); applyLPF(&cmd.Pitch, MANUALCONTROLSETTINGS_RESPONSETIME_PITCH, &settings, dT); applyLPF(&cmd.Yaw, MANUALCONTROLSETTINGS_RESPONSETIME_YAW, &settings, dT); #endif // USE_INPUT_LPF - 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_TIMEOUT) + 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_TIMEOUT) { cmd.Collective = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_COLLECTIVE]; + } AccessoryDesiredData accessory; // Set Accessory 0 @@ -380,8 +380,9 @@ static void manualControlTask(void *parameters) #ifdef USE_INPUT_LPF applyLPF(&accessory.AccessoryVal, MANUALCONTROLSETTINGS_RESPONSETIME_ACCESSORY0, &settings, dT); #endif - if (AccessoryDesiredInstSet(0, &accessory) != 0) + if (AccessoryDesiredInstSet(0, &accessory) != 0) { AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING); + } } // Set Accessory 1 if (settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY1] != MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) { @@ -389,8 +390,9 @@ static void manualControlTask(void *parameters) #ifdef USE_INPUT_LPF applyLPF(&accessory.AccessoryVal, MANUALCONTROLSETTINGS_RESPONSETIME_ACCESSORY1, &settings, dT); #endif - if (AccessoryDesiredInstSet(1, &accessory) != 0) + if (AccessoryDesiredInstSet(1, &accessory) != 0) { AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING); + } } // Set Accessory 2 if (settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY2] != MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) { @@ -398,12 +400,12 @@ static void manualControlTask(void *parameters) #ifdef USE_INPUT_LPF applyLPF(&accessory.AccessoryVal, MANUALCONTROLSETTINGS_RESPONSETIME_ACCESSORY2, &settings, dT); #endif - if (AccessoryDesiredInstSet(2, &accessory) != 0) + if (AccessoryDesiredInstSet(2, &accessory) != 0) { AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING); + } } processFlightMode(&settings, flightMode); - } // 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 (pios_usb_rctx_id) { PIOS_USB_RCTX_Update(pios_usb_rctx_id, - cmd.Channel, - settings.ChannelMin, - settings.ChannelMax, - NELEMENTS(cmd.Channel)); + cmd.Channel, + settings.ChannelMin, + settings.ChannelMax, + NELEMENTS(cmd.Channel)); } -#endif /* PIOS_INCLUDE_USB_RCTX */ - +#endif /* PIOS_INCLUDE_USB_RCTX */ } else { ManualControlCommandGet(&cmd); /* Under GCS control */ } @@ -430,48 +431,48 @@ static void manualControlTask(void *parameters) // Depending on the mode update the Stabilization or Actuator objects static uint8_t lastFlightMode = FLIGHTSTATUS_FLIGHTMODE_MANUAL; switch (PARSE_FLIGHT_MODE(flightStatus.FlightMode)) { - case FLIGHTMODE_UNDEFINED: - // This reflects a bug in the code architecture! + case FLIGHTMODE_UNDEFINED: + // 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); - 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); - } - break; + } + break; } lastFlightMode = flightStatus.FlightMode; } } -static void resetRcvrActivity(struct rcvr_activity_fsm * fsm) +static void resetRcvrActivity(struct rcvr_activity_fsm *fsm) { ReceiverActivityData data; bool updated = false; @@ -479,7 +480,7 @@ static void resetRcvrActivity(struct rcvr_activity_fsm * fsm) /* Clear all channel activity flags */ ReceiverActivityGet(&data); if (data.ActiveGroup != RECEIVERACTIVITY_ACTIVEGROUP_NONE && data.ActiveChannel != 255) { - data.ActiveGroup = RECEIVERACTIVITY_ACTIVEGROUP_NONE; + data.ActiveGroup = RECEIVERACTIVITY_ACTIVEGROUP_NONE; data.ActiveChannel = 255; 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; /* Compare the current value to the previous sampled value */ for (uint8_t channel = 1; channel <= RCVR_ACTIVITY_MONITOR_CHANNELS_PER_GROUP; channel++) { 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); if (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. */ switch (fsm->group) { - case MANUALCONTROLSETTINGS_CHANNELGROUPS_PWM: - group = RECEIVERACTIVITY_ACTIVEGROUP_PWM; - break; - case MANUALCONTROLSETTINGS_CHANNELGROUPS_PPM: - group = RECEIVERACTIVITY_ACTIVEGROUP_PPM; - break; - case MANUALCONTROLSETTINGS_CHANNELGROUPS_DSMMAINPORT: - group = RECEIVERACTIVITY_ACTIVEGROUP_DSMMAINPORT; - break; - case MANUALCONTROLSETTINGS_CHANNELGROUPS_DSMFLEXIPORT: - group = RECEIVERACTIVITY_ACTIVEGROUP_DSMFLEXIPORT; - break; - case MANUALCONTROLSETTINGS_CHANNELGROUPS_SBUS: - group = RECEIVERACTIVITY_ACTIVEGROUP_SBUS; - break; - case MANUALCONTROLSETTINGS_CHANNELGROUPS_GCS: - group = RECEIVERACTIVITY_ACTIVEGROUP_GCS; - break; - case MANUALCONTROLSETTINGS_CHANNELGROUPS_OPLINK: - group = RECEIVERACTIVITY_ACTIVEGROUP_OPLINK; - break; - default: - PIOS_Assert(0); - break; + case MANUALCONTROLSETTINGS_CHANNELGROUPS_PWM: + group = RECEIVERACTIVITY_ACTIVEGROUP_PWM; + break; + case MANUALCONTROLSETTINGS_CHANNELGROUPS_PPM: + group = RECEIVERACTIVITY_ACTIVEGROUP_PPM; + break; + case MANUALCONTROLSETTINGS_CHANNELGROUPS_DSMMAINPORT: + group = RECEIVERACTIVITY_ACTIVEGROUP_DSMMAINPORT; + break; + case MANUALCONTROLSETTINGS_CHANNELGROUPS_DSMFLEXIPORT: + group = RECEIVERACTIVITY_ACTIVEGROUP_DSMFLEXIPORT; + break; + case MANUALCONTROLSETTINGS_CHANNELGROUPS_SBUS: + group = RECEIVERACTIVITY_ACTIVEGROUP_SBUS; + break; + case MANUALCONTROLSETTINGS_CHANNELGROUPS_GCS: + group = RECEIVERACTIVITY_ACTIVEGROUP_GCS; + break; + case MANUALCONTROLSETTINGS_CHANNELGROUPS_OPLINK: + group = RECEIVERACTIVITY_ACTIVEGROUP_OPLINK; + break; + default: + PIOS_Assert(0); + break; } - ReceiverActivityActiveGroupSet((uint8_t*) &group); + ReceiverActivityActiveGroupSet((uint8_t *)&group); ReceiverActivityActiveChannelSet(&channel); 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; @@ -574,13 +575,13 @@ static bool updateRcvrActivity(struct rcvr_activity_fsm * fsm) /* Take a sample of each channel in this group */ updateRcvrActivitySample(pios_rcvr_group_map[fsm->group], fsm->prev, NELEMENTS(fsm->prev)); fsm->sample_count++; - return (false); + return false; } /* Compare with previous sample */ activity_updated = updateRcvrActivityCompare(pios_rcvr_group_map[fsm->group], fsm); - group_completed: +group_completed: /* Reset the sample counter */ 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; + ActuatorDesiredGet(&actuator); - actuator.Roll = cmd->Roll; - actuator.Pitch = cmd->Pitch; - actuator.Yaw = cmd->Yaw; + actuator.Roll = cmd->Roll; + actuator.Pitch = cmd->Pitch; + actuator.Yaw = cmd->Yaw; actuator.Throttle = (cmd->Throttle < 0) ? -1 : cmd->Throttle; ActuatorDesiredSet(&actuator); } -static void updateStabilizationDesired(ManualControlCommandData * cmd, ManualControlSettingsData * settings) +static void updateStabilizationDesired(ManualControlCommandData *cmd, ManualControlSettingsData *settings) { StabilizationDesiredData stabilization; + StabilizationDesiredGet(&stabilization); StabilizationSettingsData stabSettings; StabilizationSettingsGet(&stabSettings); - uint8_t * stab_settings; + uint8_t *stab_settings; FlightStatusData flightStatus; FlightStatusGet(&flightStatus); switch (flightStatus.FlightMode) { - case FLIGHTSTATUS_FLIGHTMODE_STABILIZED1: - stab_settings = settings->Stabilization1Settings; - break; - case FLIGHTSTATUS_FLIGHTMODE_STABILIZED2: - stab_settings = settings->Stabilization2Settings; - break; - case FLIGHTSTATUS_FLIGHTMODE_STABILIZED3: - stab_settings = settings->Stabilization3Settings; - break; - default: - // Major error, this should not occur because only enter this block when one of these is true - AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_CRITICAL); - return; + case FLIGHTSTATUS_FLIGHTMODE_STABILIZED1: + stab_settings = settings->Stabilization1Settings; + break; + case FLIGHTSTATUS_FLIGHTMODE_STABILIZED2: + stab_settings = settings->Stabilization2Settings; + break; + case FLIGHTSTATUS_FLIGHTMODE_STABILIZED3: + stab_settings = settings->Stabilization3Settings; + break; + default: + // Major error, this should not occur because only enter this block when one of these is true + AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_CRITICAL); + return; } // 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_YAW] = stab_settings[2]; + stabilization.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = stab_settings[2]; stabilization.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_WEAKLEVELING) ? - cmd->Roll * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_ROLL] : - (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_VIRTUALBAR) ? cmd->Roll : - (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE) ? - 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_NONE) ? cmd->Roll : + (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) ? cmd->Roll * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_ROLL] : + (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) ? + cmd->Roll * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_ROLL] : + (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_VIRTUALBAR) ? cmd->Roll : + (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE) ? + cmd->Roll * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_ROLL] : + (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYATTITUDE) ? cmd->Roll * stabSettings.RollMax : 0; // this is an invalid mode ; stabilization.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_WEAKLEVELING) ? - cmd->Pitch * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_PITCH] : - (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) ? cmd->Pitch * stabSettings.PitchMax : - (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) ? - cmd->Pitch * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_PITCH] : - (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR) ? cmd->Pitch : - (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE) ? - 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_NONE) ? cmd->Pitch : + (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) ? cmd->Pitch * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_PITCH] : + (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) ? + cmd->Pitch * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_PITCH] : + (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) ? cmd->Pitch * stabSettings.PitchMax : + (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) ? + cmd->Pitch * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_PITCH] : + (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR) ? cmd->Pitch : + (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE) ? + cmd->Pitch * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_PITCH] : + (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYATTITUDE) ? cmd->Pitch * stabSettings.PitchMax : 0; // this is an invalid mode stabilization.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_WEAKLEVELING) ? - cmd->Yaw * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_YAW] : - (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_VIRTUALBAR) ? cmd->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_NONE) ? cmd->Yaw : + (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) ? cmd->Yaw * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_YAW] : + (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) ? + cmd->Yaw * stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_YAW] : + (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_VIRTUALBAR) ? cmd->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 stabilization.Throttle = (cmd->Throttle < 0) ? -1 : cmd->Throttle; StabilizationDesiredSet(&stabilization); @@ -696,10 +699,11 @@ static void updateStabilizationDesired(ManualControlCommandData * cmd, ManualCon * @brief Update the position desired to current location when * 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; portTickType thisSysTime = xTaskGetTickCount(); + /* float dT = (thisSysTime - lastSysTime) / portTICK_RATE_MS / 1000.0f; */ lastSysTime = thisSysTime; @@ -711,16 +715,16 @@ static void updatePathDesired(ManualControlCommandData * cmd, bool changed,bool PathDesiredData pathDesired; PathDesiredGet(&pathDesired); pathDesired.Start[PATHDESIRED_START_NORTH] = 0; - pathDesired.Start[PATHDESIRED_START_EAST] = 0; - pathDesired.Start[PATHDESIRED_START_DOWN] = positionActual.Down - 10; - pathDesired.End[PATHDESIRED_END_NORTH] = 0; + pathDesired.Start[PATHDESIRED_START_EAST] = 0; + pathDesired.Start[PATHDESIRED_START_DOWN] = positionActual.Down - 10; + pathDesired.End[PATHDESIRED_END_NORTH] = 0; pathDesired.End[PATHDESIRED_END_EAST] = 0; pathDesired.End[PATHDESIRED_END_DOWN] = positionActual.Down - 10; - pathDesired.StartingVelocity=1; - pathDesired.EndingVelocity=0; + pathDesired.StartingVelocity = 1; + pathDesired.EndingVelocity = 0; pathDesired.Mode = PATHDESIRED_MODE_FLYENDPOINT; PathDesiredSet(&pathDesired); - } else if(changed) { + } else if (changed) { // After not being in this mode for a while init at current height PositionActualData positionActual; PositionActualGet(&positionActual); @@ -728,28 +732,28 @@ static void updatePathDesired(ManualControlCommandData * cmd, bool changed,bool PathDesiredData pathDesired; PathDesiredGet(&pathDesired); pathDesired.Start[PATHDESIRED_START_NORTH] = positionActual.North; - pathDesired.Start[PATHDESIRED_START_EAST] = positionActual.East; - pathDesired.Start[PATHDESIRED_START_DOWN] = positionActual.Down; - pathDesired.End[PATHDESIRED_END_NORTH] = positionActual.North; + pathDesired.Start[PATHDESIRED_START_EAST] = positionActual.East; + pathDesired.Start[PATHDESIRED_START_DOWN] = positionActual.Down; + pathDesired.End[PATHDESIRED_END_NORTH] = positionActual.North; pathDesired.End[PATHDESIRED_END_EAST] = positionActual.East; pathDesired.End[PATHDESIRED_END_DOWN] = positionActual.Down; - pathDesired.StartingVelocity=1; - pathDesired.EndingVelocity=0; + pathDesired.StartingVelocity = 1; + pathDesired.EndingVelocity = 0; pathDesired.Mode = PATHDESIRED_MODE_FLYENDPOINT; PathDesiredSet(&pathDesired); /* Disable this section, until such time as proper discussion can be had about how to implement it for all types of crafts. - } else { - PathDesiredData pathDesired; - PathDesiredGet(&pathDesired); - pathDesired.End[PATHDESIRED_END_NORTH] += dT * -cmd->Pitch; - pathDesired.End[PATHDESIRED_END_EAST] += dT * cmd->Roll; - pathDesired.Mode = PATHDESIRED_MODE_FLYENDPOINT; - PathDesiredSet(&pathDesired); + } else { + PathDesiredData pathDesired; + PathDesiredGet(&pathDesired); + pathDesired.End[PATHDESIRED_END_NORTH] += dT * -cmd->Pitch; + pathDesired.End[PATHDESIRED_END_EAST] += dT * cmd->Roll; + pathDesired.Mode = PATHDESIRED_MODE_FLYENDPOINT; + PathDesiredSet(&pathDesired); */ } } -static void updateLandDesired(ManualControlCommandData * cmd, bool changed) +static void updateLandDesired(ManualControlCommandData *cmd, bool changed) { static portTickType lastSysTime; portTickType thisSysTime; @@ -764,19 +768,19 @@ static void updateLandDesired(ManualControlCommandData * cmd, bool changed) PathDesiredData pathDesired; PathDesiredGet(&pathDesired); - if(changed) { + if (changed) { // After not being in this mode for a while init at current height pathDesired.Start[PATHDESIRED_START_NORTH] = positionActual.North; - pathDesired.Start[PATHDESIRED_START_EAST] = positionActual.East; - pathDesired.Start[PATHDESIRED_START_DOWN] = positionActual.Down; - pathDesired.End[PATHDESIRED_END_NORTH] = positionActual.North; + pathDesired.Start[PATHDESIRED_START_EAST] = positionActual.East; + pathDesired.Start[PATHDESIRED_START_DOWN] = positionActual.Down; + pathDesired.End[PATHDESIRED_END_NORTH] = positionActual.North; pathDesired.End[PATHDESIRED_END_EAST] = positionActual.East; pathDesired.End[PATHDESIRED_END_DOWN] = positionActual.Down; - pathDesired.StartingVelocity=1; - pathDesired.EndingVelocity=0; + pathDesired.StartingVelocity = 1; + pathDesired.EndingVelocity = 0; pathDesired.Mode = PATHDESIRED_MODE_FLYENDPOINT; } - pathDesired.End[PATHDESIRED_END_DOWN] = positionActual.Down+5; + pathDesired.End[PATHDESIRED_END_DOWN] = positionActual.Down + 5; PathDesiredSet(&pathDesired); } @@ -785,16 +789,20 @@ static void updateLandDesired(ManualControlCommandData * cmd, bool changed) * enabled and enable altitude mode for stabilization * @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_LOW = 0.45; + const float DEADBAND = .20f; + 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 bool zeroed = false; portTickType thisSysTime; float dT; AltitudeHoldDesiredData altitudeHoldDesired; + AltitudeHoldDesiredGet(&altitudeHoldDesired); StabilizationSettingsData stabSettings; @@ -804,47 +812,48 @@ static void altitudeHoldDesired(ManualControlCommandData * cmd, bool changed) dT = (thisSysTime - lastSysTime) / portTICK_RATE_MS / 1000.0f; lastSysTime = thisSysTime; - altitudeHoldDesired.Roll = cmd->Roll * stabSettings.RollMax; + altitudeHoldDesired.Roll = cmd->Roll * stabSettings.RollMax; 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; PositionActualDownGet(¤tDown); - if(changed) { + if (changed) { // After not being in this mode for a while init at current height altitudeHoldDesired.Altitude = 0; zeroed = false; } 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) { - 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) { // Require the stick to enter the dead band before they can move height zeroed = true; } - AltitudeHoldDesiredSet(&altitudeHoldDesired); } -#else +#else /* if defined(REVOLUTION) */ // 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 // 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); } -static void updateLandDesired(ManualControlCommandData * cmd, bool changed) +static void updateLandDesired(ManualControlCommandData *cmd, bool changed) { 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); } -#endif +#endif /* if defined(REVOLUTION) */ /** * 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 if ((max > min && value >= neutral) || (min > max && value <= neutral)) { if (max != neutral) { - valueScaled = (float) (value - neutral) / (float) (max - neutral); + valueScaled = (float)(value - neutral) / (float)(max - neutral); } else { valueScaled = 0; } } else { if (min != neutral) { - valueScaled = (float) (value - neutral) / (float) (neutral - min); + valueScaled = (float)(value - neutral) / (float)(neutral - min); } else { valueScaled = 0; } @@ -893,13 +902,15 @@ static bool okToArm(void) { // read alarms SystemAlarmsData alarms; + SystemAlarmsGet(&alarms); // Check each alarm for (int i = 0; i < SYSTEMALARMS_ALARM_NUMELEM; i++) { - if (alarms.Alarm[i] >= SYSTEMALARMS_ALARM_ERROR) { // found an alarm thats set - if (i == SYSTEMALARMS_ALARM_GPS || i == SYSTEMALARMS_ALARM_TELEMETRY) + if (alarms.Alarm[i] >= SYSTEMALARMS_ALARM_ERROR) { // found an alarm thats set + if (i == SYSTEMALARMS_ALARM_GPS || i == SYSTEMALARMS_ALARM_TELEMETRY) { continue; + } return false; } @@ -915,6 +926,7 @@ static bool forcedDisArm(void) { // read alarms SystemAlarmsData alarms; + SystemAlarmsGet(&alarms); if (alarms.Alarm[SYSTEMALARMS_ALARM_GUIDANCE] == SYSTEMALARMS_ALARM_CRITICAL) { @@ -930,6 +942,7 @@ static bool forcedDisArm(void) static void setArmedIfChanged(uint8_t val) { FlightStatusData flightStatus; + FlightStatusGet(&flightStatus); 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[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; if (forcedDisArm()) { @@ -959,22 +971,23 @@ static void processArm(ManualControlCommandData * cmd, ManualControlSettingsData setArmedIfChanged(FLIGHTSTATUS_ARMED_DISARMED); } else { // Not really needed since this function not called when disconnected - if (cmd->Connected == MANUALCONTROLCOMMAND_CONNECTED_FALSE) + if (cmd->Connected == MANUALCONTROLCOMMAND_CONNECTED_FALSE) { lowThrottle = true; + } // 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; + 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; } @@ -994,72 +1007,76 @@ static void processArm(ManualControlCommandData * cmd, ManualControlSettingsData // 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; + 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 manualArm = false; bool manualDisarm = false; - if (armingInputLevel <= -ARMED_THRESHOLD) + if (armingInputLevel <= -ARMED_THRESHOLD) { manualArm = true; - else if (armingInputLevel >= +ARMED_THRESHOLD) + } else if (armingInputLevel >= +ARMED_THRESHOLD) { manualDisarm = true; + } switch (armState) { - case ARM_STATE_DISARMED: - setArmedIfChanged(FLIGHTSTATUS_ARMED_DISARMED); + case ARM_STATE_DISARMED: + setArmedIfChanged(FLIGHTSTATUS_ARMED_DISARMED); - // only allow arming if it's OK too - 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 + // only allow arming if it's OK too + if (manualArm && okToArm()) { armedDisarmStart = lastSysTime; - armState = ARM_STATE_DISARMING_TIMEOUT; - setArmedIfChanged(FLIGHTSTATUS_ARMED_ARMED); - break; + armState = ARM_STATE_ARMING_MANUAL; + } + 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; + case ARM_STATE_ARMING_MANUAL: + setArmedIfChanged(FLIGHTSTATUS_ARMED_ARMING); - // Switch to disarming due to manual control when needed - if (manualDisarm) { - armedDisarmStart = lastSysTime; - armState = ARM_STATE_DISARMING_MANUAL; - } - break; + if (manualArm && (timeDifferenceMs(armedDisarmStart, lastSysTime) > ARMED_TIME_MS)) { + armState = ARM_STATE_ARMED; + } else if (!manualArm) { + armState = ARM_STATE_DISARMED; + } + 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 + 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; + setArmedIfChanged(FLIGHTSTATUS_ARMED_ARMED); + 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) { FlightStatusData flightStatus; + FlightStatusGet(&flightStatus); // 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; - if (pos >= settings->FlightModeNumber) + if (pos >= settings->FlightModeNumber) { pos = settings->FlightModeNumber - 1; + } uint8_t newMode = settings->FlightModePosition[pos]; @@ -1098,7 +1117,7 @@ bool validInputRange(int16_t min, int16_t max, uint16_t value) min = max; 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) { - if (fabs(*value) < deadband) + if (fabs(*value) < deadband) { *value = 0.0f; - else if (*value > 0.0f) + } else if (*value > 0.0f) { *value -= deadband; - else + } else { *value += deadband; + } } #ifdef USE_INPUT_LPF @@ -1130,7 +1150,7 @@ static void applyLPF(float *value, ManualControlSettingsResponseTimeElem channel /** * Called whenever a critical configuration component changes */ -static void configurationUpdatedCb(UAVObjEvent * ev) +static void configurationUpdatedCb(UAVObjEvent *ev) { configuration_check(); }