Initial cut on storing waypoints in configuration. Not supported on F1 targets

docs/Controls.md

@@ -31,13 +31,12 @@ The stick positions are combined to activate different functions:
 | Calibrate Gyro                | LOW      | LOW     | LOW    | CENTER |
 | Calibrate Acc                 | HIGH     | LOW     | LOW    | CENTER |
 | Calibrate Mag/Compass         | HIGH     | HIGH    | LOW    | CENTER |
-| Inflight calibration controls | LOW      | LOW     | HIGH   | HIGH   |
 | Trim Acc Left                 | HIGH     | CENTER  | CENTER | LOW    |
 | Trim Acc Right                | HIGH     | CENTER  | CENTER | HIGH   |
 | Trim Acc Forwards             | HIGH     | CENTER  | HIGH   | CENTER |
 | Trim Acc Backwards            | HIGH     | CENTER  | LOW    | CENTER |
-| Disable LCD Page Cycling      | LOW      | CENTER  | HIGH   | LOW    |
+| Save current waypoint mission | LOW      | CENTER  | HIGH   | LOW    |
-| Enable LCD Page Cycling       | LOW      | CENTER  | HIGH   | HIGH   |
+| Load current waypoint mission | LOW      | CENTER  | HIGH   | HIGH   |
 | Save setting                  | LOW      | LOW     | LOW    | HIGH   |
 
 ![Stick Positions](assets/images/StickPositions.png)

src/main/config/parameter_group_ids.h

@@ -88,7 +88,8 @@
 #define PG_OSD_CONFIG 1004
 #define PG_BEEPER_CONFIG 1005
 #define PG_RANGEFINDER_CONFIG 1006
-#define PG_INAV_END 1006
+#define PG_WAYPOINT_MISSION_STORAGE 1007
+#define PG_INAV_END 1007
 
 // OSD configuration (subject to change)
 //#define PG_OSD_FONT_CONFIG 2047

src/main/fc/rc_controls.c

@@ -212,12 +212,27 @@ void processRcStickPositions(throttleStatus_e throttleStatus, bool disarm_kill_s
     // actions during not armed
     i = 0;
 
-    if (rcSticks == THR_LO + YAW_LO + PIT_LO + ROL_CE) {
     // GYRO calibration
+    if (rcSticks == THR_LO + YAW_LO + PIT_LO + ROL_CE) {
         gyroSetCalibrationCycles(CALIBRATING_GYRO_CYCLES);
         return;
     }
 
+
+#if defined(NAV_NON_VOLATILE_WAYPOINT_STORAGE)
+    // Save waypoint list
+    if (rcSticks == THR_LO + YAW_CE + PIT_HI + ROL_LO) {
+        const bool success = saveNonVolatileWaypointList();
+        beeper(success ? BEEPER_ACTION_SUCCESS : BEEPER_ACTION_FAIL);
+    }
+
+    // Load waypoint list
+    if (rcSticks == THR_LO + YAW_CE + PIT_HI + ROL_HI) {
+        const bool success = loadNonVolatileWaypointList();
+        beeper(success ? BEEPER_ACTION_SUCCESS : BEEPER_ACTION_FAIL);
+    }
+#endif
+    
     // Multiple configuration profiles
     if (rcSticks == THR_LO + YAW_LO + PIT_CE + ROL_LO)          // ROLL left  -> Profile 1
         i = 1;
@@ -230,17 +245,17 @@ void processRcStickPositions(throttleStatus_e throttleStatus, bool disarm_kill_s
         return;
     }
 
-
+    // Save config
     if (rcSticks == THR_LO + YAW_LO + PIT_LO + ROL_HI) {
         saveConfigAndNotify();
     }
 
 
+    // Arming by sticks
     if (isUsingSticksToArm) {
         if (STATE(FIXED_WING) && feature(FEATURE_MOTOR_STOP) && fixed_wing_auto_arm) {
             // Auto arm on throttle when using fixedwing and motorstop
             if (throttleStatus != THROTTLE_LOW) {
-                // Arm via YAW
                 mwArm();
                 return;
             }
@@ -255,15 +270,15 @@ void processRcStickPositions(throttleStatus_e throttleStatus, bool disarm_kill_s
     }
 
 
-    if (rcSticks == THR_HI + YAW_LO + PIT_LO + ROL_CE) {
     // Calibrating Acc
+    if (rcSticks == THR_HI + YAW_LO + PIT_LO + ROL_CE) {
         accSetCalibrationCycles(CALIBRATING_ACC_CYCLES);
         return;
     }
 
 
-    if (rcSticks == THR_HI + YAW_HI + PIT_LO + ROL_CE) {
     // Calibrating Mag
+    if (rcSticks == THR_HI + YAW_HI + PIT_LO + ROL_CE) {
         ENABLE_STATE(CALIBRATE_MAG);
         return;
     }

src/main/io/beeper.c

@@ -175,8 +175,8 @@ typedef struct beeperTableEntry_s {
     { BEEPER_ENTRY(BEEPER_BAT_LOW,               8, beep_lowBatteryBeep,   "BAT_LOW") },
     { BEEPER_ENTRY(BEEPER_GPS_STATUS,            9, beep_multiBeeps,       "GPS_STATUS") },
     { BEEPER_ENTRY(BEEPER_RX_SET,                10, beep_shortBeep,       "RX_SET") },
-    { BEEPER_ENTRY(BEEPER_ACC_CALIBRATION,       11, beep_2shortBeeps,     "ACC_CALIBRATION") },
+    { BEEPER_ENTRY(BEEPER_ACTION_SUCCESS,        11, beep_2shortBeeps,     "ACTION_SUCCESS") },
-    { BEEPER_ENTRY(BEEPER_ACC_CALIBRATION_FAIL,  12, beep_2longerBeeps,    "ACC_CALIBRATION_FAIL") },
+    { BEEPER_ENTRY(BEEPER_ACTION_FAIL,           12, beep_2longerBeeps,    "ACTION_FAIL") },
     { BEEPER_ENTRY(BEEPER_READY_BEEP,            13, beep_readyBeep,       "READY_BEEP") },
     { BEEPER_ENTRY(BEEPER_MULTI_BEEPS,           14, beep_multiBeeps,      "MULTI_BEEPS") }, // FIXME having this listed makes no sense since the beep array will not be initialised.
     { BEEPER_ENTRY(BEEPER_DISARM_REPEAT,         15, beep_disarmRepeatBeep, "DISARM_REPEAT") },

src/main/io/beeper.h

@@ -34,8 +34,8 @@ typedef enum {
     BEEPER_GPS_STATUS,              // FIXME **** Disable beeper when connected to USB ****
     BEEPER_RX_SET,                  // Beeps when aux channel is set for beep or beep sequence how many satellites has found if GPS enabled
     BEEPER_DISARM_REPEAT,           // Beeps sounded while stick held in disarm position
-    BEEPER_ACC_CALIBRATION,         // ACC inflight calibration completed confirmation
+    BEEPER_ACTION_SUCCESS,          // Action success (various actions)
-    BEEPER_ACC_CALIBRATION_FAIL,    // ACC inflight calibration failed
+    BEEPER_ACTION_FAIL,             // Action fail (varions actions)
     BEEPER_READY_BEEP,              // Ring a tone when GPS is locked and ready
     BEEPER_MULTI_BEEPS,             // Internal value used by 'beeperConfirmationBeeps()'.
     BEEPER_ARMED,                   // Warning beeps when board is armed (repeats until board is disarmed or throttle is increased)

src/main/navigation/navigation.c

@@ -60,6 +60,9 @@ uint16_t      GPS_distanceToHome;        // distance to home point in meters
 int16_t       GPS_directionToHome;       // direction to home point in degrees
 
 #if defined(NAV)
+PG_DECLARE_ARRAY(navWaypoint_t, NAV_MAX_WAYPOINTS, nonVolatileWaypointList);
+PG_REGISTER_ARRAY(navWaypoint_t, NAV_MAX_WAYPOINTS, nonVolatileWaypointList, PG_WAYPOINT_MISSION_STORAGE, 0);
+
 PG_REGISTER_WITH_RESET_TEMPLATE(navConfig_t, navConfig, PG_NAV_CONFIG, 0);
 
 PG_RESET_TEMPLATE(navConfig_t, navConfig,
@@ -2234,6 +2237,46 @@ void resetWaypointList(void)
     }
 }
 
+#ifdef NAV_NON_VOLATILE_WAYPOINT_STORAGE
+bool loadNonVolatileWaypointList(void)
+{
+    if (ARMING_FLAG(ARMED))
+        return false;
+
+    resetWaypointList();
+
+    for (int i = 0; i < NAV_MAX_WAYPOINTS; i++) {
+        // Load waypoint 
+        setWaypoint(i + 1, nonVolatileWaypointList(i));
+
+        // Check if this is the last waypoint
+        if (nonVolatileWaypointList(i)->flag == NAV_WP_FLAG_LAST)
+            break;
+    }
+
+    // Mission sanity check failed - reset the list
+    if (!posControl.waypointListValid) {
+        resetWaypointList();
+    }
+
+    return posControl.waypointListValid;
+}
+
+bool saveNonVolatileWaypointList(void)
+{
+    if (ARMING_FLAG(ARMED) || !posControl.waypointListValid)
+        return false;
+
+    for (int i = 0; i < NAV_MAX_WAYPOINTS; i++) {
+        getWaypoint(i + 1, nonVolatileWaypointListMutable(i));
+    }
+
+    saveConfigAndNotify();
+
+    return true;
+}
+#endif
+
 static void mapWaypointToLocalPosition(t_fp_vector * localPos, const navWaypoint_t * waypoint)
 {
     gpsLocation_t wpLLH;

src/main/navigation/navigation.h

@@ -267,6 +267,8 @@ int32_t getTotalTravelDistance(void);
 void getWaypoint(uint8_t wpNumber, navWaypoint_t * wpData);
 void setWaypoint(uint8_t wpNumber, const navWaypoint_t * wpData);
 void resetWaypointList(void);
+bool loadNonVolatileWaypointList(void);
+bool saveNonVolatileWaypointList(void);
 
 /* Geodetic functions */
 typedef enum {

src/main/target/common.h

@@ -66,6 +66,7 @@
 #define GPS_PROTO_NAZA
 #define NAV_AUTO_MAG_DECLINATION
 #define NAV_GPS_GLITCH_DETECTION
+#define NAV_NON_VOLATILE_WAYPOINT_STORAGE
 #define TELEMETRY_HOTT
 #define TELEMETRY_IBUS
 #define TELEMETRY_MAVLINK