From 55b3b52c4f71bac2011b8bea2d2bb730ab3bc883 Mon Sep 17 00:00:00 2001 From: abeck70 Date: Fri, 22 May 2015 17:34:43 +1000 Subject: [PATCH 1/4] OP-1898 VelocityRoam flight mode Replace PositionRoam with VelocityRoam Make VelocityRoam an explicit flight mode to simplify access Disable PositionRoam as not supported and not suitable to retain. --- flight/libraries/sanitycheck.c | 2 +- flight/modules/ManualControl/manualcontrol.c | 4 +-- .../ManualControl/pathfollowerhandler.c | 16 +++--------- flight/modules/Notify/inc/sequences.h | 3 ++- .../flightmodesettings.xml | 26 +++++++++---------- shared/uavobjectdefinition/flightstatus.xml | 2 +- 6 files changed, 23 insertions(+), 30 deletions(-) diff --git a/flight/libraries/sanitycheck.c b/flight/libraries/sanitycheck.c index cf60a63d4..bfb4b792b 100644 --- a/flight/libraries/sanitycheck.c +++ b/flight/libraries/sanitycheck.c @@ -146,7 +146,7 @@ int32_t configuration_check() ADDSEVERITY(!gps_assisted); } case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_POSITIONHOLD: - case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_POSITIONROAM: + case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_VELOCITYROAM: case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_LAND: case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_AUTOTAKEOFF: ADDSEVERITY(!coptercontrol); diff --git a/flight/modules/ManualControl/manualcontrol.c b/flight/modules/ManualControl/manualcontrol.c index 633e390e6..12a6bb61a 100644 --- a/flight/modules/ManualControl/manualcontrol.c +++ b/flight/modules/ManualControl/manualcontrol.c @@ -411,7 +411,7 @@ static void manualControlTask(void) // set in stabi settings. Once if we decide to always have this on, it can // can be directly set here...i.e. set the flight mode assist as required. case FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD: - case FLIGHTSTATUS_FLIGHTMODE_POSITIONROAM: + case FLIGHTSTATUS_FLIGHTMODE_VELOCITYROAM: case FLIGHTSTATUS_FLIGHTMODE_LAND: case FLIGHTSTATUS_FLIGHTMODE_AUTOTAKEOFF: newFlightModeAssist = isAssistedFlightMode(position, newMode, &modeSettings); @@ -531,7 +531,7 @@ static uint8_t isAssistedFlightMode(uint8_t position, uint8_t flightMode, Flight thrustMode = modeSettings->Stabilization6Settings.Thrust; break; case FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD: - case FLIGHTSTATUS_FLIGHTMODE_POSITIONROAM: + case FLIGHTSTATUS_FLIGHTMODE_VELOCITYROAM: // we hard code the "GPS Assisted" PostionHold/Roam to use alt-vario which // is a more appropriate throttle mode. "GPSAssist" adds braking // and a better throttle management to the standard Position Hold. diff --git a/flight/modules/ManualControl/pathfollowerhandler.c b/flight/modules/ManualControl/pathfollowerhandler.c index dc71cebf3..202c833b8 100644 --- a/flight/modules/ManualControl/pathfollowerhandler.c +++ b/flight/modules/ManualControl/pathfollowerhandler.c @@ -80,12 +80,8 @@ void pathFollowerHandler(bool newinit) case FLIGHTSTATUS_FLIGHTMODE_COURSELOCK: plan_setup_CourseLock(); break; - case FLIGHTSTATUS_FLIGHTMODE_POSITIONROAM: - if (flightModeAssist == FLIGHTSTATUS_FLIGHTMODEASSIST_NONE) { - plan_setup_PositionRoam(); - } else { - plan_setup_VelocityRoam(); - } + case FLIGHTSTATUS_FLIGHTMODE_VELOCITYROAM: + plan_setup_VelocityRoam(); break; case FLIGHTSTATUS_FLIGHTMODE_HOMELEASH: plan_setup_HomeLeash(); @@ -130,12 +126,8 @@ void pathFollowerHandler(bool newinit) case FLIGHTSTATUS_FLIGHTMODE_COURSELOCK: plan_run_CourseLock(); break; - case FLIGHTSTATUS_FLIGHTMODE_POSITIONROAM: - if (flightModeAssist == FLIGHTSTATUS_FLIGHTMODEASSIST_NONE) { - plan_run_PositionRoam(); - } else { - plan_run_VelocityRoam(); - } + case FLIGHTSTATUS_FLIGHTMODE_VELOCITYROAM: + plan_run_VelocityRoam(); break; case FLIGHTSTATUS_FLIGHTMODE_HOMELEASH: plan_run_HomeLeash(); diff --git a/flight/modules/Notify/inc/sequences.h b/flight/modules/Notify/inc/sequences.h index 86c4f153a..3cf46d832 100644 --- a/flight/modules/Notify/inc/sequences.h +++ b/flight/modules/Notify/inc/sequences.h @@ -176,13 +176,14 @@ const LedSequence_t *flightModeMap[] = { [FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER] = ¬ifications[NOTIFY_SEQUENCE_ARMED_FM_AUTO], [FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD] = ¬ifications[NOTIFY_SEQUENCE_ARMED_FM_GPS], [FLIGHTSTATUS_FLIGHTMODE_COURSELOCK] = ¬ifications[NOTIFY_SEQUENCE_ARMED_FM_GPS], - [FLIGHTSTATUS_FLIGHTMODE_POSITIONROAM] = ¬ifications[NOTIFY_SEQUENCE_ARMED_FM_GPS], + [FLIGHTSTATUS_FLIGHTMODE_VELOCITYROAM] = ¬ifications[NOTIFY_SEQUENCE_ARMED_FM_GPS], [FLIGHTSTATUS_FLIGHTMODE_HOMELEASH] = ¬ifications[NOTIFY_SEQUENCE_ARMED_FM_GPS], [FLIGHTSTATUS_FLIGHTMODE_ABSOLUTEPOSITION] = ¬ifications[NOTIFY_SEQUENCE_ARMED_FM_GPS], [FLIGHTSTATUS_FLIGHTMODE_RETURNTOBASE] = ¬ifications[NOTIFY_SEQUENCE_ARMED_FM_RTH], [FLIGHTSTATUS_FLIGHTMODE_LAND] = ¬ifications[NOTIFY_SEQUENCE_ARMED_FM_LAND], [FLIGHTSTATUS_FLIGHTMODE_POI] = ¬ifications[NOTIFY_SEQUENCE_ARMED_FM_GPS], [FLIGHTSTATUS_FLIGHTMODE_AUTOCRUISE] = ¬ifications[NOTIFY_SEQUENCE_ARMED_FM_GPS], + [FLIGHTSTATUS_FLIGHTMODE_AUTOTAKEOFF] = ¬ifications[NOTIFY_SEQUENCE_ARMED_FM_LAND], }; // List of alarms to show with attached sequences for each status diff --git a/shared/uavobjectdefinition/flightmodesettings.xml b/shared/uavobjectdefinition/flightmodesettings.xml index e5ed1d2eb..22c6fc8bc 100644 --- a/shared/uavobjectdefinition/flightmodesettings.xml +++ b/shared/uavobjectdefinition/flightmodesettings.xml @@ -78,31 +78,31 @@ units="" type="enum" elements="6" - options="Manual,Stabilized1,Stabilized2,Stabilized3,Stabilized4,Stabilized5,Stabilized6,PositionHold,CourseLock,PositionRoam,HomeLeash,AbsolutePosition,ReturnToBase,Land,PathPlanner,POI,AutoCruise,AutoTakeoff" + options="Manual,Stabilized1,Stabilized2,Stabilized3,Stabilized4,Stabilized5,Stabilized6,PositionHold,CourseLock,VelocityRoam,HomeLeash,AbsolutePosition,ReturnToBase,Land,PathPlanner,POI,AutoCruise,AutoTakeoff" defaultvalue="Stabilized1,Stabilized2,Stabilized3,Stabilized4,Stabilized5,Stabilized6" limits="\ - %0401NE:PositionHold:CourseLock:PositionRoam:HomeLeash:AbsolutePosition:ReturnToBase:Land:PathPlanner:POI:AutoCruise:AutoTakeoff\ - %0402NE:PositionHold:CourseLock:PositionRoam:HomeLeash:AbsolutePosition:ReturnToBase:Land:PathPlanner:POI:AutoCruise:AutoTakeoff\ + %0401NE:PositionHold:CourseLock:VelocityRoam:HomeLeash:AbsolutePosition:ReturnToBase:Land:PathPlanner:POI:AutoCruise:AutoTakeoff\ + %0402NE:PositionHold:CourseLock:VelocityRoam:HomeLeash:AbsolutePosition:ReturnToBase:Land:PathPlanner:POI:AutoCruise:AutoTakeoff\ %0903NE:POI:PathPlanner:AutoCruise;\ \ - %0401NE:PositionHold:CourseLock:PositionRoam:HomeLeash:AbsolutePosition:ReturnToBase:Land:PathPlanner:POI:AutoCruise:AutoTakeoff\ - %0402NE:PositionHold:CourseLock:PositionRoam:HomeLeash:AbsolutePosition:ReturnToBase:Land:PathPlanner:POI:AutoCruise:AutoTakeoff\ + %0401NE:PositionHold:CourseLock:VelocityRoam:HomeLeash:AbsolutePosition:ReturnToBase:Land:PathPlanner:POI:AutoCruise:AutoTakeoff\ + %0402NE:PositionHold:CourseLock:VelocityRoam:HomeLeash:AbsolutePosition:ReturnToBase:Land:PathPlanner:POI:AutoCruise:AutoTakeoff\ %0903NE:POI:PathPlanner:AutoCruise;\ \ - %0401NE:PositionHold:CourseLock:PositionRoam:HomeLeash:AbsolutePosition:ReturnToBase:Land:PathPlanner:POI:AutoCruise:AutoTakeoff\ - %0402NE:PositionHold:CourseLock:PositionRoam:HomeLeash:AbsolutePosition:ReturnToBase:Land:PathPlanner:POI:AutoCruise:AutoTakeoff\ + %0401NE:PositionHold:CourseLock:VelocityRoam:HomeLeash:AbsolutePosition:ReturnToBase:Land:PathPlanner:POI:AutoCruise:AutoTakeoff\ + %0402NE:PositionHold:CourseLock:VelocityRoam:HomeLeash:AbsolutePosition:ReturnToBase:Land:PathPlanner:POI:AutoCruise:AutoTakeoff\ %0903NE:POI:PathPlanner:AutoCruise;\ \ - %0401NE:PositionHold:CourseLock:PositionRoam:HomeLeash:AbsolutePosition:ReturnToBase:Land:PathPlanner:POI:AutoCruise:AutoTakeoff\ - %0402NE:PositionHold:CourseLock:PositionRoam:HomeLeash:AbsolutePosition:ReturnToBase:Land:PathPlanner:POI:AutoCruise:AutoTakeoff\ + %0401NE:PositionHold:CourseLock:VelocityRoam:HomeLeash:AbsolutePosition:ReturnToBase:Land:PathPlanner:POI:AutoCruise:AutoTakeoff\ + %0402NE:PositionHold:CourseLock:VelocityRoam:HomeLeash:AbsolutePosition:ReturnToBase:Land:PathPlanner:POI:AutoCruise:AutoTakeoff\ %0903NE:POI:PathPlanner:AutoCruise;\ \ - %0401NE:PositionHold:CourseLock:PositionRoam:HomeLeash:AbsolutePosition:ReturnToBase:Land:PathPlanner:POI:AutoCruise:AutoTakeoff\ - %0402NE:PositionHold:CourseLock:PositionRoam:HomeLeash:AbsolutePosition:ReturnToBase:Land:PathPlanner:POI:AutoCruise:AutoTakeoff\ + %0401NE:PositionHold:CourseLock:VelocityRoam:HomeLeash:AbsolutePosition:ReturnToBase:Land:PathPlanner:POI:AutoCruise:AutoTakeoff\ + %0402NE:PositionHold:CourseLock:VelocityRoam:HomeLeash:AbsolutePosition:ReturnToBase:Land:PathPlanner:POI:AutoCruise:AutoTakeoff\ %0903NE:POI:PathPlanner:AutoCruise;\ \ - %0401NE:PositionHold:CourseLock:PositionRoam:HomeLeash:AbsolutePosition:ReturnToBase:Land:PathPlanner:POI:AutoCruise:AutoTakeoff\ - %0402NE:PositionHold:CourseLock:PositionRoam:HomeLeash:AbsolutePosition:ReturnToBase:Land:PathPlanner:POI:AutoCruise:AutoTakeoff\ + %0401NE:PositionHold:CourseLock:VelocityRoam:HomeLeash:AbsolutePosition:ReturnToBase:Land:PathPlanner:POI:AutoCruise:AutoTakeoff\ + %0402NE:PositionHold:CourseLock:VelocityRoam:HomeLeash:AbsolutePosition:ReturnToBase:Land:PathPlanner:POI:AutoCruise:AutoTakeoff\ %0903NE:POI:PathPlanner:AutoCruise;"/> diff --git a/shared/uavobjectdefinition/flightstatus.xml b/shared/uavobjectdefinition/flightstatus.xml index 395486c58..7703f38ca 100644 --- a/shared/uavobjectdefinition/flightstatus.xml +++ b/shared/uavobjectdefinition/flightstatus.xml @@ -4,7 +4,7 @@ - + From 446866f769a1779f0e6950d04526ed7474e3d888 Mon Sep 17 00:00:00 2001 From: abeck70 Date: Fri, 22 May 2015 18:56:04 +1000 Subject: [PATCH 2/4] OP-1898 Velocity Roam Update tooltips on GPSAssist --- .../openpilotgcs/src/plugins/config/input.ui | 24 +++++++++---------- 1 file changed, 12 insertions(+), 12 deletions(-) diff --git a/ground/openpilotgcs/src/plugins/config/input.ui b/ground/openpilotgcs/src/plugins/config/input.ui index ed0be6807..2bf8f1aac 100644 --- a/ground/openpilotgcs/src/plugins/config/input.ui +++ b/ground/openpilotgcs/src/plugins/config/input.ui @@ -116,8 +116,8 @@ 0 0 - 1228 - 661 + 1220 + 655 @@ -490,8 +490,8 @@ 0 0 - 1228 - 669 + 1220 + 655 @@ -1960,7 +1960,7 @@ font:bold; 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> + <html><head/><body><p>GPSAssist adds a brake/hold sequence to Stabilization when pitch & roll is centered, and on initiation of PositionHold. GPSAssist also enables pitch & roll control during the auto landing flight mode.</p></body></html> @@ -1986,7 +1986,7 @@ font:bold; 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> + <html><head/><body><p>GPSAssist adds a brake/hold sequence to Stabilization when pitch & roll is centered, and on initiation of PositionHold. GPSAssist also enables pitch & roll control during the auto landing flight mode.</p></body></html> @@ -2012,7 +2012,7 @@ font:bold; 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> + <html><head/><body><p>GPSAssist adds a brake/hold sequence to Stabilization when pitch & roll is centered, and on initiation of PositionHold. GPSAssist also enables pitch & roll control during the auto landing flight mode.</p></body></html> @@ -2041,7 +2041,7 @@ font:bold; 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> + <html><head/><body><p>GPSAssist adds a brake/hold sequence to Stabilization when pitch & roll is centered, and on initiation of PositionHold. GPSAssist also enables pitch & roll control during the auto landing flight mode.</p></body></html> @@ -2070,7 +2070,7 @@ font:bold; 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> + <html><head/><body><p>GPSAssist adds a brake/hold sequence to Stabilization when pitch & roll is centered, and on initiation of PositionHold. GPSAssist also enables pitch & roll control during the auto landing flight mode.</p></body></html> @@ -2099,7 +2099,7 @@ font:bold; 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> + <html><head/><body><p>GPSAssist adds a brake/hold sequence to Stabilization when pitch & roll is centered, and on initiation of PositionHold. GPSAssist also enables pitch & roll control during the auto landing flight mode.</p></body></html> @@ -2219,8 +2219,8 @@ font:bold; 0 0 - 1228 - 669 + 1220 + 655 From c272c4d4fcb301cf3ab88237df7a30a5118659a3 Mon Sep 17 00:00:00 2001 From: abeck70 Date: Fri, 22 May 2015 19:07:06 +1000 Subject: [PATCH 3/4] OP-1898 VelocityRoam AltVario update tuning values based on FTT feedback --- shared/uavobjectdefinition/altitudeholdsettings.xml | 4 ++-- shared/uavobjectdefinition/vtolpathfollowersettings.xml | 6 +++--- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/shared/uavobjectdefinition/altitudeholdsettings.xml b/shared/uavobjectdefinition/altitudeholdsettings.xml index 7058041e2..d621b1ca6 100644 --- a/shared/uavobjectdefinition/altitudeholdsettings.xml +++ b/shared/uavobjectdefinition/altitudeholdsettings.xml @@ -5,8 +5,8 @@ - - + + diff --git a/shared/uavobjectdefinition/vtolpathfollowersettings.xml b/shared/uavobjectdefinition/vtolpathfollowersettings.xml index 62469c113..47e43f06f 100644 --- a/shared/uavobjectdefinition/vtolpathfollowersettings.xml +++ b/shared/uavobjectdefinition/vtolpathfollowersettings.xml @@ -6,9 +6,9 @@ - + - + @@ -21,7 +21,7 @@ - + From 064a5e831b57efa3c8d306c02e6452d460ecd033 Mon Sep 17 00:00:00 2001 From: abeck70 Date: Fri, 22 May 2015 21:23:10 +1000 Subject: [PATCH 4/4] OP-1898 VelocityRoam gets its own MaxRollPitch This allows smoother velocityroam flight --- flight/modules/PathFollower/vtolvelocitycontroller.cpp | 4 ++-- shared/uavobjectdefinition/vtolpathfollowersettings.xml | 1 + 2 files changed, 3 insertions(+), 2 deletions(-) diff --git a/flight/modules/PathFollower/vtolvelocitycontroller.cpp b/flight/modules/PathFollower/vtolvelocitycontroller.cpp index 68018c5ea..968d30cfc 100644 --- a/flight/modules/PathFollower/vtolvelocitycontroller.cpp +++ b/flight/modules/PathFollower/vtolvelocitycontroller.cpp @@ -113,7 +113,7 @@ void VtolVelocityController::SettingsUpdated(void) controlNE.UpdatePositionalParameters(vtolPathFollowerSettings->HorizontalPosP); - controlNE.UpdateCommandParameters(-vtolPathFollowerSettings->MaxRollPitch, vtolPathFollowerSettings->MaxRollPitch, vtolPathFollowerSettings->VelocityFeedforward); + controlNE.UpdateCommandParameters(-vtolPathFollowerSettings->VelocityRoamMaxRollPitch, vtolPathFollowerSettings->VelocityRoamMaxRollPitch, vtolPathFollowerSettings->VelocityFeedforward); } int32_t VtolVelocityController::Initialize(VtolPathFollowerSettingsData *ptr_vtolPathFollowerSettings) @@ -172,7 +172,7 @@ int8_t VtolVelocityController::UpdateStabilizationDesired(__attribute__((unused) float angle_radians = DEG2RAD(attitudeState.Yaw); float cos_angle = cosf(angle_radians); float sine_angle = sinf(angle_radians); - float maxPitch = vtolPathFollowerSettings->MaxRollPitch; + float maxPitch = vtolPathFollowerSettings->VelocityRoamMaxRollPitch; stabDesired.StabilizationMode.Pitch = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; stabDesired.Pitch = boundf(-northCommand * cos_angle - eastCommand * sine_angle, -maxPitch, maxPitch); stabDesired.StabilizationMode.Roll = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; diff --git a/shared/uavobjectdefinition/vtolpathfollowersettings.xml b/shared/uavobjectdefinition/vtolpathfollowersettings.xml index 47e43f06f..5445f1ed0 100644 --- a/shared/uavobjectdefinition/vtolpathfollowersettings.xml +++ b/shared/uavobjectdefinition/vtolpathfollowersettings.xml @@ -18,6 +18,7 @@ +