support SET_HEAD and SET_POI (#5851)

* support SET_HEAD and SET_POI

* [DOC] update Navigation.md for SET_POI and SET_HEAD

docs/Navigation.md

@@ -58,12 +58,14 @@ When deciding what altitude to maintain, RTH has 4 different modes of operation
 
 Parameters:
 
-  * `<action>` - The action to be taken at the WP. The following are enumerations are available in inav 2.5 and later:
+  * `<action>` - The action to be taken at the WP. The following are enumerations are available in inav 2.6 and later:
       *  0 - Unused / Unassigned
       *  1 - WAYPOINT
       *  3 - POSHOLD_TIME
       *  4 - RTH
+	  *  5 - SET_POI
       *  6 - JUMP
+      *  7 - SET_HEAD
       *  8 - LAND
 
   * `<lat>` - Latitude (WGS84), in degrees * 1E7 (for example 123456789 means 12.3456789).
@@ -72,7 +74,7 @@ Parameters:
 
   * `<alt>` - Altitude in cm.
 
-  * `<p1>` - For a RTH waypoint, p1 > 0 enables landing. For a normal waypoint it is the speed to this waypoint (cm/s), it is taken into account only for multicopters and when > 50 and < nav_auto_speed. For POSHOLD TIME waypoint it is time to loiter in seconds. For JUMP it is the target WP **index** (not number).
+  * `<p1>` - For a RTH waypoint, p1 > 0 enables landing. For a normal waypoint it is the speed to this waypoint (cm/s), it is taken into account only for multicopters and when > 50 and < nav_auto_speed. For POSHOLD TIME waypoint it is time to loiter in seconds. For JUMP it is the target WP **index** (not number). For SET_HEAD, it is the desired heading (0-359) or -1 to cancel a previous SET_HEAD or SET_POI.
 
   * `<p2>` - For a POSHOLD TIME it is the speed to this waypoint (cm/s), it is taken into account only for multicopters and when > 50 and < nav_auto_speed. For JUMP it is the number of iterations of the JUMP.
 

src/main/fc/cli.c

@@ -1338,7 +1338,7 @@ static void cliWaypoints(char *cmdline)
     } else if (sl_strcasecmp(cmdline, "save") == 0) {
         posControl.waypointListValid = false;
         for (int i = 0; i < NAV_MAX_WAYPOINTS; i++) {
-            if (!(posControl.waypointList[i].action == NAV_WP_ACTION_WAYPOINT || posControl.waypointList[i].action == NAV_WP_ACTION_JUMP || posControl.waypointList[i].action == NAV_WP_ACTION_RTH || posControl.waypointList[i].action == NAV_WP_ACTION_HOLD_TIME || posControl.waypointList[i].action == NAV_WP_ACTION_LAND)) break;
+            if (!(posControl.waypointList[i].action == NAV_WP_ACTION_WAYPOINT || posControl.waypointList[i].action == NAV_WP_ACTION_JUMP || posControl.waypointList[i].action == NAV_WP_ACTION_RTH || posControl.waypointList[i].action == NAV_WP_ACTION_HOLD_TIME || posControl.waypointList[i].action == NAV_WP_ACTION_LAND || posControl.waypointList[i].action == NAV_WP_ACTION_SET_POI || posControl.waypointList[i].action == NAV_WP_ACTION_SET_HEAD)) break;
             if (posControl.waypointList[i].flag == NAV_WP_FLAG_LAST) {
                 posControl.waypointCount = i + 1;
                 posControl.waypointListValid = true;

src/main/navigation/navigation.c

@@ -172,6 +172,7 @@ PG_RESET_TEMPLATE(navConfig_t, navConfig,
     }
 );
 
+static navWapointHeading_t wpHeadingControl;
 navigationPosControl_t  posControl;
 navSystemStatus_t       NAV_Status;
 
@@ -208,6 +209,8 @@ void calculateInitialHoldPosition(fpVector3_t * pos);
 void calculateFarAwayTarget(fpVector3_t * farAwayPos, int32_t yaw, int32_t distance);
 void calculateNewCruiseTarget(fpVector3_t * origin, int32_t yaw, int32_t distance);
 static bool isWaypointPositionReached(const fpVector3_t * pos, const bool isWaypointHome);
+static void mapWaypointToLocalPosition(fpVector3_t * localPos, const navWaypoint_t * waypoint);
+static navigationFSMEvent_t nextForNonGeoStates(void);
 
 void initializeRTHSanityChecker(const fpVector3_t * pos);
 bool validateRTHSanityChecker(void);
@@ -1415,6 +1418,21 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_INITIALIZE(nav
     }
 }
 
+static navigationFSMEvent_t nextForNonGeoStates(void)
+{
+        /* simple helper for non-geographical states that just set other data */
+    const bool isLastWaypoint = (posControl.waypointList[posControl.activeWaypointIndex].flag == NAV_WP_FLAG_LAST) || (posControl.activeWaypointIndex >= (posControl.waypointCount - 1));
+
+    if (isLastWaypoint) {
+            // non-geo state is the last waypoint, switch to finish.
+        return NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_FINISHED;
+    } else {
+            // Finished non-geo,  move to next WP
+        posControl.activeWaypointIndex++;
+        return NAV_FSM_EVENT_NONE; // re-process the state passing to the next WP
+    }
+}
+
 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_PRE_ACTION(navigationFSMState_t previousState)
 {
     /* A helper function to do waypoint-specific action */
@@ -1434,28 +1452,36 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_PRE_ACTION(nav
             if(posControl.waypointList[posControl.activeWaypointIndex].p3 != -1){
                 if(posControl.waypointList[posControl.activeWaypointIndex].p3 == 0){
                     resetJumpCounter();
-                    const bool isLastWaypoint = (posControl.waypointList[posControl.activeWaypointIndex].flag == NAV_WP_FLAG_LAST) ||
+                    return nextForNonGeoStates();
-                          (posControl.activeWaypointIndex >= (posControl.waypointCount - 1));
-
-                        if (isLastWaypoint) {
-                            // JUMP is the last waypoint and we reached the last jump, switch to finish.
-                            return NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_FINISHED;
-                        } else {
-                                // Finished JUMP, move to next WP
-                            posControl.activeWaypointIndex++;
-                            return NAV_FSM_EVENT_NONE; // re-process the state passing to the next WP
-                        }
                 }
                 else
                 {
                     posControl.waypointList[posControl.activeWaypointIndex].p3--;
                 }
             }
-
             posControl.activeWaypointIndex = posControl.waypointList[posControl.activeWaypointIndex].p1;
-
             return NAV_FSM_EVENT_NONE; // re-process the state passing to the next WP
 
+        case NAV_WP_ACTION_SET_POI:
+            if (STATE(MULTIROTOR)) {
+                wpHeadingControl.mode = NAV_WP_HEAD_MODE_POI;
+                mapWaypointToLocalPosition(&wpHeadingControl.poi_pos,
+                                           &posControl.waypointList[posControl.activeWaypointIndex]);
+            }
+            return nextForNonGeoStates();
+
+        case NAV_WP_ACTION_SET_HEAD:
+            if (STATE(MULTIROTOR)) {
+                if (posControl.waypointList[posControl.activeWaypointIndex].p1 < 0 ||
+                    posControl.waypointList[posControl.activeWaypointIndex].p1 > 359) {
+                    wpHeadingControl.mode = NAV_WP_HEAD_MODE_NONE;
+                } else {
+                    wpHeadingControl.mode = NAV_WP_HEAD_MODE_FIXED;
+                    wpHeadingControl.heading = DEGREES_TO_CENTIDEGREES(posControl.waypointList[posControl.activeWaypointIndex].p1);
+                }
+            }
+            return nextForNonGeoStates();
+
         case NAV_WP_ACTION_RTH:
             posControl.rthState.rthInitialDistance = posControl.homeDistance;
             initializeRTHSanityChecker(&navGetCurrentActualPositionAndVelocity()->pos);
@@ -1487,11 +1513,25 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_IN_PROGRESS(na
                         posControl.wpInitialDistance, posControl.wpInitialDistance / 10.0f,
                         posControl.wpInitialAltitude, posControl.activeWaypoint.pos.z);
                     setDesiredPosition(&tmpWaypoint, 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_BEARING);
+                    if(STATE(MULTIROTOR)) {
+                        switch (wpHeadingControl.mode) {
+                            case NAV_WP_HEAD_MODE_NONE:
+                                break;
+                            case NAV_WP_HEAD_MODE_FIXED:
+                                setDesiredPosition(NULL, wpHeadingControl.heading, NAV_POS_UPDATE_HEADING);
+                                break;
+                            case NAV_WP_HEAD_MODE_POI:
+                                setDesiredPosition(&wpHeadingControl.poi_pos, 0, NAV_POS_UPDATE_BEARING);
+                                break;
+                        }
+                    }
                     return NAV_FSM_EVENT_NONE;      // will re-process state in >10ms
                 }
                 break;
 
             case NAV_WP_ACTION_JUMP:
+            case NAV_WP_ACTION_SET_HEAD:
+            case NAV_WP_ACTION_SET_POI:
                  UNREACHABLE();
 
             case NAV_WP_ACTION_RTH:
@@ -1529,6 +1569,8 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_REACHED(naviga
             return NAV_FSM_EVENT_SUCCESS;   // NAV_STATE_WAYPOINT_NEXT
 
         case NAV_WP_ACTION_JUMP:
+        case NAV_WP_ACTION_SET_HEAD:
+        case NAV_WP_ACTION_SET_POI:
             UNREACHABLE();
 
         case NAV_WP_ACTION_RTH:
@@ -2753,7 +2795,7 @@ void setWaypoint(uint8_t wpNumber, const navWaypoint_t * wpData)
     }
     // WP #1 - #NAV_MAX_WAYPOINTS - common waypoints - pre-programmed mission
     else if ((wpNumber >= 1) && (wpNumber <= NAV_MAX_WAYPOINTS) && !ARMING_FLAG(ARMED)) {
-        if (wpData->action == NAV_WP_ACTION_WAYPOINT || wpData->action == NAV_WP_ACTION_JUMP || wpData->action == NAV_WP_ACTION_RTH || wpData->action == NAV_WP_ACTION_HOLD_TIME || wpData->action == NAV_WP_ACTION_LAND) {
+        if (wpData->action == NAV_WP_ACTION_WAYPOINT || wpData->action == NAV_WP_ACTION_JUMP || wpData->action == NAV_WP_ACTION_RTH || wpData->action == NAV_WP_ACTION_HOLD_TIME || wpData->action == NAV_WP_ACTION_LAND || wpData->action == NAV_WP_ACTION_SET_POI || wpData->action == NAV_WP_ACTION_SET_HEAD ) {
             // Only allow upload next waypoint (continue upload mission) or first waypoint (new mission)
             if (wpNumber == (posControl.waypointCount + 1) || wpNumber == 1) {
                 posControl.waypointList[wpNumber - 1] = *wpData;

src/main/navigation/navigation.h

@@ -237,10 +237,18 @@ typedef enum {
     NAV_WP_ACTION_WAYPOINT  = 0x01,
     NAV_WP_ACTION_HOLD_TIME = 0x03,
     NAV_WP_ACTION_RTH       = 0x04,
+    NAV_WP_ACTION_SET_POI   = 0x05,
     NAV_WP_ACTION_JUMP      = 0x06,
+    NAV_WP_ACTION_SET_HEAD  = 0x07,
     NAV_WP_ACTION_LAND      = 0x08
 } navWaypointActions_e;
 
+typedef enum {
+    NAV_WP_HEAD_MODE_NONE  = 0,
+    NAV_WP_HEAD_MODE_POI   = 1,
+    NAV_WP_HEAD_MODE_FIXED = 2
+} navWaypointHeadings_e;
+
 typedef enum {
     NAV_WP_FLAG_LAST = 0xA5
 } navWaypointFlags_e;
@@ -254,6 +262,12 @@ typedef struct {
     uint8_t flag;
 } navWaypoint_t;
 
+typedef struct {
+    navWaypointHeadings_e  mode;
+    uint32_t heading; // fixed heading * 100 (SET_HEAD)
+    fpVector3_t poi_pos; // POI location in local coordinates (SET_POI)
+} navWapointHeading_t;
+
 typedef struct radar_pois_s {
     gpsLocation_t gps;
     uint8_t state;