OP-1350 refactor into a plans library all functions dealing with initialization and execution of PathFollower/pathplanner commands for manualcontrol

flight/libraries/inc/plans.h

@@ -0,0 +1,69 @@
+/**
+ ******************************************************************************
+ * @addtogroup OpenPilotLibraries OpenPilot Libraries
+ * @{
+ * @addtogroup Navigation
+ * @brief setups RTH/PH and other pathfollower/pathplanner status
+ * @{
+ *
+ * @file       plan.h
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014.
+ *
+ * @see        The GNU Public License (GPL) Version 3
+ *
+ ******************************************************************************/
+/*
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
+ * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
+ * for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ */
+
+#ifndef PLANS_H_
+#define PLANS_H_
+#include <pios_math.h>
+
+/** \page standard Plans
+   How to use this library
+   \li Call plan_initialize() to init all needed struct and uavos at startup.<br>
+   It may be safely called more than once.<br>
+
+   \li Functions called plan_setup_* needs to be called once, every time the requested function is engaged.<br>
+
+   \li Functions called plan_run_* are to be periodically called while the requested mode is engaged.<br>
+ */
+
+/**
+ * @brief initialize UAVOs and structs used by this library
+ */
+void plan_initialize();
+
+/**
+ * @brief setup pathplanner/pathfollower for positionhold
+ */
+void plan_setup_positionHold();
+
+/**
+ * @brief setup pathplanner/pathfollower for return to base
+ */
+void plan_setup_returnToBase();
+
+/**
+ * @brief setup pathplanner/pathfollower for land
+ */
+void plan_setup_land();
+
+/**
+ * @brief execute land
+ */
+void plan_run_land();
+#endif /* PLANS_H_ */

flight/libraries/plans.c

@@ -0,0 +1,130 @@
+/**
+ ******************************************************************************
+ * @addtogroup OpenPilotLibraries OpenPilot Libraries
+ * @{
+ * @addtogroup Navigation
+ * @brief setups RTH/PH and other pathfollower/pathplanner status
+ * @{
+ *
+ * @file       plan.c
+ * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014.
+ *
+ * @see        The GNU Public License (GPL) Version 3
+ *
+ ******************************************************************************/
+/*
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
+ * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
+ * for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ */
+
+#include <plans.h>
+#include <pios.h>
+#include <attitudesettings.h>
+#include <takeofflocation.h>
+#include <pathdesired.h>
+#include <positionstate.h>
+#include <flightmodesettings.h>
+
+/**
+ * @brief initialize UAVOs and structs used by this library
+ */
+void plan_initialize()
+{
+    TakeOffLocationInitialize();
+    PositionStateInitialize();
+    PathDesiredInitialize();
+    FlightModeSettingsInitialize();
+}
+
+/**
+ * @brief setup pathplanner/pathfollower for positionhold
+ */
+void plan_setup_positionHold()
+{
+    PositionStateData positionState;
+
+    PositionStateGet(&positionState);
+    PathDesiredData pathDesired;
+    PathDesiredGet(&pathDesired);
+
+    pathDesired.Start.North      = positionState.North;
+    pathDesired.Start.East       = positionState.East;
+    pathDesired.Start.Down       = positionState.Down;
+    pathDesired.End.North        = positionState.North;
+    pathDesired.End.East         = positionState.East;
+    pathDesired.End.Down         = positionState.Down;
+    pathDesired.StartingVelocity = 1;
+    pathDesired.EndingVelocity   = 0;
+    pathDesired.Mode = PATHDESIRED_MODE_FLYENDPOINT;
+
+    PathDesiredSet(&pathDesired);
+}
+
+/**
+ * @brief setup pathplanner/pathfollower for return to base
+ */
+void plan_setup_returnToBase()
+{
+    // Simple Return To Base mode - keep altitude the same applying configured delta, fly to takeoff position
+    float positionStateDown;
+
+    PositionStateDownGet(&positionStateDown);
+
+    PathDesiredData pathDesired;
+    PathDesiredGet(&pathDesired);
+
+    TakeOffLocationData takeoffLocation;
+    TakeOffLocationGet(&takeoffLocation);
+
+    // TODO: right now VTOLPF does fly straight to destination altitude.
+    // For a safer RTB destination altitude will be the higher between takeofflocation and current position (corrected with safety margin)
+
+    float destDown;
+    FlightModeSettingsReturnToBaseAltitudeOffsetGet(&destDown);
+    destDown -= MIN(positionStateDown, takeoffLocation.Down);
+
+    pathDesired.Start.North      = takeoffLocation.North;
+    pathDesired.Start.East       = takeoffLocation.East;
+    pathDesired.Start.Down       = destDown;
+
+    pathDesired.End.North        = takeoffLocation.North;
+    pathDesired.End.East         = takeoffLocation.East;
+    pathDesired.End.Down         = destDown;
+
+    pathDesired.StartingVelocity = 1;
+    pathDesired.EndingVelocity   = 0;
+    pathDesired.Mode = PATHDESIRED_MODE_FLYENDPOINT;
+
+    PathDesiredSet(&pathDesired);
+}
+
+void plan_setup_land()
+{
+    plan_setup_positionHold();
+}
+
+/**
+ * @brief execute land
+ */
+void plan_run_land()
+{
+    PathDesiredEndData pathDesiredEnd;
+
+    PathDesiredEndGet(&pathDesiredEnd);
+
+    PositionStateDownGet(&pathDesiredEnd.Down);
+    pathDesiredEnd.Down += 5;
+
+    PathDesiredEndSet(&pathDesiredEnd);
+}

flight/modules/ManualControl/pathfollowerhandler.c

@@ -34,10 +34,10 @@
 #include <flightstatus.h>
 #include <positionstate.h>
 #include <flightmodesettings.h>
-#include <pios_math.h>
 
 #if defined(REVOLUTION)
-#include <takeofflocation.h>
+#include <plans.h>
+
 // Private constants
 
 // Private types
@@ -52,56 +52,30 @@
 void pathFollowerHandler(bool newinit)
 {
     if (newinit) {
-        PathDesiredInitialize();
+        plan_initialize();
-        PositionStateInitialize();
     }
 
-    FlightStatusData flightStatus;
+    uint8_t flightMode;
-    FlightStatusGet(&flightStatus);
+    FlightStatusFlightModeGet(&flightMode);
 
     if (newinit) {
         // After not being in this mode for a while init at current height
-        PositionStateData positionState;
+        switch (flightMode) {
-        PositionStateGet(&positionState);
-        FlightModeSettingsData settings;
-        FlightModeSettingsGet(&settings);
-        PathDesiredData pathDesired;
-        PathDesiredGet(&pathDesired);
-        TakeOffLocationData takeoffLocation;
-        TakeOffLocationGet(&takeoffLocation);
-        switch (flightStatus.FlightMode) {
         case FLIGHTSTATUS_FLIGHTMODE_RETURNTOBASE:
-        {
+            plan_setup_returnToBase();
-            // Simple Return To Base mode - keep altitude the same applying configured delta, fly to takeoff position
+            break;
 
-            // TODO: right now VTOLPF does fly straight to destination altitude.
+        case FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD:
-            // For a safer RTB destination altitude will be the higher between takeofflocation and current position (corrected with safety margin)
+            plan_setup_positionHold();
-            float destDown = MIN(positionState.Down, takeoffLocation.Down) - settings.ReturnToBaseAltitudeOffset;
+            break;
 
-            pathDesired.Start.North      = takeoffLocation.North;
+        case FLIGHTSTATUS_FLIGHTMODE_LAND:
-            pathDesired.Start.East       = takeoffLocation.East;
+            plan_setup_land();
-            pathDesired.Start.Down       = destDown;
+            break;
 
-            pathDesired.End.North        = takeoffLocation.North;
-            pathDesired.End.East         = takeoffLocation.East;
-            pathDesired.End.Down         = destDown;
-
-            pathDesired.StartingVelocity = 1;
-            pathDesired.EndingVelocity   = 0;
-            pathDesired.Mode = PATHDESIRED_MODE_FLYENDPOINT;
-        }
-        break;
         default:
+            plan_setup_positionHold();
 
-            pathDesired.Start.North      = positionState.North;
-            pathDesired.Start.East       = positionState.East;
-            pathDesired.Start.Down       = positionState.Down;
-            pathDesired.End.North        = positionState.North;
-            pathDesired.End.East         = positionState.East;
-            pathDesired.End.Down         = positionState.Down;
-            pathDesired.StartingVelocity = 1;
-            pathDesired.EndingVelocity   = 0;
-            pathDesired.Mode = PATHDESIRED_MODE_FLYENDPOINT;
             /* Disable this section, until such time as proper discussion can be had about how to implement it for all types of crafts.
                } else {
                PathDesiredData pathDesired;
@@ -113,17 +87,15 @@ void pathFollowerHandler(bool newinit)
              */
             break;
         }
-        PathDesiredSet(&pathDesired);
     }
 
+    switch (flightMode) {
     // special handling of autoland - behaves like positon hold but with slow altitude decrease
-    if (flightStatus.FlightMode == FLIGHTSTATUS_FLIGHTMODE_LAND) {
+    case FLIGHTSTATUS_FLIGHTMODE_LAND:
-        PositionStateData positionState;
+        plan_run_land();
-        PositionStateGet(&positionState);
+        break;
-        PathDesiredData pathDesired;
+    default:
-        PathDesiredGet(&pathDesired);
+        break;
-        pathDesired.End.Down = positionState.Down + 5;
-        PathDesiredSet(&pathDesired);
     }
 }
 

flight/modules/PathPlanner/pathplanner.c

@@ -45,6 +45,7 @@
 #include "flightmodesettings.h"
 #include <pios_struct_helper.h>
 #include "paths.h"
+#include "plans.h"
 
 // Private constants
 #define STACK_SIZE_BYTES            1024
@@ -89,6 +90,7 @@ static bool pathplanner_active = false;
  */
 int32_t PathPlannerStart()
 {
+    plan_initialize();
     // when the active waypoint changes, update pathDesired
     WaypointConnectCallback(commandUpdated);
     WaypointActiveConnectCallback(commandUpdated);
@@ -169,21 +171,7 @@ static void pathPlannerTask()
         if (!failsafeRTHset) {
             failsafeRTHset = 1;
             // copy pasta: same calculation as in manualcontrol, set return to home coordinates
-            PositionStateData positionState;
+            plan_setup_positionHold();
-            PositionStateGet(&positionState);
-            FlightModeSettingsData settings;
-            FlightModeSettingsGet(&settings);
-
-            pathDesired.Start.North      = 0;
-            pathDesired.Start.East       = 0;
-            pathDesired.Start.Down       = positionState.Down - settings.ReturnToBaseAltitudeOffset;
-            pathDesired.End.North        = 0;
-            pathDesired.End.East         = 0;
-            pathDesired.End.Down         = positionState.Down - settings.ReturnToBaseAltitudeOffset;
-            pathDesired.StartingVelocity = 1;
-            pathDesired.EndingVelocity   = 0;
-            pathDesired.Mode = PATHDESIRED_MODE_FLYENDPOINT;
-            PathDesiredSet(&pathDesired);
         }
         AlarmsSet(SYSTEMALARMS_ALARM_PATHPLAN, SYSTEMALARMS_ALARM_ERROR);
 

flight/targets/boards/discoveryf4bare/firmware/Makefile

@@ -82,6 +82,7 @@ ifndef TESTAPP
 
     ## Misc library functions
     SRC += $(FLIGHTLIB)/paths.c
+	SRC += $(FLIGHTLIB)/plans.c
     SRC += $(FLIGHTLIB)/WorldMagModel.c
     SRC += $(FLIGHTLIB)/insgps13state.c
 

flight/targets/boards/revolution/firmware/Makefile

@@ -84,6 +84,7 @@ ifndef TESTAPP
 
     ## Misc library functions
     SRC += $(FLIGHTLIB)/paths.c
+    SRC += $(FLIGHTLIB)/plans.c
     SRC += $(FLIGHTLIB)/WorldMagModel.c
     SRC += $(FLIGHTLIB)/insgps13state.c
 

flight/targets/boards/revoproto/firmware/Makefile

@@ -83,6 +83,7 @@ ifndef TESTAPP
 
     ## Misc library functions
     SRC += $(FLIGHTLIB)/paths.c
+    SRC += $(FLIGHTLIB)/plans.c
     SRC += $(FLIGHTLIB)/WorldMagModel.c
     SRC += $(FLIGHTLIB)/insgps13state.c