Merge branch 'abeck/OP-1858-autotakeoff' into next

flight/modules/ManualControl/armhandler.c

@@ -317,6 +317,7 @@ static bool okToArm(void)
         return false;
 
     case FLIGHTSTATUS_FLIGHTMODE_AUTOTAKEOFF:
+    case FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER:
         return true;
 
     default:

flight/modules/PathFollower/vtolautotakeoffcontroller.cpp

@@ -101,33 +101,12 @@ void VtolAutoTakeoffController::ObjectiveUpdated(void)
 {
     // Set the objective's target velocity
 
-    if (flightStatus->ControlChain.PathPlanner != FLIGHTSTATUS_CONTROLCHAIN_TRUE) {
     controlDown.UpdateVelocitySetpoint(pathDesired->ModeParameters[PATHDESIRED_MODEPARAMETER_AUTOTAKEOFF_DOWN]);
     controlNE.UpdateVelocitySetpoint(pathDesired->ModeParameters[PATHDESIRED_MODEPARAMETER_AUTOTAKEOFF_NORTH],
                                      pathDesired->ModeParameters[PATHDESIRED_MODEPARAMETER_AUTOTAKEOFF_EAST]);
     controlNE.UpdatePositionSetpoint(pathDesired->End.North, pathDesired->End.East);
     controlDown.UpdatePositionSetpoint(pathDesired->End.Down);
     fsm->setControlState((StatusVtolAutoTakeoffControlStateOptions)pathDesired->ModeParameters[PATHDESIRED_MODEPARAMETER_AUTOTAKEOFF_CONTROLSTATE]);
-    } else {
-        float velocity_down;
-        FlightModeSettingsAutoTakeOffVelocityGet(&velocity_down);
-        controlDown.UpdateVelocitySetpoint(-velocity_down);
-        controlNE.UpdateVelocitySetpoint(0.0f, 0.0f);
-        // pathplanner mode the waypoint provides the final destination including altitude. Takeoff would
-        // often be waypoint 0, in which case the starting location is the current location, and the end location
-        // is the waypoint location which really needs to be the same as the end in north and east. If it is not,
-        // it will fly to that location the during ascent.
-        // Note in pathplanner mode we use the start location which is the current initial location as the
-        // target NE to control.  Takeoff only ascends vertically.
-        controlNE.UpdatePositionSetpoint(pathDesired->Start.North, pathDesired->Start.East);
-        // Sanity check that the end location is at least a reasonable height above the start location in the down direction
-        float autotakeoff_height = pathDesired->Start.Down - pathDesired->End.Down;
-        if (autotakeoff_height < 2.0f) {
-            pathDesired->End.Down = pathDesired->Start.Down - 2.0f;
-        }
-        controlDown.UpdatePositionSetpoint(pathDesired->End.Down); // the altitude is set by the end location.
-        fsm->setControlState(STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_INITIATE);
-    }
 }
 void VtolAutoTakeoffController::Deactivate(void)
 {

flight/modules/PathPlanner/pathplanner.c

@@ -48,6 +48,8 @@
 #include "plans.h"
 #include <sanitycheck.h>
 #include <vtolpathfollowersettings.h>
+#include <statusvtolautotakeoff.h>
+#include <manualcontrolcommand.h>
 
 // Private constants
 #define STACK_SIZE_BYTES            1024
@@ -77,6 +79,8 @@ static uint8_t conditionPointingTowardsNext();
 static uint8_t conditionPythonScript();
 static uint8_t conditionImmediate();
 static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev);
+static void planner_setup_pathdesired_takeoff(PathDesiredData *pathDesired);
+static void planner_setup_pathdesired(PathDesiredData *pathDesired);
 
 
 // Private variables
@@ -167,6 +171,7 @@ static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
     }
 }
 
+#define AUTOTAKEOFF_THROTTLE_LIMIT_TO_ALLOW_TAKEOFF_START 0.3f
 
 /**
  * Module task
@@ -219,6 +224,17 @@ static void pathPlannerTask()
 
     WaypointActiveGet(&waypointActive);
 
+    // with the introduction of takeoff, we allow for arming
+    // whilst in pathplanner mode.  Previously it was just an assumption that
+    // a user never armed in pathplanner mode.  This check allows a user to select
+    // pathplanner, to upload waypoints, and then arm in pathplanner.
+    if (!flightStatus.Armed) {
+        return;
+    }
+
+
+    // the transition from pathplanner to another flightmode back to pathplanner
+    // triggers a reset back to 0 index in the waypoint list
     if (pathplanner_active == false) {
         pathplanner_active   = true;
 
@@ -245,6 +261,22 @@ static void pathPlannerTask()
         return;
     }
 
+    // check start conditions
+    // autotakeoff requires midpoint thrust if we are in a pending takeoff situation
+    if (pathAction.Mode == PATHACTION_MODE_AUTOTAKEOFF) {
+        pathAction.EndCondition = PATHACTION_ENDCONDITION_LEGREMAINING;
+        if ((uint8_t)pathDesired.ModeParameters[PATHDESIRED_MODEPARAMETER_AUTOTAKEOFF_CONTROLSTATE] == STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_WAITFORMIDTHROTTLE) {
+            ManualControlCommandData cmd;
+            ManualControlCommandGet(&cmd);
+            if (cmd.Throttle > AUTOTAKEOFF_THROTTLE_LIMIT_TO_ALLOW_TAKEOFF_START) {
+                pathDesired.ModeParameters[PATHDESIRED_MODEPARAMETER_AUTOTAKEOFF_CONTROLSTATE] = (float)STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_INITIATE;
+                PathDesiredSet(&pathDesired);
+            }
+            return;
+        }
+    }
+
+
     // check if condition has been met
     endCondition = pathConditionCheck();
     // decide what to do
@@ -283,6 +315,35 @@ static void pathPlannerTask()
     }
 }
 
+// callback function when waypoints changed in any way, update pathDesired
+void updatePathDesired()
+{
+    // only ever touch pathDesired if pathplanner is enabled
+    if (!pathplanner_active) {
+        return;
+    }
+
+    PathDesiredData pathDesired;
+
+    // find out current waypoint
+    WaypointActiveGet(&waypointActive);
+
+    WaypointInstGet(waypointActive.Index, &waypoint);
+    PathActionInstGet(waypoint.Action, &pathAction);
+
+    switch (pathAction.Mode) {
+    case PATHACTION_MODE_AUTOTAKEOFF:
+        planner_setup_pathdesired_takeoff(&pathDesired);
+        break;
+    default:
+        planner_setup_pathdesired(&pathDesired);
+        break;
+    }
+
+    PathDesiredSet(&pathDesired);
+}
+
+
 // safety checks for path plan integrity
 static uint8_t checkPathPlan()
 {
@@ -367,33 +428,20 @@ void statusUpdated(__attribute__((unused)) UAVObjEvent *ev)
     PIOS_CALLBACKSCHEDULER_Dispatch(pathPlannerHandle);
 }
 
-
+// Standard setup of a pathDesired command from the waypoint path plan
-// callback function when waypoints changed in any way, update pathDesired
+static void planner_setup_pathdesired(PathDesiredData *pathDesired)
-void updatePathDesired()
 {
-    // only ever touch pathDesired if pathplanner is enabled
+    pathDesired->End.North = waypoint.Position.North;
-    if (!pathplanner_active) {
+    pathDesired->End.East  = waypoint.Position.East;
-        return;
+    pathDesired->End.Down  = waypoint.Position.Down;
-    }
+    pathDesired->EndingVelocity    = waypoint.Velocity;
+    pathDesired->Mode = pathAction.Mode;
+    pathDesired->ModeParameters[0] = pathAction.ModeParameters[0];
+    pathDesired->ModeParameters[1] = pathAction.ModeParameters[1];
+    pathDesired->ModeParameters[2] = pathAction.ModeParameters[2];
+    pathDesired->ModeParameters[3] = pathAction.ModeParameters[3];
+    pathDesired->UID = waypointActive.Index;
 
-    PathDesiredData pathDesired;
-
-    // find out current waypoint
-    WaypointActiveGet(&waypointActive);
-
-    WaypointInstGet(waypointActive.Index, &waypoint);
-    PathActionInstGet(waypoint.Action, &pathAction);
-
-    pathDesired.End.North = waypoint.Position.North;
-    pathDesired.End.East  = waypoint.Position.East;
-    pathDesired.End.Down  = waypoint.Position.Down;
-    pathDesired.EndingVelocity    = waypoint.Velocity;
-    pathDesired.Mode = pathAction.Mode;
-    pathDesired.ModeParameters[0] = pathAction.ModeParameters[0];
-    pathDesired.ModeParameters[1] = pathAction.ModeParameters[1];
-    pathDesired.ModeParameters[2] = pathAction.ModeParameters[2];
-    pathDesired.ModeParameters[3] = pathAction.ModeParameters[3];
-    pathDesired.UID = waypointActive.Index;
 
     if (waypointActive.Index == 0) {
         PositionStateData positionState;
@@ -405,23 +453,62 @@ void updatePathDesired()
            pathDesired.Start[PATHDESIRED_START_DOWN] =  waypoint.Position[WAYPOINT_POSITION_DOWN];*/
         // note takeoff relies on the start being the current location as it merely ascends and using
         // the start as assumption current NE location
-        pathDesired.Start.North = positionState.North;
+        pathDesired->Start.North = positionState.North;
-        pathDesired.Start.East  = positionState.East;
+        pathDesired->Start.East  = positionState.East;
-        pathDesired.Start.Down  = positionState.Down;
+        pathDesired->Start.Down  = positionState.Down;
-        pathDesired.StartingVelocity = pathDesired.EndingVelocity;
+        pathDesired->StartingVelocity = pathDesired->EndingVelocity;
     } else {
         // Get previous waypoint as start point
         WaypointData waypointPrev;
         WaypointInstGet(waypointActive.Index - 1, &waypointPrev);
 
-        pathDesired.Start.North = waypointPrev.Position.North;
+        pathDesired->Start.North = waypointPrev.Position.North;
-        pathDesired.Start.East  = waypointPrev.Position.East;
+        pathDesired->Start.East  = waypointPrev.Position.East;
-        pathDesired.Start.Down  = waypointPrev.Position.Down;
+        pathDesired->Start.Down  = waypointPrev.Position.Down;
-        pathDesired.StartingVelocity = waypointPrev.Velocity;
+        pathDesired->StartingVelocity = waypointPrev.Velocity;
     }
-    PathDesiredSet(&pathDesired);
 }
 
+#define AUTOTAKEOFF_TO_INCREMENTAL_HEIGHT_MIN 2.0f
+#define AUTOTAKEOFF_TO_INCREMENTAL_HEIGHT_MAX 50.0f
+static void planner_setup_pathdesired_takeoff(PathDesiredData *pathDesired)
+{
+    PositionStateData positionState;
+
+    PositionStateGet(&positionState);
+    float velocity_down;
+    float autotakeoff_height;
+    FlightModeSettingsAutoTakeOffVelocityGet(&velocity_down);
+    FlightModeSettingsAutoTakeOffHeightGet(&autotakeoff_height);
+    autotakeoff_height = fabsf(autotakeoff_height);
+    if (autotakeoff_height < AUTOTAKEOFF_TO_INCREMENTAL_HEIGHT_MIN) {
+        autotakeoff_height = AUTOTAKEOFF_TO_INCREMENTAL_HEIGHT_MIN;
+    } else if (autotakeoff_height > AUTOTAKEOFF_TO_INCREMENTAL_HEIGHT_MAX) {
+        autotakeoff_height = AUTOTAKEOFF_TO_INCREMENTAL_HEIGHT_MAX;
+    }
+
+
+    pathDesired->Start.North = positionState.North;
+    pathDesired->Start.East  = positionState.East;
+    pathDesired->Start.Down  = positionState.Down;
+    pathDesired->ModeParameters[PATHDESIRED_MODEPARAMETER_AUTOTAKEOFF_NORTH] = 0.0f;
+    pathDesired->ModeParameters[PATHDESIRED_MODEPARAMETER_AUTOTAKEOFF_EAST]  = 0.0f;
+    pathDesired->ModeParameters[PATHDESIRED_MODEPARAMETER_AUTOTAKEOFF_DOWN]  = -velocity_down;
+    // initially halt takeoff.
+    pathDesired->ModeParameters[PATHDESIRED_MODEPARAMETER_AUTOTAKEOFF_CONTROLSTATE] = (float)STATUSVTOLAUTOTAKEOFF_CONTROLSTATE_WAITFORMIDTHROTTLE;
+
+    pathDesired->End.North = positionState.North;
+    pathDesired->End.East  = positionState.East;
+    pathDesired->End.Down  = positionState.Down - autotakeoff_height;
+
+    pathDesired->StartingVelocity = 0.0f;
+    pathDesired->EndingVelocity   = 0.0f;
+    pathDesired->Mode = PATHDESIRED_MODE_AUTOTAKEOFF;
+
+    pathDesired->UID  = waypointActive.Index;
+}
+
+
 // helper function to go to a specific waypoint
 static void setWaypoint(uint16_t num)
 {