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

add define use_multi_mission

Browse source
This commit is contained in:
breadoven 2021-11-16 22:26:50 +00:00
parent f996170e95
commit cc4ced8a05
10 changed files with 82 additions and 16 deletions
Download patch file Download diff file Expand all files Collapse all files

3
src/main/cms/cms_menu_navigation.c
View file

@ -195,8 +195,9 @@ static const OSD_Entry cmsx_menuMissionSettingsEntries[] =
OSD_SETTING_ENTRY("WP LOAD ON BOOT", SETTING_NAV_WP_LOAD_ON_BOOT),
OSD_SETTING_ENTRY("WP REACHED RADIUS", SETTING_NAV_WP_RADIUS),
OSD_SETTING_ENTRY("WP SAFE DISTANCE", SETTING_NAV_WP_SAFE_DISTANCE),
#ifdef USE_MULTI_MISSION
OSD_SETTING_ENTRY("MULTI MISSION NUMBER", SETTING_NAV_WP_MULTI_MISSION_INDEX),
#endif
OSD_BACK_AND_END_ENTRY,
};

24
src/main/fc/cli.c
View file

@ -1395,8 +1395,9 @@ static void printWaypoints(uint8_t dumpMask, const navWaypoint_t *navWaypoint, c
static void cliWaypoints(char *cmdline)
{
#ifdef USE_MULTI_MISSION
static int8_t multiMissionWPCounter = 0;
#endif
if (isEmpty(cmdline)) {
printWaypoints(DUMP_MASTER, posControl.waypointList, NULL);
} else if (sl_strcasecmp(cmdline, "reset") == 0) {
@ -1408,6 +1409,7 @@ static void cliWaypoints(char *cmdline)
for (int i = 0; i < NAV_MAX_WAYPOINTS; i++) {
if (!(posControl.waypointList[i].action == NAV_WP_ACTION_WAYPOINT || posControl.waypointList[i].action == NAV_WP_ACTION_JUMP || posControl.waypointList[i].action == NAV_WP_ACTION_RTH || posControl.waypointList[i].action == NAV_WP_ACTION_HOLD_TIME || posControl.waypointList[i].action == NAV_WP_ACTION_LAND || posControl.waypointList[i].action == NAV_WP_ACTION_SET_POI || posControl.waypointList[i].action == NAV_WP_ACTION_SET_HEAD)) break;
if (posControl.waypointList[i].flag == NAV_WP_FLAG_LAST) {
#ifdef USE_MULTI_MISSION
if (posControl.multiMissionCount == 1) {
posControl.waypointCount = i + 1;
posControl.waypointListValid = true;
@ -1417,6 +1419,11 @@ static void cliWaypoints(char *cmdline)
} else {
posControl.multiMissionCount -= 1;
}
#else
posControl.waypointCount = i + 1;
posControl.waypointListValid = true;
break;
#endif
}
}
if (posControl.waypointListValid) {
@ -1431,7 +1438,11 @@ static void cliWaypoints(char *cmdline)
uint8_t validArgumentCount = 0;
const char *ptr = cmdline;
i = fastA2I(ptr);
#ifdef USE_MULTI_MISSION
if (i + multiMissionWPCounter >= 0 && i + multiMissionWPCounter < NAV_MAX_WAYPOINTS) {
#else
if (i >= 0 && i < NAV_MAX_WAYPOINTS) {
#endif
ptr = nextArg(ptr);
if (ptr) {
action = fastA2I(ptr);
@ -1484,6 +1495,7 @@ static void cliWaypoints(char *cmdline)
} else if (!(action == 0 || action == NAV_WP_ACTION_WAYPOINT || action == NAV_WP_ACTION_RTH || action == NAV_WP_ACTION_JUMP || action == NAV_WP_ACTION_HOLD_TIME || action == NAV_WP_ACTION_LAND || action == NAV_WP_ACTION_SET_POI || action == NAV_WP_ACTION_SET_HEAD) || !(flag == 0 || flag == NAV_WP_FLAG_LAST || flag == NAV_WP_FLAG_HOME)) {
cliShowParseError();
} else {
#ifdef USE_MULTI_MISSION
if (i + multiMissionWPCounter == 0) {
posControl.multiMissionCount = 0;
}
@ -1503,6 +1515,16 @@ static void cliWaypoints(char *cmdline)
multiMissionWPCounter += i + 1;
posControl.multiMissionCount += 1;
}
#else
posControl.waypointList[i].action = action;
posControl.waypointList[i].lat = lat;
posControl.waypointList[i].lon = lon;
posControl.waypointList[i].alt = alt;
posControl.waypointList[i].p1 = p1;
posControl.waypointList[i].p2 = p2;
posControl.waypointList[i].p3 = p3;
posControl.waypointList[i].flag = flag;
#endif
}
} else {
cliShowArgumentRangeError("wp index", 0, NAV_MAX_WAYPOINTS - 1);

3
src/main/fc/fc_core.c
View file

@ -515,8 +515,9 @@ void releaseSharedTelemetryPorts(void) {
void tryArm(void)
{
#ifdef USE_MULTI_MISSION
setMultiMissionOnArm();
#endif
updateArmingStatus();
#ifdef USE_DSHOT

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

@ -263,7 +263,7 @@ void processRcStickPositions(throttleStatus_e throttleStatus)
const bool success = loadNonVolatileWaypointList(false);
beeper(success ? BEEPER_ACTION_SUCCESS : BEEPER_ACTION_FAIL);
}
#ifdef USE_MULTI_MISSION
// Increment multi mission index up
if (rcSticks == THR_LO + YAW_CE + PIT_CE + ROL_HI) {
selectMultiMissionIndex(1);
@ -277,7 +277,7 @@ void processRcStickPositions(throttleStatus_e throttleStatus)
rcDelayCommand = 0;
return;
}
#endif
if (rcSticks == THR_LO + YAW_CE + PIT_LO + ROL_HI) {
resetWaypointList();
beeper(BEEPER_ACTION_FAIL); // The above cannot fail, but traditionally, we play FAIL for not-loading

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

@ -2429,6 +2429,7 @@ groups:
description: "Index of mission selected from multi mission WP entry loaded in flight controller. 1 is the first useable WP mission in the entry. Limited to a maximum of 9 missions. Set index to 0 to display current active WP count in OSD Mission field."
default_value: 1
field: general.waypoint_multi_mission_index
condition: USE_MULTI_MISSION
min: 0
max: 9
- name: nav_auto_speed

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

@ -3003,7 +3003,9 @@ static bool osdDrawSingleElement(uint8_t item)
tfp_sprintf(buff, "%s>%2uWP", buf, posControl.wpPlannerActiveWPIndex);
} else if (posControl.wpPlannerActiveWPIndex){
tfp_sprintf(buff, "PLAN>%2uWP", posControl.waypointCount); // mission planner mision active
} else {
}
#ifdef USE_MULTI_MISSION
else {
if (ARMING_FLAG(ARMED)){
// Limit field size when Armed, only show selected mission
tfp_sprintf(buff, "M%u ", posControl.loadedMultiMissionIndex);
@ -3023,6 +3025,7 @@ static bool osdDrawSingleElement(uint8_t item)
tfp_sprintf(buff, "WP CNT>%2u", posControl.waypointCount);
}
}
#endif
displayWrite(osdDisplayPort, elemPosX, elemPosY, buff);
return true;
}
@ -3877,8 +3880,12 @@ static void osdShowArmed(void)
}
#if defined(USE_NAV)
if (posControl.waypointListValid && posControl.waypointCount > 0) {
#ifdef USE_MULTI_MISSION
tfp_sprintf(buf, "MISSION %u/%u (%u WP)", posControl.loadedMultiMissionIndex, posControl.multiMissionCount, posControl.waypointCount);
displayWrite(osdDisplayPort, 6, y, buf);
#else
displayWrite(osdDisplayPort, 7, y, "*MISSION LOADED*");
#endif
}
#endif
y += 1;

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

@ -39,7 +39,9 @@
#include "fc/rc_controls.h"
#include "fc/rc_modes.h"
#include "fc/runtime_config.h"
#ifdef USE_MULTI_MISSION
#include "fc/cli.h"
#endif
#include "fc/settings.h"
#include "flight/imu.h"
@ -125,7 +127,9 @@ PG_RESET_TEMPLATE(navConfig_t, navConfig,
.pos_failure_timeout = SETTING_NAV_POSITION_TIMEOUT_DEFAULT, // 5 sec
.waypoint_radius = SETTING_NAV_WP_RADIUS_DEFAULT, // 2m diameter
.waypoint_safe_distance = SETTING_NAV_WP_SAFE_DISTANCE_DEFAULT, // centimeters - first waypoint should be closer than this
#ifdef USE_MULTI_MISSION
.waypoint_multi_mission_index = SETTING_NAV_WP_MULTI_MISSION_INDEX_DEFAULT, // mission index selected from multi mission WP entry
#endif
.waypoint_load_on_boot = SETTING_NAV_WP_LOAD_ON_BOOT_DEFAULT, // load waypoints automatically during boot
.auto_speed = SETTING_NAV_AUTO_SPEED_DEFAULT, // speed in autonomous modes (3 m/s = 10.8 km/h)
.max_auto_speed = SETTING_NAV_MAX_AUTO_SPEED_DEFAULT, // max allowed speed autonomous modes
@ -2882,9 +2886,11 @@ void resetWaypointList(void)
posControl.waypointCount = 0;
posControl.waypointListValid = false;
posControl.geoWaypointCount = 0;
#ifdef USE_MULTI_MISSION
posControl.loadedMultiMissionIndex = 0;
posControl.loadedMultiMissionStartWP = 0;
posControl.loadedMultiMissionWPCount = 0;
#endif
}
}
@ -2897,7 +2903,7 @@ int getWaypointCount(void)
{
return posControl.waypointCount;
}
#ifdef USE_MULTI_MISSION
void selectMultiMissionIndex(int8_t increment)
{
if (posControl.multiMissionCount > 1) { // stick selection only active when multi mission loaded
@ -2934,13 +2940,17 @@ bool checkMissionCount(int8_t waypoint)
}
return false;
}
#endif // multi mission
#ifdef NAV_NON_VOLATILE_WAYPOINT_STORAGE
bool loadNonVolatileWaypointList(bool clearIfLoaded)
{
#ifdef USE_MULTI_MISSION
/* 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) {
#else
if (ARMING_FLAG(ARMED) || posControl.wpPlannerActiveWPIndex) {
#endif
return false;
}
@ -2949,7 +2959,7 @@ bool loadNonVolatileWaypointList(bool clearIfLoaded)
resetWaypointList();
return false;
}
#ifdef USE_MULTI_MISSION
/* Reset multi mission index to 1 if exceeds number of available missions */
if (navConfig()->general.waypoint_multi_mission_index > posControl.multiMissionCount) {
navConfigMutable()->general.waypoint_multi_mission_index = 1;
@ -2957,10 +2967,10 @@ bool loadNonVolatileWaypointList(bool clearIfLoaded)
posControl.multiMissionCount = 0;
posControl.loadedMultiMissionWPCount = 0;
int8_t loadedMultiMissionGeoWPCount;
#endif
for (int i = 0; i < NAV_MAX_WAYPOINTS; i++) {
setWaypoint(i + 1, nonVolatileWaypointList(i));
#ifdef USE_MULTI_MISSION
/* store details of selected mission */
if ((posControl.multiMissionCount + 1 == navConfig()->general.waypoint_multi_mission_index)) {
// mission start WP
@ -2988,6 +2998,14 @@ bool loadNonVolatileWaypointList(bool clearIfLoaded)
/* Mission sanity check failed - reset the list
* Also reset if no selected mission loaded (shouldn't happen) */
if (!posControl.waypointListValid || !posControl.loadedMultiMissionWPCount) {
#else
// check this is the last waypoint
if (nonVolatileWaypointList(i)->flag == NAV_WP_FLAG_LAST) {
break;
}
// Mission sanity check failed - reset the list
if (!posControl.waypointListValid) {
#endif
resetWaypointList();
}
@ -3002,8 +3020,9 @@ bool saveNonVolatileWaypointList(void)
for (int i = 0; i < NAV_MAX_WAYPOINTS; i++) {
getWaypoint(i + 1, nonVolatileWaypointListMutable(i));
}
#ifdef USE_MULTI_MISSION
navConfigMutable()->general.waypoint_multi_mission_index = 1; // reset selected mission to 1 when new entries saved
#endif
saveConfigAndNotify();
return true;
@ -3472,7 +3491,11 @@ bool navigationTerrainFollowingEnabled(void)
uint32_t distanceToFirstWP(void)
{
fpVector3_t startingWaypointPos;
#ifdef USE_MULTI_MISSION
mapWaypointToLocalPosition(&startingWaypointPos, &posControl.waypointList[posControl.loadedMultiMissionStartWP], GEO_ALT_RELATIVE);
#else
mapWaypointToLocalPosition(&startingWaypointPos, &posControl.waypointList[0], GEO_ALT_RELATIVE);
#endif
return calculateDistanceToDestination(&startingWaypointPos);
}
@ -3520,9 +3543,13 @@ navArmingBlocker_e navigationIsBlockingArming(bool *usedBypass)
* Can't jump beyond WP list
* Only jump to geo-referenced WP types
*
#ifdef USE_MULTI_MISSION
* Only perform check when mission loaded at start of posControl.waypointList
*/
if (posControl.waypointCount && !posControl.loadedMultiMissionStartWP) {
#else
if (posControl.waypointCount) {
#endif
for (uint8_t wp = 0; wp < posControl.waypointCount; wp++){
if (posControl.waypointList[wp].action == NAV_WP_ACTION_JUMP){
if((wp == 0) || ((posControl.waypointList[wp].p1 > (wp-2)) && (posControl.waypointList[wp].p1 < (wp+2)) ) || (posControl.waypointList[wp].p1 >= posControl.waypointCount) || (posControl.waypointList[wp].p2 < -1)) {
@ -3779,9 +3806,10 @@ void navigationInit(void)
posControl.waypointListValid = false;
posControl.wpPlannerActiveWPIndex = 0;
posControl.flags.wpMissionPlannerActive = false;
#ifdef USE_MULTI_MISSION
posControl.multiMissionCount = 0;
posControl.loadedMultiMissionStartWP = 0;
#endif
/* Set initial surface invalid */
posControl.actualState.surfaceMin = -1.0f;
@ -3802,6 +3830,7 @@ void navigationInit(void)
}
#if defined(NAV_NON_VOLATILE_WAYPOINT_STORAGE)
/* configure WP missions at boot */
#ifdef USE_MULTI_MISSION
for (int8_t i = 0; i < NAV_MAX_WAYPOINTS; i++) { // check number missions in NVM
if (checkMissionCount(i)) {
break;
@ -3811,6 +3840,7 @@ void navigationInit(void)
if (navConfig()->general.waypoint_multi_mission_index > posControl.multiMissionCount) {
navConfigMutable()->general.waypoint_multi_mission_index = 1;
}
#endif
/* load mission on boot */
if (navConfig()->general.waypoint_load_on_boot) {
loadNonVolatileWaypointList(false);

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

@ -229,7 +229,9 @@ typedef struct navConfig_s {
uint8_t pos_failure_timeout; // Time to wait before switching to emergency landing (0 - disable)
uint16_t waypoint_radius; // if we are within this distance to a waypoint then we consider it reached (distance is in cm)
uint16_t waypoint_safe_distance; // Waypoint mission sanity check distance
#ifdef USE_MULTI_MISSION
uint8_t waypoint_multi_mission_index; // Index of mission to be loaded in multi mission entry
#endif
bool waypoint_load_on_boot; // load waypoints automatically during boot
uint16_t auto_speed; // autonomous navigation speed cm/sec
uint16_t max_auto_speed; // maximum allowed autonomous navigation speed cm/sec
@ -492,9 +494,10 @@ void setWaypoint(uint8_t wpNumber, const navWaypoint_t * wpData);
void resetWaypointList(void);
bool loadNonVolatileWaypointList(bool clearIfLoaded);
bool saveNonVolatileWaypointList(void);
#ifdef USE_MULTI_MISSION
void selectMultiMissionIndex(int8_t increment);
void setMultiMissionOnArm(void);
#endif
float getFinalRTHAltitude(void);
int16_t fixedWingPitchToThrottleCorrection(int16_t pitch, timeUs_t currentTimeUs);

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

@ -366,13 +366,13 @@ typedef struct {
/* WP Mission planner */
int8_t wpMissionPlannerStatus; // WP save status for setting in flight WP mission planner
int8_t wpPlannerActiveWPIndex;
#ifdef USE_MULTI_MISSION
/* 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
int8_t loadedMultiMissionWPCount; // number of WPs in selected multi mission
#endif
navWaypointPosition_t activeWaypoint; // Local position and initial bearing, filled on waypoint activation
int8_t activeWaypointIndex;
float wpInitialAltitude; // Altitude at start of WP

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

@ -191,6 +191,7 @@
#define USE_SERIAL_PASSTHROUGH
#define NAV_MAX_WAYPOINTS 120
#define USE_RCDEVICE
#define USE_MULTI_MISSION
//Enable VTX control
#define USE_VTX_CONTROL