Mirror/inav
1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-26 09:45:33 +03:00
Code Issues Wiki Activity

Merge remote-tracking branch 'upstream/master' into abo_fixed_wing_soaring_mode

Browse source
This commit is contained in:
breadoven 2021-11-07 16:40:19 +00:00
commit 896f2da5bf
9 changed files with 208 additions and 26 deletions
Download patch file Download diff file Expand all files Collapse all files

30
docs/Settings.md
View file

@ -3712,6 +3712,16 @@ Minimum distance from homepoint when RTH full procedure will be activated [cm].
---
### nav_mission_planner_reset
With Reset ON WP Mission Planner waypoint count can be reset to 0 by toggling the mode switch ON-OFF-ON.
| Default | Min | Max |
| --- | --- | --- |
| ON | | |
---
### nav_overrides_motor_stop
When set to OFF the navigation system will not take over the control of the motor if the throttle is low (motor will stop). When set to OFF_ALWAYS the navigation system will not take over the control of the motor if the throttle was low even when failsafe is triggered. When set to AUTO_ONLY the navigation system will only take over the control of the throttle in autonomous navigation modes (NAV WP and NAV RTH). When set to ALL_NAV (default) the navigation system will take over the control of the motor completely and never allow the motor to stop even when the throttle is low. This setting only has an effect on NAV modes which take control of the throttle when combined with MOTOR_STOP and is likely to cause a stall if fw_min_throttle_down_pitch isn't set correctly or the pitch estimation is wrong for fixed wing models when not set to ALL_NAV
@ -4002,6 +4012,26 @@ AHI width in pixels (pixel OSD only)
---
### osd_airspeed_alarm_max
Airspeed above which the airspeed OSD element will start blinking (cm/s)
| Default | Min | Max |
| --- | --- | --- |
| 0 | 0 | 27000 |
---
### osd_airspeed_alarm_min
Airspeed under which the airspeed OSD element will start blinking (cm/s)
| Default | Min | Max |
| --- | --- | --- |
| 0 | 0 | 27000 |
---
### osd_alt_alarm
Value above which to make the OSD relative altitude indicator blink (meters)

5
src/main/fc/fc_msp_box.c
View file

@ -92,7 +92,8 @@ static const box_t boxes[CHECKBOX_ITEM_COUNT + 1] = {
{ BOXTURTLE, "TURTLE", 52 },
{ BOXNAVCRUISE, "NAV CRUISE", 53 },
{ BOXAUTOLEVEL, "AUTO LEVEL", 54 },
{ BOXSOARING, "SOARING", 55 },
{ BOXPLANWPMISSION, "WP PLANNER", 55 },
{ BOXSOARING, "SOARING", 56 },
{ CHECKBOX_ITEM_COUNT, NULL, 0xFF }
};
@ -219,6 +220,7 @@ void initActiveBoxIds(void)
activeBoxIds[activeBoxIdCount++] = BOXNAVWP;
activeBoxIds[activeBoxIdCount++] = BOXHOMERESET;
activeBoxIds[activeBoxIdCount++] = BOXGCSNAV;
activeBoxIds[activeBoxIdCount++] = BOXPLANWPMISSION;
if (STATE(AIRPLANE)) {
activeBoxIds[activeBoxIdCount++] = BOXNAVCOURSEHOLD;
@ -385,6 +387,7 @@ void packBoxModeFlags(boxBitmask_t * mspBoxModeFlags)
CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXMSPRCOVERRIDE)), BOXMSPRCOVERRIDE);
#endif
CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXAUTOLEVEL)), BOXAUTOLEVEL);
CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXPLANWPMISSION)), BOXPLANWPMISSION);
CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXSOARING)), BOXSOARING);
memset(mspBoxModeFlags, 0, sizeof(boxBitmask_t));

3
src/main/fc/rc_modes.h Executable file → Normal file
View file

@ -72,7 +72,8 @@ typedef enum {
BOXTURTLE = 43,
BOXNAVCRUISE = 44,
BOXAUTOLEVEL = 45,
BOXSOARING = 46,
BOXPLANWPMISSION = 46,
BOXSOARING = 47,
CHECKBOX_ITEM_COUNT
} boxId_e;

20
src/main/fc/settings.yaml
View file

@ -2597,6 +2597,11 @@ groups:
default_value: "RTH"
field: general.flags.safehome_usage_mode
table: safehome_usage_mode
- name: nav_mission_planner_reset
description: "With Reset ON WP Mission Planner waypoint count can be reset to 0 by toggling the mode switch ON-OFF-ON."
default_value: ON
field: general.flags.mission_planner_reset
type: bool
- name: nav_mc_bank_angle
description: "Maximum banking angle (deg) that multicopter navigation is allowed to set. Machine must be able to satisfy this angle without loosing altitude"
default_value: 30
@ -3229,7 +3234,20 @@ groups:
condition: USE_TEMPERATURE_SENSOR
table: osd_alignment
type: uint8_t
- name: osd_airspeed_alarm_min
condition: USE_PITOT
description: "Airspeed under which the airspeed OSD element will start blinking (cm/s)"
default_value: 0
field: airspeed_alarm_min
min: 0
max: 27000 # (1000km/h * 1000 * 100 / 60 / 60)
- name: osd_airspeed_alarm_max
condition: USE_PITOT
description: "Airspeed above which the airspeed OSD element will start blinking (cm/s)"
default_value: 0
field: airspeed_alarm_max
min: 0
max: 27000 # (1000km/h * 1000 * 100 / 60 / 60)
- name: osd_ahi_reverse_roll
field: ahi_reverse_roll
type: bool

36
src/main/io/osd.c
View file

@ -2483,6 +2483,11 @@ static bool osdDrawSingleElement(uint8_t item)
#ifdef USE_PITOT
buff[0] = SYM_AIR;
osdFormatVelocityStr(buff + 1, pitot.airSpeed, false, false);
if ((osdConfig()->airspeed_alarm_min != 0 && pitot.airSpeed < osdConfig()->airspeed_alarm_min) ||
(osdConfig()->airspeed_alarm_max != 0 && pitot.airSpeed > osdConfig()->airspeed_alarm_max)) {
TEXT_ATTRIBUTES_ADD_BLINK(elemAttr);
}
#else
return false;
#endif
@ -2980,6 +2985,25 @@ static bool osdDrawSingleElement(uint8_t item)
#if defined(USE_NAV)
case OSD_MISSION:
{
if (IS_RC_MODE_ACTIVE(BOXPLANWPMISSION)) {
char buf[5];
switch (posControl.wpMissionPlannerStatus) {
case WP_PLAN_WAIT:
strcpy(buf, "WAIT");
break;
case WP_PLAN_SAVE:
strcpy(buf, "SAVE");
break;
case WP_PLAN_OK:
strcpy(buf, " OK ");
break;
case WP_PLAN_FULL:
strcpy(buf, "FULL");
}
tfp_sprintf(buff, "%s>%2uWP", buf, posControl.wpPlannerActiveWPIndex);
} else if (posControl.wpPlannerActiveWPIndex){
tfp_sprintf(buff, "PLAN>%2uWP", posControl.waypointCount); // mission planner mision active
} else {
if (ARMING_FLAG(ARMED)){
// Limit field size when Armed, only show selected mission
tfp_sprintf(buff, "M%u ", posControl.loadedMultiMissionIndex);
@ -2987,7 +3011,7 @@ static bool osdDrawSingleElement(uint8_t item)
if (navConfig()->general.waypoint_multi_mission_index != posControl.loadedMultiMissionIndex) {
tfp_sprintf(buff, "M%u/%u>LOAD", navConfig()->general.waypoint_multi_mission_index, posControl.multiMissionCount);
} else {
// wpCount source for selected mission changes when Armed/Disarmed
// wpCount source for selected mission changes after Arming (until next mission load)
int8_t wpCount = posControl.loadedMultiMissionWPCount ? posControl.loadedMultiMissionWPCount : posControl.waypointCount;
if (posControl.waypointListValid && wpCount > 0) {
tfp_sprintf(buff, "M%u/%u>%2uWP", posControl.loadedMultiMissionIndex, posControl.multiMissionCount, wpCount);
@ -2998,6 +3022,7 @@ static bool osdDrawSingleElement(uint8_t item)
} else { // multi_mission_index 0 - show active WP count
tfp_sprintf(buff, "WP CNT>%2u", posControl.waypointCount);
}
}
displayWrite(osdDisplayPort, elemPosX, elemPosY, buff);
return true;
}
@ -3168,6 +3193,10 @@ PG_RESET_TEMPLATE(osdConfig_t, osdConfig,
#ifdef USE_TEMPERATURE_SENSOR
.temp_label_align = SETTING_OSD_TEMP_LABEL_ALIGN_DEFAULT,
#endif
#ifdef USE_PITOT
.airspeed_alarm_min = SETTING_OSD_AIRSPEED_ALARM_MIN_DEFAULT,
.airspeed_alarm_max = SETTING_OSD_AIRSPEED_ALARM_MAX_DEFAULT,
#endif
.video_system = SETTING_OSD_VIDEO_SYSTEM_DEFAULT,
.row_shiftdown = SETTING_OSD_ROW_SHIFTDOWN_DEFAULT,
@ -4244,6 +4273,11 @@ textAttributes_t osdGetSystemMessage(char *buff, size_t buff_size, bool isCenter
if (FLIGHT_MODE(SOARING_MODE)) {
messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_NAV_SOARING);
}
#if defined(USE_NAV)
if (posControl.flags.wpMissionPlannerActive) {
messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_MISSION_PLANNER);
}
#endif
}
}
} else if (ARMING_FLAG(ARMING_DISABLED_ALL_FLAGS)) {

5
src/main/io/osd.h
View file

@ -85,6 +85,7 @@
#define OSD_MSG_HEADING_HOME "EN ROUTE TO HOME"
#define OSD_MSG_WP_FINISHED "WP END>HOLDING POSITION"
#define OSD_MSG_PREPARE_NEXT_WP "PREPARING FOR NEXT WAYPOINT"
#define OSD_MSG_MISSION_PLANNER "(WP MISSION PLANNER)"
#define OSD_MSG_WP_RTH_CANCEL "CANCEL WP TO EXIT RTH"
#define OSD_MSG_WP_MISSION_LOADED "* MISSION LOADED *"
#define OSD_MSG_EMERG_LANDING "EMERGENCY LANDING"
@ -330,6 +331,10 @@ typedef struct osdConfig_s {
#ifdef USE_TEMPERATURE_SENSOR
osd_alignment_e temp_label_align;
#endif
#ifdef USE_PITOT
float airspeed_alarm_min;
float airspeed_alarm_max;
#endif
videoSystem_e video_system;
uint8_t row_shiftdown;

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

@ -115,8 +115,9 @@ PG_RESET_TEMPLATE(navConfig_t, navConfig,
.rth_alt_control_override = SETTING_NAV_RTH_ALT_CONTROL_OVERRIDE_DEFAULT, // Override RTH Altitude and Climb First using Pitch and Roll stick
.nav_overrides_motor_stop = SETTING_NAV_OVERRIDES_MOTOR_STOP_DEFAULT,
.safehome_usage_mode = SETTING_SAFEHOME_USAGE_MODE_DEFAULT,
.soaring_motor_stop = SETTING_NAV_FW_SOARING_MOTOR_STOP_DEFAULT, // stops motor when Saoring mode enabled
.mission_planner_reset = SETTING_NAV_MISSION_PLANNER_RESET_DEFAULT, // Allow mode switch toggle to reset Mission Planner WPs
.waypoint_mission_restart = SETTING_NAV_WP_MISSION_RESTART_DEFAULT, // WP mission restart action
.soaring_motor_stop = SETTING_NAV_FW_SOARING_MOTOR_STOP_DEFAULT, // stops motor when Saoring mode enabled
},
// General navigation parameters
@ -241,6 +242,7 @@ static bool isWaypointPositionReached(const fpVector3_t * pos, const bool isWayp
static void mapWaypointToLocalPosition(fpVector3_t * localPos, const navWaypoint_t * waypoint, geoAltitudeConversionMode_e altConv);
static navigationFSMEvent_t nextForNonGeoStates(void);
static bool isWaypointMissionValid(void);
void missionPlannerSetWaypoint(void);
void initializeRTHSanityChecker(const fpVector3_t * pos);
bool validateRTHSanityChecker(void);
@ -2909,8 +2911,9 @@ bool checkMissionCount(int8_t waypoint)
#ifdef NAV_NON_VOLATILE_WAYPOINT_STORAGE
bool loadNonVolatileWaypointList(bool clearIfLoaded)
{
/* multi_mission_index 0 only used for non NVM missions - don't load */
if (ARMING_FLAG(ARMED) || !navConfig()->general.waypoint_multi_mission_index) {
/* multi_mission_index 0 only used for non NVM missions - don't load.
* Don't load if mission planner WP count > 0 */
if (ARMING_FLAG(ARMED) || !navConfig()->general.waypoint_multi_mission_index || posControl.wpPlannerActiveWPIndex) {
return false;
}
@ -3309,9 +3312,11 @@ static navigationFSMEvent_t selectNavEventFromBoxModeInput(void)
return NAV_FSM_EVENT_SWITCH_TO_RTH;
}
// Pilot-triggered RTH (can override MANUAL), also fall-back for WP if there is no mission loaded
// Prevent MANUAL falling back to RTH if selected during active mission (canActivateWaypoint is set false on MANUAL selection)
if (IS_RC_MODE_ACTIVE(BOXNAVRTH) || (IS_RC_MODE_ACTIVE(BOXNAVWP) && !canActivateWaypoint && !IS_RC_MODE_ACTIVE(BOXMANUAL))) {
/* Pilot-triggered RTH (can override MANUAL), also fall-back for WP if there is no mission loaded
* Prevent MANUAL falling back to RTH if selected during active mission (canActivateWaypoint is set false on MANUAL selection)
* Also prevent WP falling back to RTH if WP mission planner is active */
const bool blockWPFallback = IS_RC_MODE_ACTIVE(BOXMANUAL) || posControl.flags.wpMissionPlannerActive;
if (IS_RC_MODE_ACTIVE(BOXNAVRTH) || (IS_RC_MODE_ACTIVE(BOXNAVWP) && !canActivateWaypoint && !blockWPFallback)) {
// Check for isExecutingRTH to prevent switching our from RTH in case of a brief GPS loss
// If don't keep this, loss of any of the canActivateNavigation && canActivateAltHold
// will kick us out of RTH state machine via NAV_FSM_EVENT_SWITCH_TO_IDLE and will prevent any of the fall-back
@ -3328,7 +3333,8 @@ static navigationFSMEvent_t selectNavEventFromBoxModeInput(void)
}
// Pilot-activated waypoint mission. Fall-back to RTH in case of no mission loaded
if (IS_RC_MODE_ACTIVE(BOXNAVWP)) {
// Block activation if using WP Mission Planner
if (IS_RC_MODE_ACTIVE(BOXNAVWP) && !posControl.flags.wpMissionPlannerActive) {
if (FLIGHT_MODE(NAV_WP_MODE) || (canActivateWaypoint && canActivateNavigation && canActivateAltHold && STATE(GPS_FIX_HOME)))
return NAV_FSM_EVENT_SWITCH_TO_WAYPOINT;
}
@ -3535,6 +3541,71 @@ void updateFlightBehaviorModifiers(void)
posControl.flags.isGCSAssistedNavigationEnabled = IS_RC_MODE_ACTIVE(BOXGCSNAV);
}
/* On the fly WP mission planner mode allows WP missions to be setup during navigation.
* Uses the WP mode switch to save WP at current location (WP mode disabled when active)
* Mission can be flown after mission planner mode switched off and saved after disarm. */
void updateWpMissionPlanner(void)
{
static timeMs_t resetTimerStart = 0;
if (IS_RC_MODE_ACTIVE(BOXPLANWPMISSION) && !(FLIGHT_MODE(NAV_WP_MODE) || isWaypointMissionRTHActive())) {
const bool positionTrusted = posControl.flags.estAltStatus == EST_TRUSTED && posControl.flags.estPosStatus == EST_TRUSTED && STATE(GPS_FIX);
posControl.flags.wpMissionPlannerActive = true;
if (millis() - resetTimerStart < 1000 && navConfig()->general.flags.mission_planner_reset) {
posControl.waypointCount = posControl.wpPlannerActiveWPIndex = 0;
posControl.waypointListValid = false;
posControl.wpMissionPlannerStatus = WP_PLAN_WAIT;
}
if (positionTrusted && posControl.wpMissionPlannerStatus != WP_PLAN_FULL) {
missionPlannerSetWaypoint();
} else {
posControl.wpMissionPlannerStatus = posControl.wpMissionPlannerStatus == WP_PLAN_FULL ? WP_PLAN_FULL : WP_PLAN_WAIT;
}
} else if (posControl.flags.wpMissionPlannerActive) {
posControl.flags.wpMissionPlannerActive = false;
posControl.activeWaypointIndex = 0;
resetTimerStart = millis();
}
}
void missionPlannerSetWaypoint(void)
{
static bool boxWPModeIsReset = true;
boxWPModeIsReset = !boxWPModeIsReset ? !IS_RC_MODE_ACTIVE(BOXNAVWP) : boxWPModeIsReset; // only able to save new WP when WP mode reset
posControl.wpMissionPlannerStatus = boxWPModeIsReset ? boxWPModeIsReset : posControl.wpMissionPlannerStatus; // hold save status until WP mode reset
if (!boxWPModeIsReset || !IS_RC_MODE_ACTIVE(BOXNAVWP)) {
return;
}
if (!posControl.wpPlannerActiveWPIndex) { // reset existing mission data before adding first WP
resetWaypointList();
}
gpsLocation_t wpLLH;
geoConvertLocalToGeodetic(&wpLLH, &posControl.gpsOrigin, &navGetCurrentActualPositionAndVelocity()->pos);
posControl.waypointList[posControl.wpPlannerActiveWPIndex].action = 1;
posControl.waypointList[posControl.wpPlannerActiveWPIndex].lat = wpLLH.lat;
posControl.waypointList[posControl.wpPlannerActiveWPIndex].lon = wpLLH.lon;
posControl.waypointList[posControl.wpPlannerActiveWPIndex].alt = wpLLH.alt;
posControl.waypointList[posControl.wpPlannerActiveWPIndex].p1 = posControl.waypointList[posControl.wpPlannerActiveWPIndex].p2 = 0;
posControl.waypointList[posControl.wpPlannerActiveWPIndex].p3 = 1; // use absolute altitude datum
posControl.waypointList[posControl.wpPlannerActiveWPIndex].flag = NAV_WP_FLAG_LAST;
posControl.waypointListValid = true;
if (posControl.wpPlannerActiveWPIndex) {
posControl.waypointList[posControl.wpPlannerActiveWPIndex - 1].flag = 0; // rollling reset of previous end of mission flag when new WP added
}
posControl.wpPlannerActiveWPIndex += 1;
posControl.waypointCount = posControl.wpPlannerActiveWPIndex;
posControl.wpMissionPlannerStatus = posControl.waypointCount == NAV_MAX_WAYPOINTS ? WP_PLAN_FULL : WP_PLAN_OK;
boxWPModeIsReset = false;
}
/**
* Process NAV mode transition and WP/RTH state machine
* Update rate: RX (data driven or 50Hz)
@ -3562,6 +3633,9 @@ void updateWaypointsAndNavigationMode(void)
// Map navMode back to enabled flight modes
switchNavigationFlightModes();
// Update WP mission planner
updateWpMissionPlanner();
//Update Blackbox data
navCurrentState = (int16_t)posControl.navPersistentId;
}
@ -3676,6 +3750,8 @@ void navigationInit(void)
posControl.waypointCount = 0;
posControl.activeWaypointIndex = 0;
posControl.waypointListValid = false;
posControl.wpPlannerActiveWPIndex = 0;
posControl.flags.wpMissionPlannerActive = false;
posControl.multiMissionCount = 0;
posControl.loadedMultiMissionStartWP = 0;

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

@ -142,6 +142,13 @@ typedef enum {
ON_FW_SPIRAL,
} navRTHClimbFirst_e;
typedef enum {
WP_PLAN_WAIT,
WP_PLAN_SAVE,
WP_PLAN_OK,
WP_PLAN_FULL,
} wpMissionPlannerStatus_e;
typedef enum {
WP_MISSION_START,
WP_MISSION_RESUME,
@ -203,6 +210,7 @@ typedef struct navConfig_s {
uint8_t nav_overrides_motor_stop; // Autonomous modes override motor_stop setting and user command to stop motor
uint8_t safehome_usage_mode; // Controls when safehomes are used
uint8_t soaring_motor_stop; // stop motor when Soaring mode enabled
uint8_t mission_planner_reset; // Allow WP Mission Planner reset using mode toggle (resets WPs to 0)
uint8_t waypoint_mission_restart; // Waypoint mission restart action
} flags;

7
src/main/navigation/navigation_private.h
View file

@ -93,6 +93,8 @@ typedef struct navigationFlags_s {
// Failsafe actions
bool forcedRTHActivated;
bool forcedEmergLandingActivated;
bool wpMissionPlannerActive; // Activation status of WP mission planner
} navigationFlags_t;
typedef struct {
@ -360,6 +362,11 @@ typedef struct {
int8_t geoWaypointCount; // total geospatial WPs in mission
bool wpMissionRestart; // mission restart from first waypoint
/* WP Mission planner */
int8_t wpMissionPlannerStatus; // WP save status for setting in flight WP mission planner
int8_t wpPlannerActiveWPIndex;
/* Multi Missions */
int8_t multiMissionCount; // number of missions in multi mission entry
int8_t loadedMultiMissionIndex; // index of selected multi mission
int8_t loadedMultiMissionStartWP; // selected multi mission start WP