From f4d1fc13ece0c26da7922579985c50d7671ab790 Mon Sep 17 00:00:00 2001 From: abeck70 Date: Thu, 14 May 2015 20:07:07 +1000 Subject: [PATCH 1/5] OP-1847 gps assist fixes 1. Update vtol vertical pids 2. Reduce deadband on throttle in gpsassist auto throttle to avoid large deviations in throttle change from neutral 3. Reinitialise assisted control state on mode change where the flight is the same but only gpsassist is going from disabled to enabled or enabled to disabled. --- flight/modules/ManualControl/manualcontrol.c | 18 ++++++++++++++---- .../vtolpathfollowersettings.xml | 4 ++-- 2 files changed, 16 insertions(+), 6 deletions(-) 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/shared/uavobjectdefinition/vtolpathfollowersettings.xml b/shared/uavobjectdefinition/vtolpathfollowersettings.xml index 4f6758035..61e49cb8d 100644 --- a/shared/uavobjectdefinition/vtolpathfollowersettings.xml +++ b/shared/uavobjectdefinition/vtolpathfollowersettings.xml @@ -7,8 +7,8 @@ - - + + From 7d603bc6cb4f125945b8cd12bd7cf58e31ea7dc8 Mon Sep 17 00:00:00 2001 From: abeck70 Date: Sat, 16 May 2015 20:33:03 +1000 Subject: [PATCH 2/5] OP-1847 gpsassist fix Set assisted control state to HOLD when Brake controller transitions to HOLD. This impacts throttle stick behavior as it is different during braking and on brake completion in the hold state. Also cleanuped and removed unused/old code that triggered HOLD in plans.c --- flight/libraries/inc/plans.h | 2 +- flight/libraries/plans.c | 118 ++++++++---------- .../ManualControl/pathfollowerhandler.c | 4 +- .../PathFollower/vtolbrakecontroller.cpp | 15 +++ 4 files changed, 67 insertions(+), 72 deletions(-) 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/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 From 1e9f7d989205246d6187b41fdf5ad58ee5bacd1b Mon Sep 17 00:00:00 2001 From: abeck70 Date: Sat, 16 May 2015 22:31:42 +1000 Subject: [PATCH 3/5] OP-1847 update vertical velicity Kp --- shared/uavobjectdefinition/altitudeholdsettings.xml | 2 +- shared/uavobjectdefinition/vtolpathfollowersettings.xml | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) 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/vtolpathfollowersettings.xml b/shared/uavobjectdefinition/vtolpathfollowersettings.xml index 61e49cb8d..cae6d957b 100644 --- a/shared/uavobjectdefinition/vtolpathfollowersettings.xml +++ b/shared/uavobjectdefinition/vtolpathfollowersettings.xml @@ -8,7 +8,7 @@ - + From 8cb7aeacf70a31c296e78fab73e2592c65288ca4 Mon Sep 17 00:00:00 2001 From: abeck70 Date: Sat, 16 May 2015 22:32:22 +1000 Subject: [PATCH 4/5] OP-1882 set thrust control to auto as default --- shared/uavobjectdefinition/vtolpathfollowersettings.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/shared/uavobjectdefinition/vtolpathfollowersettings.xml b/shared/uavobjectdefinition/vtolpathfollowersettings.xml index cae6d957b..62469c113 100644 --- a/shared/uavobjectdefinition/vtolpathfollowersettings.xml +++ b/shared/uavobjectdefinition/vtolpathfollowersettings.xml @@ -11,7 +11,7 @@ - + From f76fee6810c018b98318412609342973297854a5 Mon Sep 17 00:00:00 2001 From: abeck70 Date: Sat, 16 May 2015 22:47:34 +1000 Subject: [PATCH 5/5] OP-1884 New defaults for GyroTau and Lowpass mpu filter --- shared/uavobjectdefinition/mpu6000settings.xml | 2 +- shared/uavobjectdefinition/stabilizationsettings.xml | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) 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 @@ - +