diff --git a/flight/modules/FixedWingPathFollower/fixedwingpathfollower.c b/flight/modules/FixedWingPathFollower/fixedwingpathfollower.c index 604d09698..64edbe53d 100644 --- a/flight/modules/FixedWingPathFollower/fixedwingpathfollower.c +++ b/flight/modules/FixedWingPathFollower/fixedwingpathfollower.c @@ -49,7 +49,6 @@ #include "attitudestate.h" #include "pathdesired.h" // object that will be updated by the module #include "positionstate.h" -#include "manualcontrol.h" #include "flightstatus.h" #include "pathstatus.h" #include "airspeedstate.h" diff --git a/flight/modules/Receiver/receiver.c b/flight/modules/Receiver/receiver.c index 940e5cf5c..8182e3fb1 100644 --- a/flight/modules/Receiver/receiver.c +++ b/flight/modules/Receiver/receiver.c @@ -2,17 +2,16 @@ ****************************************************************************** * @addtogroup OpenPilotModules OpenPilot Modules * @{ - * @addtogroup ManualControlModule Manual Control Module + * @addtogroup ReceiverModule 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) + * Reads in the ManualControlCommand from receiver then + * pass it to ManualControl * - * @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. + * @file receiver.c + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014. + * @brief Receiver module. Handles safety R/C link and flight mode. * * @see The GNU Public License (GPL) Version 3 * @@ -35,23 +34,14 @@ #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 +#include +#include +#include +#include +#include +#include +#include #include #if defined(PIOS_INCLUDE_USB_RCTX) @@ -59,8 +49,8 @@ #endif /* PIOS_INCLUDE_USB_RCTX */ // Private constants -#if defined(PIOS_MANUAL_STACK_SIZE) -#define STACK_SIZE_BYTES PIOS_MANUAL_STACK_SIZE +#if defined(PIOS_RECEIVER_STACK_SIZE) +#define STACK_SIZE_BYTES PIOS_RECEIVER_STACK_SIZE #else #define STACK_SIZE_BYTES 1152 #endif @@ -73,17 +63,9 @@ #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 @@ -92,20 +74,9 @@ 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 void receiverTask(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); @@ -125,16 +96,22 @@ 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) +#define assumptions \ + ( \ + ((int)MANUALCONTROLCOMMAND_CHANNEL_NUMELEM == (int)MANUALCONTROLSETTINGS_CHANNELGROUPS_NUMELEM) && \ + ((int)MANUALCONTROLCOMMAND_CHANNEL_NUMELEM == (int)MANUALCONTROLSETTINGS_CHANNELNUMBER_NUMELEM) && \ + ((int)MANUALCONTROLCOMMAND_CHANNEL_NUMELEM == (int)MANUALCONTROLSETTINGS_CHANNELMIN_NUMELEM) && \ + ((int)MANUALCONTROLCOMMAND_CHANNEL_NUMELEM == (int)MANUALCONTROLSETTINGS_CHANNELMAX_NUMELEM) && \ + ((int)MANUALCONTROLCOMMAND_CHANNEL_NUMELEM == (int)MANUALCONTROLSETTINGS_CHANNELNEUTRAL_NUMELEM)) /** * Module starting */ -int32_t ManualControlStart() +int32_t ReceiverStart() { // Start main task - xTaskCreate(manualControlTask, (signed char *)"ManualControl", STACK_SIZE_BYTES / 4, NULL, TASK_PRIORITY, &taskHandle); - PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_MANUALCONTROL, taskHandle); + xTaskCreate(receiverTask, (signed char *)"Receiver", STACK_SIZE_BYTES / 4, NULL, TASK_PRIORITY, &taskHandle); + PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_RECEIVER, taskHandle); #ifdef PIOS_INCLUDE_WDG PIOS_WDG_RegisterFlag(PIOS_WDG_MANUAL); #endif @@ -145,35 +122,27 @@ int32_t ManualControlStart() /** * Module initialization */ -int32_t ManualControlInitialize() +int32_t ReceiverInitialize() { /* Check the assumptions about uavobject enum's are correct */ - if (!assumptions) { - return -1; - } + PIOS_STATIC_ASSERT(assumptions); - AccessoryDesiredInitialize(); ManualControlCommandInitialize(); - FlightStatusInitialize(); - StabilizationDesiredInitialize(); ReceiverActivityInitialize(); ManualControlSettingsInitialize(); - FlightModeSettingsInitialize(); return 0; } -MODULE_INITCALL(ManualControlInitialize, ManualControlStart); +MODULE_INITCALL(ReceiverInitialize, ReceiverStart); /** * Module task */ -static void manualControlTask(__attribute__((unused)) void *parameters) +static void receiverTask(__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; @@ -183,20 +152,10 @@ static void manualControlTask(__attribute__((unused)) void *parameters) 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(); @@ -217,11 +176,10 @@ static void manualControlTask(__attribute__((unused)) void *parameters) // Read settings ManualControlSettingsGet(&settings); - FlightModeSettingsGet(&modeSettings); SystemSettingsThrustControlGet(&thrustType); /* Update channel activity monitor */ - if (flightStatus.Armed == ARM_STATE_DISARMED) { + if (flightStatus.Armed == FLIGHTSTATUS_ARMED_DISARMED) { if (updateRcvrActivity(&activity_fsm)) { /* Reset the aging timer because activity was detected */ lastActivityTime = lastSysTime; @@ -244,301 +202,252 @@ static void manualControlTask(__attribute__((unused)) void *parameters) } } - if (!ManualControlCommandReadOnly()) { - bool valid_input_detected = true; + 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[]; + // 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]); - } + 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]); } - // 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; + // 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]); } - - // 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! + // 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 collective if required + (thrustType == SYSTEMSETTINGS_THRUSTCONTROL_COLLECTIVE && ( + settings.ChannelGroups.Collective >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE + || cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_COLLECTIVE] == (uint16_t)PIOS_RCVR_INVALID + || cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_COLLECTIVE] == (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); - 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); + cmd.Connected = MANUALCONTROLCOMMAND_CONNECTED_FALSE; + ManualControlCommandSet(&cmd); + + 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]); + + if (settings.ChannelGroups.Collective != MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) { + valid_input_detected &= validInputRange(settings.ChannelMin.Collective, + settings.ChannelMax.Collective, cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_COLLECTIVE]); + } + if (settings.ChannelGroups.Accessory0 != MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) { + valid_input_detected &= validInputRange(settings.ChannelMin.Accessory0, + settings.ChannelMax.Accessory0, cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY0]); + } + if (settings.ChannelGroups.Accessory1 != MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) { + valid_input_detected &= validInputRange(settings.ChannelMin.Accessory1, + settings.ChannelMax.Accessory1, cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY1]); + } + if (settings.ChannelGroups.Accessory2 != MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) { + valid_input_detected &= validInputRange(settings.ChannelMin.Accessory2, + settings.ChannelMax.Accessory2, cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY2]); + } + + // 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; + } + + 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 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); + case SYSTEMSETTINGS_THRUSTCONTROL_COLLECTIVE: + cmd.Thrust = cmd.Collective; break; default: - AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_CRITICAL); + break; + } + if (settings.FailsafeFlightModeSwitchPosition >= 0 && settings.FailsafeFlightModeSwitchPosition < settings.FlightModeNumber) { + cmd.FlightModeSwitchPosition = (uint8_t)settings.FailsafeFlightModeSwitchPosition; + } + 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]; + // Convert flightMode value into the switch position in the range [0..N-1] + cmd.FlightModeSwitchPosition = ((int16_t)(scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_FLIGHTMODE] * 256.0f) + 256) * settings.FlightModeNumber >> 9; + if (cmd.FlightModeSwitchPosition >= settings.FlightModeNumber) { + cmd.FlightModeSwitchPosition = settings.FlightModeNumber - 1; + } + + // 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]; + if (settings.Deadband > 0.0f) { + applyDeadband(&cmd.Collective, settings.Deadband); + } +#ifdef USE_INPUT_LPF + applyLPF(&cmd.Collective, MANUALCONTROLSETTINGS_RESPONSETIME_COLLECTIVE, &settings, dT); +#endif // USE_INPUT_LPF + } + + 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 (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 (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 (AccessoryDesiredInstSet(2, &accessory) != 0) { + AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING); + } } - break; } - lastFlightMode = flightStatus.FlightMode; + + // 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 */ } } @@ -678,283 +587,6 @@ group_completed: 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. */ @@ -992,261 +624,6 @@ 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 @@ -1289,13 +666,6 @@ static void applyLPF(float *value, ManualControlSettingsResponseTimeElem channel } } #endif // USE_INPUT_LPF -/** - * Called whenever a critical configuration component changes - */ -static void configurationUpdatedCb(__attribute__((unused)) UAVObjEvent *ev) -{ - configuration_check(); -} /** * @} diff --git a/flight/targets/boards/revolution/firmware/Makefile b/flight/targets/boards/revolution/firmware/Makefile index 3202b2e1e..025ae2470 100644 --- a/flight/targets/boards/revolution/firmware/Makefile +++ b/flight/targets/boards/revolution/firmware/Makefile @@ -37,6 +37,7 @@ MODULES += AltitudeHold MODULES += Stabilization MODULES += VtolPathFollower MODULES += ManualControl +MODULES += Receiver MODULES += Actuator MODULES += GPS MODULES += TxPID diff --git a/shared/uavobjectdefinition/manualcontrolsettings.xml b/shared/uavobjectdefinition/manualcontrolsettings.xml index d4fbe858b..acdb3a5dc 100644 --- a/shared/uavobjectdefinition/manualcontrolsettings.xml +++ b/shared/uavobjectdefinition/manualcontrolsettings.xml @@ -13,7 +13,7 @@ + elementnames="Roll,Pitch,Yaw,Collective,Accessory0,Accessory1,Accessory2"/> diff --git a/shared/uavobjectdefinition/taskinfo.xml b/shared/uavobjectdefinition/taskinfo.xml index c5ddc5058..981054a2c 100644 --- a/shared/uavobjectdefinition/taskinfo.xml +++ b/shared/uavobjectdefinition/taskinfo.xml @@ -11,6 +11,7 @@ CallbackScheduler3 ManualControl + Receiver Stabilization Actuator Sensors @@ -45,6 +46,7 @@ CallbackScheduler3 ManualControl + Receiver Stabilization Actuator Sensors @@ -83,6 +85,7 @@ CallbackScheduler3 ManualControl + Receiver Stabilization Actuator Sensors