diff --git a/src/main/cms/cms_menu_navigation.c b/src/main/cms/cms_menu_navigation.c index f8cdf92b25..b1fd83af19 100644 --- a/src/main/cms/cms_menu_navigation.c +++ b/src/main/cms/cms_menu_navigation.c @@ -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, }; diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c index 1ba70ed38f..bb8a12bc4d 100644 --- a/src/main/fc/cli.c +++ b/src/main/fc/cli.c @@ -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); diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c index 3e28f41be8..1b3d26e5f9 100755 --- a/src/main/fc/fc_core.c +++ b/src/main/fc/fc_core.c @@ -515,8 +515,9 @@ void releaseSharedTelemetryPorts(void) { void tryArm(void) { +#ifdef USE_MULTI_MISSION setMultiMissionOnArm(); - +#endif updateArmingStatus(); #ifdef USE_DSHOT diff --git a/src/main/fc/rc_controls.c b/src/main/fc/rc_controls.c index e75be364a7..cbd995aec9 100644 --- a/src/main/fc/rc_controls.c +++ b/src/main/fc/rc_controls.c @@ -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 diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 22dd0335b1..2efdc2987b 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -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 diff --git a/src/main/io/osd.c b/src/main/io/osd.c index b489aa1573..cb46c7d713 100644 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -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; diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index 22ad2f0c38..644e298b91 100644 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -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); diff --git a/src/main/navigation/navigation.h b/src/main/navigation/navigation.h index 2894b22b53..9a2154e21b 100644 --- a/src/main/navigation/navigation.h +++ b/src/main/navigation/navigation.h @@ -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); diff --git a/src/main/navigation/navigation_private.h b/src/main/navigation/navigation_private.h index c20b0fbef6..23e61927c2 100644 --- a/src/main/navigation/navigation_private.h +++ b/src/main/navigation/navigation_private.h @@ -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 diff --git a/src/main/target/common.h b/src/main/target/common.h index 6817ec7e37..a2b9dbaefd 100755 --- a/src/main/target/common.h +++ b/src/main/target/common.h @@ -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