1
0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2024-12-13 20:48:42 +01:00
LibrePilot/flight/modules/ManualControl/manualcontrol.c

616 lines
26 KiB
C

/**
******************************************************************************
* @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) 2014.
* @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 "inc/manualcontrol.h"
#include <sanitycheck.h>
#include <manualcontrolsettings.h>
#include <manualcontrolcommand.h>
#include <accessorydesired.h>
#include <vtolselftuningstats.h>
#include <flightmodesettings.h>
#include <flightstatus.h>
#include <systemsettings.h>
#include <stabilizationdesired.h>
#include <callbackinfo.h>
#include <stabilizationsettings.h>
#ifndef PIOS_EXCLUDE_ADVANCED_FEATURES
#include <vtolpathfollowersettings.h>
#endif
// Private constants
#if defined(PIOS_MANUAL_STACK_SIZE)
#define STACK_SIZE_BYTES PIOS_MANUAL_STACK_SIZE
#else
#define STACK_SIZE_BYTES 1152
#endif
#define CALLBACK_PRIORITY CALLBACK_PRIORITY_REGULAR
#define CBTASK_PRIORITY CALLBACK_TASK_FLIGHTCONTROL
#define ASSISTEDCONTROL_NEUTRALTHROTTLERANGE_FACTOR 0.2f
#define ASSISTEDCONTROL_BRAKETHRUST_DEADBAND_FACTOR_LO 0.96f
#define ASSISTEDCONTROL_BRAKETHRUST_DEADBAND_FACTOR_HI 1.04f
#define ALWAYSTABILIZEACCESSORY_THRESHOLD 0.50f
// defined handlers
static const controlHandler handler_MANUAL = {
.controlChain = {
.Stabilization = false,
.PathFollower = false,
.PathPlanner = false,
},
.handler = &manualHandler,
};
static const controlHandler handler_STABILIZED = {
.controlChain = {
.Stabilization = true,
.PathFollower = false,
.PathPlanner = false,
},
.handler = &stabilizedHandler,
};
#ifndef PIOS_EXCLUDE_ADVANCED_FEATURES
static const controlHandler handler_PATHFOLLOWER = {
.controlChain = {
.Stabilization = true,
.PathFollower = true,
.PathPlanner = false,
},
.handler = &pathFollowerHandler,
};
static const controlHandler handler_PATHPLANNER = {
.controlChain = {
.Stabilization = true,
.PathFollower = true,
.PathPlanner = true,
},
.handler = &pathPlannerHandler,
};
static float thrustAtBrakeStart = 0.0f;
static float thrustLo = 0.0f;
static float thrustHi = 0.0f;
#endif /* ifndef PIOS_EXCLUDE_ADVANCED_FEATURES */
// Private variables
static DelayedCallbackInfo *callbackHandle;
static FrameType_t frameType = FRAME_TYPE_MULTIROTOR;
// Private functions
static void configurationUpdatedCb(UAVObjEvent *ev);
static void commandUpdatedCb(UAVObjEvent *ev);
static void manualControlTask(void);
#ifndef PIOS_EXCLUDE_ADVANCED_FEATURES
static uint8_t isAssistedFlightMode(uint8_t position, uint8_t flightMode, FlightModeSettingsData *modeSettings);
#endif
static void SettingsUpdatedCb(UAVObjEvent *ev);
#define assumptions (assumptions1 && assumptions2 && assumptions3 && assumptions4 && assumptions5 && assumptions6 && assumptions7 && assumptions_flightmode)
/**
* Module starting
*/
int32_t ManualControlStart()
{
// 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);
ManualControlCommandConnectCallback(commandUpdatedCb);
// clear alarms
AlarmsClear(SYSTEMALARMS_ALARM_MANUALCONTROL);
SettingsUpdatedCb(NULL);
// Make sure unarmed on power up
armHandler(true, frameType);
#ifndef PIOS_EXCLUDE_ADVANCED_FEATURES
takeOffLocationHandlerInit();
#endif
// Start main task
PIOS_CALLBACKSCHEDULER_Dispatch(callbackHandle);
return 0;
}
/**
* Module initialization
*/
int32_t ManualControlInitialize()
{
/* Check the assumptions about uavobject enum's are correct */
PIOS_STATIC_ASSERT(assumptions);
ManualControlCommandInitialize();
FlightStatusInitialize();
ManualControlSettingsInitialize();
FlightModeSettingsInitialize();
SystemSettingsInitialize();
StabilizationSettingsInitialize();
AccessoryDesiredInitialize();
#ifndef PIOS_EXCLUDE_ADVANCED_FEATURES
VtolSelfTuningStatsInitialize();
VtolPathFollowerSettingsInitialize();
VtolPathFollowerSettingsConnectCallback(&SettingsUpdatedCb);
SystemSettingsConnectCallback(&SettingsUpdatedCb);
#endif
callbackHandle = PIOS_CALLBACKSCHEDULER_Create(&manualControlTask, CALLBACK_PRIORITY, CBTASK_PRIORITY, CALLBACKINFO_RUNNING_MANUALCONTROL, STACK_SIZE_BYTES);
return 0;
}
MODULE_INITCALL(ManualControlInitialize, ManualControlStart);
static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
{
frameType = GetCurrentFrameType();
#ifndef PIOS_EXCLUDE_ADVANCED_FEATURES
VtolPathFollowerSettingsTreatCustomCraftAsOptions TreatCustomCraftAs;
VtolPathFollowerSettingsTreatCustomCraftAsGet(&TreatCustomCraftAs);
if (frameType == FRAME_TYPE_CUSTOM) {
switch (TreatCustomCraftAs) {
case VTOLPATHFOLLOWERSETTINGS_TREATCUSTOMCRAFTAS_FIXEDWING:
frameType = FRAME_TYPE_FIXED_WING;
break;
case VTOLPATHFOLLOWERSETTINGS_TREATCUSTOMCRAFTAS_VTOL:
frameType = FRAME_TYPE_MULTIROTOR;
break;
case VTOLPATHFOLLOWERSETTINGS_TREATCUSTOMCRAFTAS_GROUND:
frameType = FRAME_TYPE_GROUND;
break;
}
}
#endif
}
/**
* Module task
*/
static void manualControlTask(void)
{
// Process Arming
armHandler(false, frameType);
#ifndef PIOS_EXCLUDE_ADVANCED_FEATURES
takeOffLocationHandler();
#endif
// Process flight mode
FlightStatusData flightStatus;
FlightStatusGet(&flightStatus);
ManualControlCommandData cmd;
ManualControlCommandGet(&cmd);
AccessoryDesiredData acc;
#ifndef PIOS_EXCLUDE_ADVANCED_FEATURES
VtolPathFollowerSettingsThrustLimitsData thrustLimits;
VtolPathFollowerSettingsThrustLimitsGet(&thrustLimits);
#endif
FlightModeSettingsData modeSettings;
FlightModeSettingsGet(&modeSettings);
uint8_t position = cmd.FlightModeSwitchPosition;
uint8_t newMode = flightStatus.FlightMode;
uint8_t newAlwaysStabilized = flightStatus.AlwaysStabilizeWhenArmed;
uint8_t newFlightModeAssist = flightStatus.FlightModeAssist;
uint8_t newAssistedControlState = flightStatus.AssistedControlState;
uint8_t newAssistedThrottleState = flightStatus.AssistedThrottleState;
if (position < FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_NUMELEM) {
newMode = modeSettings.FlightModePosition[position];
}
// if a mode change occurs we default the assist mode and states here
// to avoid having to add it to all of the below modes that are
// otherwise unrelated
if (newMode != flightStatus.FlightMode) {
// set assist mode to none to avoid an assisted flight mode position
// carrying over and impacting a newly selected non-assisted flight mode pos
newFlightModeAssist = FLIGHTSTATUS_FLIGHTMODEASSIST_NONE;
// The following are equivalent to none effectively. Code should always
// check the flightmodeassist state.
newAssistedControlState = FLIGHTSTATUS_ASSISTEDCONTROLSTATE_PRIMARY;
newAssistedThrottleState = FLIGHTSTATUS_ASSISTEDTHROTTLESTATE_MANUAL;
}
// Depending on the mode update the Stabilization or Actuator objects
const controlHandler *handler = &handler_MANUAL;
switch (newMode) {
case FLIGHTSTATUS_FLIGHTMODE_MANUAL:
handler = &handler_MANUAL;
break;
case FLIGHTSTATUS_FLIGHTMODE_STABILIZED1:
case FLIGHTSTATUS_FLIGHTMODE_STABILIZED2:
case FLIGHTSTATUS_FLIGHTMODE_STABILIZED3:
case FLIGHTSTATUS_FLIGHTMODE_STABILIZED4:
case FLIGHTSTATUS_FLIGHTMODE_STABILIZED5:
case FLIGHTSTATUS_FLIGHTMODE_STABILIZED6:
handler = &handler_STABILIZED;
#ifndef PIOS_EXCLUDE_ADVANCED_FEATURES
newFlightModeAssist = isAssistedFlightMode(position, newMode, &modeSettings);
if (newFlightModeAssist != flightStatus.FlightModeAssist) {
// On change of assist mode reinitialise control state. This is required
// for the scenario where a flight position change reuses a flight mode
// but adds assistedcontrol.
newAssistedControlState = FLIGHTSTATUS_ASSISTEDCONTROLSTATE_PRIMARY;
newAssistedThrottleState = FLIGHTSTATUS_ASSISTEDTHROTTLESTATE_MANUAL;
}
if (newFlightModeAssist) {
// assess roll/pitch state
bool flagRollPitchHasInput = (fabsf(cmd.Roll) > 0.0f || fabsf(cmd.Pitch) > 0.0f);
// assess throttle state
bool throttleNeutral = false;
float neutralThrustOffset = 0.0f;
VtolSelfTuningStatsNeutralThrustOffsetGet(&neutralThrustOffset);
float throttleRangeDelta = (thrustLimits.Neutral + neutralThrustOffset) * ASSISTEDCONTROL_NEUTRALTHROTTLERANGE_FACTOR;
float throttleNeutralLow = (thrustLimits.Neutral + neutralThrustOffset) - throttleRangeDelta;
float throttleNeutralHi = (thrustLimits.Neutral + neutralThrustOffset) + throttleRangeDelta;
if (cmd.Thrust > throttleNeutralLow && cmd.Thrust < throttleNeutralHi) {
throttleNeutral = true;
}
// determine default thrust mode for hold/brake states
uint8_t pathfollowerthrustmode = FLIGHTSTATUS_ASSISTEDTHROTTLESTATE_MANUAL;
if (newFlightModeAssist == FLIGHTSTATUS_FLIGHTMODEASSIST_GPSASSIST) {
pathfollowerthrustmode = FLIGHTSTATUS_ASSISTEDTHROTTLESTATE_AUTO;
}
switch (newAssistedControlState) {
case FLIGHTSTATUS_ASSISTEDCONTROLSTATE_PRIMARY:
if (!flagRollPitchHasInput) {
// no stick input on roll/pitch so enter brake state
newAssistedControlState = FLIGHTSTATUS_ASSISTEDCONTROLSTATE_BRAKE;
newAssistedThrottleState = pathfollowerthrustmode;
handler = &handler_PATHFOLLOWER;
// retain thrust cmd for later comparison with actual in braking
thrustAtBrakeStart = cmd.Thrust;
// calculate hi and low value of +-4% as a mini-deadband
// for use in auto-override in brake sequence
thrustLo = ASSISTEDCONTROL_BRAKETHRUST_DEADBAND_FACTOR_LO * thrustAtBrakeStart;
thrustHi = ASSISTEDCONTROL_BRAKETHRUST_DEADBAND_FACTOR_HI * thrustAtBrakeStart;
// The purpose for auto throttle assist is to go from a mid to high thrust range to a
// neutral vertical-holding/maintaining ~50% thrust range. It is not designed/intended
// to go from near zero to 50%...we don't want an auto-takeoff feature here!
// Also for rapid decents a user might have a bit of forward stick and low throttle
// then stick-off for auto-braking...but if below the vtol min of 20% it will not
// kick in...the flyer needs to manually manage throttle to slow down decent,
// and the next time they put in a bit of stick, revert to primary, and then
// sticks-off it will brake and hold with auto-thrust
if (thrustAtBrakeStart < thrustLimits.Min) {
newAssistedThrottleState = FLIGHTSTATUS_ASSISTEDTHROTTLESTATE_MANUAL; // Effectively None
}
} else {
// stick input so stay in primary mode control state
// newAssistedControlState = FLIGHTSTATUS_ASSISTEDCONTROLSTATE_PRIMARY;
newAssistedThrottleState = FLIGHTSTATUS_ASSISTEDTHROTTLESTATE_MANUAL; // Effectively None
}
break;
case FLIGHTSTATUS_ASSISTEDCONTROLSTATE_BRAKE:
if (flagRollPitchHasInput) {
// stick input during brake sequence allows immediate resumption of control
newAssistedControlState = FLIGHTSTATUS_ASSISTEDCONTROLSTATE_PRIMARY;
newAssistedThrottleState = FLIGHTSTATUS_ASSISTEDTHROTTLESTATE_MANUAL; // Effectively None
} else {
// no stick input, stay in brake mode
newAssistedThrottleState = pathfollowerthrustmode;
handler = &handler_PATHFOLLOWER;
// if auto thrust and user adjusts thrust outside of a deadband in which case revert to manual
if ((newAssistedThrottleState == FLIGHTSTATUS_ASSISTEDTHROTTLESTATE_AUTO) &&
(cmd.Thrust < thrustLo || cmd.Thrust > thrustHi)) {
newAssistedThrottleState = FLIGHTSTATUS_ASSISTEDTHROTTLESTATE_MANUAL;
}
}
break;
case FLIGHTSTATUS_ASSISTEDCONTROLSTATE_HOLD:
if (newFlightModeAssist == FLIGHTSTATUS_FLIGHTMODEASSIST_GPSASSIST_PRIMARYTHRUST ||
(newFlightModeAssist == FLIGHTSTATUS_FLIGHTMODEASSIST_GPSASSIST &&
newAssistedThrottleState == FLIGHTSTATUS_ASSISTEDTHROTTLESTATE_MANUAL)) {
// for manual or primary throttle states/modes, stick control immediately reverts to primary mode control
if (flagRollPitchHasInput) {
newAssistedControlState = FLIGHTSTATUS_ASSISTEDCONTROLSTATE_PRIMARY;
newAssistedThrottleState = FLIGHTSTATUS_ASSISTEDTHROTTLESTATE_MANUAL; // Effectively None
} else {
// otherwise stay in the hold state
// newAssistedControlState = FLIGHTSTATUS_ASSISTEDCONTROLSTATE_HOLD;
newAssistedThrottleState = FLIGHTSTATUS_ASSISTEDTHROTTLESTATE_MANUAL;
handler = &handler_PATHFOLLOWER;
}
} else if (newFlightModeAssist == FLIGHTSTATUS_FLIGHTMODEASSIST_GPSASSIST &&
newAssistedThrottleState != FLIGHTSTATUS_ASSISTEDTHROTTLESTATE_MANUAL) {
// ok auto thrust mode applies in hold unless overridden
if (flagRollPitchHasInput) {
// throttle is neutral approximately and stick input present so revert to primary mode control
newAssistedControlState = FLIGHTSTATUS_ASSISTEDCONTROLSTATE_PRIMARY;
newAssistedThrottleState = FLIGHTSTATUS_ASSISTEDTHROTTLESTATE_MANUAL; // Effectively None
} else {
// otherwise hold, autothrust, no stick input...we watch throttle for need to change mode
switch (newAssistedThrottleState) {
case FLIGHTSTATUS_ASSISTEDTHROTTLESTATE_AUTO:
// whilst in auto, watch for neutral range, from which we allow override
if (throttleNeutral) {
pathfollowerthrustmode = FLIGHTSTATUS_ASSISTEDTHROTTLESTATE_AUTOOVERRIDE;
} else { pathfollowerthrustmode = FLIGHTSTATUS_ASSISTEDTHROTTLESTATE_AUTO; }
break;
case FLIGHTSTATUS_ASSISTEDTHROTTLESTATE_AUTOOVERRIDE:
// previously user has set throttle to neutral range, apply a deadband and revert to manual
// if moving out of deadband. This allows for landing in hold state.
if (!throttleNeutral) {
pathfollowerthrustmode = FLIGHTSTATUS_ASSISTEDTHROTTLESTATE_MANUAL;
} else { pathfollowerthrustmode = FLIGHTSTATUS_ASSISTEDTHROTTLESTATE_AUTOOVERRIDE; }
break;
case FLIGHTSTATUS_ASSISTEDTHROTTLESTATE_MANUAL:
pathfollowerthrustmode = FLIGHTSTATUS_ASSISTEDTHROTTLESTATE_MANUAL;
break;
}
newAssistedThrottleState = pathfollowerthrustmode;
handler = &handler_PATHFOLLOWER;
}
}
break;
}
}
#endif /* ifndef PIOS_EXCLUDE_ADVANCED_FEATURES */
break;
#ifndef PIOS_EXCLUDE_ADVANCED_FEATURES
// During development the assistedcontrol implementation is optional and set
// set in stabi settings. Once if we decide to always have this on, it can
// can be directly set here...i.e. set the flight mode assist as required.
case FLIGHTSTATUS_FLIGHTMODE_VELOCITYROAM:
newFlightModeAssist = FLIGHTSTATUS_FLIGHTMODEASSIST_GPSASSIST_PRIMARYTHRUST;
newAssistedThrottleState = FLIGHTSTATUS_ASSISTEDTHROTTLESTATE_MANUAL;
handler = &handler_PATHFOLLOWER;
break;
case FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD:
case FLIGHTSTATUS_FLIGHTMODE_LAND:
case FLIGHTSTATUS_FLIGHTMODE_AUTOTAKEOFF:
newFlightModeAssist = isAssistedFlightMode(position, newMode, &modeSettings);
if (newFlightModeAssist) {
// Set the default thrust state
switch (newFlightModeAssist) {
case FLIGHTSTATUS_FLIGHTMODEASSIST_GPSASSIST_PRIMARYTHRUST:
newAssistedThrottleState = FLIGHTSTATUS_ASSISTEDTHROTTLESTATE_MANUAL;
break;
case FLIGHTSTATUS_FLIGHTMODEASSIST_GPSASSIST:
newAssistedThrottleState = FLIGHTSTATUS_ASSISTEDTHROTTLESTATE_AUTO;
break;
case FLIGHTSTATUS_FLIGHTMODEASSIST_NONE:
default:
newAssistedThrottleState = FLIGHTSTATUS_ASSISTEDTHROTTLESTATE_MANUAL; // Effectively None
break;
}
}
handler = &handler_PATHFOLLOWER;
break;
case FLIGHTSTATUS_FLIGHTMODE_COURSELOCK:
case FLIGHTSTATUS_FLIGHTMODE_HOMELEASH:
case FLIGHTSTATUS_FLIGHTMODE_ABSOLUTEPOSITION:
case FLIGHTSTATUS_FLIGHTMODE_RETURNTOBASE:
case FLIGHTSTATUS_FLIGHTMODE_POI:
case FLIGHTSTATUS_FLIGHTMODE_AUTOCRUISE:
handler = &handler_PATHFOLLOWER;
break;
case FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER:
handler = &handler_PATHPLANNER;
break;
#endif /* ifndef PIOS_EXCLUDE_ADVANCED_FEATURES */
// There is no default, so if a flightmode is forgotten the compiler can throw a warning!
}
bool alwaysStabilizedSwitch = false;
// Check for a AlwaysStabilizeWhenArmed accessory switch
switch (modeSettings.AlwaysStabilizeWhenArmedSwitch) {
case FLIGHTMODESETTINGS_ALWAYSSTABILIZEWHENARMEDSWITCH_ACCESSORY0:
AccessoryDesiredInstGet(0, &acc);
alwaysStabilizedSwitch = true;
break;
case FLIGHTMODESETTINGS_ALWAYSSTABILIZEWHENARMEDSWITCH_ACCESSORY1:
AccessoryDesiredInstGet(1, &acc);
alwaysStabilizedSwitch = true;
break;
case FLIGHTMODESETTINGS_ALWAYSSTABILIZEWHENARMEDSWITCH_ACCESSORY2:
AccessoryDesiredInstGet(2, &acc);
alwaysStabilizedSwitch = true;
break;
case FLIGHTMODESETTINGS_ALWAYSSTABILIZEWHENARMEDSWITCH_ACCESSORY3:
AccessoryDesiredInstGet(3, &acc);
alwaysStabilizedSwitch = true;
break;
default:
break;
}
if (alwaysStabilizedSwitch) {
if (acc.AccessoryVal <= -ALWAYSTABILIZEACCESSORY_THRESHOLD) {
newAlwaysStabilized = FLIGHTSTATUS_ALWAYSSTABILIZEWHENARMED_FALSE;
} else if (acc.AccessoryVal >= ALWAYSTABILIZEACCESSORY_THRESHOLD) {
newAlwaysStabilized = FLIGHTSTATUS_ALWAYSSTABILIZEWHENARMED_TRUE;
}
} else {
newAlwaysStabilized = FLIGHTSTATUS_ALWAYSSTABILIZEWHENARMED_FALSE;
}
bool newinit = false;
// FlightMode needs to be set correctly on first run (otherwise ControlChain is invalid)
static bool firstRun = true;
if (flightStatus.AlwaysStabilizeWhenArmed != newAlwaysStabilized ||
flightStatus.FlightMode != newMode || firstRun ||
newFlightModeAssist != flightStatus.FlightModeAssist ||
newAssistedControlState != flightStatus.AssistedControlState ||
flightStatus.AssistedThrottleState != newAssistedThrottleState) {
firstRun = false;
flightStatus.ControlChain = handler->controlChain;
flightStatus.FlightMode = newMode;
flightStatus.AlwaysStabilizeWhenArmed = newAlwaysStabilized;
flightStatus.FlightModeAssist = newFlightModeAssist;
flightStatus.AssistedControlState = newAssistedControlState;
flightStatus.AssistedThrottleState = newAssistedThrottleState;
FlightStatusSet(&flightStatus);
newinit = true;
}
if (handler->handler) {
handler->handler(newinit);
}
}
/**
* Called whenever a critical configuration component changes
*/
static void configurationUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
{
configuration_check();
}
/**
* Called whenever a critical configuration component changes
*/
static void commandUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
{
PIOS_CALLBACKSCHEDULER_Dispatch(callbackHandle);
}
/**
* Check and set modes for gps assisted stablised flight modes
*/
#ifndef PIOS_EXCLUDE_ADVANCED_FEATURES
static uint8_t isAssistedFlightMode(uint8_t position, uint8_t flightMode, FlightModeSettingsData *modeSettings)
{
uint8_t flightModeAssistOption = STABILIZATIONSETTINGS_FLIGHTMODEASSISTMAP_NONE;
uint8_t isAssistedFlag = FLIGHTSTATUS_FLIGHTMODEASSIST_NONE;
StabilizationSettingsFlightModeAssistMapOptions FlightModeAssistMap[STABILIZATIONSETTINGS_FLIGHTMODEASSISTMAP_NUMELEM];
StabilizationSettingsFlightModeAssistMapGet(FlightModeAssistMap);
if (position < STABILIZATIONSETTINGS_FLIGHTMODEASSISTMAP_NUMELEM) {
flightModeAssistOption = FlightModeAssistMap[position];
}
switch (flightModeAssistOption) {
case STABILIZATIONSETTINGS_FLIGHTMODEASSISTMAP_NONE:
break;
case STABILIZATIONSETTINGS_FLIGHTMODEASSISTMAP_GPSASSIST:
{
// default to cruise control.
FlightModeSettingsStabilization1SettingsOptions thrustMode = FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_CRUISECONTROL;
switch (flightMode) {
case FLIGHTSTATUS_FLIGHTMODE_STABILIZED1:
thrustMode = modeSettings->Stabilization1Settings.Thrust;
break;
case FLIGHTSTATUS_FLIGHTMODE_STABILIZED2:
thrustMode = modeSettings->Stabilization2Settings.Thrust;
break;
case FLIGHTSTATUS_FLIGHTMODE_STABILIZED3:
thrustMode = modeSettings->Stabilization3Settings.Thrust;
break;
case FLIGHTSTATUS_FLIGHTMODE_STABILIZED4:
thrustMode = modeSettings->Stabilization4Settings.Thrust;
break;
case FLIGHTSTATUS_FLIGHTMODE_STABILIZED5:
thrustMode = modeSettings->Stabilization5Settings.Thrust;
break;
case FLIGHTSTATUS_FLIGHTMODE_STABILIZED6:
thrustMode = modeSettings->Stabilization6Settings.Thrust;
break;
case FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD:
case FLIGHTSTATUS_FLIGHTMODE_VELOCITYROAM:
// we hard code the "GPS Assisted" PostionHold/Roam to use alt-vario which
// is a more appropriate throttle mode. "GPSAssist" adds braking
// and a better throttle management to the standard Position Hold.
thrustMode = FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_ALTITUDEVARIO;
break;
case FLIGHTSTATUS_FLIGHTMODE_LAND:
case FLIGHTSTATUS_FLIGHTMODE_AUTOTAKEOFF:
thrustMode = FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_CRUISECONTROL;
break;
// other modes will use cruisecontrol as default
}
switch (thrustMode) {
case FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_ALTITUDEHOLD:
case FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_ALTITUDEVARIO:
// this is only for use with stabi mods with althold/vario.
isAssistedFlag = FLIGHTSTATUS_FLIGHTMODEASSIST_GPSASSIST_PRIMARYTHRUST;
break;
case FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_MANUAL:
case FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_CRUISECONTROL:
default:
// this is the default for non stabi modes also
isAssistedFlag = FLIGHTSTATUS_FLIGHTMODEASSIST_GPSASSIST;
break;
}
}
break;
}
return isAssistedFlag;
}
#endif /* ifndef PIOS_EXCLUDE_ADVANCED_FEATURES */
/**
* @}
* @}
*/