diff --git a/docs/Controls.md b/docs/Controls.md index e302a7f253..4afbd6f1da 100644 --- a/docs/Controls.md +++ b/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 | -| Enable LCD Page Cycling | LOW | CENTER | HIGH | HIGH | +| Save current waypoint mission | LOW | CENTER | HIGH | LOW | +| Load current waypoint mission | LOW | CENTER | HIGH | HIGH | | Save setting | LOW | LOW | LOW | HIGH | ![Stick Positions](assets/images/StickPositions.png) diff --git a/src/main/config/parameter_group_ids.h b/src/main/config/parameter_group_ids.h index 08bf1f4020..76a648650b 100644 --- a/src/main/config/parameter_group_ids.h +++ b/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 diff --git a/src/main/fc/rc_controls.c b/src/main/fc/rc_controls.c index 7484c1a93d..540f82ed10 100644 --- a/src/main/fc/rc_controls.c +++ b/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; + // GYRO calibration if (rcSticks == THR_LO + YAW_LO + PIT_LO + ROL_CE) { - // GYRO calibration 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 } + // Calibrating Acc if (rcSticks == THR_HI + YAW_LO + PIT_LO + ROL_CE) { - // Calibrating Acc accSetCalibrationCycles(CALIBRATING_ACC_CYCLES); return; } + // Calibrating Mag if (rcSticks == THR_HI + YAW_HI + PIT_LO + ROL_CE) { - // Calibrating Mag ENABLE_STATE(CALIBRATE_MAG); return; } diff --git a/src/main/io/beeper.c b/src/main/io/beeper.c index 580ea6c72e..3b8fe483b2 100644 --- a/src/main/io/beeper.c +++ b/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_ACC_CALIBRATION_FAIL, 12, beep_2longerBeeps, "ACC_CALIBRATION_FAIL") }, + { BEEPER_ENTRY(BEEPER_ACTION_SUCCESS, 11, beep_2shortBeeps, "ACTION_SUCCESS") }, + { 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") }, diff --git a/src/main/io/beeper.h b/src/main/io/beeper.h index 95b607ce12..fd900ebda3 100644 --- a/src/main/io/beeper.h +++ b/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_ACC_CALIBRATION_FAIL, // ACC inflight calibration failed + BEEPER_ACTION_SUCCESS, // Action success (various actions) + 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) diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index a1e135eea9..2838c508e0 100755 --- a/src/main/navigation/navigation.c +++ b/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; diff --git a/src/main/navigation/navigation.h b/src/main/navigation/navigation.h index a85948ce60..af36a6f662 100755 --- a/src/main/navigation/navigation.h +++ b/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 { diff --git a/src/main/target/common.h b/src/main/target/common.h index 35aa32dffd..197c4f81b6 100644 --- a/src/main/target/common.h +++ b/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