Mirror/inav
1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-24 00:35:34 +03:00
Code Issues Wiki Activity

Merge pull request #1256 from iNavFlight/non-volatile-wp-mission

Browse source 
Non-volatile storage for waypoint missions
This commit is contained in:
Konstantin Sharlaimov 2017-02-10 00:34:28 +10:00 • committed by GitHub
commit c305dba225
10 changed files with 93 additions and 13 deletions
Download patch file Download diff file Expand all files Collapse all files

5
docs/Controls.md
View file

@ -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)

3
src/main/config/parameter_group_ids.h
View file

@ -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

16
src/main/fc/fc_msp.c
View file

@ -1980,6 +1980,22 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
break;
#endif
#ifdef NAV_NON_VOLATILE_WAYPOINT_STORAGE
case MSP_WP_MISSION_LOAD:
sbufReadU8(src); // Mission ID (reserved)
if (!loadNonVolatileWaypointList()) {
return MSP_RESULT_ERROR;
}
break;
case MSP_WP_MISSION_SAVE:
sbufReadU8(src); // Mission ID (reserved)
if (!saveNonVolatileWaypointList()) {
return MSP_RESULT_ERROR;
}
break;
#endif
default:
return MSP_RESULT_ERROR;
}

25
src/main/fc/rc_controls.c
View file

@ -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;
}

4
src/main/io/beeper.c
View file

@ -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") },

4
src/main/io/beeper.h
View file

@ -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)

3
src/main/msp/msp_protocol.h
View file

@ -114,6 +114,9 @@
#define MSP_POSITION_ESTIMATION_CONFIG 16
#define MSP_SET_POSITION_ESTIMATION_CONFIG 17
#define MSP_WP_MISSION_LOAD 18 // Load mission from NVRAM
#define MSP_WP_MISSION_SAVE 19 // Save mission to NVRAM
//
// MSP commands for Cleanflight original features
//

43
src/main/navigation/navigation.c
View file

@ -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;

2
src/main/navigation/navigation.h
View file

@ -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 {

1
src/main/target/common.h
View file

@ -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