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

fixes + flags

Browse source
This commit is contained in:
breadoven 2022-08-23 00:17:36 +01:00
parent a670d1d216
commit 09626ea0c5
7 changed files with 63 additions and 55 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -1576,7 +1576,7 @@ static void mspFcWaypointOutCommand(sbuf_t *dst, sbuf_t *src)
{ {
const uint8_t msp_wp_no = sbufReadU8(src); // get the wp number const uint8_t msp_wp_no = sbufReadU8(src); // get the wp number
navWaypoint_t msp_wp; navWaypoint_t msp_wp;
getWaypoint(msp_wp_no + wpMissionStartIndex, &msp_wp); // CR74 getWaypoint(msp_wp_no + wpMissionStartIndex, &msp_wp);
sbufWriteU8(dst, msp_wp_no); // wp_no sbufWriteU8(dst, msp_wp_no); // wp_no
sbufWriteU8(dst, msp_wp.action); // action (WAYPOINT) sbufWriteU8(dst, msp_wp.action); // action (WAYPOINT)
sbufWriteU32(dst, msp_wp.lat); // lat sbufWriteU32(dst, msp_wp.lat); // lat

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

@ -95,7 +95,7 @@ static const box_t boxes[CHECKBOX_ITEM_COUNT + 1] = {
{ .boxId = BOXAUTOLEVEL, .boxName = "AUTO LEVEL", .permanentId = 54 }, { .boxId = BOXAUTOLEVEL, .boxName = "AUTO LEVEL", .permanentId = 54 },
{ .boxId = BOXPLANWPMISSION, .boxName = "WP PLANNER", .permanentId = 55 }, { .boxId = BOXPLANWPMISSION, .boxName = "WP PLANNER", .permanentId = 55 },
{ .boxId = BOXSOARING, .boxName = "SOARING", .permanentId = 56 }, { .boxId = BOXSOARING, .boxName = "SOARING", .permanentId = 56 },
{ .boxId = BOXCHANGEMISSION, .boxName = "MISSION CHANGE", .permanentId = 57 }, // CR74 { .boxId = BOXCHANGEMISSION, .boxName = "MISSION CHANGE", .permanentId = 57 },
{ .boxId = CHECKBOX_ITEM_COUNT, .boxName = NULL, .permanentId = 0xFF } { .boxId = CHECKBOX_ITEM_COUNT, .boxName = NULL, .permanentId = 0xFF }
}; };
@ -228,8 +228,8 @@ void initActiveBoxIds(void)
ADD_ACTIVE_BOX(BOXHOMERESET); ADD_ACTIVE_BOX(BOXHOMERESET);
ADD_ACTIVE_BOX(BOXGCSNAV); ADD_ACTIVE_BOX(BOXGCSNAV);
ADD_ACTIVE_BOX(BOXPLANWPMISSION); ADD_ACTIVE_BOX(BOXPLANWPMISSION);
#ifdef USE_MULTI_MISSION // CR74 #ifdef USE_MULTI_MISSION
ADD_ACTIVE_BOX(BOXCHANGEMISSION); // CR74 ADD_ACTIVE_BOX(BOXCHANGEMISSION);
#endif #endif
} }
@ -404,8 +404,8 @@ void packBoxModeFlags(boxBitmask_t * mspBoxModeFlags)
CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXAUTOLEVEL)), BOXAUTOLEVEL); 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(BOXPLANWPMISSION)), BOXPLANWPMISSION);
CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXSOARING)), BOXSOARING); CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXSOARING)), BOXSOARING);
#ifdef USE_MULTI_MISSION // CR74 #ifdef USE_MULTI_MISSION
CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXCHANGEMISSION)), BOXCHANGEMISSION); // CR74 CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXCHANGEMISSION)), BOXCHANGEMISSION);
#endif #endif
memset(mspBoxModeFlags, 0, sizeof(boxBitmask_t)); memset(mspBoxModeFlags, 0, sizeof(boxBitmask_t));

2
src/main/fc/rc_modes.h
View file

@ -75,7 +75,7 @@ typedef enum {
BOXPLANWPMISSION = 46, BOXPLANWPMISSION = 46,
BOXSOARING = 47, BOXSOARING = 47,
BOXUSER3 = 48, BOXUSER3 = 48,
BOXCHANGEMISSION = 49, // CR74 BOXCHANGEMISSION = 49,
CHECKBOX_ITEM_COUNT CHECKBOX_ITEM_COUNT
} boxId_e; } boxId_e;

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

@ -1514,7 +1514,7 @@ int8_t getGeoWaypointNumber(int8_t waypointIndex)
if (waypointIndex != lastWaypointIndex) { if (waypointIndex != lastWaypointIndex) {
lastWaypointIndex = geoWaypointIndex = waypointIndex; lastWaypointIndex = geoWaypointIndex = waypointIndex;
for (uint8_t i = wpMissionStartIndex; i <= waypointIndex; i++) { // CR74 for (uint8_t i = wpMissionStartIndex; i <= waypointIndex; i++) {
if (posControl.waypointList[i].action == NAV_WP_ACTION_SET_POI || if (posControl.waypointList[i].action == NAV_WP_ACTION_SET_POI ||
posControl.waypointList[i].action == NAV_WP_ACTION_SET_HEAD || posControl.waypointList[i].action == NAV_WP_ACTION_SET_HEAD ||
posControl.waypointList[i].action == NAV_WP_ACTION_JUMP) { posControl.waypointList[i].action == NAV_WP_ACTION_JUMP) {
@ -1523,7 +1523,7 @@ int8_t getGeoWaypointNumber(int8_t waypointIndex)
} }
} }
return geoWaypointIndex - wpMissionStartIndex + 1; // CR74 return geoWaypointIndex - wpMissionStartIndex + 1;
} }
void osdDisplaySwitchIndicator(const char *swName, int rcValue, char *buff) { void osdDisplaySwitchIndicator(const char *swName, int rcValue, char *buff) {
@ -2218,7 +2218,7 @@ static bool osdDrawSingleElement(uint8_t item)
for (int i = osdConfig()->hud_wp_disp - 1; i >= 0 ; i--) { // Display in reverse order so the next WP is always written on top for (int i = osdConfig()->hud_wp_disp - 1; i >= 0 ; i--) { // Display in reverse order so the next WP is always written on top
j = posControl.activeWaypointIndex + i; j = posControl.activeWaypointIndex + i;
if (j > wpMissionStartIndex + posControl.waypointCount - 1) { // limit to max WP index for mission // CR74 if (j > wpMissionStartIndex + posControl.waypointCount - 1) { // limit to max WP index for mission
break; break;
} }
if (posControl.waypointList[j].lat != 0 && posControl.waypointList[j].lon != 0) { if (posControl.waypointList[j].lat != 0 && posControl.waypointList[j].lon != 0) {
@ -3164,7 +3164,7 @@ static bool osdDrawSingleElement(uint8_t item)
} }
#ifdef USE_MULTI_MISSION #ifdef USE_MULTI_MISSION
else { else {
if (ARMING_FLAG(ARMED) && !(IS_RC_MODE_ACTIVE(BOXCHANGEMISSION) && posControl.multiMissionCount > 1)){ // CR74 if (ARMING_FLAG(ARMED) && !(IS_RC_MODE_ACTIVE(BOXCHANGEMISSION) && posControl.multiMissionCount > 1)){
// Limit field size when Armed, only show selected mission // Limit field size when Armed, only show selected mission
tfp_sprintf(buff, "M%u ", posControl.loadedMultiMissionIndex); tfp_sprintf(buff, "M%u ", posControl.loadedMultiMissionIndex);
} else if (posControl.multiMissionCount && navConfig()->general.waypoint_multi_mission_index){ } else if (posControl.multiMissionCount && navConfig()->general.waypoint_multi_mission_index){
@ -3172,7 +3172,7 @@ static bool osdDrawSingleElement(uint8_t item)
tfp_sprintf(buff, "M%u/%u>LOAD", navConfig()->general.waypoint_multi_mission_index, posControl.multiMissionCount); tfp_sprintf(buff, "M%u/%u>LOAD", navConfig()->general.waypoint_multi_mission_index, posControl.multiMissionCount);
} else { } else {
// wpCount source for selected mission changes after Arming (until next mission load) // wpCount source for selected mission changes after Arming (until next mission load)
int8_t wpCount = posControl.loadedMissionWPCount; // CR74 int8_t wpCount = posControl.loadedMissionWPCount;
if (posControl.waypointListValid && wpCount > 0) { if (posControl.waypointListValid && wpCount > 0) {
tfp_sprintf(buff, "M%u/%u>%2uWP", posControl.loadedMultiMissionIndex, posControl.multiMissionCount, wpCount); tfp_sprintf(buff, "M%u/%u>%2uWP", posControl.loadedMultiMissionIndex, posControl.multiMissionCount, wpCount);
} else { } else {

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

@ -91,7 +91,7 @@ PG_REGISTER_ARRAY(navSafeHome_t, MAX_SAFE_HOMES, safeHomeConfig, PG_SAFE_HOME_CO
#endif #endif
uint8_t wpMissionStartIndex = 0; // WP mode start waypoint index CR74 uint8_t wpMissionStartIndex = 0; // WP mode start waypoint index
// waypoint 254, 255 are special waypoints // waypoint 254, 255 are special waypoints
STATIC_ASSERT(NAV_MAX_WAYPOINTS < 254, NAV_MAX_WAYPOINTS_exceeded_allowable_range); STATIC_ASSERT(NAV_MAX_WAYPOINTS < 254, NAV_MAX_WAYPOINTS_exceeded_allowable_range);
@ -212,7 +212,7 @@ PG_RESET_TEMPLATE(navConfig_t, navConfig,
.soaring_pitch_deadband = SETTING_NAV_FW_SOARING_PITCH_DEADBAND_DEFAULT,// pitch angle mode deadband when Saoring mode enabled .soaring_pitch_deadband = SETTING_NAV_FW_SOARING_PITCH_DEADBAND_DEFAULT,// pitch angle mode deadband when Saoring mode enabled
.auto_disarm_delay = SETTING_NAV_FW_AUTO_DISARM_DELAY_DEFAULT, // ms - time delay to disarm when auto disarm after landing enabled .auto_disarm_delay = SETTING_NAV_FW_AUTO_DISARM_DELAY_DEFAULT, // ms - time delay to disarm when auto disarm after landing enabled
.wp_tracking_accuracy = SETTING_NAV_FW_WP_TRACKING_ACCURACY_DEFAULT, // 0, improves course tracking accuracy during FW WP missions .wp_tracking_accuracy = SETTING_NAV_FW_WP_TRACKING_ACCURACY_DEFAULT, // 0, improves course tracking accuracy during FW WP missions
.wp_tracking_max_angle = SETTING_NAV_FW_WP_TRACKING_MAX_ANGLE_DEFAULT, // 60 degs CR67 .wp_tracking_max_angle = SETTING_NAV_FW_WP_TRACKING_MAX_ANGLE_DEFAULT, // 60 degs
.wp_turn_smoothing = SETTING_NAV_FW_WP_TURN_SMOOTHING_DEFAULT, // 0, smooths turns during FW WP mode missions .wp_turn_smoothing = SETTING_NAV_FW_WP_TURN_SMOOTHING_DEFAULT, // 0, smooths turns during FW WP mode missions
} }
); );
@ -1528,14 +1528,14 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_INITIALIZE(nav
Use p3 as the volatile jump counter, allowing embedded, rearmed jumps Use p3 as the volatile jump counter, allowing embedded, rearmed jumps
Using p3 minimises the risk of saving an invalid counter if a mission is aborted. Using p3 minimises the risk of saving an invalid counter if a mission is aborted.
*/ */
if (posControl.activeWaypointIndex == wpMissionStartIndex || posControl.wpMissionRestart) { // CR74 if (posControl.activeWaypointIndex == wpMissionStartIndex || posControl.wpMissionRestart) {
setupJumpCounters(); setupJumpCounters();
posControl.activeWaypointIndex = wpMissionStartIndex; // CR74 posControl.activeWaypointIndex = wpMissionStartIndex;
wpHeadingControl.mode = NAV_WP_HEAD_MODE_NONE; wpHeadingControl.mode = NAV_WP_HEAD_MODE_NONE;
} }
if (navConfig()->general.flags.waypoint_mission_restart == WP_MISSION_SWITCH) { if (navConfig()->general.flags.waypoint_mission_restart == WP_MISSION_SWITCH) {
posControl.wpMissionRestart = posControl.activeWaypointIndex > wpMissionStartIndex ? !posControl.wpMissionRestart : false; // CR74 posControl.wpMissionRestart = posControl.activeWaypointIndex > wpMissionStartIndex ? !posControl.wpMissionRestart : false;
} else { } else {
posControl.wpMissionRestart = navConfig()->general.flags.waypoint_mission_restart == WP_MISSION_START; posControl.wpMissionRestart = navConfig()->general.flags.waypoint_mission_restart == WP_MISSION_START;
} }
@ -1547,7 +1547,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_INITIALIZE(nav
static navigationFSMEvent_t nextForNonGeoStates(void) static navigationFSMEvent_t nextForNonGeoStates(void)
{ {
/* simple helper for non-geographical states that just set other data */ /* simple helper for non-geographical states that just set other data */
if (isLastMissionWaypoint()) { // non-geo state is the last waypoint, switch to finish. CR74 if (isLastMissionWaypoint()) { // non-geo state is the last waypoint, switch to finish.
return NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_FINISHED; return NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_FINISHED;
} else { // Finished non-geo, move to next WP } else { // Finished non-geo, move to next WP
posControl.activeWaypointIndex++; posControl.activeWaypointIndex++;
@ -1582,7 +1582,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_PRE_ACTION(nav
posControl.waypointList[posControl.activeWaypointIndex].p3--; posControl.waypointList[posControl.activeWaypointIndex].p3--;
} }
} }
posControl.activeWaypointIndex = posControl.waypointList[posControl.activeWaypointIndex].p1 + wpMissionStartIndex; // CR74 posControl.activeWaypointIndex = posControl.waypointList[posControl.activeWaypointIndex].p1 + wpMissionStartIndex;
return NAV_FSM_EVENT_NONE; // re-process the state passing to the next WP return NAV_FSM_EVENT_NONE; // re-process the state passing to the next WP
case NAV_WP_ACTION_SET_POI: case NAV_WP_ACTION_SET_POI:
@ -1929,7 +1929,7 @@ static void navProcessFSMEvents(navigationFSMEvent_t injectedEvent)
if (posControl.flags.isAdjustingPosition) NAV_Status.flags |= MW_NAV_FLAG_ADJUSTING_POSITION; if (posControl.flags.isAdjustingPosition) NAV_Status.flags |= MW_NAV_FLAG_ADJUSTING_POSITION;
if (posControl.flags.isAdjustingAltitude) NAV_Status.flags |= MW_NAV_FLAG_ADJUSTING_ALTITUDE; if (posControl.flags.isAdjustingAltitude) NAV_Status.flags |= MW_NAV_FLAG_ADJUSTING_ALTITUDE;
NAV_Status.activeWpNumber = posControl.activeWaypointIndex - wpMissionStartIndex + 1; // CR74 NAV_Status.activeWpNumber = posControl.activeWaypointIndex - wpMissionStartIndex + 1;
NAV_Status.activeWpAction = 0; NAV_Status.activeWpAction = 0;
if ((posControl.activeWaypointIndex >= 0) && (posControl.activeWaypointIndex < NAV_MAX_WAYPOINTS)) { if ((posControl.activeWaypointIndex >= 0) && (posControl.activeWaypointIndex < NAV_MAX_WAYPOINTS)) {
NAV_Status.activeWpAction = posControl.waypointList[posControl.activeWaypointIndex].action; NAV_Status.activeWpAction = posControl.waypointList[posControl.activeWaypointIndex].action;
@ -2228,8 +2228,8 @@ static bool getLocalPosNextWaypoint(fpVector3_t * nextWpPos)
if (nextWpAction == NAV_WP_ACTION_JUMP) { if (nextWpAction == NAV_WP_ACTION_JUMP) {
if (posControl.waypointList[posControl.activeWaypointIndex + 1].p3 != 0 || if (posControl.waypointList[posControl.activeWaypointIndex + 1].p3 != 0 ||
posControl.waypointList[posControl.activeWaypointIndex + 1].p2 == -1) { posControl.waypointList[posControl.activeWaypointIndex + 1].p2 == -1) {
nextWpIndex = posControl.waypointList[posControl.activeWaypointIndex + 1].p1 + wpMissionStartIndex; // CR74 nextWpIndex = posControl.waypointList[posControl.activeWaypointIndex + 1].p1 + wpMissionStartIndex;
} else if (posControl.activeWaypointIndex + 2 <= wpMissionStartIndex + posControl.waypointCount - 1) { // CR74 } else if (posControl.activeWaypointIndex + 2 <= wpMissionStartIndex + posControl.waypointCount - 1) {
if (posControl.waypointList[posControl.activeWaypointIndex + 2].action != NAV_WP_ACTION_JUMP) { if (posControl.waypointList[posControl.activeWaypointIndex + 2].action != NAV_WP_ACTION_JUMP) {
nextWpIndex++; nextWpIndex++;
} else { } else {
@ -2940,7 +2940,7 @@ static bool adjustAltitudeFromRCInput(void)
*-----------------------------------------------------------*/ *-----------------------------------------------------------*/
static void setupJumpCounters(void) static void setupJumpCounters(void)
{ {
for (uint8_t wp = wpMissionStartIndex; wp < posControl.waypointCount + wpMissionStartIndex; wp++) { // CR74 for (uint8_t wp = wpMissionStartIndex; wp < posControl.waypointCount + wpMissionStartIndex; wp++) {
if (posControl.waypointList[wp].action == NAV_WP_ACTION_JUMP){ if (posControl.waypointList[wp].action == NAV_WP_ACTION_JUMP){
posControl.waypointList[wp].p3 = posControl.waypointList[wp].p2; posControl.waypointList[wp].p3 = posControl.waypointList[wp].p2;
} }
@ -2955,7 +2955,7 @@ static void resetJumpCounter(void)
static void clearJumpCounters(void) static void clearJumpCounters(void)
{ {
for (uint8_t wp = wpMissionStartIndex; wp < posControl.waypointCount + wpMissionStartIndex; wp++) { // CR74 for (uint8_t wp = wpMissionStartIndex; wp < posControl.waypointCount + wpMissionStartIndex; wp++) {
if (posControl.waypointList[wp].action == NAV_WP_ACTION_JUMP) { if (posControl.waypointList[wp].action == NAV_WP_ACTION_JUMP) {
posControl.waypointList[wp].p3 = 0; posControl.waypointList[wp].p3 = 0;
} }
@ -3074,7 +3074,7 @@ void getWaypoint(uint8_t wpNumber, navWaypoint_t * wpData)
} }
// WP #1 - #60 - common waypoints - pre-programmed mission // WP #1 - #60 - common waypoints - pre-programmed mission
else if ((wpNumber >= 1) && (wpNumber <= NAV_MAX_WAYPOINTS)) { else if ((wpNumber >= 1) && (wpNumber <= NAV_MAX_WAYPOINTS)) {
if (wpNumber <= posControl.waypointCount + wpMissionStartIndex) { // CR74 if (wpNumber <= posControl.waypointCount + wpMissionStartIndex) {
*wpData = posControl.waypointList[wpNumber - 1]; *wpData = posControl.waypointList[wpNumber - 1];
if(wpData->action == NAV_WP_ACTION_JUMP) { if(wpData->action == NAV_WP_ACTION_JUMP) {
wpData->p1 += 1; // make WP # (vice index) wpData->p1 += 1; // make WP # (vice index)
@ -3156,9 +3156,10 @@ void resetWaypointList(void)
posControl.waypointCount = 0; posControl.waypointCount = 0;
posControl.waypointListValid = false; posControl.waypointListValid = false;
posControl.geoWaypointCount = 0; posControl.geoWaypointCount = 0;
wpMissionStartIndex = 0; // CR74 wpMissionStartIndex = 0;
posControl.loadedMissionWPCount = 0; // CR74 posControl.loadedMissionWPCount = 0;
#ifdef USE_MULTI_MISSION #ifdef USE_MULTI_MISSION
posControl.totalMultiMissionWpCount = 0;
posControl.loadedMultiMissionIndex = 0; posControl.loadedMultiMissionIndex = 0;
#endif #endif
} }
@ -3180,7 +3181,7 @@ void selectMultiMissionIndex(int8_t increment)
navConfigMutable()->general.waypoint_multi_mission_index = constrain(navConfigMutable()->general.waypoint_multi_mission_index + increment, 0, posControl.multiMissionCount); navConfigMutable()->general.waypoint_multi_mission_index = constrain(navConfigMutable()->general.waypoint_multi_mission_index + increment, 0, posControl.multiMissionCount);
} }
} }
// CR74
void loadSelectedMultiMission(uint8_t missionIndex) void loadSelectedMultiMission(uint8_t missionIndex)
{ {
uint8_t missionCount = 1; uint8_t missionCount = 1;
@ -3210,25 +3211,24 @@ void loadSelectedMultiMission(uint8_t missionIndex)
} }
} }
if (posControl.multiMissionCount > 1 && posControl.loadedMissionWPCount && ARMING_FLAG(ARMED)) {
posControl.waypointCount = posControl.loadedMissionWPCount;
}
posControl.loadedMultiMissionIndex = posControl.multiMissionCount ? missionIndex : 0; posControl.loadedMultiMissionIndex = posControl.multiMissionCount ? missionIndex : 0;
posControl.wpMissionRestart = true; posControl.wpMissionRestart = true;
} }
bool updateWpMissionChange(void) bool updateWpMissionChange(void)
{ {
if (!ARMING_FLAG(ARMED) || posControl.multiMissionCount <= 1 || posControl.wpPlannerActiveWPIndex) { /* Function only called when ARMED */
if (posControl.multiMissionCount <= 1 || posControl.wpPlannerActiveWPIndex) {
posControl.multiMissionCount = posControl.wpPlannerActiveWPIndex ? 0 : posControl.multiMissionCount; posControl.multiMissionCount = posControl.wpPlannerActiveWPIndex ? 0 : posControl.multiMissionCount;
return true; return true;
} }
if (!IS_RC_MODE_ACTIVE(BOXCHANGEMISSION)) { if (!IS_RC_MODE_ACTIVE(BOXCHANGEMISSION)) {
if (posControl.loadedMultiMissionIndex != navConfig()->general.waypoint_multi_mission_index) { if (posControl.loadedMultiMissionIndex != navConfig()->general.waypoint_multi_mission_index ||
(navConfig()->general.waypoint_multi_mission_index > 1 && wpMissionStartIndex == 0)) {
loadSelectedMultiMission(navConfig()->general.waypoint_multi_mission_index); loadSelectedMultiMission(navConfig()->general.waypoint_multi_mission_index);
} else if (posControl.waypointCount > posControl.loadedMissionWPCount) { } else if (posControl.waypointCount != posControl.loadedMissionWPCount) {
posControl.waypointCount = posControl.loadedMissionWPCount; posControl.waypointCount = posControl.loadedMissionWPCount;
} }
return true; return true;
@ -3247,7 +3247,7 @@ bool updateWpMissionChange(void)
} }
return false; return false;
} }
// CR74
bool checkMissionCount(int8_t waypoint) bool checkMissionCount(int8_t waypoint)
{ {
if (nonVolatileWaypointList(waypoint)->flag == NAV_WP_FLAG_LAST) { if (nonVolatileWaypointList(waypoint)->flag == NAV_WP_FLAG_LAST) {
@ -3295,8 +3295,8 @@ bool loadNonVolatileWaypointList(bool clearIfLoaded)
break; break;
} }
} }
posControl.totalMultiMissionWpCount = posControl.waypointCount;
loadSelectedMultiMission(navConfig()->general.waypoint_multi_mission_index); loadSelectedMultiMission(navConfig()->general.waypoint_multi_mission_index);
posControl.loadedMultiMissionIndex = posControl.multiMissionCount ? navConfig()->general.waypoint_multi_mission_index : 0;
/* Mission sanity check failed - reset the list /* Mission sanity check failed - reset the list
* Also reset if no selected mission loaded (shouldn't happen) */ * Also reset if no selected mission loaded (shouldn't happen) */
@ -3307,7 +3307,7 @@ bool loadNonVolatileWaypointList(bool clearIfLoaded)
break; break;
} }
} }
posControl.loadedMissionWPCount = posControl.waypointCount; // CR74 posControl.loadedMissionWPCount = posControl.waypointCount;
// Mission sanity check failed - reset the list // Mission sanity check failed - reset the list
if (!posControl.waypointListValid) { if (!posControl.waypointListValid) {
@ -3401,7 +3401,7 @@ static void calculateAndSetActiveWaypoint(const navWaypoint_t * waypoint)
bool isLastMissionWaypoint(void) bool isLastMissionWaypoint(void)
{ {
return FLIGHT_MODE(NAV_WP_MODE) && (posControl.activeWaypointIndex >= (wpMissionStartIndex + posControl.waypointCount - 1) || return FLIGHT_MODE(NAV_WP_MODE) && (posControl.activeWaypointIndex >= (wpMissionStartIndex + posControl.waypointCount - 1) ||
(posControl.waypointList[posControl.activeWaypointIndex].flag == NAV_WP_FLAG_LAST)); // CR74 (posControl.waypointList[posControl.activeWaypointIndex].flag == NAV_WP_FLAG_LAST));
} }
/* Checks if Nav hold position is active */ /* Checks if Nav hold position is active */
@ -3512,11 +3512,18 @@ void applyWaypointNavigationAndAltitudeHold(void)
posControl.flags.forcedRTHActivated = false; posControl.flags.forcedRTHActivated = false;
posControl.flags.forcedEmergLandingActivated = false; posControl.flags.forcedEmergLandingActivated = false;
// ensure WP missions always restart from first waypoint after disarm // ensure WP missions always restart from first waypoint after disarm
posControl.activeWaypointIndex = wpMissionStartIndex; // CR74 posControl.activeWaypointIndex = wpMissionStartIndex;
// Reset RTH trackback // Reset RTH trackback
posControl.activeRthTBPointIndex = -1; posControl.activeRthTBPointIndex = -1;
posControl.flags.rthTrackbackActive = false; posControl.flags.rthTrackbackActive = false;
posControl.rthTBWrapAroundCounter = -1; posControl.rthTBWrapAroundCounter = -1;
// Reset active WP count and wpMissionStartIndex
#ifdef USE_MULTI_MISSION
if (wpMissionStartIndex || posControl.waypointCount < posControl.totalMultiMissionWpCount) {
posControl.waypointCount = posControl.totalMultiMissionWpCount;
wpMissionStartIndex = 0;
}
#endif
return; return;
} }
@ -3702,11 +3709,11 @@ static navigationFSMEvent_t selectNavEventFromBoxModeInput(void)
// Pilot-activated waypoint mission. Fall-back to RTH in case of no mission loaded // Pilot-activated waypoint mission. Fall-back to RTH in case of no mission loaded
// Block activation if using WP Mission Planner // Block activation if using WP Mission Planner
// Also check multimission mission change status before activating WP mode // CR74 // Also check multimission mission change status before activating WP mode
#ifdef USE_MULTI_MISSION #ifdef USE_MULTI_MISSION
if (updateWpMissionChange() && IS_RC_MODE_ACTIVE(BOXNAVWP) && !posControl.flags.wpMissionPlannerActive) { // CR74 if (updateWpMissionChange() && IS_RC_MODE_ACTIVE(BOXNAVWP) && !posControl.flags.wpMissionPlannerActive) {
#else #else
if (IS_RC_MODE_ACTIVE(BOXNAVWP) && !posControl.flags.wpMissionPlannerActive) { // CR74 if (IS_RC_MODE_ACTIVE(BOXNAVWP) && !posControl.flags.wpMissionPlannerActive) {
#endif #endif
if (FLIGHT_MODE(NAV_WP_MODE) || (canActivateWaypoint && canActivateNavigation && canActivateAltHold && STATE(GPS_FIX_HOME))) if (FLIGHT_MODE(NAV_WP_MODE) || (canActivateWaypoint && canActivateNavigation && canActivateAltHold && STATE(GPS_FIX_HOME)))
return NAV_FSM_EVENT_SWITCH_TO_WAYPOINT; return NAV_FSM_EVENT_SWITCH_TO_WAYPOINT;
@ -3818,7 +3825,7 @@ bool navigationTerrainFollowingEnabled(void)
uint32_t distanceToFirstWP(void) uint32_t distanceToFirstWP(void)
{ {
fpVector3_t startingWaypointPos; fpVector3_t startingWaypointPos;
mapWaypointToLocalPosition(&startingWaypointPos, &posControl.waypointList[wpMissionStartIndex], GEO_ALT_RELATIVE); // CR74 mapWaypointToLocalPosition(&startingWaypointPos, &posControl.waypointList[wpMissionStartIndex], GEO_ALT_RELATIVE);
return calculateDistanceToDestination(&startingWaypointPos); return calculateDistanceToDestination(&startingWaypointPos);
} }
@ -3868,15 +3875,15 @@ navArmingBlocker_e navigationIsBlockingArming(bool *usedBypass)
* Only jump to geo-referenced WP types * Only jump to geo-referenced WP types
*/ */
uint8_t wpCount = posControl.loadedMissionWPCount; uint8_t wpCount = posControl.loadedMissionWPCount;
if (wpCount) { // CR74 if (wpCount) {
for (uint8_t wp = wpMissionStartIndex; wp < wpCount + wpMissionStartIndex; wp++){ // CR74 for (uint8_t wp = wpMissionStartIndex; wp < wpCount + wpMissionStartIndex; wp++){
if (posControl.waypointList[wp].action == NAV_WP_ACTION_JUMP){ if (posControl.waypointList[wp].action == NAV_WP_ACTION_JUMP){
if (wp == wpMissionStartIndex || posControl.waypointList[wp].p1 == (wp - wpMissionStartIndex + 1) || posControl.waypointList[wp].p1 == (wp - wpMissionStartIndex - 1) || posControl.waypointList[wp].p1 >= wpCount || posControl.waypointList[wp].p2 < -1) { // CR74 if (wp == wpMissionStartIndex || posControl.waypointList[wp].p1 == (wp - wpMissionStartIndex + 1) || posControl.waypointList[wp].p1 == (wp - wpMissionStartIndex - 1) || posControl.waypointList[wp].p1 >= wpCount || posControl.waypointList[wp].p2 < -1) {
return NAV_ARMING_BLOCKER_JUMP_WAYPOINT_ERROR; return NAV_ARMING_BLOCKER_JUMP_WAYPOINT_ERROR;
} }
/* check for target geo-ref sanity */ /* check for target geo-ref sanity */
uint16_t target = posControl.waypointList[wp].p1 + wpMissionStartIndex; // CR74 uint16_t target = posControl.waypointList[wp].p1 + wpMissionStartIndex;
if (!(posControl.waypointList[target].action == NAV_WP_ACTION_WAYPOINT || posControl.waypointList[target].action == NAV_WP_ACTION_HOLD_TIME || posControl.waypointList[target].action == NAV_WP_ACTION_LAND)) { if (!(posControl.waypointList[target].action == NAV_WP_ACTION_WAYPOINT || posControl.waypointList[target].action == NAV_WP_ACTION_HOLD_TIME || posControl.waypointList[target].action == NAV_WP_ACTION_LAND)) {
return NAV_ARMING_BLOCKER_JUMP_WAYPOINT_ERROR; return NAV_ARMING_BLOCKER_JUMP_WAYPOINT_ERROR;
} }
@ -3933,7 +3940,7 @@ void updateWpMissionPlanner(void)
} }
} else if (posControl.flags.wpMissionPlannerActive) { } else if (posControl.flags.wpMissionPlannerActive) {
posControl.flags.wpMissionPlannerActive = false; posControl.flags.wpMissionPlannerActive = false;
posControl.activeWaypointIndex = 0; // CR74 ???? posControl.activeWaypointIndex = 0;
resetTimerStart = millis(); resetTimerStart = millis();
} }
} }
@ -3964,7 +3971,7 @@ void missionPlannerSetWaypoint(void)
posControl.waypointList[posControl.wpPlannerActiveWPIndex].p3 = 1; // use absolute altitude datum posControl.waypointList[posControl.wpPlannerActiveWPIndex].p3 = 1; // use absolute altitude datum
posControl.waypointList[posControl.wpPlannerActiveWPIndex].flag = NAV_WP_FLAG_LAST; posControl.waypointList[posControl.wpPlannerActiveWPIndex].flag = NAV_WP_FLAG_LAST;
posControl.waypointListValid = true; posControl.waypointListValid = true;
wpMissionStartIndex = 0; // CR74 wpMissionStartIndex = 0;
if (posControl.wpPlannerActiveWPIndex) { if (posControl.wpPlannerActiveWPIndex) {
posControl.waypointList[posControl.wpPlannerActiveWPIndex - 1].flag = 0; // rollling reset of previous end of mission flag when new WP added posControl.waypointList[posControl.wpPlannerActiveWPIndex - 1].flag = 0; // rollling reset of previous end of mission flag when new WP added
@ -4133,7 +4140,7 @@ void navigationInit(void)
posControl.waypointListValid = false; posControl.waypointListValid = false;
posControl.wpPlannerActiveWPIndex = 0; posControl.wpPlannerActiveWPIndex = 0;
posControl.flags.wpMissionPlannerActive = false; posControl.flags.wpMissionPlannerActive = false;
wpMissionStartIndex = 0; // CR74 wpMissionStartIndex = 0;
#ifdef USE_MULTI_MISSION #ifdef USE_MULTI_MISSION
posControl.multiMissionCount = 0; posControl.multiMissionCount = 0;
#endif #endif
@ -4295,7 +4302,7 @@ bool navigationRTHAllowsLanding(void)
{ {
// WP mission RTH landing setting // WP mission RTH landing setting
if (isWaypointMissionRTHActive() && isWaypointMissionValid()) { if (isWaypointMissionRTHActive() && isWaypointMissionValid()) {
return posControl.waypointList[wpMissionStartIndex + posControl.waypointCount - 1].p1 > 0; // CR74 return posControl.waypointList[wpMissionStartIndex + posControl.waypointCount - 1].p1 > 0;
} }
// normal RTH landing setting // normal RTH landing setting

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

@ -68,7 +68,7 @@ bool findNearestSafeHome(void); // Find nearest safehome
#endif // defined(USE_SAFE_HOME) #endif // defined(USE_SAFE_HOME)
extern uint8_t wpMissionStartIndex; // first waypoint index in mission CR74 extern uint8_t wpMissionStartIndex; // first waypoint index in mission
#ifndef NAV_MAX_WAYPOINTS #ifndef NAV_MAX_WAYPOINTS
#define NAV_MAX_WAYPOINTS 15 #define NAV_MAX_WAYPOINTS 15

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

@ -382,8 +382,8 @@ typedef struct {
/* Waypoint list */ /* Waypoint list */
navWaypoint_t waypointList[NAV_MAX_WAYPOINTS]; navWaypoint_t waypointList[NAV_MAX_WAYPOINTS];
bool waypointListValid; bool waypointListValid;
int8_t waypointCount; // number of WPs loaded from WP source CR74 int8_t waypointCount; // number of WPs loaded from WP source
int8_t loadedMissionWPCount; // number of WPs in loaded mission CR74 int8_t loadedMissionWPCount; // number of WPs in loaded mission
int8_t geoWaypointCount; // total geospatial WPs in mission int8_t geoWaypointCount; // total geospatial WPs in mission
bool wpMissionRestart; // mission restart from first waypoint bool wpMissionRestart; // mission restart from first waypoint
@ -394,6 +394,7 @@ typedef struct {
/* Multi Missions */ /* Multi Missions */
int8_t multiMissionCount; // number of missions in multi mission entry int8_t multiMissionCount; // number of missions in multi mission entry
int8_t loadedMultiMissionIndex; // index of selected multi mission int8_t loadedMultiMissionIndex; // index of selected multi mission
int8_t totalMultiMissionWpCount; // total number of waypoints in all multi missions
#endif #endif
navWaypointPosition_t activeWaypoint; // Local position, current bearing and turn angle to next WP, filled on waypoint activation navWaypointPosition_t activeWaypoint; // Local position, current bearing and turn angle to next WP, filled on waypoint activation
int8_t activeWaypointIndex; int8_t activeWaypointIndex;