Waypoint code rework. Upload/download mission works. Waypoint actions RTH and WAYPOINT are supported (untested)

src/main/config/config.c

@@ -233,8 +233,6 @@ void resetNavConfig(navConfig_t * navConfig)
     navConfig->inav.w_xy_gps_p = 1.0f;
     navConfig->inav.w_xy_gps_v = 2.0f;
 
-    navConfig->inav.w_xy_dr_v = 0.25f;
-
     navConfig->inav.w_z_res_v = 0.5f;
     navConfig->inav.w_xy_res_v = 0.5f;
 

src/main/flight/navigation_rewrite.c

@@ -74,6 +74,7 @@ static void setupAltitudeController(void);
 void resetNavigation(void);
 
 static void calcualteAndSetActiveWaypoint(navWaypoint_t * waypoint);
+static void calcualteAndSetActiveWaypointToLocalPosition(t_fp_vector * pos);
 void calculateInitialHoldPosition(t_fp_vector * pos);
 
 /*************************************************************************************************/
@@ -99,6 +100,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_3D_LANDING(navigati
 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_3D_FINISHING(navigationFSMState_t previousState);
 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_3D_FINISHED(navigationFSMState_t previousState);
 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_INITIALIZE(navigationFSMState_t previousState);
+static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_PRE_ACTION(navigationFSMState_t previousState);
 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_IN_PROGRESS(navigationFSMState_t previousState);
 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_REACHED(navigationFSMState_t previousState);
 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_FINISHED(navigationFSMState_t previousState);
@@ -418,6 +420,19 @@ static navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
         .timeoutMs = 0,
         .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_WP,
         .mapToFlightModes = NAV_WP_MODE | NAV_ALTHOLD_MODE,
+        .onEvent = {
+            [NAV_FSM_EVENT_SUCCESS]                     = NAV_STATE_WAYPOINT_PRE_ACTION,
+            [NAV_FSM_EVENT_ERROR]                       = NAV_STATE_IDLE,
+            [NAV_FSM_EVENT_SWITCH_TO_IDLE]              = NAV_STATE_IDLE,
+            [NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_FINISHED] = NAV_STATE_WAYPOINT_FINISHED,
+        }
+    },
+
+    [NAV_STATE_WAYPOINT_PRE_ACTION] = {
+        .onEntry = navOnEnteringState_NAV_STATE_WAYPOINT_PRE_ACTION,
+        .timeoutMs = 0,
+        .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_WP,
+        .mapToFlightModes = NAV_WP_MODE | NAV_ALTHOLD_MODE,
         .onEvent = {
             [NAV_FSM_EVENT_SUCCESS]                     = NAV_STATE_WAYPOINT_IN_PROGRESS,
             [NAV_FSM_EVENT_ERROR]                       = NAV_STATE_IDLE,
@@ -449,7 +464,7 @@ static navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
         .stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_WP,
         .mapToFlightModes = NAV_WP_MODE | NAV_ALTHOLD_MODE,
         .onEvent = {
-            [NAV_FSM_EVENT_SUCCESS]                     = NAV_STATE_WAYPOINT_IN_PROGRESS,
+            [NAV_FSM_EVENT_SUCCESS]                     = NAV_STATE_WAYPOINT_PRE_ACTION,
             [NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_FINISHED] = NAV_STATE_WAYPOINT_FINISHED,
             [NAV_FSM_EVENT_SWITCH_TO_IDLE]              = NAV_STATE_IDLE,
             [NAV_FSM_EVENT_SWITCH_TO_ALTHOLD]           = NAV_STATE_ALTHOLD_INITIALIZE,
@@ -828,12 +843,27 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_INITIALIZE(nav
         setupAltitudeController();
 
         posControl.activeWaypointIndex = 0;
-
+        return NAV_FSM_EVENT_SUCCESS;   // will switch to NAV_STATE_WAYPOINT_PRE_ACTION
-        calcualteAndSetActiveWaypoint(&posControl.waypointList[posControl.activeWaypointIndex]);
-        return NAV_FSM_EVENT_SUCCESS;
     }
 }
 
+static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_PRE_ACTION(navigationFSMState_t previousState)
+{
+    /* A helper function to do waypoint-specific action */
+    UNUSED(previousState);
+
+    switch (posControl.waypointList[posControl.activeWaypointIndex].action) {
+        case NAV_WP_ACTION_WAYPOINT:
+            calcualteAndSetActiveWaypoint(&posControl.waypointList[posControl.activeWaypointIndex]);
+            return NAV_FSM_EVENT_SUCCESS;       // will switch to NAV_STATE_WAYPOINT_IN_PROGRESS
+
+        case NAV_WP_ACTION_RTH:
+        default:
+            calcualteAndSetActiveWaypointToLocalPosition(&posControl.homeWaypointAbove.pos);
+            return NAV_FSM_EVENT_SUCCESS;       // will switch to NAV_STATE_WAYPOINT_IN_PROGRESS
+    };
+}
+
 static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_IN_PROGRESS(navigationFSMState_t previousState)
 {
     UNUSED(previousState);
@@ -843,14 +873,19 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_IN_PROGRESS(na
         return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING;
     }
 
-    if (isWaypointReached(&posControl.activeWaypoint) || isWaypointMissed(&posControl.activeWaypoint)) {
+    switch (posControl.waypointList[posControl.activeWaypointIndex].action) {
-        // Waypoint reached
+        case NAV_WP_ACTION_WAYPOINT:
-        return NAV_FSM_EVENT_SUCCESS;
+        case NAV_WP_ACTION_RTH:
-    }
+        default:
-    else {
+            if (isWaypointReached(&posControl.activeWaypoint) || isWaypointMissed(&posControl.activeWaypoint)) {
-        // Update XY-position target to active waypoint
+                // Waypoint reached
-        setDesiredPosition(&posControl.activeWaypoint.pos, 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_BEARING);
+                return NAV_FSM_EVENT_SUCCESS;   // will switch to NAV_STATE_WAYPOINT_REACHED
-        return NAV_FSM_EVENT_NONE;
+            }
+            else {
+                // Update XY-position target to active waypoint
+                setDesiredPosition(&posControl.activeWaypoint.pos, 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_BEARING);
+                return NAV_FSM_EVENT_NONE;      // will re-process state in >10ms
+            }
     }
 }
 
@@ -862,14 +897,13 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_REACHED(naviga
                           (posControl.activeWaypointIndex >= (posControl.waypointCount - 1));
 
     if (isLastWaypoint) {
-        // This is the last waypoint - finalize
+        // Last waypoint reached
         return NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_FINISHED;
     }
     else {
         // Waypoint reached, do something and move on to next waypoint
         posControl.activeWaypointIndex++;
-        calcualteAndSetActiveWaypoint(&posControl.waypointList[posControl.activeWaypointIndex]);
+        return NAV_FSM_EVENT_SUCCESS;   // will switch to NAV_STATE_WAYPOINT_PRE_ACTION
-        return NAV_FSM_EVENT_SUCCESS;
     }
 }
 
@@ -1527,7 +1561,7 @@ void getWaypoint(uint8_t wpNumber, navWaypoint_t * wpData)
         wpData->alt = wpLLH.alt;
     }
     // WP #1 - #15 - common waypoints - pre-programmed mission
-    else if ((wpNumber >= 1) && (wpNumber <= NAV_MAX_WAYPOINTS) && posControl.waypointListValid) {
+    else if ((wpNumber >= 1) && (wpNumber <= NAV_MAX_WAYPOINTS)) {
         if (wpNumber <= posControl.waypointCount) {
             *wpData = posControl.waypointList[wpNumber - 1];
         }
@@ -1583,21 +1617,28 @@ void setWaypoint(uint8_t wpNumber, navWaypoint_t * wpData)
     }
 }
 
+static void calcualteAndSetActiveWaypointToLocalPosition(t_fp_vector * pos)
+{
+    posControl.activeWaypoint.pos = *pos;
+
+    // Calculate initial bearing towards waypoint and store it in waypoint yaw parameter (this will further be used to detect missed waypoints)
+    posControl.activeWaypoint.yaw = calculateBearingToDestination(pos);
+
+    // Set desired position to next waypoint (XYZ-controller)
+    setDesiredPosition(&posControl.activeWaypoint.pos, posControl.activeWaypoint.yaw, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING);
+}
+
 static void calcualteAndSetActiveWaypoint(navWaypoint_t * waypoint)
 {
     gpsLocation_t wpLLH;
+    t_fp_vector localPos;
 
     wpLLH.lat = waypoint->lat;
     wpLLH.lon = waypoint->lon;
     wpLLH.alt = waypoint->alt;
 
-    geoConvertGeodeticToLocal(&posControl.gpsOrigin, &wpLLH, &posControl.activeWaypoint.pos);
+    geoConvertGeodeticToLocal(&posControl.gpsOrigin, &wpLLH, &localPos);
-
+    calcualteAndSetActiveWaypointToLocalPosition(&localPos);
-    // Calculate initial bearing towards waypoint and store it in waypoint yaw parameter (this will further be used to detect missed waypoints)
-    posControl.activeWaypoint.yaw = calculateBearingToDestination(&posControl.activeWaypoint.pos);
-
-    // Set desired position to next waypoint (XYZ-controller)
-    setDesiredPosition(&posControl.activeWaypoint.pos, posControl.activeWaypoint.yaw, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING);
 }
 
 /**

src/main/flight/navigation_rewrite.h

@@ -83,8 +83,6 @@ typedef struct navConfig_s {
         float w_xy_gps_p;   // Weight (cutoff frequency) for GPS position measurements
         float w_xy_gps_v;   // Weight (cutoff frequency) for GPS velocity measurements
 
-        float w_xy_dr_v;    // When we are using dead reckoning, loosely assume that we don't move and stay at the same place
-
         float w_z_res_v;    // When velocity sources lost slowly decrease estimated velocity with this weight
         float w_xy_res_v;
 
@@ -109,9 +107,9 @@ typedef struct navConfig_s {
     uint16_t fw_cruise_throttle;            // Cruise throttle
     uint16_t fw_min_throttle;               // Minimum allowed throttle in auto mode
     uint16_t fw_max_throttle;               // Maximum allowed throttle in auto mode
-    uint8_t fw_max_bank_angle;              // Fixed wing max banking angle (deg)
+    uint8_t  fw_max_bank_angle;             // Fixed wing max banking angle (deg)
-    uint8_t fw_max_climb_angle;             // Fixed wing max banking angle (deg)
+    uint8_t  fw_max_climb_angle;            // Fixed wing max banking angle (deg)
-    uint8_t fw_max_dive_angle;              // Fixed wing max banking angle (deg)
+    uint8_t  fw_max_dive_angle;             // Fixed wing max banking angle (deg)
     uint8_t  fw_pitch_to_throttle;          // Pitch angle (in deg) to throttle gain (in 1/1000's of throttle) (*10)
 } navConfig_t;
 

src/main/flight/navigation_rewrite_private.h

@@ -174,13 +174,14 @@ typedef enum {
     NAV_STATE_RTH_3D_FINISHED,                  // 21
 
     NAV_STATE_WAYPOINT_INITIALIZE,              // 22
-    NAV_STATE_WAYPOINT_IN_PROGRESS,             // 23
+    NAV_STATE_WAYPOINT_PRE_ACTION,              // 23
-    NAV_STATE_WAYPOINT_REACHED,                 // 24
+    NAV_STATE_WAYPOINT_IN_PROGRESS,             // 24
-    NAV_STATE_WAYPOINT_FINISHED,                // 25
+    NAV_STATE_WAYPOINT_REACHED,                 // 25
+    NAV_STATE_WAYPOINT_FINISHED,                // 26
 
-    NAV_STATE_EMERGENCY_LANDING_INITIALIZE,     // 26
+    NAV_STATE_EMERGENCY_LANDING_INITIALIZE,     // 27
-    NAV_STATE_EMERGENCY_LANDING_IN_PROGRESS,    // 27
+    NAV_STATE_EMERGENCY_LANDING_IN_PROGRESS,    // 28
-    NAV_STATE_EMERGENCY_LANDING_FINISHED,       // 28
+    NAV_STATE_EMERGENCY_LANDING_FINISHED,       // 29
 
     NAV_STATE_COUNT,
 } navigationFSMState_t;

src/main/io/serial_cli.c

@@ -544,7 +544,6 @@ const clivalue_t valueTable[] = {
     { "inav_w_z_gps_v",             VAR_FLOAT  | MASTER_VALUE, &masterConfig.navConfig.inav.w_z_gps_v, .config.minmax = { 0,  10 }, 0 },
     { "inav_w_xy_gps_p",            VAR_FLOAT  | MASTER_VALUE, &masterConfig.navConfig.inav.w_xy_gps_p, .config.minmax = { 0,  10 }, 0 },
     { "inav_w_xy_gps_v",            VAR_FLOAT  | MASTER_VALUE, &masterConfig.navConfig.inav.w_xy_gps_v, .config.minmax = { 0,  10 }, 0 },
-    { "inav_w_xy_dr_v",             VAR_FLOAT  | MASTER_VALUE, &masterConfig.navConfig.inav.w_xy_dr_v, .config.minmax = { 0,  10 }, 0 },
     { "inav_w_z_res_v",             VAR_FLOAT  | MASTER_VALUE, &masterConfig.navConfig.inav.w_z_res_v, .config.minmax = { 0,  10 }, 0 },
     { "inav_w_xy_res_v",            VAR_FLOAT  | MASTER_VALUE, &masterConfig.navConfig.inav.w_xy_res_v, .config.minmax = { 0,  10 }, 0 },
     { "inav_w_acc_bias",            VAR_FLOAT  | MASTER_VALUE, &masterConfig.navConfig.inav.w_acc_bias, .config.minmax = { 0,  1 }, 0 },