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

fixes + improvements

Browse source
This commit is contained in:
breadoven 2022-08-21 22:27:25 +01:00
parent f357482966
commit a670d1d216
3 changed files with 35 additions and 52 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -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)){ // CR74 if (ARMING_FLAG(ARMED) && !(IS_RC_MODE_ACTIVE(BOXCHANGEMISSION) && posControl.multiMissionCount > 1)){ // CR74
// 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.loadedMultiMissionWPCount ? posControl.loadedMultiMissionWPCount : posControl.waypointCount; int8_t wpCount = posControl.loadedMissionWPCount; // CR74
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 {

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

@ -3157,9 +3157,9 @@ void resetWaypointList(void)
posControl.waypointListValid = false; posControl.waypointListValid = false;
posControl.geoWaypointCount = 0; posControl.geoWaypointCount = 0;
wpMissionStartIndex = 0; // CR74 wpMissionStartIndex = 0; // CR74
posControl.loadedMissionWPCount = 0; // CR74
#ifdef USE_MULTI_MISSION #ifdef USE_MULTI_MISSION
posControl.loadedMultiMissionIndex = 0; posControl.loadedMultiMissionIndex = 0;
posControl.loadedMultiMissionWPCount = 0;
#endif #endif
} }
} }
@ -3181,10 +3181,10 @@ void selectMultiMissionIndex(int8_t increment)
} }
} }
// CR74 // CR74
void loadMultiMission(uint8_t missionIndex) void loadSelectedMultiMission(uint8_t missionIndex)
{ {
uint8_t missionCount = 1; uint8_t missionCount = 1;
posControl.loadedMultiMissionWPCount = 0; posControl.loadedMissionWPCount = 0;
posControl.geoWaypointCount = 0; posControl.geoWaypointCount = 0;
for (int i = 0; i < NAV_MAX_WAYPOINTS; i++) { for (int i = 0; i < NAV_MAX_WAYPOINTS; i++) {
@ -3196,13 +3196,13 @@ void loadMultiMission(uint8_t missionIndex)
} }
/* store details of selected mission */ /* store details of selected mission */
// mission start WP // mission start WP
if (posControl.loadedMultiMissionWPCount == 0) { if (posControl.loadedMissionWPCount == 0) {
posControl.loadedMultiMissionWPCount = 1; // start marker only, value here unimportant (but not 0) posControl.loadedMissionWPCount = 1; // start marker only, value here unimportant (but not 0)
wpMissionStartIndex = i; wpMissionStartIndex = i;
} }
// mission end WP // mission end WP
if (posControl.waypointList[i].flag == NAV_WP_FLAG_LAST) { if (posControl.waypointList[i].flag == NAV_WP_FLAG_LAST) {
posControl.loadedMultiMissionWPCount = i - wpMissionStartIndex + 1; posControl.loadedMissionWPCount = i - wpMissionStartIndex + 1;
break; break;
} }
} else if (posControl.waypointList[i].flag == NAV_WP_FLAG_LAST) { } else if (posControl.waypointList[i].flag == NAV_WP_FLAG_LAST) {
@ -3210,26 +3210,30 @@ void loadMultiMission(uint8_t missionIndex)
} }
} }
if (posControl.multiMissionCount > 1 && posControl.loadedMultiMissionWPCount) { if (posControl.multiMissionCount > 1 && posControl.loadedMissionWPCount && ARMING_FLAG(ARMED)) {
posControl.waypointCount = posControl.loadedMultiMissionWPCount; posControl.waypointCount = posControl.loadedMissionWPCount;
} }
posControl.loadedMultiMissionIndex = posControl.multiMissionCount ? missionIndex : 0; posControl.loadedMultiMissionIndex = posControl.multiMissionCount ? missionIndex : 0;
posControl.wpMissionRestart = true; posControl.wpMissionRestart = true;
} }
void updateWpMissionChange(void) bool updateWpMissionChange(void)
{ {
if (!ARMING_FLAG(ARMED) || posControl.multiMissionCount <= 1) return; if (!ARMING_FLAG(ARMED) || posControl.multiMissionCount <= 1 || posControl.wpPlannerActiveWPIndex) {
posControl.multiMissionCount = posControl.wpPlannerActiveWPIndex ? 0 : posControl.multiMissionCount;
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) {
loadMultiMission(navConfig()->general.waypoint_multi_mission_index); loadSelectedMultiMission(navConfig()->general.waypoint_multi_mission_index);
} else if (posControl.waypointCount > posControl.loadedMultiMissionWPCount) { } else if (posControl.waypointCount > posControl.loadedMissionWPCount) {
posControl.waypointCount = posControl.loadedMultiMissionWPCount; posControl.waypointCount = posControl.loadedMissionWPCount;
} }
return; return true;
} }
static bool toggleFlag = true; static bool toggleFlag = true;
if (IS_RC_MODE_ACTIVE(BOXNAVWP) && toggleFlag) { if (IS_RC_MODE_ACTIVE(BOXNAVWP) && toggleFlag) {
if (navConfig()->general.waypoint_multi_mission_index == posControl.multiMissionCount) { if (navConfig()->general.waypoint_multi_mission_index == posControl.multiMissionCount) {
@ -3241,6 +3245,7 @@ void updateWpMissionChange(void)
} else if (!IS_RC_MODE_ACTIVE(BOXNAVWP)) { } else if (!IS_RC_MODE_ACTIVE(BOXNAVWP)) {
toggleFlag = true; toggleFlag = true;
} }
return false;
} }
// CR74 // CR74
bool checkMissionCount(int8_t waypoint) bool checkMissionCount(int8_t waypoint)
@ -3280,44 +3285,30 @@ bool loadNonVolatileWaypointList(bool clearIfLoaded)
navConfigMutable()->general.waypoint_multi_mission_index = 1; navConfigMutable()->general.waypoint_multi_mission_index = 1;
} }
posControl.multiMissionCount = 0; posControl.multiMissionCount = 0;
posControl.loadedMultiMissionWPCount = 0; posControl.loadedMissionWPCount = 0;
int8_t loadedMultiMissionGeoWPCount;
#endif #endif
for (int i = 0; i < NAV_MAX_WAYPOINTS; i++) { for (int i = 0; i < NAV_MAX_WAYPOINTS; i++) {
setWaypoint(i + 1, nonVolatileWaypointList(i)); setWaypoint(i + 1, nonVolatileWaypointList(i));
#ifdef USE_MULTI_MISSION #ifdef USE_MULTI_MISSION
/* store details of selected mission */ /* count up number of missions and exit after last multi mission */
if ((posControl.multiMissionCount + 1 == navConfig()->general.waypoint_multi_mission_index)) {
// mission start WP
if (posControl.loadedMultiMissionWPCount == 0) {
posControl.loadedMultiMissionWPCount = 1; // start marker only, value here unimportant (but not 0)
wpMissionStartIndex = i; // CR74
loadedMultiMissionGeoWPCount = posControl.geoWaypointCount;
}
// mission end WP
if (posControl.waypointList[i].flag == NAV_WP_FLAG_LAST) {
posControl.loadedMultiMissionWPCount = i - wpMissionStartIndex + 1; // CR74
loadedMultiMissionGeoWPCount = posControl.geoWaypointCount - loadedMultiMissionGeoWPCount + 1;
}
}
/* count up number of missions */
if (checkMissionCount(i)) { if (checkMissionCount(i)) {
break; break;
} }
} }
posControl.geoWaypointCount = loadedMultiMissionGeoWPCount; loadSelectedMultiMission(navConfig()->general.waypoint_multi_mission_index);
posControl.loadedMultiMissionIndex = posControl.multiMissionCount ? navConfig()->general.waypoint_multi_mission_index : 0; 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) */
if (!posControl.waypointListValid || !posControl.loadedMultiMissionWPCount) { if (!posControl.waypointListValid || !posControl.loadedMissionWPCount) {
#else #else
// check this is the last waypoint // check this is the last waypoint
if (nonVolatileWaypointList(i)->flag == NAV_WP_FLAG_LAST) { if (nonVolatileWaypointList(i)->flag == NAV_WP_FLAG_LAST) {
break; break;
} }
} }
posControl.loadedMissionWPCount = posControl.waypointCount; // CR74
// Mission sanity check failed - reset the list // Mission sanity check failed - reset the list
if (!posControl.waypointListValid) { if (!posControl.waypointListValid) {
#endif #endif
@ -3710,11 +3701,10 @@ 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 or BOXCHANGEMISSION mode active CR74 // Block activation if using WP Mission Planner
// Update WP mission change before activating WP mode // CR74 // Also check multimission mission change status before activating WP mode // CR74
#ifdef USE_MULTI_MISSION #ifdef USE_MULTI_MISSION
if (IS_RC_MODE_ACTIVE(BOXNAVWP) && !posControl.flags.wpMissionPlannerActive && !IS_RC_MODE_ACTIVE(BOXCHANGEMISSION)) { // CR74 if (updateWpMissionChange() && IS_RC_MODE_ACTIVE(BOXNAVWP) && !posControl.flags.wpMissionPlannerActive) { // CR74
updateWpMissionChange(); // CR74
#else #else
if (IS_RC_MODE_ACTIVE(BOXNAVWP) && !posControl.flags.wpMissionPlannerActive) { // CR74 if (IS_RC_MODE_ACTIVE(BOXNAVWP) && !posControl.flags.wpMissionPlannerActive) { // CR74
#endif #endif
@ -3828,11 +3818,7 @@ bool navigationTerrainFollowingEnabled(void)
uint32_t distanceToFirstWP(void) uint32_t distanceToFirstWP(void)
{ {
fpVector3_t startingWaypointPos; fpVector3_t startingWaypointPos;
// #ifdef USE_MULTI_MISSION
mapWaypointToLocalPosition(&startingWaypointPos, &posControl.waypointList[wpMissionStartIndex], GEO_ALT_RELATIVE); // CR74 mapWaypointToLocalPosition(&startingWaypointPos, &posControl.waypointList[wpMissionStartIndex], GEO_ALT_RELATIVE); // CR74
// #else
// mapWaypointToLocalPosition(&startingWaypointPos, &posControl.waypointList[0], GEO_ALT_RELATIVE);
// #endif
return calculateDistanceToDestination(&startingWaypointPos); return calculateDistanceToDestination(&startingWaypointPos);
} }
@ -3881,18 +3867,14 @@ navArmingBlocker_e navigationIsBlockingArming(bool *usedBypass)
* Can't jump beyond WP list * Can't jump beyond WP list
* Only jump to geo-referenced WP types * Only jump to geo-referenced WP types
*/ */
// CR74 uint8_t wpCount = posControl.loadedMissionWPCount;
uint8_t wpCount = posControl.waypointCount;
#ifdef USE_MULTI_MISSION
wpCount = posControl.loadedMultiMissionWPCount;
#endif
if (wpCount) { // CR74 if (wpCount) { // CR74
for (uint8_t wp = wpMissionStartIndex; wp < wpCount + wpMissionStartIndex; wp++){ // CR74 for (uint8_t wp = wpMissionStartIndex; wp < wpCount + wpMissionStartIndex; wp++){ // CR74
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) { // CR74
return NAV_ARMING_BLOCKER_JUMP_WAYPOINT_ERROR; return NAV_ARMING_BLOCKER_JUMP_WAYPOINT_ERROR;
} }
// CR74
/* 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; // CR74
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)) {
@ -3982,6 +3964,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
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

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

@ -382,7 +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; int8_t waypointCount; // number of WPs loaded from WP source CR74
int8_t loadedMissionWPCount; // number of WPs in loaded mission CR74
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
@ -393,7 +394,6 @@ 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 loadedMultiMissionWPCount; // number of WPs in selected multi mission
#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;