From 227aab80b9d1352da514334558617d83e0c204da Mon Sep 17 00:00:00 2001 From: Corvus Corax Date: Sun, 16 Feb 2014 13:49:51 +0100 Subject: [PATCH] OP-1217 moved manualcontrol to receiver --- .../{manualcontrol.c => control.c} | 0 flight/modules/Receiver/receiver.c | 1303 +++++++++++++++++ 2 files changed, 1303 insertions(+) rename flight/modules/ManualControl/{manualcontrol.c => control.c} (100%) create mode 100644 flight/modules/Receiver/receiver.c diff --git a/flight/modules/ManualControl/manualcontrol.c b/flight/modules/ManualControl/control.c similarity index 100% rename from flight/modules/ManualControl/manualcontrol.c rename to flight/modules/ManualControl/control.c diff --git a/flight/modules/Receiver/receiver.c b/flight/modules/Receiver/receiver.c new file mode 100644 index 000000000..940e5cf5c --- /dev/null +++ b/flight/modules/Receiver/receiver.c @@ -0,0 +1,1303 @@ +/** + ****************************************************************************** + * @addtogroup OpenPilotModules OpenPilot Modules + * @{ + * @addtogroup ManualControlModule Manual Control Module + * @brief Provide manual control or allow it alter flight mode. + * @{ + * + * Reads in the ManualControlCommand FlightMode setting from receiver then either + * pass the settings straght to ActuatorDesired object (manual mode) or to + * AttitudeDesired object (stabilized mode) + * + * @file manualcontrol.c + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * @brief ManualControl module. Handles safety R/C link and flight mode. + * + * @see The GNU Public License (GPL) Version 3 + * + *****************************************************************************/ +/* + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY + * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License + * for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program; if not, write to the Free Software Foundation, Inc., + * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + */ + +#include +#include +#include "accessorydesired.h" +#include "actuatordesired.h" +#include "altitudeholddesired.h" +#include "flighttelemetrystats.h" +#include "flightstatus.h" +#include "sanitycheck.h" +#include "manualcontrol.h" +#include "manualcontrolsettings.h" +#include "manualcontrolcommand.h" +#include "flightmodesettings.h" +#include "positionstate.h" +#include "pathdesired.h" +#include "stabilizationbank.h" +#include "stabilizationdesired.h" +#include "receiveractivity.h" +#include "systemsettings.h" +#include +#include + +#if defined(PIOS_INCLUDE_USB_RCTX) +#include "pios_usb_rctx.h" +#endif /* PIOS_INCLUDE_USB_RCTX */ + +// Private constants +#if defined(PIOS_MANUAL_STACK_SIZE) +#define STACK_SIZE_BYTES PIOS_MANUAL_STACK_SIZE +#else +#define STACK_SIZE_BYTES 1152 +#endif + +#define TASK_PRIORITY (tskIDLE_PRIORITY + 3) // 3 = flight control +#define UPDATE_PERIOD_MS 20 +#define THROTTLE_FAILSAFE -0.1f +#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 { + ARM_STATE_DISARMED, + ARM_STATE_ARMING_MANUAL, + ARM_STATE_ARMED, + ARM_STATE_DISARMING_MANUAL, + ARM_STATE_DISARMING_TIMEOUT +} ArmState_t; + +// Private variables +static xTaskHandle taskHandle; +static ArmState_t armState; +static portTickType lastSysTime; + +#ifdef USE_INPUT_LPF +static portTickType lastSysTimeLPF; +static float inputFiltered[MANUALCONTROLSETTINGS_RESPONSETIME_NUMELEM]; +#endif + +// Private functions +static void updateActuatorDesired(ManualControlCommandData *cmd); +static void updateStabilizationDesired(ManualControlCommandData *cmd, FlightModeSettingsData *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, FlightModeSettingsData *modeSettings, float flightMode, ManualControlCommandData *cmd); +static void processArm(ManualControlCommandData *cmd, FlightModeSettingsData *settings, int8_t armSwitch); +static void setArmedIfChanged(uint8_t val); +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); +static uint32_t timeDifferenceMs(portTickType start_time, portTickType end_time); +static bool okToArm(void); +static bool validInputRange(int16_t min, int16_t max, uint16_t value); +static void applyDeadband(float *value, float deadband); + +#ifdef USE_INPUT_LPF +static void applyLPF(float *value, ManualControlSettingsResponseTimeElem channel, ManualControlSettingsData *settings, float dT); +#endif + +#define RCVR_ACTIVITY_MONITOR_CHANNELS_PER_GROUP 12 +#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); + +#define assumptions (assumptions1 && assumptions3 && assumptions5 && assumptions_flightmode && assumptions_channelcount) + +/** + * Module starting + */ +int32_t ManualControlStart() +{ + // Start main task + xTaskCreate(manualControlTask, (signed char *)"ManualControl", STACK_SIZE_BYTES / 4, NULL, TASK_PRIORITY, &taskHandle); + PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_MANUALCONTROL, taskHandle); +#ifdef PIOS_INCLUDE_WDG + PIOS_WDG_RegisterFlag(PIOS_WDG_MANUAL); +#endif + + return 0; +} + +/** + * Module initialization + */ +int32_t ManualControlInitialize() +{ + /* Check the assumptions about uavobject enum's are correct */ + if (!assumptions) { + return -1; + } + + AccessoryDesiredInitialize(); + ManualControlCommandInitialize(); + FlightStatusInitialize(); + StabilizationDesiredInitialize(); + ReceiverActivityInitialize(); + ManualControlSettingsInitialize(); + FlightModeSettingsInitialize(); + + return 0; +} +MODULE_INITCALL(ManualControlInitialize, ManualControlStart); + +/** + * Module task + */ +static void manualControlTask(__attribute__((unused)) void *parameters) +{ + ManualControlSettingsData settings; + FlightModeSettingsData modeSettings; + ManualControlCommandData cmd; + FlightStatusData flightStatus; + float flightMode = 0; + + uint8_t disconnected_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 + AccessoryDesiredCreateInstance(); + AccessoryDesiredCreateInstance(); + + // Run this initially to make sure the configuration is checked + configuration_check(); + + // Whenever the configuration changes, make sure it is safe to fly + SystemSettingsConnectCallback(configurationUpdatedCb); + ManualControlSettingsConnectCallback(configurationUpdatedCb); + + // Whenever the configuration changes, make sure it is safe to fly + + // Make sure unarmed on power up + ManualControlCommandGet(&cmd); + FlightStatusGet(&flightStatus); + flightStatus.Armed = FLIGHTSTATUS_ARMED_DISARMED; + armState = ARM_STATE_DISARMED; + + /* Initialize the RcvrActivty FSM */ + portTickType lastActivityTime = xTaskGetTickCount(); + resetRcvrActivity(&activity_fsm); + + // Main task loop + lastSysTime = xTaskGetTickCount(); + + float scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_NUMELEM] = { 0 }; + SystemSettingsThrustControlOptions thrustType; + + while (1) { + // Wait until next update + vTaskDelayUntil(&lastSysTime, UPDATE_PERIOD_MS / portTICK_RATE_MS); +#ifdef PIOS_INCLUDE_WDG + PIOS_WDG_UpdateFlag(PIOS_WDG_MANUAL); +#endif + + // Read settings + ManualControlSettingsGet(&settings); + FlightModeSettingsGet(&modeSettings); + SystemSettingsThrustControlGet(&thrustType); + + /* Update channel activity monitor */ + if (flightStatus.Armed == ARM_STATE_DISARMED) { + if (updateRcvrActivity(&activity_fsm)) { + /* Reset the aging timer because activity was detected */ + lastActivityTime = lastSysTime; + } + } + if (timeDifferenceMs(lastActivityTime, lastSysTime) > 5000) { + resetRcvrActivity(&activity_fsm); + lastActivityTime = lastSysTime; + } + + if (ManualControlCommandReadOnly()) { + FlightTelemetryStatsData flightTelemStats; + FlightTelemetryStatsGet(&flightTelemStats); + if (flightTelemStats.Status != FLIGHTTELEMETRYSTATS_STATUS_CONNECTED) { + /* trying to fly via GCS and lost connection. fall back to transmitter */ + UAVObjMetadata metadata; + ManualControlCommandGetMetadata(&metadata); + UAVObjSetAccess(&metadata, ACCESS_READWRITE); + ManualControlCommandSetMetadata(&metadata); + } + } + + if (!ManualControlCommandReadOnly()) { + bool valid_input_detected = true; + + // Read channel values in us + for (uint8_t n = 0; n < MANUALCONTROLSETTINGS_CHANNELGROUPS_NUMELEM && n < MANUALCONTROLCOMMAND_CHANNEL_NUMELEM; ++n) { + extern uint32_t pios_rcvr_group_map[]; + + if (cast_struct_to_array(settings.ChannelGroups, settings.ChannelGroups.Roll)[n] >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) { + cmd.Channel[n] = PIOS_RCVR_INVALID; + } else { + cmd.Channel[n] = PIOS_RCVR_Read(pios_rcvr_group_map[ + cast_struct_to_array(settings.ChannelGroups, settings.ChannelGroups.Pitch)[n]], + cast_struct_to_array(settings.ChannelNumber, settings.ChannelNumber.Pitch)[n]); + } + + // 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] == (uint16_t)PIOS_RCVR_TIMEOUT) { + valid_input_detected = false; + } else { + scaledChannel[n] = scaleChannel(cmd.Channel[n], + cast_struct_to_array(settings.ChannelMax, settings.ChannelMax.Pitch)[n], + cast_struct_to_array(settings.ChannelMin, settings.ChannelMin.Pitch)[n], + cast_struct_to_array(settings.ChannelNeutral, settings.ChannelNeutral.Pitch)[n]); + } + } + + // Check settings, if error raise alarm + if (settings.ChannelGroups.Roll >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE + || settings.ChannelGroups.Pitch >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE + || settings.ChannelGroups.Yaw >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE + || settings.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 > FLIGHTMODESETTINGS_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.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); + + // Need to do this here since we don't process armed status. Since this shouldn't happen in flight (changed config) + // immediately disarm + setArmedIfChanged(FLIGHTSTATUS_ARMED_DISARMED); + + continue; + } + + // decide if we have valid manual input or not + valid_input_detected &= validInputRange(settings.ChannelMin.Throttle, + settings.ChannelMax.Throttle, cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE]) + && validInputRange(settings.ChannelMin.Roll, + settings.ChannelMax.Roll, cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL]) + && validInputRange(settings.ChannelMin.Yaw, + settings.ChannelMax.Yaw, cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW]) + && validInputRange(settings.ChannelMin.Pitch, + settings.ChannelMax.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; + disconnected_count = 0; + } else if (!valid_input_detected && (++disconnected_count > 10)) { + cmd.Connected = MANUALCONTROLCOMMAND_CONNECTED_FALSE; + connected_count = 0; + disconnected_count = 0; + } + + int8_t armSwitch = 0; + if (cmd.Connected == MANUALCONTROLCOMMAND_CONNECTED_FALSE) { + cmd.Throttle = settings.FailsafeChannel.Throttle; + cmd.Roll = settings.FailsafeChannel.Roll; + cmd.Pitch = settings.FailsafeChannel.Pitch; + cmd.Yaw = settings.FailsafeChannel.Yaw; + cmd.Collective = settings.FailsafeChannel.Collective; + switch (thrustType) { + case SYSTEMSETTINGS_THRUSTCONTROL_THROTTLE: + cmd.Thrust = cmd.Throttle; + break; + case SYSTEMSETTINGS_THRUSTCONTROL_COLLECTIVE: + cmd.Thrust = cmd.Collective; + break; + default: + break; + } + if (settings.FailsafeFlightModeSwitchPosition >= 0 && settings.FailsafeFlightModeSwitchPosition < settings.FlightModeNumber) { + FlightStatusGet(&flightStatus); + + cmd.FlightModeSwitchPosition = (uint8_t)settings.FailsafeFlightModeSwitchPosition; + flightStatus.FlightMode = modeSettings.FlightModePosition[settings.FailsafeFlightModeSwitchPosition]; + FlightStatusSet(&flightStatus); + } + AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING); + + AccessoryDesiredData accessory; + // Set Accessory 0 + if (settings.ChannelGroups.Accessory0 != MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) { + accessory.AccessoryVal = settings.FailsafeChannel.Accessory0; + if (AccessoryDesiredInstSet(0, &accessory) != 0) { + AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING); + } + } + // Set Accessory 1 + if (settings.ChannelGroups.Accessory1 != MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) { + accessory.AccessoryVal = settings.FailsafeChannel.Accessory1; + if (AccessoryDesiredInstSet(1, &accessory) != 0) { + AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING); + } + } + // Set Accessory 2 + if (settings.ChannelGroups.Accessory2 != MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) { + accessory.AccessoryVal = settings.FailsafeChannel.Accessory2; + 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.Throttle = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE]; + flightMode = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_FLIGHTMODE]; + + // Apply deadband for Roll/Pitch/Yaw stick inputs + if (settings.Deadband > 0.0f) { + applyDeadband(&cmd.Roll, settings.Deadband); + applyDeadband(&cmd.Pitch, settings.Deadband); + applyDeadband(&cmd.Yaw, settings.Deadband); + } +#ifdef USE_INPUT_LPF + // 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; + 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) { + cmd.Collective = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_COLLECTIVE]; + } + + switch (thrustType) { + case SYSTEMSETTINGS_THRUSTCONTROL_THROTTLE: + cmd.Thrust = cmd.Throttle; + break; + case SYSTEMSETTINGS_THRUSTCONTROL_COLLECTIVE: + cmd.Thrust = cmd.Collective; + break; + default: + break; + } + + AccessoryDesiredData accessory; + // Set Accessory 0 + if (settings.ChannelGroups.Accessory0 != MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) { + accessory.AccessoryVal = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY0]; +#ifdef USE_INPUT_LPF + applyLPF(&accessory.AccessoryVal, MANUALCONTROLSETTINGS_RESPONSETIME_ACCESSORY0, &settings, dT); +#endif + if (modeSettings.Arming == FLIGHTMODESETTINGS_ARMING_ACCESSORY0) { + if (accessory.AccessoryVal > ARMED_THRESHOLD) { + armSwitch = 1; + } else if (accessory.AccessoryVal < -ARMED_THRESHOLD) { + armSwitch = -1; + } + } + if (AccessoryDesiredInstSet(0, &accessory) != 0) { + AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING); + } + } + // Set Accessory 1 + if (settings.ChannelGroups.Accessory1 != MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) { + accessory.AccessoryVal = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY1]; +#ifdef USE_INPUT_LPF + applyLPF(&accessory.AccessoryVal, MANUALCONTROLSETTINGS_RESPONSETIME_ACCESSORY1, &settings, dT); +#endif + if (modeSettings.Arming == FLIGHTMODESETTINGS_ARMING_ACCESSORY1) { + if (accessory.AccessoryVal > ARMED_THRESHOLD) { + armSwitch = 1; + } else if (accessory.AccessoryVal < -ARMED_THRESHOLD) { + armSwitch = -1; + } + } + if (AccessoryDesiredInstSet(1, &accessory) != 0) { + AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING); + } + } + // Set Accessory 2 + if (settings.ChannelGroups.Accessory2 != MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) { + accessory.AccessoryVal = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY2]; +#ifdef USE_INPUT_LPF + applyLPF(&accessory.AccessoryVal, MANUALCONTROLSETTINGS_RESPONSETIME_ACCESSORY2, &settings, dT); +#endif + if (modeSettings.Arming == FLIGHTMODESETTINGS_ARMING_ACCESSORY2) { + if (accessory.AccessoryVal > ARMED_THRESHOLD) { + armSwitch = 1; + } else if (accessory.AccessoryVal < -ARMED_THRESHOLD) { + armSwitch = -1; + } + } + + if (AccessoryDesiredInstSet(2, &accessory) != 0) { + AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING); + } + } + + processFlightMode(&settings, &modeSettings, flightMode, &cmd); + } + + // Process arming outside conditional so system will disarm when disconnected + processArm(&cmd, &modeSettings, armSwitch); + + // Update cmd object + ManualControlCommandSet(&cmd); +#if defined(PIOS_INCLUDE_USB_RCTX) + if (pios_usb_rctx_id) { + PIOS_USB_RCTX_Update(pios_usb_rctx_id, + cmd.Channel, + cast_struct_to_array(settings.ChannelMin, settings.ChannelMin.Roll), + cast_struct_to_array(settings.ChannelMax, settings.ChannelMax.Roll), + NELEMENTS(cmd.Channel)); + } +#endif /* PIOS_INCLUDE_USB_RCTX */ + } else { + ManualControlCommandGet(&cmd); /* Under GCS control */ + } + + FlightStatusGet(&flightStatus); + + // 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! + AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_CRITICAL); + break; + case FLIGHTMODE_MANUAL: + updateActuatorDesired(&cmd); + break; + case FLIGHTMODE_STABILIZED: + updateStabilizationDesired(&cmd, &modeSettings); + 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: + case FLIGHTSTATUS_FLIGHTMODE_ALTITUDEVARIO: + 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; + } +} + +static void resetRcvrActivity(struct rcvr_activity_fsm *fsm) +{ + ReceiverActivityData data; + bool updated = false; + + /* Clear all channel activity flags */ + ReceiverActivityGet(&data); + if (data.ActiveGroup != RECEIVERACTIVITY_ACTIVEGROUP_NONE && data.ActiveChannel != 255) { + data.ActiveGroup = RECEIVERACTIVITY_ACTIVEGROUP_NONE; + data.ActiveChannel = 255; + updated = true; + } + if (updated) { + ReceiverActivitySet(&data); + } + + /* Reset the FSM state */ + fsm->group = 0; + fsm->sample_count = 0; +} + +static void updateRcvrActivitySample(uint32_t rcvr_id, uint16_t samples[], uint8_t max_channels) +{ + for (uint8_t channel = 1; channel <= max_channels; channel++) { + // Subtract 1 because channels are 1 indexed + samples[channel - 1] = PIOS_RCVR_Read(rcvr_id, channel); + } +} + +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 curr = PIOS_RCVR_Read(rcvr_id, channel); + if (curr > prev) { + delta = curr - prev; + } else { + delta = prev - curr; + } + + if (delta > RCVR_ACTIVITY_MONITOR_MIN_RANGE) { + /* Mark this channel as active */ + ReceiverActivityActiveGroupOptions group; + + /* 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; + } + + ReceiverActivityActiveGroupSet((uint8_t *)&group); + ReceiverActivityActiveChannelSet(&channel); + activity_updated = true; + } + } + return activity_updated; +} + +static bool updateRcvrActivity(struct rcvr_activity_fsm *fsm) +{ + bool activity_updated = false; + + if (fsm->group >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) { + /* We're out of range, reset things */ + resetRcvrActivity(fsm); + } + + extern uint32_t pios_rcvr_group_map[]; + if (!pios_rcvr_group_map[fsm->group]) { + /* Unbound group, skip it */ + goto group_completed; + } + + if (fsm->sample_count == 0) { + /* 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; + } + + /* Compare with previous sample */ + activity_updated = updateRcvrActivityCompare(pios_rcvr_group_map[fsm->group], fsm); + +group_completed: + /* Reset the sample counter */ + fsm->sample_count = 0; + + /* Find the next active group, but limit search so we can't loop forever here */ + for (uint8_t i = 0; i < MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE; i++) { + /* Move to the next group */ + fsm->group++; + if (fsm->group >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) { + /* Wrap back to the first group */ + fsm->group = 0; + } + if (pios_rcvr_group_map[fsm->group]) { + /* + * Found an active group, take a sample here to avoid an + * extra 20ms delay in the main thread so we can speed up + * this algorithm. + */ + updateRcvrActivitySample(pios_rcvr_group_map[fsm->group], fsm->prev, NELEMENTS(fsm->prev)); + fsm->sample_count++; + break; + } + } + + return activity_updated; +} + +static void updateActuatorDesired(ManualControlCommandData *cmd) +{ + ActuatorDesiredData actuator; + + ActuatorDesiredGet(&actuator); + actuator.Roll = cmd->Roll; + actuator.Pitch = cmd->Pitch; + actuator.Yaw = cmd->Yaw; + actuator.Thrust = cmd->Thrust; + ActuatorDesiredSet(&actuator); +} + +static void updateStabilizationDesired(ManualControlCommandData *cmd, FlightModeSettingsData *settings) +{ + StabilizationDesiredData stabilization; + + StabilizationDesiredGet(&stabilization); + + StabilizationBankData stabSettings; + StabilizationBankGet(&stabSettings); + + uint8_t *stab_settings; + FlightStatusData flightStatus; + FlightStatusGet(&flightStatus); + switch (flightStatus.FlightMode) { + case FLIGHTSTATUS_FLIGHTMODE_STABILIZED1: + stab_settings = cast_struct_to_array(settings->Stabilization1Settings, settings->Stabilization1Settings.Roll); + break; + case FLIGHTSTATUS_FLIGHTMODE_STABILIZED2: + stab_settings = cast_struct_to_array(settings->Stabilization2Settings, settings->Stabilization2Settings.Roll); + break; + case FLIGHTSTATUS_FLIGHTMODE_STABILIZED3: + stab_settings = cast_struct_to_array(settings->Stabilization3Settings, settings->Stabilization3Settings.Roll); + 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; + } + + stabilization.Roll = + (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) ? cmd->Roll : + (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) ? cmd->Roll * stabSettings.ManualRate.Roll : + (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) ? cmd->Roll * stabSettings.ManualRate.Roll : + (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) ? cmd->Roll * stabSettings.RollMax : + (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) ? cmd->Roll * stabSettings.ManualRate.Roll : + (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR) ? cmd->Roll : + (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATTITUDE) ? cmd->Roll : + (stab_settings[0] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE) ? cmd->Roll * stabSettings.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.Pitch : + (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) ? cmd->Pitch * stabSettings.ManualRate.Pitch : + (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) ? cmd->Pitch * stabSettings.PitchMax : + (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) ? cmd->Pitch * stabSettings.ManualRate.Pitch : + (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR) ? cmd->Pitch : + (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATTITUDE) ? cmd->Pitch : + (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE) ? cmd->Pitch * stabSettings.ManualRate.Pitch : + (stab_settings[1] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYATTITUDE) ? cmd->Pitch * stabSettings.PitchMax : + 0; // this is an invalid mode + + // TOOD: Add assumption about order of stabilization desired and manual control stabilization mode fields having same order + stabilization.StabilizationMode.Roll = stab_settings[0]; + stabilization.StabilizationMode.Pitch = stab_settings[1]; + // Other axes (yaw) cannot be Rattitude, so use Rate + // Should really do this for Attitude mode as well? + if (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATTITUDE) { + stabilization.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_RATE; + stabilization.Yaw = cmd->Yaw * stabSettings.ManualRate.Yaw; + } else { + stabilization.StabilizationMode.Yaw = stab_settings[2]; + stabilization.Yaw = + (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_NONE) ? cmd->Yaw : + (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATE) ? cmd->Yaw * stabSettings.ManualRate.Yaw : + (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) ? cmd->Yaw * stabSettings.ManualRate.Yaw : + (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) ? cmd->Yaw * stabSettings.YawMax : + (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK) ? cmd->Yaw * stabSettings.ManualRate.Yaw : + (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR) ? cmd->Yaw : + (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RATTITUDE) ? cmd->Yaw : + (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYRATE) ? cmd->Yaw * stabSettings.ManualRate.Yaw : + (stab_settings[2] == STABILIZATIONDESIRED_STABILIZATIONMODE_RELAYATTITUDE) ? cmd->Yaw * stabSettings.YawMax : + 0; // this is an invalid mode + } + + stabilization.Thrust = cmd->Thrust; + StabilizationDesiredSet(&stabilization); +} + +#if defined(REVOLUTION) +// TODO: Need compile flag to exclude this from copter control +/** + * @brief Update the position desired to current location when + * enabled and allow the waypoint to be moved by transmitter + */ +static void updatePathDesired(__attribute__((unused)) ManualControlCommandData *cmd, bool changed, bool home) +{ + /* + static portTickType lastSysTime; + portTickType thisSysTime = xTaskGetTickCount(); + dT = ((thisSysTime == lastSysTime)? 0.001f : (thisSysTime - lastSysTime) * portTICK_RATE_MS * 0.001f); + lastSysTime = thisSysTime; + */ + + if (home && changed) { + // Simple Return To Base mode - keep altitude the same, fly to home position + PositionStateData positionState; + PositionStateGet(&positionState); + FlightModeSettingsData settings; + FlightModeSettingsGet(&settings); + + PathDesiredData pathDesired; + PathDesiredGet(&pathDesired); + pathDesired.Start.North = 0; + pathDesired.Start.East = 0; + pathDesired.Start.Down = positionState.Down - settings.ReturnToHomeAltitudeOffset; + pathDesired.End.North = 0; + pathDesired.End.East = 0; + pathDesired.End.Down = positionState.Down - settings.ReturnToHomeAltitudeOffset; + pathDesired.StartingVelocity = 1; + pathDesired.EndingVelocity = 0; + pathDesired.Mode = PATHDESIRED_MODE_FLYENDPOINT; + PathDesiredSet(&pathDesired); + } else if (changed) { + // After not being in this mode for a while init at current height + PositionStateData positionState; + PositionStateGet(&positionState); + + PathDesiredData pathDesired; + PathDesiredGet(&pathDesired); + pathDesired.Start.North = positionState.North; + pathDesired.Start.East = positionState.East; + pathDesired.Start.Down = positionState.Down; + pathDesired.End.North = positionState.North; + pathDesired.End.East = positionState.East; + pathDesired.End.Down = positionState.Down; + 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); + */ + } +} + +static void updateLandDesired(__attribute__((unused)) ManualControlCommandData *cmd, bool changed) +{ + /* + static portTickType lastSysTime; + portTickType thisSysTime; + float dT; + + thisSysTime = xTaskGetTickCount(); + dT = ((thisSysTime == lastSysTime)? 0.001f : (thisSysTime - lastSysTime) * portTICK_RATE_MS * 0.001f); + lastSysTime = thisSysTime; + */ + + PositionStateData positionState; + + PositionStateGet(&positionState); + + PathDesiredData pathDesired; + PathDesiredGet(&pathDesired); + if (changed) { + // After not being in this mode for a while init at current height + pathDesired.Start.North = positionState.North; + pathDesired.Start.East = positionState.East; + pathDesired.Start.Down = positionState.Down; + pathDesired.End.North = positionState.North; + pathDesired.End.East = positionState.East; + pathDesired.End.Down = positionState.Down; + pathDesired.StartingVelocity = 1; + pathDesired.EndingVelocity = 0; + pathDesired.Mode = PATHDESIRED_MODE_FLYENDPOINT; + } + pathDesired.End.Down = positionState.Down + 5; + PathDesiredSet(&pathDesired); +} + +/** + * @brief Update the altitude desired to current altitude when + * enabled and enable altitude mode for stabilization + * @todo: Need compile flag to exclude this from copter control + */ +static void altitudeHoldDesired(ManualControlCommandData *cmd, bool changed) +{ + const float DEADBAND = 0.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 thrust + float thrustRate; + uint8_t thrustExp; + + static uint8_t flightMode; + static bool newaltitude = true; + + FlightStatusFlightModeGet(&flightMode); + + AltitudeHoldDesiredData altitudeHoldDesiredData; + AltitudeHoldDesiredGet(&altitudeHoldDesiredData); + + AltitudeHoldSettingsThrustExpGet(&thrustExp); + AltitudeHoldSettingsThrustRateGet(&thrustRate); + + StabilizationBankData stabSettings; + StabilizationBankGet(&stabSettings); + + PositionStateData posState; + PositionStateGet(&posState); + + altitudeHoldDesiredData.Roll = cmd->Roll * stabSettings.RollMax; + altitudeHoldDesiredData.Pitch = cmd->Pitch * stabSettings.PitchMax; + altitudeHoldDesiredData.Yaw = cmd->Yaw * stabSettings.ManualRate.Yaw; + + if (changed) { + newaltitude = true; + } + + uint8_t cutOff; + AltitudeHoldSettingsCutThrustWhenZeroGet(&cutOff); + if (cutOff && cmd->Thrust < 0) { + // Cut thrust if desired + altitudeHoldDesiredData.SetPoint = cmd->Thrust; + altitudeHoldDesiredData.ControlMode = ALTITUDEHOLDDESIRED_CONTROLMODE_THRUST; + newaltitude = true; + } else if (flightMode == FLIGHTSTATUS_FLIGHTMODE_ALTITUDEVARIO && cmd->Thrust > DEADBAND_HIGH) { + // being the two band symmetrical I can divide by DEADBAND_LOW to scale it to a value betweeon 0 and 1 + // then apply an "exp" f(x,k) = (k*x*x*x + (255-k)*x) / 255 + altitudeHoldDesiredData.SetPoint = -((thrustExp * powf((cmd->Thrust - DEADBAND_HIGH) / (DEADBAND_LOW), 3) + (255 - thrustExp) * (cmd->Thrust - DEADBAND_HIGH) / DEADBAND_LOW) / 255 * thrustRate); + altitudeHoldDesiredData.ControlMode = ALTITUDEHOLDDESIRED_CONTROLMODE_VELOCITY; + newaltitude = true; + } else if (flightMode == FLIGHTSTATUS_FLIGHTMODE_ALTITUDEVARIO && cmd->Thrust < DEADBAND_LOW) { + altitudeHoldDesiredData.SetPoint = -(-(thrustExp * powf((DEADBAND_LOW - (cmd->Thrust < 0 ? 0 : cmd->Thrust)) / DEADBAND_LOW, 3) + (255 - thrustExp) * (DEADBAND_LOW - cmd->Thrust) / DEADBAND_LOW) / 255 * thrustRate); + altitudeHoldDesiredData.ControlMode = ALTITUDEHOLDDESIRED_CONTROLMODE_VELOCITY; + newaltitude = true; + } else if (newaltitude == true) { + altitudeHoldDesiredData.SetPoint = posState.Down; + altitudeHoldDesiredData.ControlMode = ALTITUDEHOLDDESIRED_CONTROLMODE_ALTITUDE; + newaltitude = false; + } + + AltitudeHoldDesiredSet(&altitudeHoldDesiredData); +} +#else /* if defined(REVOLUTION) */ + +// TODO: These functions should never be accessible on CC. Any configuration that +// could allow them to be called should already throw an error to prevent this happening +// in flight +static void updatePathDesired(__attribute__((unused)) ManualControlCommandData *cmd, + __attribute__((unused)) bool changed, + __attribute__((unused)) bool home) +{ + AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_ERROR); +} + +static void updateLandDesired(__attribute__((unused)) ManualControlCommandData *cmd, + __attribute__((unused)) bool changed) +{ + AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_ERROR); +} + +static void altitudeHoldDesired(__attribute__((unused)) ManualControlCommandData *cmd, + __attribute__((unused)) bool changed) +{ + AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_ERROR); +} +#endif /* if defined(REVOLUTION) */ +/** + * Convert channel from servo pulse duration (microseconds) to scaled -1/+1 range. + */ +static float scaleChannel(int16_t value, int16_t max, int16_t min, int16_t neutral) +{ + float valueScaled; + + // Scale + if ((max > min && value >= neutral) || (min > max && value <= neutral)) { + if (max != neutral) { + valueScaled = (float)(value - neutral) / (float)(max - neutral); + } else { + valueScaled = 0; + } + } else { + if (min != neutral) { + valueScaled = (float)(value - neutral) / (float)(neutral - min); + } else { + valueScaled = 0; + } + } + + // Bound + if (valueScaled > 1.0f) { + valueScaled = 1.0f; + } else if (valueScaled < -1.0f) { + valueScaled = -1.0f; + } + + return valueScaled; +} + +static uint32_t timeDifferenceMs(portTickType start_time, portTickType end_time) +{ + return (end_time - start_time) * portTICK_RATE_MS; +} + +/** + * @brief Determine if the aircraft is safe to arm + * @returns True if safe to arm, false otherwise + */ +static bool okToArm(void) +{ + // update checks + configuration_check(); + + // read alarms + SystemAlarmsData alarms; + + SystemAlarmsGet(&alarms); + + // Check each alarm + for (int i = 0; i < SYSTEMALARMS_ALARM_NUMELEM; i++) { + if (cast_struct_to_array(alarms.Alarm, alarms.Alarm.Actuator)[i] >= SYSTEMALARMS_ALARM_ERROR) { // found an alarm thats set + if (i == SYSTEMALARMS_ALARM_GPS || i == SYSTEMALARMS_ALARM_TELEMETRY) { + continue; + } + + return false; + } + } + + uint8_t flightMode; + FlightStatusFlightModeGet(&flightMode); + switch (flightMode) { + case FLIGHTSTATUS_FLIGHTMODE_MANUAL: + case FLIGHTSTATUS_FLIGHTMODE_STABILIZED1: + case FLIGHTSTATUS_FLIGHTMODE_STABILIZED2: + case FLIGHTSTATUS_FLIGHTMODE_STABILIZED3: + return true; + + default: + return false; + } +} +/** + * @brief Determine if the aircraft is forced to disarm by an explicit alarm + * @returns True if safe to arm, false otherwise + */ +static bool forcedDisArm(void) +{ + // read alarms + SystemAlarmsData alarms; + + SystemAlarmsGet(&alarms); + + if (alarms.Alarm.Guidance == SYSTEMALARMS_ALARM_CRITICAL) { + return true; + } + return false; +} + +/** + * @brief Update the flightStatus object only if value changed. Reduces callbacks + * @param[in] val The new value + */ +static void setArmedIfChanged(uint8_t val) +{ + FlightStatusData flightStatus; + + FlightStatusGet(&flightStatus); + + if (flightStatus.Armed != val) { + flightStatus.Armed = val; + FlightStatusSet(&flightStatus); + } +} + +/** + * @brief Process the inputs and determine whether to arm or not + * @param[out] cmd The structure to set the armed in + * @param[in] settings Settings indicating the necessary position + */ +static void processArm(ManualControlCommandData *cmd, FlightModeSettingsData *settings, int8_t armSwitch) +{ + bool lowThrottle = cmd->Throttle < 0; + + /** + * do NOT check throttle if disarming via switch, must be instant + */ + switch (settings->Arming) { + case FLIGHTMODESETTINGS_ARMING_ACCESSORY0: + case FLIGHTMODESETTINGS_ARMING_ACCESSORY1: + case FLIGHTMODESETTINGS_ARMING_ACCESSORY2: + if (armSwitch < 0) { + lowThrottle = true; + } + break; + default: + break; + } + + if (forcedDisArm()) { + // PathPlanner forces explicit disarming due to error condition (crash, impact, fire, ...) + setArmedIfChanged(FLIGHTSTATUS_ARMED_DISARMED); + return; + } + + if (settings->Arming == FLIGHTMODESETTINGS_ARMING_ALWAYSDISARMED) { + // In this configuration we always disarm + setArmedIfChanged(FLIGHTSTATUS_ARMED_DISARMED); + } else { + // Not really needed since this function not called when disconnected + 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; + } + return; + } + + // The rest of these cases throttle is low + if (settings->Arming == FLIGHTMODESETTINGS_ARMING_ALWAYSARMED) { + // In this configuration, we go into armed state as soon as the throttle is low, never disarm + setArmedIfChanged(FLIGHTSTATUS_ARMED_ARMED); + return; + } + + // When the configuration is not "Always armed" and no "Always disarmed", + // the state will not be changed when the throttle is not low + static portTickType armedDisarmStart; + float armingInputLevel = 0; + + // Calc channel see assumptions7 + switch (settings->Arming) { + case FLIGHTMODESETTINGS_ARMING_ROLLLEFT: + armingInputLevel = 1.0f * cmd->Roll; + break; + case FLIGHTMODESETTINGS_ARMING_ROLLRIGHT: + armingInputLevel = -1.0f * cmd->Roll; + break; + case FLIGHTMODESETTINGS_ARMING_PITCHFORWARD: + armingInputLevel = 1.0f * cmd->Pitch; + break; + case FLIGHTMODESETTINGS_ARMING_PITCHAFT: + armingInputLevel = -1.0f * cmd->Pitch; + break; + case FLIGHTMODESETTINGS_ARMING_YAWLEFT: + armingInputLevel = 1.0f * cmd->Yaw; + break; + case FLIGHTMODESETTINGS_ARMING_YAWRIGHT: + armingInputLevel = -1.0f * cmd->Yaw; + break; + case FLIGHTMODESETTINGS_ARMING_ACCESSORY0: + case FLIGHTMODESETTINGS_ARMING_ACCESSORY1: + case FLIGHTMODESETTINGS_ARMING_ACCESSORY2: + armingInputLevel = -1.0f * (float)armSwitch; + break; + } + + bool manualArm = false; + bool manualDisarm = false; + + if (armingInputLevel <= -ARMED_THRESHOLD) { + manualArm = true; + } else if (armingInputLevel >= +ARMED_THRESHOLD) { + manualDisarm = true; + } + + switch (armState) { + case ARM_STATE_DISARMED: + 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) > settings->ArmingSequenceTime)) { + armState = ARM_STATE_ARMED; + } else if (!manualArm) { + armState = ARM_STATE_DISARMED; + } + break; + + case ARM_STATE_ARMED: + // When we get here, the throttle is low, + // we go immediately to disarming due to timeout, also when the disarming mechanism is not enabled + armedDisarmStart = lastSysTime; + armState = ARM_STATE_DISARMING_TIMEOUT; + 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) > settings->DisarmingSequenceTime)) { + armState = ARM_STATE_DISARMED; + } else if (!manualDisarm) { + armState = ARM_STATE_ARMED; + } + break; + } // End Switch + } +} + +/** + * @brief Determine which of N positions the flight mode switch is in and set flight mode accordingly + * @param[out] cmd Pointer to the command structure to set the flight mode in + * @param[in] settings The settings which indicate which position is which mode + * @param[in] flightMode the value of the switch position + */ +static void processFlightMode(ManualControlSettingsData *settings, FlightModeSettingsData *modeSettings, float flightMode, ManualControlCommandData *cmd) +{ + 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) { + pos = settings->FlightModeNumber - 1; + } + + cmd->FlightModeSwitchPosition = pos; + + uint8_t newMode = modeSettings->FlightModePosition[pos]; + + if (flightStatus.FlightMode != newMode) { + flightStatus.FlightMode = newMode; + FlightStatusSet(&flightStatus); + } +} + +/** + * @brief Determine if the manual input value is within acceptable limits + * @returns return TRUE if so, otherwise return FALSE + */ +bool validInputRange(int16_t min, int16_t max, uint16_t value) +{ + if (min > max) { + int16_t tmp = min; + min = max; + max = tmp; + } + return value >= min - CONNECTION_OFFSET && value <= max + CONNECTION_OFFSET; +} + +/** + * @brief Apply deadband to Roll/Pitch/Yaw channels + */ +static void applyDeadband(float *value, float deadband) +{ + if (fabsf(*value) < deadband) { + *value = 0.0f; + } else if (*value > 0.0f) { + *value -= deadband; + } else { + *value += deadband; + } +} + +#ifdef USE_INPUT_LPF +/** + * @brief Apply Low Pass Filter to Throttle/Roll/Pitch/Yaw or Accessory channel + */ +static void applyLPF(float *value, ManualControlSettingsResponseTimeElem channel, ManualControlSettingsData *settings, float dT) +{ + if (cast_struct_to_array(settings->ResponseTime, settings->ResponseTime.Roll)[channel]) { + float rt = (float)cast_struct_to_array(settings->ResponseTime, settings->ResponseTime.Roll)[channel]; + inputFiltered[channel] = ((rt * inputFiltered[channel]) + (dT * (*value))) / (rt + dT); + *value = inputFiltered[channel]; + } +} +#endif // USE_INPUT_LPF +/** + * Called whenever a critical configuration component changes + */ +static void configurationUpdatedCb(__attribute__((unused)) UAVObjEvent *ev) +{ + configuration_check(); +} + +/** + * @} + * @} + */