diff --git a/flight/libraries/inc/plans.h b/flight/libraries/inc/plans.h
index 64159ea36..c241dd85d 100644
--- a/flight/libraries/inc/plans.h
+++ b/flight/libraries/inc/plans.h
@@ -67,7 +67,7 @@ void plan_setup_AutoTakeoff();
/**
* @brief setup pathplanner/pathfollower for braking
*/
-void plan_setup_assistedcontrol(uint8_t timeout_occurred);
+void plan_setup_assistedcontrol();
#define PATHDESIRED_MODEPARAMETER_BRAKE_STARTVELOCITYVECTOR_NORTH 0
#define PATHDESIRED_MODEPARAMETER_BRAKE_STARTVELOCITYVECTOR_EAST 1
diff --git a/flight/libraries/plans.c b/flight/libraries/plans.c
index 4005a9d6f..7055744b8 100644
--- a/flight/libraries/plans.c
+++ b/flight/libraries/plans.c
@@ -615,7 +615,7 @@ void plan_run_VelocityRoam()
// avoid brake then hold sequence to continue descent.
plan_setup_land_from_velocityroam();
} else {
- plan_setup_assistedcontrol(false);
+ plan_setup_assistedcontrol();
}
}
// otherwise nothing to do in braking/hold modes
@@ -824,7 +824,7 @@ void plan_run_AutoCruise()
#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)
+void plan_setup_assistedcontrol()
{
PositionStateData positionState;
@@ -832,75 +832,55 @@ void plan_setup_assistedcontrol(uint8_t timeout_occurred)
PathDesiredData pathDesired;
FlightStatusAssistedControlStateOptions assistedControlFlightMode;
- FlightStatusAssistedControlStateGet(&assistedControlFlightMode);
- if (timeout_occurred) {
- FlightStatusFlightModeOptions flightMode;
- FlightStatusFlightModeGet(&flightMode);
- if (flightMode == FLIGHTSTATUS_FLIGHTMODE_LAND) {
- plan_setup_land_helper(&pathDesired);
- } else {
- 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_GOTOENDPOINT;
- }
- 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;
+ 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/modules/ManualControl/manualcontrol.c b/flight/modules/ManualControl/manualcontrol.c
index ca7ebeba5..633e390e6 100644
--- a/flight/modules/ManualControl/manualcontrol.c
+++ b/flight/modules/ManualControl/manualcontrol.c
@@ -59,8 +59,8 @@
#define CBTASK_PRIORITY CALLBACK_TASK_FLIGHTCONTROL
#define ASSISTEDCONTROL_NEUTRALTHROTTLERANGE_FACTOR 0.2f
-#define ASSISTEDCONTROL_BRAKETHRUST_DEADBAND_FACTOR_LO 0.92f
-#define ASSISTEDCONTROL_BRAKETHRUST_DEADBAND_FACTOR_HI 1.08f
+#define ASSISTEDCONTROL_BRAKETHRUST_DEADBAND_FACTOR_LO 0.96f
+#define ASSISTEDCONTROL_BRAKETHRUST_DEADBAND_FACTOR_HI 1.04f
// defined handlers
@@ -242,7 +242,7 @@ static void manualControlTask(void)
// 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
+ // The following are equivalent to none effectively. Code should always
// check the flightmodeassist state.
newAssistedControlState = FLIGHTSTATUS_ASSISTEDCONTROLSTATE_PRIMARY;
newAssistedThrottleState = FLIGHTSTATUS_ASSISTEDTHROTTLESTATE_MANUAL;
@@ -264,6 +264,16 @@ static void manualControlTask(void)
#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);
@@ -301,7 +311,7 @@ static void manualControlTask(void)
// retain thrust cmd for later comparison with actual in braking
thrustAtBrakeStart = cmd.Thrust;
- // calculate hi and low value of +-8% as a mini-deadband
+ // 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;
diff --git a/flight/modules/ManualControl/pathfollowerhandler.c b/flight/modules/ManualControl/pathfollowerhandler.c
index 86912a168..dc71cebf3 100644
--- a/flight/modules/ManualControl/pathfollowerhandler.c
+++ b/flight/modules/ManualControl/pathfollowerhandler.c
@@ -72,7 +72,7 @@ void pathFollowerHandler(bool newinit)
if ((flightModeAssist != FLIGHTSTATUS_FLIGHTMODEASSIST_NONE) &&
(assistedControlFlightMode == FLIGHTSTATUS_ASSISTEDCONTROLSTATE_PRIMARY)) {
// Switch from primary (just entered this PH flight mode) into brake
- plan_setup_assistedcontrol(false);
+ plan_setup_assistedcontrol();
} else {
plan_setup_positionHold();
}
@@ -116,7 +116,7 @@ void pathFollowerHandler(bool newinit)
case FLIGHTSTATUS_FLIGHTMODE_STABILIZED6:
if (assistedControlFlightMode == FLIGHTSTATUS_ASSISTEDCONTROLSTATE_BRAKE) {
// Just initiated braking after returning from stabi control
- plan_setup_assistedcontrol(false);
+ plan_setup_assistedcontrol();
}
break;
diff --git a/flight/modules/PathFollower/vtolbrakecontroller.cpp b/flight/modules/PathFollower/vtolbrakecontroller.cpp
index 862011057..ffa492461 100644
--- a/flight/modules/PathFollower/vtolbrakecontroller.cpp
+++ b/flight/modules/PathFollower/vtolbrakecontroller.cpp
@@ -193,6 +193,21 @@ void VtolBrakeController::UpdateVelocityDesired()
if (!mManualThrust) {
controlDown.UpdatePositionSetpoint(positionState.Down);
}
+
+
+ FlightStatusFlightModeAssistOptions flightModeAssist;
+ FlightStatusFlightModeAssistGet(&flightModeAssist);
+ if (flightModeAssist != FLIGHTSTATUS_FLIGHTMODEASSIST_NONE) {
+ // Notify manualcommand via setting hold state in flightstatus assistedcontrolstate
+ FlightStatusAssistedControlStateOptions assistedControlFlightMode;
+ FlightStatusAssistedControlStateGet(&assistedControlFlightMode);
+ // sanity check that we are in brake state according to flight status, which means
+ // we are being used for gpsassist
+ if (assistedControlFlightMode == FLIGHTSTATUS_ASSISTEDCONTROLSTATE_BRAKE) {
+ assistedControlFlightMode = FLIGHTSTATUS_ASSISTEDCONTROLSTATE_HOLD;
+ FlightStatusAssistedControlStateSet(&assistedControlFlightMode);
+ }
+ }
}
// Update position state and control position to create inputs to velocity control
diff --git a/shared/uavobjectdefinition/altitudeholdsettings.xml b/shared/uavobjectdefinition/altitudeholdsettings.xml
index e85ff09e6..7058041e2 100644
--- a/shared/uavobjectdefinition/altitudeholdsettings.xml
+++ b/shared/uavobjectdefinition/altitudeholdsettings.xml
@@ -6,7 +6,7 @@
-
+
diff --git a/shared/uavobjectdefinition/mpu6000settings.xml b/shared/uavobjectdefinition/mpu6000settings.xml
index 943243fa7..4813a9c71 100644
--- a/shared/uavobjectdefinition/mpu6000settings.xml
+++ b/shared/uavobjectdefinition/mpu6000settings.xml
@@ -18,7 +18,7 @@
-
+
diff --git a/shared/uavobjectdefinition/stabilizationsettings.xml b/shared/uavobjectdefinition/stabilizationsettings.xml
index 9187d13f3..644bfbf41 100644
--- a/shared/uavobjectdefinition/stabilizationsettings.xml
+++ b/shared/uavobjectdefinition/stabilizationsettings.xml
@@ -18,7 +18,7 @@
-
+
diff --git a/shared/uavobjectdefinition/vtolpathfollowersettings.xml b/shared/uavobjectdefinition/vtolpathfollowersettings.xml
index 4f6758035..62469c113 100644
--- a/shared/uavobjectdefinition/vtolpathfollowersettings.xml
+++ b/shared/uavobjectdefinition/vtolpathfollowersettings.xml
@@ -7,11 +7,11 @@
-
-
+
+
-
+