From b1c94e83d016a05307fd6a7098efd876fc2d37f4 Mon Sep 17 00:00:00 2001 From: abeck70 Date: Thu, 8 Jan 2015 13:20:47 +1100 Subject: [PATCH] gpsassist: One commit new branch Collapsed into one branch for a new code review. Some unnecssary committed files removed. --- flight/libraries/inc/plans.h | 11 + flight/libraries/inc/sanitycheck.h | 2 +- flight/libraries/paths.c | 1 + flight/libraries/plans.c | 93 +++ flight/libraries/sanitycheck.c | 45 +- flight/modules/ManualControl/armhandler.c | 17 +- flight/modules/ManualControl/manualcontrol.c | 290 +++++++- .../ManualControl/pathfollowerhandler.c | 22 +- .../modules/ManualControl/stabilizedhandler.c | 3 +- flight/modules/PathFollower/pathfollower.c | 512 +++++++++++--- flight/modules/Receiver/receiver.c | 105 ++- .../discoveryf4bare/firmware/UAVObjects.inc | 2 + .../boards/revolution/firmware/UAVObjects.inc | 2 + .../boards/revoproto/firmware/UAVObjects.inc | 2 + .../boards/simposix/firmware/UAVObjects.inc | 2 + .../src/plugins/config/configinputwidget.cpp | 13 + .../openpilotgcs/src/plugins/config/input.ui | 630 ++++++++++++------ .../plugins/uavobjects/uavobject.m.template | 14 +- .../src/plugins/uavobjects/uavobjects.pro | 4 + shared/uavobjectdefinition/adjustments.xml | 15 + shared/uavobjectdefinition/flightstatus.xml | 4 + .../manualcontrolsettings.xml | 2 + shared/uavobjectdefinition/pathdesired.xml | 4 +- shared/uavobjectdefinition/pathstatus.xml | 1 + shared/uavobjectdefinition/pathsummary.xml | 19 + .../stabilizationsettings.xml | 4 + .../vtolpathfollowersettings.xml | 4 + 27 files changed, 1471 insertions(+), 352 deletions(-) create mode 100644 shared/uavobjectdefinition/adjustments.xml create mode 100644 shared/uavobjectdefinition/pathsummary.xml diff --git a/flight/libraries/inc/plans.h b/flight/libraries/inc/plans.h index fcf20a633..478de25af 100644 --- a/flight/libraries/inc/plans.h +++ b/flight/libraries/inc/plans.h @@ -30,6 +30,7 @@ #ifndef PLANS_H_ #define PLANS_H_ +#include #include /** \page standard Plans @@ -67,6 +68,16 @@ void plan_setup_land(); */ void plan_run_land(); +/** + * @brief setup pathplanner/pathfollower for braking + */ +void plan_setup_assistedcontrol(uint8_t timeout_occurred); + +#define PATHDESIRED_MODEPARAMETER_BRAKE_STARTVELOCITYVECTOR_NORTH 0 +#define PATHDESIRED_MODEPARAMETER_BRAKE_STARTVELOCITYVECTOR_EAST 1 +#define PATHDESIRED_MODEPARAMETER_BRAKE_STARTVELOCITYVECTOR_DOWN 2 +#define PATHDESIRED_MODEPARAMETER_BRAKE_TIMEOUT 3 + /** * @brief setup pathfollower for positionvario */ diff --git a/flight/libraries/inc/sanitycheck.h b/flight/libraries/inc/sanitycheck.h index 9576d1030..4c6d8e990 100644 --- a/flight/libraries/inc/sanitycheck.h +++ b/flight/libraries/inc/sanitycheck.h @@ -53,6 +53,6 @@ typedef enum { extern int32_t configuration_check(); -FrameType_t GetCurrentFrameType(); +extern FrameType_t GetCurrentFrameType(); #endif /* SANITYCHECK_H */ diff --git a/flight/libraries/paths.c b/flight/libraries/paths.c index 3bcd0a03d..cdfa27f04 100644 --- a/flight/libraries/paths.c +++ b/flight/libraries/paths.c @@ -47,6 +47,7 @@ static void path_circle(PathDesiredData *path, float *cur_point, struct path_sta void path_progress(PathDesiredData *path, float *cur_point, struct path_status *status) { switch (path->Mode) { + case PATHDESIRED_MODE_BRAKE: // should never get here... case PATHDESIRED_MODE_FLYVECTOR: return path_vector(path, cur_point, status, true); diff --git a/flight/libraries/plans.c b/flight/libraries/plans.c index fda8b3f74..bf43d3d17 100644 --- a/flight/libraries/plans.c +++ b/flight/libraries/plans.c @@ -35,8 +35,11 @@ #include #include #include +#include +#include #include #include +#include #include #define UPDATE_EXPECTED 0.02f @@ -53,8 +56,11 @@ void plan_initialize() PositionStateInitialize(); PathDesiredInitialize(); FlightModeSettingsInitialize(); + FlightStatusInitialize(); AttitudeStateInitialize(); ManualControlCommandInitialize(); + VelocityStateInitialize(); + VtolPathFollowerSettingsInitialize(); } /** @@ -84,6 +90,7 @@ void plan_setup_positionHold() PathDesiredSet(&pathDesired); } + /** * @brief setup pathplanner/pathfollower for return to base */ @@ -504,3 +511,89 @@ void plan_run_AutoCruise() pathDesired.Start.Down = pathDesired.End.Down; PathDesiredSet(&pathDesired); } + +/** + * @brief setup pathplanner/pathfollower for braking in positionroam + * timeout_occurred = false: Attempt to enter flyvector for braking + * timeout_occurred = true: Revert to position hold + */ +#define ASSISTEDCONTROL_BRAKERATE_MINIMUM 0.2f // m/s2 +#define ASSISTEDCONTROL_TIMETOSTOP_MINIMUM 0.2f // seconds +#define ASSISTEDCONTROL_TIMETOSTOP_MAXIMUM 9.0f // seconds +#define ASSISTEDCONTROL_DELAY_TO_BRAKE 1.0f // seconds +#define ASSISTEDCONTROL_TIMEOUT_MULTIPLIER 2.0f // actual deceleration rate can be 50% of desired...timeouts need to cater for this +void plan_setup_assistedcontrol(uint8_t timeout_occurred) +{ + PositionStateData positionState; + + PositionStateGet(&positionState); + PathDesiredData pathDesired; + PathDesiredGet(&pathDesired); + + FlightStatusAssistedControlStateOptions assistedControlFlightMode; + FlightStatusAssistedControlStateGet(&assistedControlFlightMode); + + if (timeout_occurred) { + pathDesired.End.North = positionState.North; + pathDesired.End.East = positionState.East; + pathDesired.End.Down = positionState.Down; + pathDesired.Start.North = positionState.North; + pathDesired.Start.East = positionState.East; + pathDesired.Start.Down = positionState.Down; + pathDesired.StartingVelocity = 0.0f; + pathDesired.EndingVelocity = 0.0f; + pathDesired.Mode = PATHDESIRED_MODE_FLYENDPOINT; + assistedControlFlightMode = FLIGHTSTATUS_ASSISTEDCONTROLSTATE_HOLD; + } else { + VelocityStateData velocityState; + VelocityStateGet(&velocityState); + float brakeRate; + VtolPathFollowerSettingsBrakeRateGet(&brakeRate); + if (brakeRate < ASSISTEDCONTROL_BRAKERATE_MINIMUM) { + brakeRate = ASSISTEDCONTROL_BRAKERATE_MINIMUM; // set a minimum to avoid a divide by zero potential below + } + // Calculate the velocity + float velocity = velocityState.North * velocityState.North + velocityState.East * velocityState.East + velocityState.Down * velocityState.Down; + velocity = sqrtf(velocity); + + // Calculate the desired time to zero velocity. + float time_to_stopped = ASSISTEDCONTROL_DELAY_TO_BRAKE; // we allow at least 0.5 seconds to rotate to a brake angle. + time_to_stopped += velocity / brakeRate; + + // Sanity check the brake rate by ensuring that the time to stop is within a range. + if (time_to_stopped < ASSISTEDCONTROL_TIMETOSTOP_MINIMUM) { + time_to_stopped = ASSISTEDCONTROL_TIMETOSTOP_MINIMUM; + } else if (time_to_stopped > ASSISTEDCONTROL_TIMETOSTOP_MAXIMUM) { + time_to_stopped = ASSISTEDCONTROL_TIMETOSTOP_MAXIMUM; + } + + // calculate the distance we will travel + float north_delta = velocityState.North * ASSISTEDCONTROL_DELAY_TO_BRAKE; // we allow at least 0.5s to rotate to brake angle + north_delta += (time_to_stopped - ASSISTEDCONTROL_DELAY_TO_BRAKE) * 0.5f * velocityState.North; // area under the linear deceleration plot + float east_delta = velocityState.East * ASSISTEDCONTROL_DELAY_TO_BRAKE; // we allow at least 0.5s to rotate to brake angle + east_delta += (time_to_stopped - ASSISTEDCONTROL_DELAY_TO_BRAKE) * 0.5f * velocityState.East; // area under the linear deceleration plot + float down_delta = velocityState.Down * ASSISTEDCONTROL_DELAY_TO_BRAKE; + down_delta += (time_to_stopped - ASSISTEDCONTROL_DELAY_TO_BRAKE) * 0.5f * velocityState.Down; // area under the linear deceleration plot + float net_delta = east_delta * east_delta + north_delta * north_delta + down_delta * down_delta; + net_delta = sqrtf(net_delta); + + pathDesired.Start.North = positionState.North; + pathDesired.Start.East = positionState.East; + pathDesired.Start.Down = positionState.Down; + pathDesired.ModeParameters[PATHDESIRED_MODEPARAMETER_BRAKE_STARTVELOCITYVECTOR_NORTH] = velocityState.North; + pathDesired.ModeParameters[PATHDESIRED_MODEPARAMETER_BRAKE_STARTVELOCITYVECTOR_EAST] = velocityState.East; + pathDesired.ModeParameters[PATHDESIRED_MODEPARAMETER_BRAKE_STARTVELOCITYVECTOR_DOWN] = velocityState.Down; + + pathDesired.End.North = positionState.North + north_delta; + pathDesired.End.East = positionState.East + east_delta; + pathDesired.End.Down = positionState.Down + down_delta; + + pathDesired.StartingVelocity = velocity; + pathDesired.EndingVelocity = 0.0f; + pathDesired.Mode = PATHDESIRED_MODE_BRAKE; + pathDesired.ModeParameters[PATHDESIRED_MODEPARAMETER_BRAKE_TIMEOUT] = time_to_stopped * ASSISTEDCONTROL_TIMEOUT_MULTIPLIER; + assistedControlFlightMode = FLIGHTSTATUS_ASSISTEDCONTROLSTATE_BRAKE; + } + FlightStatusAssistedControlStateSet(&assistedControlFlightMode); + PathDesiredSet(&pathDesired); +} diff --git a/flight/libraries/sanitycheck.c b/flight/libraries/sanitycheck.c index 32f4e41e5..85c074ef9 100644 --- a/flight/libraries/sanitycheck.c +++ b/flight/libraries/sanitycheck.c @@ -36,6 +36,7 @@ #include #include #include +#include #include #include #include @@ -46,7 +47,7 @@ // ! Check a stabilization mode switch position for safety -static bool check_stabilization_settings(int index, bool multirotor, bool coptercontrol); +static bool check_stabilization_settings(int index, bool multirotor, bool coptercontrol, bool gpsassisted); /** * Run a preflight check over the hardware configuration @@ -95,31 +96,41 @@ int32_t configuration_check() // modes uint8_t num_modes; uint8_t modes[FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_NUMELEM]; + uint8_t FlightModeAssistMap[STABILIZATIONSETTINGS_FLIGHTMODEASSISTMAP_NUMELEM]; ManualControlSettingsFlightModeNumberGet(&num_modes); + StabilizationSettingsFlightModeAssistMapGet(FlightModeAssistMap); FlightModeSettingsFlightModePositionGet(modes); for (uint32_t i = 0; i < num_modes; i++) { + uint8_t gps_assisted = FlightModeAssistMap[i]; + if (gps_assisted) { + ADDSEVERITY(!coptercontrol); + ADDSEVERITY(multirotor); + ADDSEVERITY(navCapableFusion); + } + switch (modes[i]) { case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_MANUAL: + ADDSEVERITY(!gps_assisted); ADDSEVERITY(!multirotor); break; case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_STABILIZED1: - ADDSEVERITY(check_stabilization_settings(1, multirotor, coptercontrol)); + ADDSEVERITY(check_stabilization_settings(1, multirotor, coptercontrol, gps_assisted)); break; case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_STABILIZED2: - ADDSEVERITY(check_stabilization_settings(2, multirotor, coptercontrol)); + ADDSEVERITY(check_stabilization_settings(2, multirotor, coptercontrol, gps_assisted)); break; case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_STABILIZED3: - ADDSEVERITY(check_stabilization_settings(3, multirotor, coptercontrol)); + ADDSEVERITY(check_stabilization_settings(3, multirotor, coptercontrol, gps_assisted)); break; case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_STABILIZED4: - ADDSEVERITY(check_stabilization_settings(4, multirotor, coptercontrol)); + ADDSEVERITY(check_stabilization_settings(4, multirotor, coptercontrol, gps_assisted)); break; case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_STABILIZED5: - ADDSEVERITY(check_stabilization_settings(5, multirotor, coptercontrol)); + ADDSEVERITY(check_stabilization_settings(5, multirotor, coptercontrol, gps_assisted)); break; case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_STABILIZED6: - ADDSEVERITY(check_stabilization_settings(6, multirotor, coptercontrol)); + ADDSEVERITY(check_stabilization_settings(6, multirotor, coptercontrol, gps_assisted)); break; case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_PATHPLANNER: { @@ -129,9 +140,13 @@ int32_t configuration_check() SystemAlarmsAlarmData alarms; SystemAlarmsAlarmGet(&alarms); ADDSEVERITY(alarms.PathPlan == SYSTEMALARMS_ALARM_OK); + ADDSEVERITY(!gps_assisted); } - // intentionally no break as this also needs pathfollower case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_POSITIONHOLD: + ADDSEVERITY(!coptercontrol); + ADDSEVERITY(navCapableFusion); + break; + case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_COURSELOCK: case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_POSITIONROAM: case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_HOMELEASH: @@ -140,6 +155,7 @@ int32_t configuration_check() case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_POI: case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_RETURNTOBASE: case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_AUTOCRUISE: + ADDSEVERITY(!gps_assisted); ADDSEVERITY(!coptercontrol); ADDSEVERITY(navCapableFusion); break; @@ -175,7 +191,7 @@ int32_t configuration_check() * @param[in] index Which stabilization mode to check * @returns true or false */ -static bool check_stabilization_settings(int index, bool multirotor, bool coptercontrol) +static bool check_stabilization_settings(int index, bool multirotor, bool coptercontrol, bool gpsassisted) { uint8_t modes[FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_NUMELEM]; @@ -213,6 +229,17 @@ static bool check_stabilization_settings(int index, bool multirotor, bool copter } } + if (gpsassisted) { + // For multirotors verify that roll/pitch are either attitude or rattitude + for (uint32_t i = 0; i < FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_YAW; i++) { + if (!(modes[i] == FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_ATTITUDE || + modes[i] == FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_RATTITUDE)) { + return false; + } + } + } + + // coptercontrol cannot do altitude holding if (coptercontrol) { if (modes[FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_THRUST] == FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_ALTITUDEHOLD diff --git a/flight/modules/ManualControl/armhandler.c b/flight/modules/ManualControl/armhandler.c index 01f0eb84b..b17ce8334 100644 --- a/flight/modules/ManualControl/armhandler.c +++ b/flight/modules/ManualControl/armhandler.c @@ -278,9 +278,9 @@ static bool okToArm(void) StabilizationDesiredStabilizationModeData stabDesired; - uint8_t flightMode; - FlightStatusFlightModeGet(&flightMode); - switch (flightMode) { + FlightStatusData flightStatus; + FlightStatusGet(&flightStatus); + switch (flightStatus.FlightMode) { case FLIGHTSTATUS_FLIGHTMODE_MANUAL: case FLIGHTSTATUS_FLIGHTMODE_STABILIZED1: case FLIGHTSTATUS_FLIGHTMODE_STABILIZED2: @@ -293,9 +293,16 @@ static bool okToArm(void) if (stabDesired.Thrust == STABILIZATIONDESIRED_STABILIZATIONMODE_ALTITUDEHOLD || stabDesired.Thrust == STABILIZATIONDESIRED_STABILIZATIONMODE_ALTITUDEVARIO) { return false; - } else { - return true; } + + // avoid assisted control with auto throttle. As it sits waiting to launch, + // it will move to hold, and auto thrust will auto launch otherwise! + if (flightStatus.FlightModeAssist == FLIGHTSTATUS_FLIGHTMODEASSIST_GPSASSIST) { + return false; + } + + return true; + break; default: diff --git a/flight/modules/ManualControl/manualcontrol.c b/flight/modules/ManualControl/manualcontrol.c index c695e5b43..40805dbb3 100644 --- a/flight/modules/ManualControl/manualcontrol.c +++ b/flight/modules/ManualControl/manualcontrol.c @@ -37,12 +37,16 @@ #include #include #include +#include #include #include #include #include #include - +#ifndef PIOS_EXCLUDE_ADVANCED_FEATURES +#include +#include +#endif // Private constants #if defined(PIOS_MANUAL_STACK_SIZE) @@ -93,6 +97,9 @@ static const controlHandler handler_PATHPLANNER = { }, .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 @@ -101,8 +108,10 @@ static DelayedCallbackInfo *callbackHandle; // 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 #define assumptions (assumptions1 && assumptions2 && assumptions3 && assumptions4 && assumptions5 && assumptions6 && assumptions7 && assumptions_flightmode) @@ -147,7 +156,11 @@ int32_t ManualControlInitialize() ManualControlSettingsInitialize(); FlightModeSettingsInitialize(); SystemSettingsInitialize(); - +#ifndef PIOS_EXCLUDE_ADVANCED_FEATURES + AdjustmentsInitialize(); + StabilizationSettingsInitialize(); + VtolPathFollowerSettingsInitialize(); +#endif callbackHandle = PIOS_CALLBACKSCHEDULER_Create(&manualControlTask, CALLBACK_PRIORITY, CBTASK_PRIORITY, CALLBACKINFO_RUNNING_MANUALCONTROL, STACK_SIZE_BYTES); return 0; @@ -170,16 +183,36 @@ static void manualControlTask(void) FlightStatusGet(&flightStatus); ManualControlCommandData cmd; ManualControlCommandGet(&cmd); +#ifndef PIOS_EXCLUDE_ADVANCED_FEATURES + VtolPathFollowerSettingsData vtolPathFollowerSettings; + VtolPathFollowerSettingsGet(&vtolPathFollowerSettings); +#endif FlightModeSettingsData modeSettings; FlightModeSettingsGet(&modeSettings); uint8_t position = cmd.FlightModeSwitchPosition; uint8_t newMode = flightStatus.FlightMode; + 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 eqivalent 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) { @@ -193,9 +226,171 @@ static void manualControlTask(void) case FLIGHTSTATUS_FLIGHTMODE_STABILIZED5: case FLIGHTSTATUS_FLIGHTMODE_STABILIZED6: handler = &handler_STABILIZED; + +#ifndef PIOS_EXCLUDE_ADVANCED_FEATURES + newFlightModeAssist = isAssistedFlightMode(position, newMode, &modeSettings); + if (newFlightModeAssist) { + // assess roll/pitch state + bool flagRollPitchHasInput = (fabsf(cmd.Roll) > 0.0f || fabsf(cmd.Pitch) > 0.0f); + + // assess throttle state + bool throttleNeutral = false; + bool throttleNeutralOrLow = false; + float neutralThrustOffset = 0.0f; + AdjustmentsNeutralThrustOffsetGet(&neutralThrustOffset); + float throttleRangeDelta = (vtolPathFollowerSettings.ThrustLimits.Neutral + neutralThrustOffset) * 0.2f; + float throttleNeutralLow = (vtolPathFollowerSettings.ThrustLimits.Neutral + neutralThrustOffset) - throttleRangeDelta; + float throttleNeutralHi = (vtolPathFollowerSettings.ThrustLimits.Neutral + neutralThrustOffset) + throttleRangeDelta; + if (cmd.Thrust > throttleNeutralLow && cmd.Thrust < throttleNeutralHi) { + throttleNeutral = true; + } + if (cmd.Thrust < throttleNeutralHi) { + throttleNeutralOrLow = 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 +-8% as a mini-deadband + // for use in auto-override in brake sequence + thrustLo = 0.92f * thrustAtBrakeStart; + thrustHi = 1.08f * 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 < vtolPathFollowerSettings.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 (throttleNeutralOrLow && 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 + case FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD: + newFlightModeAssist = isAssistedFlightMode(position, newMode, &modeSettings); + if (newFlightModeAssist) { + 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; + } + + switch (newAssistedControlState) { + case FLIGHTSTATUS_ASSISTEDCONTROLSTATE_BRAKE: + newAssistedControlState = FLIGHTSTATUS_ASSISTEDCONTROLSTATE_BRAKE; + break; + case FLIGHTSTATUS_ASSISTEDCONTROLSTATE_PRIMARY: + newAssistedControlState = FLIGHTSTATUS_ASSISTEDCONTROLSTATE_BRAKE; + break; + case FLIGHTSTATUS_ASSISTEDCONTROLSTATE_HOLD: + newAssistedControlState = FLIGHTSTATUS_ASSISTEDCONTROLSTATE_HOLD; + break; + } + } + handler = &handler_PATHFOLLOWER; + break; + case FLIGHTSTATUS_FLIGHTMODE_COURSELOCK: case FLIGHTSTATUS_FLIGHTMODE_POSITIONROAM: case FLIGHTSTATUS_FLIGHTMODE_HOMELEASH: @@ -209,7 +404,7 @@ static void manualControlTask(void) case FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER: handler = &handler_PATHPLANNER; break; -#endif +#endif /* ifndef PIOS_EXCLUDE_ADVANCED_FEATURES */ // There is no default, so if a flightmode is forgotten the compiler can throw a warning! } @@ -218,10 +413,16 @@ static void manualControlTask(void) // FlightMode needs to be set correctly on first run (otherwise ControlChain is invalid) static bool firstRun = true; - if (flightStatus.FlightMode != newMode || firstRun) { + if (flightStatus.FlightMode != newMode || firstRun || + newFlightModeAssist != flightStatus.FlightModeAssist || + newAssistedControlState != flightStatus.AssistedControlState || + flightStatus.AssistedThrottleState != newAssistedThrottleState) { firstRun = false; - flightStatus.ControlChain = handler->controlChain; - flightStatus.FlightMode = newMode; + flightStatus.ControlChain = handler->controlChain; + flightStatus.FlightMode = newMode; + flightStatus.FlightModeAssist = newFlightModeAssist; + flightStatus.AssistedControlState = newAssistedControlState; + flightStatus.AssistedThrottleState = newAssistedThrottleState; FlightStatusSet(&flightStatus); newinit = true; } @@ -246,6 +447,81 @@ 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; + uint8_t 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: + // we hard code the "GPS Assisted" PostionHold 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; + + // 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 */ + /** * @} * @} diff --git a/flight/modules/ManualControl/pathfollowerhandler.c b/flight/modules/ManualControl/pathfollowerhandler.c index b492f2281..fb0420317 100644 --- a/flight/modules/ManualControl/pathfollowerhandler.c +++ b/flight/modules/ManualControl/pathfollowerhandler.c @@ -56,7 +56,9 @@ void pathFollowerHandler(bool newinit) } uint8_t flightMode; + uint8_t assistedControlFlightMode; FlightStatusFlightModeGet(&flightMode); + FlightStatusAssistedControlStateGet(&assistedControlFlightMode); if (newinit) { // After not being in this mode for a while init at current height @@ -64,9 +66,13 @@ void pathFollowerHandler(bool newinit) case FLIGHTSTATUS_FLIGHTMODE_RETURNTOBASE: plan_setup_returnToBase(); break; - case FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD: - plan_setup_positionHold(); + if (assistedControlFlightMode == FLIGHTSTATUS_ASSISTEDCONTROLSTATE_BRAKE) { + // Just initiated braking after returning from stabi control + plan_setup_assistedcontrol(false); + } else { + plan_setup_positionHold(); + } break; case FLIGHTSTATUS_FLIGHTMODE_COURSELOCK: plan_setup_CourseLock(); @@ -88,6 +94,18 @@ void pathFollowerHandler(bool newinit) plan_setup_AutoCruise(); 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: + if (assistedControlFlightMode == FLIGHTSTATUS_ASSISTEDCONTROLSTATE_BRAKE) { + // Just initiated braking after returning from stabi control + plan_setup_assistedcontrol(false); + } + break; + default: plan_setup_positionHold(); break; diff --git a/flight/modules/ManualControl/stabilizedhandler.c b/flight/modules/ManualControl/stabilizedhandler.c index 085d349b4..4621a7171 100644 --- a/flight/modules/ManualControl/stabilizedhandler.c +++ b/flight/modules/ManualControl/stabilizedhandler.c @@ -93,6 +93,7 @@ void stabilizedHandler(bool newinit) uint8_t *stab_settings; FlightStatusData flightStatus; FlightStatusGet(&flightStatus); + switch (flightStatus.FlightMode) { case FLIGHTSTATUS_FLIGHTMODE_STABILIZED1: stab_settings = FlightModeSettingsStabilization1SettingsToArray(settings.Stabilization1Settings); @@ -163,8 +164,8 @@ void stabilizedHandler(bool newinit) 0; // this is an invalid mode } - stabilization.Thrust = cmd.Thrust; stabilization.StabilizationMode.Thrust = stab_settings[3]; + stabilization.Thrust = cmd.Thrust; StabilizationDesiredSet(&stabilization); } diff --git a/flight/modules/PathFollower/pathfollower.c b/flight/modules/PathFollower/pathfollower.c index 1cdcc569c..b5a38ad76 100644 --- a/flight/modules/PathFollower/pathfollower.c +++ b/flight/modules/PathFollower/pathfollower.c @@ -54,6 +54,7 @@ #include #include #include +#include "plans.h" #include @@ -61,6 +62,7 @@ #include #include #include +#include #include #include #include @@ -73,25 +75,42 @@ #include #include #include +#include +#include // Private constants -#define CALLBACK_PRIORITY CALLBACK_PRIORITY_LOW -#define CBTASK_PRIORITY CALLBACK_TASK_FLIGHTCONTROL +#define CALLBACK_PRIORITY CALLBACK_PRIORITY_LOW +#define CBTASK_PRIORITY CALLBACK_TASK_FLIGHTCONTROL -#define PF_IDLE_UPDATE_RATE_MS 100 +#define PF_IDLE_UPDATE_RATE_MS 100 -#define STACK_SIZE_BYTES 2048 +#define STACK_SIZE_BYTES 2048 + +#define DEADBAND_HIGH 0.10f +#define DEADBAND_LOW -0.10f + +#define BRAKE_FRACTIONALPROGRESS_STARTVELOCITYCHECK 0.95f +#define BRAKE_EXIT_VELOCITY_LIMIT 0.2f + +#define BRAKE_RATE_MINIMUM 0.2f + +#define NEUTRALTHRUST_PH_POSITIONAL_ERROR_LIMIT 0.5f +#define NEUTRALTHRUST_PH_VEL_DESIRED_LIMIT 0.2f +#define NEUTRALTHRUST_PH_VEL_STATE_LIMIT 0.2f +#define NEUTRALTHRUST_PH_VEL_ERROR_LIMIT 0.1f + +#define NEUTRALTHRUST_START_DELAY (2 * 20) // 2 seconds at rate of 20Hz (50ms update rate) +#define NEUTRALTHRUST_END_COUNT (NEUTRALTHRUST_START_DELAY + (4 * 20)) // 4 second sample -#define DEADBAND_HIGH 0.10f -#define DEADBAND_LOW -0.10f // Private types struct Globals { struct pid PIDposH[2]; struct pid PIDposV; - struct pid PIDvel[3]; + struct pid PIDvel[3]; // North, East, Down + struct pid BrakePIDvel[2]; // North, East struct pid PIDcourse; struct pid PIDspeed; struct pid PIDpower; @@ -100,6 +119,19 @@ struct Globals { bool vtolEmergencyFallbackSwitch; }; +struct NeutralThrustEstimation { + uint32_t count; + float sum; + float average; + float correction; + float algo_erro_check; + float min; + float max; + bool start_sampling; + bool have_correction; +}; +static struct NeutralThrustEstimation neutralThrustEst; + // Private variables static DelayedCallbackInfo *pathFollowerCBInfo; @@ -109,6 +141,8 @@ static PathStatusData pathStatus; static PathDesiredData pathDesired; static FixedWingPathFollowerSettingsData fixedWingPathFollowerSettings; static VtolPathFollowerSettingsData vtolPathFollowerSettings; +static FlightStatusData flightStatus; +static PathSummaryData pathSummary; // correct speed by measured airspeed static float indicatedAirspeedStateBias = 0.0f; @@ -126,6 +160,7 @@ static float updateCourseBearing(); static float updatePathBearing(); static float updatePOIBearing(); static void processPOI(); +static void updateBrakeVelocity(float startingVelocity, float dT, float brakeRate, float currentVelocity, float *updatedVelocity); static void updatePathVelocity(float kFF, bool limited); static uint8_t updateFixedDesiredAttitude(); static int8_t updateVtolDesiredAttitude(bool yaw_attitude, float yaw_direction); @@ -159,7 +194,9 @@ int32_t PathFollowerInitialize() FixedWingPathFollowerStatusInitialize(); VtolPathFollowerSettingsInitialize(); FlightStatusInitialize(); + FlightModeSettingsInitialize(); PathStatusInitialize(); + PathSummaryInitialize(); PathDesiredInitialize(); PositionStateInitialize(); VelocityStateInitialize(); @@ -172,6 +209,8 @@ int32_t PathFollowerInitialize() ManualControlCommandInitialize(); SystemSettingsInitialize(); StabilizationBankInitialize(); + AdjustmentsInitialize(); + // reset integrals resetGlobals(); @@ -193,8 +232,6 @@ MODULE_INITCALL(PathFollowerInitialize, PathFollowerStart); */ static void pathFollowerTask(void) { - FlightStatusData flightStatus; - FlightStatusGet(&flightStatus); if (flightStatus.ControlChain.PathFollower != FLIGHTSTATUS_CONTROLCHAIN_TRUE) { @@ -208,11 +245,21 @@ static void pathFollowerTask(void) processPOI(); } + int16_t old_uid = pathStatus.UID; pathStatus.UID = pathDesired.UID; pathStatus.Status = PATHSTATUS_STATUS_INPROGRESS; + if (pathDesired.ModeParameters[PATHDESIRED_MODEPARAMETER_BRAKE_TIMEOUT] > 0.0f) { + if (old_uid != pathStatus.UID) { + pathStatus.path_time = 0.0f; + } else { + pathStatus.path_time += updatePeriod / 1000.0f; + } + } + switch (pathDesired.Mode) { case PATHDESIRED_MODE_FLYENDPOINT: case PATHDESIRED_MODE_FLYVECTOR: + case PATHDESIRED_MODE_BRAKE: case PATHDESIRED_MODE_FLYCIRCLERIGHT: case PATHDESIRED_MODE_FLYCIRCLELEFT: { @@ -239,6 +286,7 @@ static void pathFollowerTask(void) } PathStatusSet(&pathStatus); + PIOS_CALLBACKSCHEDULER_Schedule(pathFollowerCBInfo, updatePeriod, CALLBACK_UPDATEMODE_SOONER); } @@ -258,6 +306,9 @@ static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev) pid_configure(&global.PIDvel[1], vtolPathFollowerSettings.HorizontalVelPID.Kp, vtolPathFollowerSettings.HorizontalVelPID.Ki, vtolPathFollowerSettings.HorizontalVelPID.Kd, vtolPathFollowerSettings.HorizontalVelPID.ILimit); pid_configure(&global.PIDvel[2], vtolPathFollowerSettings.VerticalVelPID.Kp, vtolPathFollowerSettings.VerticalVelPID.Ki, vtolPathFollowerSettings.VerticalVelPID.Kd, vtolPathFollowerSettings.VerticalVelPID.ILimit); + pid_configure(&global.BrakePIDvel[0], vtolPathFollowerSettings.BrakeHorizontalVelPID.Kp, vtolPathFollowerSettings.BrakeHorizontalVelPID.Ki, vtolPathFollowerSettings.BrakeHorizontalVelPID.Kd, vtolPathFollowerSettings.BrakeHorizontalVelPID.ILimit); + pid_configure(&global.BrakePIDvel[1], vtolPathFollowerSettings.BrakeHorizontalVelPID.Kp, vtolPathFollowerSettings.BrakeHorizontalVelPID.Ki, vtolPathFollowerSettings.BrakeHorizontalVelPID.Kd, vtolPathFollowerSettings.BrakeHorizontalVelPID.ILimit); + PathDesiredGet(&pathDesired); } @@ -297,12 +348,27 @@ static void resetGlobals() pid_zero(&global.PIDvel[0]); pid_zero(&global.PIDvel[1]); pid_zero(&global.PIDvel[2]); + pid_zero(&global.BrakePIDvel[0]); + pid_zero(&global.BrakePIDvel[1]); pid_zero(&global.PIDcourse); pid_zero(&global.PIDspeed); pid_zero(&global.PIDpower); global.poiRadius = 0.0f; global.vtolEmergencyFallback = 0; global.vtolEmergencyFallbackSwitch = false; + + // reset neutral thrust assessment. We restart this process + // and do once for each position hold engagement + neutralThrustEst.start_sampling = false; + neutralThrustEst.count = 0; + neutralThrustEst.sum = 0.0f; + neutralThrustEst.have_correction = false; + neutralThrustEst.average = 0.0f; + neutralThrustEst.correction = 0.0f; + neutralThrustEst.min = 0.0f; + neutralThrustEst.max = 0.0f; + + pathStatus.path_time = 0.0f; } static uint8_t updateAutoPilotByFrameType() @@ -389,28 +455,37 @@ static uint8_t updateAutoPilotVtol() // yaw behaviour is configurable in vtolpathfollower, select yaw control algorithm bool yaw_attitude = true; float yaw = 0.0f; - switch (vtolPathFollowerSettings.YawControl) { - case VTOLPATHFOLLOWERSETTINGS_YAWCONTROL_MANUAL: + + if (pathDesired.Mode == PATHDESIRED_MODE_BRAKE) { yaw_attitude = false; - break; - case VTOLPATHFOLLOWERSETTINGS_YAWCONTROL_TAILIN: - yaw = updateTailInBearing(); - break; - case VTOLPATHFOLLOWERSETTINGS_YAWCONTROL_MOVEMENTDIRECTION: - yaw = updateCourseBearing(); - break; - case VTOLPATHFOLLOWERSETTINGS_YAWCONTROL_PATHDIRECTION: - yaw = updatePathBearing(); - break; - case VTOLPATHFOLLOWERSETTINGS_YAWCONTROL_POI: - yaw = updatePOIBearing(); - break; + } else { + switch (vtolPathFollowerSettings.YawControl) { + case VTOLPATHFOLLOWERSETTINGS_YAWCONTROL_MANUAL: + yaw_attitude = false; + break; + case VTOLPATHFOLLOWERSETTINGS_YAWCONTROL_TAILIN: + yaw = updateTailInBearing(); + break; + case VTOLPATHFOLLOWERSETTINGS_YAWCONTROL_MOVEMENTDIRECTION: + yaw = updateCourseBearing(); + break; + case VTOLPATHFOLLOWERSETTINGS_YAWCONTROL_PATHDIRECTION: + yaw = updatePathBearing(); + break; + case VTOLPATHFOLLOWERSETTINGS_YAWCONTROL_POI: + yaw = updatePOIBearing(); + break; + } } result = updateVtolDesiredAttitude(yaw_attitude, yaw); - // switch to emergency follower if follower indicates problems - if (!result && (vtolPathFollowerSettings.FlyawayEmergencyFallback != VTOLPATHFOLLOWERSETTINGS_FLYAWAYEMERGENCYFALLBACK_DISABLED)) { - global.vtolEmergencyFallbackSwitch = true; + if (!result) { + if (pathDesired.Mode == PATHDESIRED_MODE_BRAKE) { + plan_setup_assistedcontrol(true); // revert braking to position hold, user can always stick override + } else if (vtolPathFollowerSettings.FlyawayEmergencyFallback != VTOLPATHFOLLOWERSETTINGS_FLYAWAYEMERGENCYFALLBACK_DISABLED) { + // switch to emergency follower if follower indicates problems + global.vtolEmergencyFallbackSwitch = true; + } } } break; @@ -427,6 +502,48 @@ static uint8_t updateAutoPilotVtol() break; } + + // Brake mode end condition checks + if (pathDesired.Mode == PATHDESIRED_MODE_BRAKE) { + bool exit_brake = false; + VelocityStateData velocityState; + if (pathStatus.path_time > pathDesired.ModeParameters[PATHDESIRED_MODEPARAMETER_BRAKE_TIMEOUT]) { // enter hold on timeout + pathSummary.brake_exit_reason = PATHSUMMARY_BRAKE_EXIT_REASON_TIMEOUT; + exit_brake = true; + } else if (pathStatus.fractional_progress > BRAKE_FRACTIONALPROGRESS_STARTVELOCITYCHECK) { + VelocityStateGet(&velocityState); + if (fabsf(velocityState.East) < BRAKE_EXIT_VELOCITY_LIMIT && fabsf(velocityState.North) < BRAKE_EXIT_VELOCITY_LIMIT) { + pathSummary.brake_exit_reason = PATHSUMMARY_BRAKE_EXIT_REASON_PATHCOMPLETED; + exit_brake = true; + } + } + + if (exit_brake) { + // Calculate the distance error between the originally desired + // stopping point and the actual brake-exit point. + + PositionStateData p; + PositionStateGet(&p); + float north_offset = pathDesired.End.North - p.North; + float east_offset = pathDesired.End.East - p.East; + float down_offset = pathDesired.End.Down - p.Down; + pathSummary.brake_distance_offset = sqrtf(north_offset * north_offset + east_offset * east_offset + down_offset * down_offset); + pathSummary.time_remaining = pathDesired.ModeParameters[PATHDESIRED_MODEPARAMETER_BRAKE_TIMEOUT] - pathStatus.path_time; + pathSummary.fractional_progress = pathStatus.fractional_progress; + float cur_velocity = velocityState.North * velocityState.North + velocityState.East * velocityState.East + velocityState.Down * velocityState.Down; + cur_velocity = sqrtf(cur_velocity); + pathSummary.decelrate = (pathDesired.StartingVelocity - cur_velocity) / pathStatus.path_time; + pathSummary.brakeRateActualDesiredRatio = pathSummary.decelrate / vtolPathFollowerSettings.BrakeRate; + pathSummary.velocityIntoHold = cur_velocity; + pathSummary.UID = pathStatus.UID; + PathSummarySet(&pathSummary); + + plan_setup_assistedcontrol(true); // braking timeout true + // having re-entered hold allow reassessment of neutral thrust + neutralThrustEst.have_correction = false; + } + } + switch (returnmode) { case RETURN_RESULT: return result; @@ -607,6 +724,28 @@ static void processPOI() } } +static void updateBrakeVelocity(float startingVelocity, float dT, float brakeRate, float currentVelocity, float *updatedVelocity) +{ + if (startingVelocity >= 0.0f) { + *updatedVelocity = startingVelocity - dT * brakeRate; + if (*updatedVelocity > currentVelocity) { + *updatedVelocity = currentVelocity; + } + if (*updatedVelocity < 0.0f) { + *updatedVelocity = 0.0f; + } + } else { + *updatedVelocity = startingVelocity + dT * brakeRate; + if (*updatedVelocity < currentVelocity) { + *updatedVelocity = currentVelocity; + } + if (*updatedVelocity > 0.0f) { + *updatedVelocity = 0.0f; + } + } +} + + /** * Compute desired velocity from the current position and path */ @@ -617,57 +756,91 @@ static void updatePathVelocity(float kFF, bool limited) PositionStateGet(&positionState); VelocityStateData velocityState; VelocityStateGet(&velocityState); + VelocityDesiredData velocityDesired; const float dT = updatePeriod / 1000.0f; - // look ahead kFF seconds - float cur[3] = { positionState.North + (velocityState.North * kFF), - positionState.East + (velocityState.East * kFF), - positionState.Down + (velocityState.Down * kFF) }; - struct path_status progress; + if (pathDesired.Mode == PATHDESIRED_MODE_BRAKE) { + float brakeRate = vtolPathFollowerSettings.BrakeRate; + if (brakeRate < BRAKE_RATE_MINIMUM) { + brakeRate = BRAKE_RATE_MINIMUM; // set a minimum to avoid a divide by zero potential below + } + updateBrakeVelocity(pathDesired.ModeParameters[PATHDESIRED_MODEPARAMETER_BRAKE_STARTVELOCITYVECTOR_NORTH], pathStatus.path_time, brakeRate, velocityState.North, &velocityDesired.North); + updateBrakeVelocity(pathDesired.ModeParameters[PATHDESIRED_MODEPARAMETER_BRAKE_STARTVELOCITYVECTOR_EAST], pathStatus.path_time, brakeRate, velocityState.East, &velocityDesired.East); + updateBrakeVelocity(pathDesired.ModeParameters[PATHDESIRED_MODEPARAMETER_BRAKE_STARTVELOCITYVECTOR_DOWN], pathStatus.path_time, brakeRate, velocityState.Down, &velocityDesired.Down); - path_progress(&pathDesired, cur, &progress); + float cur_velocity = velocityState.North * velocityState.North + velocityState.East * velocityState.East + velocityState.Down * velocityState.Down; + cur_velocity = sqrtf(cur_velocity); + float desired_velocity = velocityDesired.North * velocityDesired.North + velocityDesired.East * velocityDesired.East + velocityDesired.Down * velocityDesired.Down; + desired_velocity = sqrtf(desired_velocity); - // calculate velocity - can be zero if waypoints are too close - VelocityDesiredData velocityDesired; - velocityDesired.North = progress.path_vector[0]; - velocityDesired.East = progress.path_vector[1]; - velocityDesired.Down = progress.path_vector[2]; + // update pathstatus + pathStatus.error = cur_velocity - desired_velocity; + pathStatus.fractional_progress = 1.0f; + if (pathDesired.StartingVelocity > 0.0f) { + pathStatus.fractional_progress = (pathDesired.StartingVelocity - cur_velocity) / pathDesired.StartingVelocity; - if (limited && - // if a plane is crossing its desired flightpath facing the wrong way (away from flight direction) - // it would turn towards the flightpath to get on its desired course. This however would reverse the correction vector - // once it crosses the flightpath again, which would make it again turn towards the flightpath (but away from its desired heading) - // leading to an S-shape snake course the wrong way - // this only happens especially if HorizontalPosP is too high, as otherwise the angle between velocity desired and path_direction won't - // turn steep unless there is enough space complete the turn before crossing the flightpath - // in this case the plane effectively needs to be turned around - // indicators: - // difference between correction_direction and velocitystate >90 degrees and - // difference between path_direction and velocitystate >90 degrees ( 4th sector, facing away from everything ) - // fix: ignore correction, steer in path direction until the situation has become better (condition doesn't apply anymore) - // calculating angles < 90 degrees through dot products - (vector_lengthf(progress.path_vector, 2) > 1e-6f) && - ((progress.path_vector[0] * velocityState.North + progress.path_vector[1] * velocityState.East) < 0.0f) && - ((progress.correction_vector[0] * velocityState.North + progress.correction_vector[1] * velocityState.East) < 0.0f)) { - ; + // sometimes current velocity can exceed starting velocity due to initial acceleration + if (pathStatus.fractional_progress < 0.0f) { + pathStatus.fractional_progress = 0.0f; + } + } + pathStatus.path_direction_north = velocityDesired.North; + pathStatus.path_direction_east = velocityDesired.East; + pathStatus.path_direction_down = velocityDesired.Down; + + pathStatus.correction_direction_north = velocityDesired.North - velocityState.North; + pathStatus.correction_direction_east = velocityDesired.East - velocityState.East; + pathStatus.correction_direction_down = velocityDesired.Down - velocityState.Down; } else { - // calculate correction - velocityDesired.North += pid_apply(&global.PIDposH[0], progress.correction_vector[0], dT); - velocityDesired.East += pid_apply(&global.PIDposH[1], progress.correction_vector[1], dT); + // look ahead kFF seconds + float cur[3] = { positionState.North + (velocityState.North * kFF), + positionState.East + (velocityState.East * kFF), + positionState.Down + (velocityState.Down * kFF) }; + struct path_status progress; + path_progress(&pathDesired, cur, &progress); + + // calculate velocity - can be zero if waypoints are too close + velocityDesired.North = progress.path_vector[0]; + velocityDesired.East = progress.path_vector[1]; + velocityDesired.Down = progress.path_vector[2]; + + if (limited && + // if a plane is crossing its desired flightpath facing the wrong way (away from flight direction) + // it would turn towards the flightpath to get on its desired course. This however would reverse the correction vector + // once it crosses the flightpath again, which would make it again turn towards the flightpath (but away from its desired heading) + // leading to an S-shape snake course the wrong way + // this only happens especially if HorizontalPosP is too high, as otherwise the angle between velocity desired and path_direction won't + // turn steep unless there is enough space complete the turn before crossing the flightpath + // in this case the plane effectively needs to be turned around + // indicators: + // difference between correction_direction and velocitystate >90 degrees and + // difference between path_direction and velocitystate >90 degrees ( 4th sector, facing away from everything ) + // fix: ignore correction, steer in path direction until the situation has become better (condition doesn't apply anymore) + // calculating angles < 90 degrees through dot products + (vector_lengthf(progress.path_vector, 2) > 1e-6f) && + ((progress.path_vector[0] * velocityState.North + progress.path_vector[1] * velocityState.East) < 0.0f) && + ((progress.correction_vector[0] * velocityState.North + progress.correction_vector[1] * velocityState.East) < 0.0f)) { + ; + } else { + // calculate correction + velocityDesired.North += pid_apply(&global.PIDposH[0], progress.correction_vector[0], dT); + velocityDesired.East += pid_apply(&global.PIDposH[1], progress.correction_vector[1], dT); + } + velocityDesired.Down += pid_apply(&global.PIDposV, progress.correction_vector[2], dT); + + // update pathstatus + pathStatus.error = progress.error; + pathStatus.fractional_progress = progress.fractional_progress; + pathStatus.path_direction_north = progress.path_vector[0]; + pathStatus.path_direction_east = progress.path_vector[1]; + pathStatus.path_direction_down = progress.path_vector[2]; + + pathStatus.correction_direction_north = progress.correction_vector[0]; + pathStatus.correction_direction_east = progress.correction_vector[1]; + pathStatus.correction_direction_down = progress.correction_vector[2]; } - velocityDesired.Down += pid_apply(&global.PIDposV, progress.correction_vector[2], dT); - // update pathstatus - pathStatus.error = progress.error; - pathStatus.fractional_progress = progress.fractional_progress; - pathStatus.path_direction_north = progress.path_vector[0]; - pathStatus.path_direction_east = progress.path_vector[1]; - pathStatus.path_direction_down = progress.path_vector[2]; - - pathStatus.correction_direction_north = progress.correction_vector[0]; - pathStatus.correction_direction_east = progress.correction_vector[1]; - pathStatus.correction_direction_down = progress.correction_vector[2]; VelocityDesiredSet(&velocityDesired); } @@ -1036,8 +1209,9 @@ static bool correctCourse(float *C, float *V, float *F, float s) */ static int8_t updateVtolDesiredAttitude(bool yaw_attitude, float yaw_direction) { - const float dT = updatePeriod / 1000.0f; - uint8_t result = 1; + const float dT = updatePeriod / 1000.0f; + uint8_t result = 1; + bool manual_thrust = false; VelocityDesiredData velocityDesired; VelocityStateData velocityState; @@ -1045,6 +1219,7 @@ static int8_t updateVtolDesiredAttitude(bool yaw_attitude, float yaw_direction) AttitudeStateData attitudeState; StabilizationBankData stabSettings; SystemSettingsData systemSettings; + AdjustmentsData adjustments; float northError; float northCommand; @@ -1062,36 +1237,121 @@ static int8_t updateVtolDesiredAttitude(bool yaw_attitude, float yaw_direction) VelocityDesiredGet(&velocityDesired); AttitudeStateGet(&attitudeState); StabilizationBankGet(&stabSettings); + AdjustmentsGet(&adjustments); - // Testing code - refactor into manual control command - ManualControlCommandData manualControlData; - ManualControlCommandGet(&manualControlData); - // scale velocity if it is above configured maximum - float velH = sqrtf(velocityDesired.North * velocityDesired.North + velocityDesired.East * velocityDesired.East); - if (velH > vtolPathFollowerSettings.HorizontalVelMax) { - velocityDesired.North *= vtolPathFollowerSettings.HorizontalVelMax / velH; - velocityDesired.East *= vtolPathFollowerSettings.HorizontalVelMax / velH; - } - if (fabsf(velocityDesired.Down) > vtolPathFollowerSettings.VerticalVelMax) { - velocityDesired.Down *= vtolPathFollowerSettings.VerticalVelMax / fabsf(velocityDesired.Down); + if (pathDesired.Mode != PATHDESIRED_MODE_BRAKE) { + // scale velocity if it is above configured maximum + // for braking, we can not help it if initial velocity was greater + float velH = sqrtf(velocityDesired.North * velocityDesired.North + velocityDesired.East * velocityDesired.East); + if (velH > vtolPathFollowerSettings.HorizontalVelMax) { + velocityDesired.North *= vtolPathFollowerSettings.HorizontalVelMax / velH; + velocityDesired.East *= vtolPathFollowerSettings.HorizontalVelMax / velH; + } + if (fabsf(velocityDesired.Down) > vtolPathFollowerSettings.VerticalVelMax) { + velocityDesired.Down *= vtolPathFollowerSettings.VerticalVelMax / fabsf(velocityDesired.Down); + } } - // Compute desired north command - northError = velocityDesired.North - velocityState.North; - northCommand = pid_apply(&global.PIDvel[0], northError, dT) + velocityDesired.North * vtolPathFollowerSettings.VelocityFeedforward; + // calculate the velocity errors between desired and actual + northError = velocityDesired.North - velocityState.North; + eastError = velocityDesired.East - velocityState.East; + downError = velocityDesired.Down - velocityState.Down; - // Compute desired east command - eastError = velocityDesired.East - velocityState.East; - eastCommand = pid_apply(&global.PIDvel[1], eastError, dT) + velocityDesired.East * vtolPathFollowerSettings.VelocityFeedforward; - - // Compute desired down command - downError = velocityDesired.Down - velocityState.Down; // Must flip this sign - downError = -downError; - downCommand = pid_apply(&global.PIDvel[2], downError, dT); + downError = -downError; - stabDesired.Thrust = boundf(downCommand + vtolPathFollowerSettings.ThrustLimits.Neutral, vtolPathFollowerSettings.ThrustLimits.Min, vtolPathFollowerSettings.ThrustLimits.Max); + // Compute desired commands + if (pathDesired.Mode != PATHDESIRED_MODE_BRAKE) { + northCommand = pid_apply(&global.PIDvel[0], northError, dT) + velocityDesired.North * vtolPathFollowerSettings.VelocityFeedforward; + eastCommand = pid_apply(&global.PIDvel[1], eastError, dT) + velocityDesired.East * vtolPathFollowerSettings.VelocityFeedforward; + } else { + northCommand = pid_apply(&global.BrakePIDvel[0], northError, dT) + velocityDesired.North * vtolPathFollowerSettings.BrakeVelocityFeedforward; + eastCommand = pid_apply(&global.BrakePIDvel[1], eastError, dT) + velocityDesired.East * vtolPathFollowerSettings.BrakeVelocityFeedforward; + } + downCommand = pid_apply(&global.PIDvel[2], downError, dT); + + + if ((vtolPathFollowerSettings.ThrustControl == VTOLPATHFOLLOWERSETTINGS_THRUSTCONTROL_MANUAL && + flightStatus.FlightModeAssist == FLIGHTSTATUS_FLIGHTMODEASSIST_NONE) || + (flightStatus.FlightModeAssist && flightStatus.AssistedThrottleState == FLIGHTSTATUS_ASSISTEDTHROTTLESTATE_MANUAL)) { + manual_thrust = true; + } + + // if auto thrust and we have not run a correction calc check for PH and stability to then run an assessment + // note that arming into this flight mode is not allowed, so assumption here is that + // we have not landed. If GPSAssist+Manual/Cruise control thrust mode is used, a user overriding the + // altitude maintaining PID will have moved the throttle state to FLIGHTSTATUS_ASSISTEDTHROTTLESTATE_MANUAL. + // In manualcontrol.c the state will stay in manual throttle until the throttle command exceeds the vtol thrust min, + // avoiding auto-takeoffs. Therefore no need to check that here. + if (!manual_thrust && neutralThrustEst.have_correction != true) { + // Assess if position hold state running. This can be normal position hold or + // another mode with assist-hold active. + bool ph_active = + ((flightStatus.FlightMode == FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD && + flightStatus.FlightModeAssist == FLIGHTSTATUS_FLIGHTMODEASSIST_NONE) || + (flightStatus.FlightModeAssist != FLIGHTSTATUS_FLIGHTMODEASSIST_NONE && + flightStatus.AssistedControlState == FLIGHTSTATUS_ASSISTEDCONTROLSTATE_HOLD)); + + + bool stable = (fabsf(pathStatus.correction_direction_down) < NEUTRALTHRUST_PH_POSITIONAL_ERROR_LIMIT && + fabsf(velocityDesired.Down) < NEUTRALTHRUST_PH_VEL_DESIRED_LIMIT && + fabsf(velocityState.Down) < NEUTRALTHRUST_PH_VEL_STATE_LIMIT && + fabsf(downError) < NEUTRALTHRUST_PH_VEL_ERROR_LIMIT); + + if (ph_active && stable) { + if (neutralThrustEst.start_sampling) { + neutralThrustEst.count++; + + + // delay count for 2 seconds into hold allowing for stablisation + if (neutralThrustEst.count > NEUTRALTHRUST_START_DELAY) { + neutralThrustEst.sum += global.PIDvel[2].iAccumulator; + if (global.PIDvel[2].iAccumulator < neutralThrustEst.min) { + neutralThrustEst.min = global.PIDvel[2].iAccumulator; + } + if (global.PIDvel[2].iAccumulator > neutralThrustEst.max) { + neutralThrustEst.max = global.PIDvel[2].iAccumulator; + } + } + + if (neutralThrustEst.count >= NEUTRALTHRUST_END_COUNT) { + // 6 seconds have past + // lets take an average + neutralThrustEst.average = neutralThrustEst.sum / (float)(NEUTRALTHRUST_END_COUNT - NEUTRALTHRUST_START_DELAY); + neutralThrustEst.correction = neutralThrustEst.average / 1000.0f; + + global.PIDvel[2].iAccumulator -= neutralThrustEst.average; + + neutralThrustEst.start_sampling = false; + neutralThrustEst.have_correction = true; + + // Write a new adjustment value + // adjustments.NeutralThrustOffset was incremental adjusted above + AdjustmentsData new_adjustments; + // add the average remaining i value to the + new_adjustments.NeutralThrustOffset = adjustments.NeutralThrustOffset + neutralThrustEst.correction; + new_adjustments.NeutralThrustCorrection = neutralThrustEst.correction; // the i term thrust correction value applied + new_adjustments.NeutralThrustAccumulator = global.PIDvel[2].iAccumulator; // the actual iaccumulator value after correction + new_adjustments.NeutralThrustRange = neutralThrustEst.max - neutralThrustEst.min; + AdjustmentsSet(&new_adjustments); + } + } else { + // start a tick count + neutralThrustEst.start_sampling = true; + neutralThrustEst.count = 0; + neutralThrustEst.sum = 0.0f; + neutralThrustEst.max = 0.0f; + neutralThrustEst.min = 0.0f; + } + } else { + // reset sampling as we did't get 6 continuous seconds + neutralThrustEst.start_sampling = false; + } + } // else we already have a correction for this PH run + + // Generally in braking the downError will be an increased altitude. We really will rely on cruisecontrol to backoff. + stabDesired.Thrust = boundf(adjustments.NeutralThrustOffset + downCommand + vtolPathFollowerSettings.ThrustLimits.Neutral, vtolPathFollowerSettings.ThrustLimits.Min, vtolPathFollowerSettings.ThrustLimits.Max); // DEBUG HACK: allow user to skew compass on purpose to see if emergency failsafe kicks in @@ -1102,6 +1362,7 @@ static int8_t updateVtolDesiredAttitude(bool yaw_attitude, float yaw_direction) } } + if ( // emergency flyaway detection ( // integral already at its limit vtolPathFollowerSettings.HorizontalVelPID.ILimit - fabsf(global.PIDvel[0].iAccumulator) < 1e-6f || @@ -1124,19 +1385,22 @@ static int8_t updateVtolDesiredAttitude(bool yaw_attitude, float yaw_direction) // Project the north and east command signals into the pitch and roll based on yaw. For this to behave well the // craft should move similarly for 5 deg roll versus 5 deg pitch + + float maxPitch = vtolPathFollowerSettings.MaxRollPitch; + if (pathDesired.Mode == PATHDESIRED_MODE_BRAKE) { + maxPitch = vtolPathFollowerSettings.BrakeMaxPitch; + } + stabDesired.Pitch = boundf(-northCommand * cosf(DEG2RAD(attitudeState.Yaw)) + -eastCommand * sinf(DEG2RAD(attitudeState.Yaw)), - -vtolPathFollowerSettings.MaxRollPitch, vtolPathFollowerSettings.MaxRollPitch); + -maxPitch, maxPitch); stabDesired.Roll = boundf(-northCommand * sinf(DEG2RAD(attitudeState.Yaw)) + eastCommand * cosf(DEG2RAD(attitudeState.Yaw)), - -vtolPathFollowerSettings.MaxRollPitch, vtolPathFollowerSettings.MaxRollPitch); + -maxPitch, maxPitch); + + ManualControlCommandData manualControl; + ManualControlCommandGet(&manualControl); - if (vtolPathFollowerSettings.ThrustControl == VTOLPATHFOLLOWERSETTINGS_THRUSTCONTROL_MANUAL) { - // For now override thrust with manual control. Disable at your risk, quad goes to China. - ManualControlCommandData manualControl; - ManualControlCommandGet(&manualControl); - stabDesired.Thrust = manualControl.Thrust; - } stabDesired.StabilizationMode.Roll = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; stabDesired.StabilizationMode.Pitch = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; @@ -1145,9 +1409,51 @@ static int8_t updateVtolDesiredAttitude(bool yaw_attitude, float yaw_direction) stabDesired.Yaw = yaw_direction; } else { stabDesired.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK; - stabDesired.Yaw = stabSettings.MaximumRate.Yaw * manualControlData.Yaw; + stabDesired.Yaw = stabSettings.MaximumRate.Yaw * manualControl.Yaw; } + + // default thrust mode to cruise control stabDesired.StabilizationMode.Thrust = STABILIZATIONDESIRED_STABILIZATIONMODE_CRUISECONTROL; + + // when flight mode assist is active but in primary-thrust mode, the thrust mode must be set to the same as per the primary mode. + if (flightStatus.FlightModeAssist == FLIGHTSTATUS_FLIGHTMODEASSIST_GPSASSIST_PRIMARYTHRUST) { + FlightModeSettingsData settings; + FlightModeSettingsGet(&settings); + FlightModeSettingsStabilization1SettingsOptions thrustMode = FLIGHTMODESETTINGS_STABILIZATION1SETTINGS_CRUISECONTROL; + + switch (flightStatus.FlightMode) { + case FLIGHTSTATUS_FLIGHTMODE_STABILIZED1: + thrustMode = settings.Stabilization1Settings.Thrust; + break; + case FLIGHTSTATUS_FLIGHTMODE_STABILIZED2: + thrustMode = settings.Stabilization2Settings.Thrust; + break; + case FLIGHTSTATUS_FLIGHTMODE_STABILIZED3: + thrustMode = settings.Stabilization3Settings.Thrust; + break; + case FLIGHTSTATUS_FLIGHTMODE_STABILIZED4: + thrustMode = settings.Stabilization4Settings.Thrust; + break; + case FLIGHTSTATUS_FLIGHTMODE_STABILIZED5: + thrustMode = settings.Stabilization5Settings.Thrust; + break; + case FLIGHTSTATUS_FLIGHTMODE_STABILIZED6: + thrustMode = settings.Stabilization6Settings.Thrust; + break; + case FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD: + // we hard code the "GPS Assisted" PostionHold 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; + } + stabDesired.StabilizationMode.Thrust = thrustMode; + stabDesired.Thrust = manualControl.Thrust; + } else if (manual_thrust) { + stabDesired.Thrust = manualControl.Thrust; + } + // else thrust is set by the PID controller + StabilizationDesiredSet(&stabDesired); return result; diff --git a/flight/modules/Receiver/receiver.c b/flight/modules/Receiver/receiver.c index cf3230751..bf68b73e0 100644 --- a/flight/modules/Receiver/receiver.c +++ b/flight/modules/Receiver/receiver.c @@ -39,27 +39,33 @@ #include #include #include +#ifndef PIOS_EXCLUDE_ADVANCED_FEATURES +#include +#endif #include #include #include + #if defined(PIOS_INCLUDE_USB_RCTX) #include "pios_usb_rctx.h" #endif /* PIOS_INCLUDE_USB_RCTX */ // Private constants #if defined(PIOS_RECEIVER_STACK_SIZE) -#define STACK_SIZE_BYTES PIOS_RECEIVER_STACK_SIZE +#define STACK_SIZE_BYTES PIOS_RECEIVER_STACK_SIZE #else -#define STACK_SIZE_BYTES 1152 +#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 +#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 +#define CONNECTION_OFFSET 250 + +#define ASSISTEDCONTROL_DEADBAND_MINIMUM 0.02f // minimum value for a well bahaved Tx. // Private types @@ -79,8 +85,12 @@ static uint32_t timeDifferenceMs(portTickType start_time, portTickType end_time) static bool validInputRange(int16_t min, int16_t max, uint16_t value); static void applyDeadband(float *value, float deadband); +#ifndef PIOS_EXCLUDE_ADVANCED_FEATURES +static uint8_t isAssistedFlightMode(uint8_t position); +#endif + #ifdef USE_INPUT_LPF -static void applyLPF(float *value, ManualControlSettingsResponseTimeElem channel, ManualControlSettingsData *settings, float dT); +static void applyLPF(float *value, ManualControlSettingsResponseTimeElem channel, ManualControlSettingsResponseTimeData *responseTime, float deadband, float dT); #endif #define RCVR_ACTIVITY_MONITOR_CHANNELS_PER_GROUP 12 @@ -129,6 +139,10 @@ int32_t ReceiverInitialize() ManualControlCommandInitialize(); ReceiverActivityInitialize(); ManualControlSettingsInitialize(); +#ifndef PIOS_EXCLUDE_ADVANCED_FEATURES + StabilizationSettingsInitialize(); +#endif + return 0; } @@ -363,11 +377,30 @@ static void receiverTask(__attribute__((unused)) void *parameters) cmd.FlightModeSwitchPosition = settings.FlightModeNumber - 1; } + float deadband_checked = settings.Deadband; +#ifndef PIOS_EXCLUDE_ADVANCED_FEATURES + // AssistedControl must have deadband set for pitch/roll hence + // we default to a higher value for badly behaved TXs and also enforce a minimum value + // for well behaved TXs + uint8_t assistedMode = isAssistedFlightMode(cmd.FlightModeSwitchPosition); + if (assistedMode) { + deadband_checked = settings.DeadbandAssistedControl; + if (deadband_checked < ASSISTEDCONTROL_DEADBAND_MINIMUM) { + deadband_checked = ASSISTEDCONTROL_DEADBAND_MINIMUM; + } + + // If user has set settings.Deadband to a higher value...we use that. + if (deadband_checked < settings.Deadband) { + deadband_checked = settings.Deadband; + } + } +#endif // PIOS_EXCLUDE_ADVANCED_FEATURES + // 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); + if (deadband_checked > 0.0f) { + applyDeadband(&cmd.Roll, deadband_checked); + applyDeadband(&cmd.Pitch, deadband_checked); + applyDeadband(&cmd.Yaw, deadband_checked); } #ifdef USE_INPUT_LPF // Apply Low Pass Filter to input channels, time delta between calls in ms @@ -377,9 +410,9 @@ static void receiverTask(__attribute__((unused)) void *parameters) (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); + applyLPF(&cmd.Roll, MANUALCONTROLSETTINGS_RESPONSETIME_ROLL, &settings.ResponseTime, deadband_checked, dT); + applyLPF(&cmd.Pitch, MANUALCONTROLSETTINGS_RESPONSETIME_PITCH, &settings.ResponseTime, deadband_checked, dT); + applyLPF(&cmd.Yaw, MANUALCONTROLSETTINGS_RESPONSETIME_YAW, &settings.ResponseTime, deadband_checked, 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 @@ -389,7 +422,7 @@ static void receiverTask(__attribute__((unused)) void *parameters) applyDeadband(&cmd.Collective, settings.Deadband); } #ifdef USE_INPUT_LPF - applyLPF(&cmd.Collective, MANUALCONTROLSETTINGS_RESPONSETIME_COLLECTIVE, &settings, dT); + applyLPF(&cmd.Collective, MANUALCONTROLSETTINGS_RESPONSETIME_COLLECTIVE, &settings.ResponseTime, settings.Deadband, dT); #endif // USE_INPUT_LPF } @@ -409,7 +442,7 @@ static void receiverTask(__attribute__((unused)) void *parameters) 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); + applyLPF(&accessory.AccessoryVal, MANUALCONTROLSETTINGS_RESPONSETIME_ACCESSORY0, &settings.ResponseTime, settings.Deadband, dT); #endif if (AccessoryDesiredInstSet(0, &accessory) != 0) { AlarmsSet(SYSTEMALARMS_ALARM_RECEIVER, SYSTEMALARMS_ALARM_WARNING); @@ -419,7 +452,7 @@ static void receiverTask(__attribute__((unused)) void *parameters) 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); + applyLPF(&accessory.AccessoryVal, MANUALCONTROLSETTINGS_RESPONSETIME_ACCESSORY1, &settings.ResponseTime, settings.Deadband, dT); #endif if (AccessoryDesiredInstSet(1, &accessory) != 0) { AlarmsSet(SYSTEMALARMS_ALARM_RECEIVER, SYSTEMALARMS_ALARM_WARNING); @@ -429,7 +462,7 @@ static void receiverTask(__attribute__((unused)) void *parameters) 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); + applyLPF(&accessory.AccessoryVal, MANUALCONTROLSETTINGS_RESPONSETIME_ACCESSORY2, &settings.ResponseTime, settings.Deadband, dT); #endif if (AccessoryDesiredInstSet(2, &accessory) != 0) { @@ -440,6 +473,8 @@ static void receiverTask(__attribute__((unused)) void *parameters) // Update cmd object ManualControlCommandSet(&cmd); + + #if defined(PIOS_INCLUDE_USB_RCTX) if (pios_usb_rctx_id) { PIOS_USB_RCTX_Update(pios_usb_rctx_id, @@ -658,16 +693,42 @@ static void applyDeadband(float *value, float deadband) /** * @brief Apply Low Pass Filter to Throttle/Roll/Pitch/Yaw or Accessory channel */ -static void applyLPF(float *value, ManualControlSettingsResponseTimeElem channel, ManualControlSettingsData *settings, float dT) +static void applyLPF(float *value, ManualControlSettingsResponseTimeElem channel, ManualControlSettingsResponseTimeData *responseTime, float deadband, float dT) { - if (ManualControlSettingsResponseTimeToArray(settings->ResponseTime)[channel]) { - float rt = (float)ManualControlSettingsResponseTimeToArray(settings->ResponseTime)[channel]; + float rt = (float)ManualControlSettingsResponseTimeToArray((*responseTime))[channel]; + + if (rt > 0.0f) { inputFiltered[channel] = ((rt * inputFiltered[channel]) + (dT * (*value))) / (rt + dT); + + // avoid a long tail of non-zero data. if we have deadband, once the filtered result reduces to 1/10th + // of deadband revert to 0. We downstream rely on this to know when sticks are centered. + if (deadband > 0.0f && fabsf(inputFiltered[channel]) < deadband / 10.0f) { + inputFiltered[channel] = 0.0f; + } *value = inputFiltered[channel]; } } #endif // USE_INPUT_LPF +/** + * Check and set modes for gps assisted stablised flight modes + */ +#ifndef PIOS_EXCLUDE_ADVANCED_FEATURES +static uint8_t isAssistedFlightMode(uint8_t position) +{ + uint8_t isAssistedFlag = STABILIZATIONSETTINGS_FLIGHTMODEASSISTMAP_NONE; + uint8_t FlightModeAssistMap[STABILIZATIONSETTINGS_FLIGHTMODEASSISTMAP_NUMELEM]; + + StabilizationSettingsFlightModeAssistMapGet(FlightModeAssistMap); + + if (position < STABILIZATIONSETTINGS_FLIGHTMODEASSISTMAP_NUMELEM) { + isAssistedFlag = FlightModeAssistMap[position]; + } + + return isAssistedFlag; +} +#endif + /** * @} * @} diff --git a/flight/targets/boards/discoveryf4bare/firmware/UAVObjects.inc b/flight/targets/boards/discoveryf4bare/firmware/UAVObjects.inc index 9f2d3c902..8aeadca04 100644 --- a/flight/targets/boards/discoveryf4bare/firmware/UAVObjects.inc +++ b/flight/targets/boards/discoveryf4bare/firmware/UAVObjects.inc @@ -19,6 +19,7 @@ # These are the UAVObjects supposed to be build as part of the OpenPilot target # (all architectures) UAVOBJSRCFILENAMES = +UAVOBJSRCFILENAMES += adjustments UAVOBJSRCFILENAMES += accelgyrosettings UAVOBJSRCFILENAMES += accessorydesired UAVOBJSRCFILENAMES += actuatorcommand @@ -76,6 +77,7 @@ UAVOBJSRCFILENAMES += pathaction UAVOBJSRCFILENAMES += pathdesired UAVOBJSRCFILENAMES += pathplan UAVOBJSRCFILENAMES += pathstatus +UAVOBJSRCFILENAMES += pathsummary UAVOBJSRCFILENAMES += positionstate UAVOBJSRCFILENAMES += ratedesired UAVOBJSRCFILENAMES += ekfconfiguration diff --git a/flight/targets/boards/revolution/firmware/UAVObjects.inc b/flight/targets/boards/revolution/firmware/UAVObjects.inc index 9f2d3c902..8aeadca04 100644 --- a/flight/targets/boards/revolution/firmware/UAVObjects.inc +++ b/flight/targets/boards/revolution/firmware/UAVObjects.inc @@ -19,6 +19,7 @@ # These are the UAVObjects supposed to be build as part of the OpenPilot target # (all architectures) UAVOBJSRCFILENAMES = +UAVOBJSRCFILENAMES += adjustments UAVOBJSRCFILENAMES += accelgyrosettings UAVOBJSRCFILENAMES += accessorydesired UAVOBJSRCFILENAMES += actuatorcommand @@ -76,6 +77,7 @@ UAVOBJSRCFILENAMES += pathaction UAVOBJSRCFILENAMES += pathdesired UAVOBJSRCFILENAMES += pathplan UAVOBJSRCFILENAMES += pathstatus +UAVOBJSRCFILENAMES += pathsummary UAVOBJSRCFILENAMES += positionstate UAVOBJSRCFILENAMES += ratedesired UAVOBJSRCFILENAMES += ekfconfiguration diff --git a/flight/targets/boards/revoproto/firmware/UAVObjects.inc b/flight/targets/boards/revoproto/firmware/UAVObjects.inc index 763b6d940..91d1ba64d 100644 --- a/flight/targets/boards/revoproto/firmware/UAVObjects.inc +++ b/flight/targets/boards/revoproto/firmware/UAVObjects.inc @@ -19,6 +19,7 @@ # These are the UAVObjects supposed to be build as part of the OpenPilot target # (all architectures) UAVOBJSRCFILENAMES = +UAVOBJSRCFILENAMES += adjustments UAVOBJSRCFILENAMES += accelgyrosettings UAVOBJSRCFILENAMES += accessorydesired UAVOBJSRCFILENAMES += actuatorcommand @@ -76,6 +77,7 @@ UAVOBJSRCFILENAMES += pathaction UAVOBJSRCFILENAMES += pathdesired UAVOBJSRCFILENAMES += pathplan UAVOBJSRCFILENAMES += pathstatus +UAVOBJSRCFILENAMES += pathsummary UAVOBJSRCFILENAMES += positionstate UAVOBJSRCFILENAMES += ratedesired UAVOBJSRCFILENAMES += ekfconfiguration diff --git a/flight/targets/boards/simposix/firmware/UAVObjects.inc b/flight/targets/boards/simposix/firmware/UAVObjects.inc index 0db26345a..7e898a83a 100644 --- a/flight/targets/boards/simposix/firmware/UAVObjects.inc +++ b/flight/targets/boards/simposix/firmware/UAVObjects.inc @@ -24,6 +24,7 @@ # (all architectures) UAVOBJSRCFILENAMES = +UAVOBJSRCFILENAMES += adjustments UAVOBJSRCFILENAMES += accessorydesired UAVOBJSRCFILENAMES += actuatorcommand UAVOBJSRCFILENAMES += actuatordesired @@ -77,6 +78,7 @@ UAVOBJSRCFILENAMES += pathaction UAVOBJSRCFILENAMES += pathdesired UAVOBJSRCFILENAMES += pathplan UAVOBJSRCFILENAMES += pathstatus +UAVOBJSRCFILENAMES += pathsummary UAVOBJSRCFILENAMES += positionstate UAVOBJSRCFILENAMES += ratedesired UAVOBJSRCFILENAMES += revocalibration diff --git a/ground/openpilotgcs/src/plugins/config/configinputwidget.cpp b/ground/openpilotgcs/src/plugins/config/configinputwidget.cpp index d7b9fd7d0..6014ad6f0 100644 --- a/ground/openpilotgcs/src/plugins/config/configinputwidget.cpp +++ b/ground/openpilotgcs/src/plugins/config/configinputwidget.cpp @@ -148,6 +148,7 @@ ConfigInputWidget::ConfigInputWidget(QWidget *parent) : } addWidgetBinding("ManualControlSettings", "Deadband", ui->deadband, 0, 0.01f); + addWidgetBinding("ManualControlSettings", "DeadbandAssistedControl", ui->assistedControlDeadband, 0, 0.01f); connect(ui->configurationWizard, SIGNAL(clicked()), this, SLOT(goToWizard())); connect(ui->stackedWidget, SIGNAL(currentChanged(int)), this, SLOT(disableWizardButton(int))); @@ -1494,26 +1495,32 @@ void ConfigInputWidget::updatePositionSlider() case 6: ui->fmsModePos6->setEnabled(true); ui->pidBankSs1_5->setEnabled(true); + ui->assistControlPos6->setEnabled(true); // pass through case 5: ui->fmsModePos5->setEnabled(true); ui->pidBankSs1_4->setEnabled(true); + ui->assistControlPos5->setEnabled(true); // pass through case 4: ui->fmsModePos4->setEnabled(true); ui->pidBankSs1_3->setEnabled(true); + ui->assistControlPos4->setEnabled(true); // pass through case 3: ui->fmsModePos3->setEnabled(true); ui->pidBankSs1_2->setEnabled(true); + ui->assistControlPos3->setEnabled(true); // pass through case 2: ui->fmsModePos2->setEnabled(true); ui->pidBankSs1_1->setEnabled(true); + ui->assistControlPos2->setEnabled(true); // pass through case 1: ui->fmsModePos1->setEnabled(true); ui->pidBankSs1_0->setEnabled(true); + ui->assistControlPos1->setEnabled(true); // pass through case 0: break; @@ -1523,26 +1530,32 @@ void ConfigInputWidget::updatePositionSlider() case 0: ui->fmsModePos1->setEnabled(false); ui->pidBankSs1_0->setEnabled(false); + ui->assistControlPos1->setEnabled(false); // pass through case 1: ui->fmsModePos2->setEnabled(false); ui->pidBankSs1_1->setEnabled(false); + ui->assistControlPos2->setEnabled(false); // pass through case 2: ui->fmsModePos3->setEnabled(false); ui->pidBankSs1_2->setEnabled(false); + ui->assistControlPos3->setEnabled(false); // pass through case 3: ui->fmsModePos4->setEnabled(false); ui->pidBankSs1_3->setEnabled(false); + ui->assistControlPos4->setEnabled(false); // pass through case 4: ui->fmsModePos5->setEnabled(false); ui->pidBankSs1_4->setEnabled(false); + ui->assistControlPos5->setEnabled(false); // pass through case 5: ui->fmsModePos6->setEnabled(false); ui->pidBankSs1_5->setEnabled(false); + ui->assistControlPos6->setEnabled(false); // pass through case 6: default: diff --git a/ground/openpilotgcs/src/plugins/config/input.ui b/ground/openpilotgcs/src/plugins/config/input.ui index dbf637b81..1cc129bde 100644 --- a/ground/openpilotgcs/src/plugins/config/input.ui +++ b/ground/openpilotgcs/src/plugins/config/input.ui @@ -6,8 +6,8 @@ 0 0 - 796 - 591 + 1250 + 755 @@ -234,6 +234,29 @@ + + + + Assisted Control stick deadband + + + + + + + Assisted Control stick deadband in percents of full range (2-12) for use with GPSAssist. This can not be disabled. + + + 1 + + + 2.000000000000000 + + + 12.000000000000000 + + + @@ -1206,197 +1229,6 @@ font:bold; 6 - - - - - 80 - 20 - - - - - 16777215 - 20 - - - - background-color: qlineargradient(spread:reflect, x1:0.507, y1:0, x2:0.507, y2:0.772, stop:0.208955 rgba(74, 74, 74, 255), stop:0.78607 rgba(36, 36, 36, 255)); -color: rgb(255, 255, 255); -border-radius: 5; -margin:1px; -font:bold; - - - Settings Bank - - - Qt::AlignCenter - - - - - - - - 138 - 20 - - - - - 16777215 - 20 - - - - background-color: qlineargradient(spread:reflect, x1:0.507, y1:0, x2:0.507, y2:0.772, stop:0.208955 rgba(74, 74, 74, 255), stop:0.78607 rgba(36, 36, 36, 255)); -color: rgb(255, 255, 255); -border-radius: 5; -margin:1px; -font:bold; - - - Flight Mode Count - - - Qt::AlignCenter - - - - - - - Number of positions your FlightMode switch has. - -Default is 3. - -It will be 2 or 3 for most of setups, but it also can be up to 6. -In that case you have to configure your radio mixers so the whole range -from min to max is split into N equal intervals, and you may set arbitrary -channel value for each flight mode. - - - 1 - - - 6 - - - 3 - - - - - - - - 0 - 0 - - - - - 10 - 75 - true - - - - <html><head/><body><p>Avoid &quot;Manual&quot; for multirotors! Never select &quot;Altitude&quot;, &quot;VelocityControl&quot; or &quot;CruiseControl&quot; on a fixed wing!</p></body></html> - - - Qt::AlignCenter - - - true - - - - - - - Qt::Horizontal - - - QSizePolicy::Fixed - - - - 10 - 20 - - - - - - - - Qt::Horizontal - - - QSizePolicy::Fixed - - - - 10 - 20 - - - - - - - - Qt::Horizontal - - - QSizePolicy::Fixed - - - - 10 - 20 - - - - - - - - Qt::Vertical - - - - - - - - 138 - 20 - - - - - 16777215 - 20 - - - - background-color: qlineargradient(spread:reflect, x1:0.507, y1:0, x2:0.507, y2:0.772, stop:0.208955 rgba(74, 74, 74, 255), stop:0.78607 rgba(36, 36, 36, 255)); -color: rgb(255, 255, 255); -border-radius: 5; -margin:1px; -font:bold; - - - Flight Mode - - - Qt::AlignCenter - - - @@ -1941,6 +1773,418 @@ Setup the flight mode channel on the RC Input tab if you have not done so alread + + + + + 138 + 20 + + + + + 16777215 + 20 + + + + background-color: qlineargradient(spread:reflect, x1:0.507, y1:0, x2:0.507, y2:0.772, stop:0.208955 rgba(74, 74, 74, 255), stop:0.78607 rgba(36, 36, 36, 255)); +color: rgb(255, 255, 255); +border-radius: 5; +margin:1px; +font:bold; + + + Settings Bank + + + Qt::AlignCenter + + + + + + + + 138 + 20 + + + + + 16777215 + 20 + + + + background-color: qlineargradient(spread:reflect, x1:0.507, y1:0, x2:0.507, y2:0.772, stop:0.208955 rgba(74, 74, 74, 255), stop:0.78607 rgba(36, 36, 36, 255)); +color: rgb(255, 255, 255); +border-radius: 5; +margin:1px; +font:bold; + + + Flight Mode Count + + + Qt::AlignCenter + + + + + + + Number of positions your FlightMode switch has. + +Default is 3. + +It will be 2 or 3 for most of setups, but it also can be up to 6. +In that case you have to configure your radio mixers so the whole range +from min to max is split into N equal intervals, and you may set arbitrary +channel value for each flight mode. + + + 1 + + + 6 + + + 3 + + + + + + + + 0 + 0 + + + + + 10 + 75 + true + + + + <html><head/><body><p>Avoid &quot;Manual&quot; for multirotors! Never select &quot;Altitude&quot;, &quot;VelocityControl&quot; or &quot;CruiseControl&quot; on a fixed wing!</p></body></html> + + + Qt::AlignCenter + + + true + + + + + + + Qt::Horizontal + + + QSizePolicy::Fixed + + + + 10 + 20 + + + + + + + + Qt::Horizontal + + + QSizePolicy::Fixed + + + + 10 + 20 + + + + + + + + Qt::Horizontal + + + QSizePolicy::Fixed + + + + 10 + 20 + + + + + + + + Qt::Vertical + + + + + + + + 138 + 20 + + + + + 16777215 + 20 + + + + background-color: qlineargradient(spread:reflect, x1:0.507, y1:0, x2:0.507, y2:0.772, stop:0.208955 rgba(74, 74, 74, 255), stop:0.78607 rgba(36, 36, 36, 255)); +color: rgb(255, 255, 255); +border-radius: 5; +margin:1px; +font:bold; + + + Flight Mode + + + Qt::AlignCenter + + + + + + + + 138 + 0 + + + + Qt::LeftToRight + + + background-color: qlineargradient(spread:reflect, x1:0.507, y1:0, x2:0.507, y2:0.772, stop:0.208955 rgba(74, 74, 74, 255), stop:0.78607 rgba(36, 36, 36, 255)); +color: rgb(255, 255, 255); +border-radius: 5; +margin:1px; +font:bold; + + + Assisted Control + + + Qt::AlignCenter + + + + + + + + 75 + 0 + + + + QFrame::NoFrame + + + QFrame::Plain + + + + 1 + + + 1 + + + 1 + + + 1 + + + + + + 75 + 0 + + + + Qt::StrongFocus + + + <html><head/><body><p>Assisted Control options augment the primary flight mode. GPSAssist adds brake/hold to Stabilization and PositionHold.</p></body></html> + + + + objname:StabilizationSettings + fieldname:FlightModeAssistMap + index:0 + haslimits:no + scale:1 + buttongroup:16 + + + + + + + + + 75 + 0 + + + + Qt::StrongFocus + + + <html><head/><body><p>Assisted Control options augment the primary flight mode. GPSAssist adds brake/hold to Stabilization and PositionHold.</p></body></html> + + + + objname:StabilizationSettings + fieldname:FlightModeAssistMap + index:1 + haslimits:no + scale:1 + buttongroup:16 + + + + + + + + + 75 + 0 + + + + Qt::StrongFocus + + + <html><head/><body><p>Assisted Control options augment the primary flight mode. GPSAssist adds brake/hold to Stabilization and PositionHold.</p></body></html> + + + + objname:StabilizationSettings + fieldname:FlightModeAssistMap + index:2 + haslimits:no + scale:1 + buttongroup:16 + + + + + + + + false + + + + 75 + 0 + + + + Qt::StrongFocus + + + <html><head/><body><p>Assisted Control options augment the primary flight mode. GPSAssist adds brake/hold to Stabilization and PositionHold.</p></body></html> + + + + objname:StabilizationSettings + fieldname:FlightModeAssistMap + index:3 + haslimits:no + scale:1 + buttongroup:16 + + + + + + + + false + + + + 75 + 0 + + + + Qt::StrongFocus + + + <html><head/><body><p>Assisted Control options augment the primary flight mode. GPSAssist adds brake/hold to Stabilization and PositionHold.</p></body></html> + + + + objname:StabilizationSettings + fieldname:FlightModeAssistMap + index:4 + haslimits:no + scale:1 + buttongroup:16 + + + + + + + + false + + + + 75 + 0 + + + + Qt::StrongFocus + + + <html><head/><body><p>Assisted Control options augment the primary flight mode. GPSAssist adds brake/hold to Stabilization and PositionHold.</p></body></html> + + + + objname:StabilizationSettings + fieldname:FlightModeAssistMap + index:5 + haslimits:no + scale:1 + buttongroup:16 + + + + + + + diff --git a/ground/openpilotgcs/src/plugins/uavobjects/uavobject.m.template b/ground/openpilotgcs/src/plugins/uavobjects/uavobject.m.template index f7e683b0d..b12bea0a7 100644 --- a/ground/openpilotgcs/src/plugins/uavobjects/uavobject.m.template +++ b/ground/openpilotgcs/src/plugins/uavobjects/uavobject.m.template @@ -86,7 +86,7 @@ while (1) if (bufferlen < headerIdx + 12); break; end sync = buffer(headerIdx); % printf('%x ', sync); - headerIdx += 1; + headerIdx = headerIdx + 1; if sync ~= correctSyncByte wrongSyncByte = wrongSyncByte + 1; continue @@ -99,7 +99,7 @@ while (1) % get msg type (quint8 1 byte ) should be 0x20/0xA0, ignore the rest msgType = buffer(headerIdx); - headerIdx += 1; + headerIdx = headerIdx + 1; if msgType ~= correctMsgByte && msgType ~= correctTimestampedByte % fixme: it should read the whole message payload instead of skipping and blindly searching for next sync byte. fprintf('\nSkipping message type: %x \n', msgType); @@ -108,16 +108,16 @@ while (1) % get msg size (quint16 2 bytes) excludes crc, include msg header and data payload msgSize = uint32(typecast(buffer(headerIdx:headerIdx + 1), 'uint16')); - headerIdx += 2; + headerIdx = headerIdx + 2; % get obj id (quint32 4 bytes) objID = typecast(uint8(buffer(headerIdx:headerIdx + 3)), 'uint32'); - headerIdx += 4; + headerIdx = headerIdx + 4; % get instance id (quint16 2 bytes) instID = typecast(uint8(buffer(headerIdx:headerIdx + 1)), 'uint16'); % printf('Id %x type %x size %u Inst %x ', objID, msgType, msgSize, instID); - headerIdx += 2; + headerIdx = headerIdx + 2; % get timestamp if needed (quint32 4 bytes) if msgType == correctMsgByte @@ -125,12 +125,12 @@ while (1) elseif msgType == correctTimestampedByte timestamp = typecast(uint8(buffer(headerIdx:headerIdx + 3)), 'uint32'); % printf('ts %u'); - headerIdx += 4; + headerIdx = headerIdx + 4; datasize = msgSize - headerLen - timestampLen; end % printf('\n'); bufferIdx = headerIdx; - headerIdx += datasize + crcLen + oplHeaderLen; + headerIdx = headerIdx + datasize + crcLen + oplHeaderLen; %% Read object diff --git a/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro b/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro index d42481a99..e09cee52f 100644 --- a/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro +++ b/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro @@ -27,6 +27,7 @@ OTHER_FILES += UAVObjects.pluginspec # Add in all of the synthetic/generated uavobject files HEADERS += \ + $$UAVOBJECT_SYNTHETICS/adjustments.h \ $$UAVOBJECT_SYNTHETICS/accelgyrosettings.h \ $$UAVOBJECT_SYNTHETICS/accessorydesired.h \ $$UAVOBJECT_SYNTHETICS/barosensor.h \ @@ -82,6 +83,7 @@ HEADERS += \ $$UAVOBJECT_SYNTHETICS/pathdesired.h \ $$UAVOBJECT_SYNTHETICS/pathplan.h \ $$UAVOBJECT_SYNTHETICS/pathstatus.h \ + $$UAVOBJECT_SYNTHETICS/pathsummary.h \ $$UAVOBJECT_SYNTHETICS/gpsvelocitysensor.h \ $$UAVOBJECT_SYNTHETICS/positionstate.h \ $$UAVOBJECT_SYNTHETICS/flightbatterystate.h \ @@ -131,6 +133,7 @@ HEADERS += \ $$UAVOBJECT_SYNTHETICS/perfcounter.h SOURCES += \ + $$UAVOBJECT_SYNTHETICS/adjustments.cpp \ $$UAVOBJECT_SYNTHETICS/accelgyrosettings.cpp \ $$UAVOBJECT_SYNTHETICS/accessorydesired.cpp \ $$UAVOBJECT_SYNTHETICS/barosensor.cpp \ @@ -186,6 +189,7 @@ SOURCES += \ $$UAVOBJECT_SYNTHETICS/pathdesired.cpp \ $$UAVOBJECT_SYNTHETICS/pathplan.cpp \ $$UAVOBJECT_SYNTHETICS/pathstatus.cpp \ + $$UAVOBJECT_SYNTHETICS/pathsummary.cpp \ $$UAVOBJECT_SYNTHETICS/gpsvelocitysensor.cpp \ $$UAVOBJECT_SYNTHETICS/positionstate.cpp \ $$UAVOBJECT_SYNTHETICS/flightbatterystate.cpp \ diff --git a/shared/uavobjectdefinition/adjustments.xml b/shared/uavobjectdefinition/adjustments.xml new file mode 100644 index 000000000..c7414c71c --- /dev/null +++ b/shared/uavobjectdefinition/adjustments.xml @@ -0,0 +1,15 @@ + + + Automatically calculated adjustments to parameters used into auto flight modes. Can come from @ref PathFollower + + + + + + + + + + + + diff --git a/shared/uavobjectdefinition/flightstatus.xml b/shared/uavobjectdefinition/flightstatus.xml index 8d373b312..b3c92f8aa 100644 --- a/shared/uavobjectdefinition/flightstatus.xml +++ b/shared/uavobjectdefinition/flightstatus.xml @@ -5,6 +5,10 @@ + + + + diff --git a/shared/uavobjectdefinition/manualcontrolsettings.xml b/shared/uavobjectdefinition/manualcontrolsettings.xml index be9d807ee..997ebdce0 100644 --- a/shared/uavobjectdefinition/manualcontrolsettings.xml +++ b/shared/uavobjectdefinition/manualcontrolsettings.xml @@ -16,6 +16,8 @@ elementnames="Roll,Pitch,Yaw,Collective,Accessory0,Accessory1,Accessory2"/> + + diff --git a/shared/uavobjectdefinition/pathdesired.xml b/shared/uavobjectdefinition/pathdesired.xml index a70604aa9..8296b479c 100644 --- a/shared/uavobjectdefinition/pathdesired.xml +++ b/shared/uavobjectdefinition/pathdesired.xml @@ -3,7 +3,7 @@ The endpoint or path the craft is trying to achieve. Can come from @ref ManualControl or @ref PathPlanner - + @@ -12,7 +12,7 @@ FixedAttitude, SetAccessory, Land, - DisarmAlarm" default="0"/> + DisarmAlarm,Brake" default="0"/> diff --git a/shared/uavobjectdefinition/pathstatus.xml b/shared/uavobjectdefinition/pathstatus.xml index 812b7a1cf..0735d8374 100644 --- a/shared/uavobjectdefinition/pathstatus.xml +++ b/shared/uavobjectdefinition/pathstatus.xml @@ -13,6 +13,7 @@ + diff --git a/shared/uavobjectdefinition/pathsummary.xml b/shared/uavobjectdefinition/pathsummary.xml new file mode 100644 index 000000000..0f8bec9eb --- /dev/null +++ b/shared/uavobjectdefinition/pathsummary.xml @@ -0,0 +1,19 @@ + + + Summary of a completed path segment Can come from any @ref PathFollower module + + + + + + + + + + + + + + + + diff --git a/shared/uavobjectdefinition/stabilizationsettings.xml b/shared/uavobjectdefinition/stabilizationsettings.xml index a79798b4c..f0369782e 100644 --- a/shared/uavobjectdefinition/stabilizationsettings.xml +++ b/shared/uavobjectdefinition/stabilizationsettings.xml @@ -46,6 +46,10 @@ + diff --git a/shared/uavobjectdefinition/vtolpathfollowersettings.xml b/shared/uavobjectdefinition/vtolpathfollowersettings.xml index 82277f320..ec698b810 100644 --- a/shared/uavobjectdefinition/vtolpathfollowersettings.xml +++ b/shared/uavobjectdefinition/vtolpathfollowersettings.xml @@ -19,6 +19,10 @@ + + + +