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

first build

Browse source
This commit is contained in:
breadoven 2022-08-20 09:46:54 +01:00
parent e3e52c49c6
commit f357482966
8 changed files with 120 additions and 68 deletions
Download patch file Download diff file Expand all files Collapse all files

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

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

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

@ -1576,8 +1576,8 @@ 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, &msp_wp); getWaypoint(msp_wp_no + wpMissionStartIndex, &msp_wp); // CR74
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
sbufWriteU32(dst, msp_wp.lon); // lon sbufWriteU32(dst, msp_wp.lon); // lon

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

@ -95,6 +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 = CHECKBOX_ITEM_COUNT, .boxName = NULL, .permanentId = 0xFF } { .boxId = CHECKBOX_ITEM_COUNT, .boxName = NULL, .permanentId = 0xFF }
}; };
@ -227,6 +228,9 @@ 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
ADD_ACTIVE_BOX(BOXCHANGEMISSION); // CR74
#endif
} }
if (STATE(AIRPLANE)) { if (STATE(AIRPLANE)) {
@ -400,6 +404,9 @@ 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
CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXCHANGEMISSION)), BOXCHANGEMISSION); // CR74
#endif
memset(mspBoxModeFlags, 0, sizeof(boxBitmask_t)); memset(mspBoxModeFlags, 0, sizeof(boxBitmask_t));
for (uint32_t i = 0; i < activeBoxIdCount; i++) { for (uint32_t i = 0; i < activeBoxIdCount; i++) {

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

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

8
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 = 0; i <= waypointIndex; i++) { for (uint8_t i = wpMissionStartIndex; i <= waypointIndex; i++) { // CR74
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 + 1; return geoWaypointIndex - wpMissionStartIndex + 1; // CR74
} }
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 > posControl.waypointCount - 1) { // limit to max WP index for mission if (j > wpMissionStartIndex + posControl.waypointCount - 1) { // limit to max WP index for mission // CR74
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)){ if (ARMING_FLAG(ARMED) && !IS_RC_MODE_ACTIVE(BOXCHANGEMISSION)){ // 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){

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

@ -91,6 +91,8 @@ 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
// 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);
@ -1526,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 == 0 || posControl.wpMissionRestart) { if (posControl.activeWaypointIndex == wpMissionStartIndex || posControl.wpMissionRestart) { // CR74
setupJumpCounters(); setupJumpCounters();
posControl.activeWaypointIndex = 0; posControl.activeWaypointIndex = wpMissionStartIndex; // CR74
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 ? !posControl.wpMissionRestart : false; posControl.wpMissionRestart = posControl.activeWaypointIndex > wpMissionStartIndex ? !posControl.wpMissionRestart : false; // CR74
} else { } else {
posControl.wpMissionRestart = navConfig()->general.flags.waypoint_mission_restart == WP_MISSION_START; posControl.wpMissionRestart = navConfig()->general.flags.waypoint_mission_restart == WP_MISSION_START;
} }
@ -1544,14 +1546,10 @@ 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 */
const bool isLastWaypoint = (posControl.waypointList[posControl.activeWaypointIndex].flag == NAV_WP_FLAG_LAST) || (posControl.activeWaypointIndex >= (posControl.waypointCount - 1)); if (isLastMissionWaypoint()) { // non-geo state is the last waypoint, switch to finish. CR74
if (isLastWaypoint) {
// 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 { } else { // Finished non-geo, move to next WP
// Finished non-geo, move to next WP
posControl.activeWaypointIndex++; posControl.activeWaypointIndex++;
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
} }
@ -1574,7 +1572,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_PRE_ACTION(nav
// We use p3 as the volatile jump counter (p2 is the static value) // We use p3 as the volatile jump counter (p2 is the static value)
case NAV_WP_ACTION_JUMP: case NAV_WP_ACTION_JUMP:
if(posControl.waypointList[posControl.activeWaypointIndex].p3 != -1){ if (posControl.waypointList[posControl.activeWaypointIndex].p3 != -1){
if(posControl.waypointList[posControl.activeWaypointIndex].p3 == 0){ if(posControl.waypointList[posControl.activeWaypointIndex].p3 == 0){
resetJumpCounter(); resetJumpCounter();
return nextForNonGeoStates(); return nextForNonGeoStates();
@ -1584,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; posControl.activeWaypointIndex = posControl.waypointList[posControl.activeWaypointIndex].p1 + wpMissionStartIndex; // CR74
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:
@ -1931,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 + 1; NAV_Status.activeWpNumber = posControl.activeWaypointIndex - wpMissionStartIndex + 1; // CR74
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;
@ -2230,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; nextWpIndex = posControl.waypointList[posControl.activeWaypointIndex + 1].p1 + wpMissionStartIndex; // CR74
} else if (posControl.activeWaypointIndex + 2 <= posControl.waypointCount - 1) { } else if (posControl.activeWaypointIndex + 2 <= wpMissionStartIndex + posControl.waypointCount - 1) { // CR74
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 {
@ -2942,7 +2940,7 @@ static bool adjustAltitudeFromRCInput(void)
*-----------------------------------------------------------*/ *-----------------------------------------------------------*/
static void setupJumpCounters(void) static void setupJumpCounters(void)
{ {
for (uint8_t wp = 0; wp < posControl.waypointCount ; wp++) { for (uint8_t wp = wpMissionStartIndex; wp < posControl.waypointCount + wpMissionStartIndex; wp++) { // CR74
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;
} }
@ -2952,13 +2950,12 @@ static void setupJumpCounters(void)
static void resetJumpCounter(void) static void resetJumpCounter(void)
{ {
// reset the volatile counter from the set / static value // reset the volatile counter from the set / static value
posControl.waypointList[posControl.activeWaypointIndex].p3 = posControl.waypointList[posControl.activeWaypointIndex].p3 = posControl.waypointList[posControl.activeWaypointIndex].p2;
posControl.waypointList[posControl.activeWaypointIndex].p2;
} }
static void clearJumpCounters(void) static void clearJumpCounters(void)
{ {
for (uint8_t wp = 0; wp < posControl.waypointCount ; wp++) { for (uint8_t wp = wpMissionStartIndex; wp < posControl.waypointCount + wpMissionStartIndex; wp++) { // CR74
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;
} }
@ -3077,12 +3074,11 @@ 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) { if (wpNumber <= posControl.waypointCount + wpMissionStartIndex) { // CR74
*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)
} }
} }
} }
} }
@ -3160,9 +3156,9 @@ void resetWaypointList(void)
posControl.waypointCount = 0; posControl.waypointCount = 0;
posControl.waypointListValid = false; posControl.waypointListValid = false;
posControl.geoWaypointCount = 0; posControl.geoWaypointCount = 0;
wpMissionStartIndex = 0; // CR74
#ifdef USE_MULTI_MISSION #ifdef USE_MULTI_MISSION
posControl.loadedMultiMissionIndex = 0; posControl.loadedMultiMissionIndex = 0;
posControl.loadedMultiMissionStartWP = 0;
posControl.loadedMultiMissionWPCount = 0; posControl.loadedMultiMissionWPCount = 0;
#endif #endif
} }
@ -3184,24 +3180,69 @@ 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 setMultiMissionOnArm(void) void loadMultiMission(uint8_t missionIndex)
{ {
if (posControl.multiMissionCount > 1 && posControl.loadedMultiMissionWPCount) { uint8_t missionCount = 1;
posControl.waypointCount = posControl.loadedMultiMissionWPCount; posControl.loadedMultiMissionWPCount = 0;
posControl.geoWaypointCount = 0;
for (int8_t i = 0; i < NAV_MAX_WAYPOINTS; i++) { for (int i = 0; i < NAV_MAX_WAYPOINTS; i++) {
posControl.waypointList[i] = posControl.waypointList[i + posControl.loadedMultiMissionStartWP]; if ((missionCount == missionIndex)) {
if (i == posControl.waypointCount - 1) { 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_JUMP)) {
posControl.geoWaypointCount++;
}
/* store details of selected mission */
// mission start WP
if (posControl.loadedMultiMissionWPCount == 0) {
posControl.loadedMultiMissionWPCount = 1; // start marker only, value here unimportant (but not 0)
wpMissionStartIndex = i;
}
// mission end WP
if (posControl.waypointList[i].flag == NAV_WP_FLAG_LAST) {
posControl.loadedMultiMissionWPCount = i - wpMissionStartIndex + 1;
break; break;
} }
} else if (posControl.waypointList[i].flag == NAV_WP_FLAG_LAST) {
missionCount++;
} }
posControl.loadedMultiMissionStartWP = 0;
posControl.loadedMultiMissionWPCount = 0;
} }
if (posControl.multiMissionCount > 1 && posControl.loadedMultiMissionWPCount) {
posControl.waypointCount = posControl.loadedMultiMissionWPCount;
}
posControl.loadedMultiMissionIndex = posControl.multiMissionCount ? missionIndex : 0;
posControl.wpMissionRestart = true;
} }
void updateWpMissionChange(void)
{
if (!ARMING_FLAG(ARMED) || posControl.multiMissionCount <= 1) return;
if (!IS_RC_MODE_ACTIVE(BOXCHANGEMISSION)) {
if (posControl.loadedMultiMissionIndex != navConfig()->general.waypoint_multi_mission_index) {
loadMultiMission(navConfig()->general.waypoint_multi_mission_index);
} else if (posControl.waypointCount > posControl.loadedMultiMissionWPCount) {
posControl.waypointCount = posControl.loadedMultiMissionWPCount;
}
return;
}
static bool toggleFlag = true;
if (IS_RC_MODE_ACTIVE(BOXNAVWP) && toggleFlag) {
if (navConfig()->general.waypoint_multi_mission_index == posControl.multiMissionCount) {
navConfigMutable()->general.waypoint_multi_mission_index = 1;
} else {
selectMultiMissionIndex(1);
}
toggleFlag = false;
} else if (!IS_RC_MODE_ACTIVE(BOXNAVWP)) {
toggleFlag = true;
}
}
// 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) {
@ -3250,12 +3291,12 @@ bool loadNonVolatileWaypointList(bool clearIfLoaded)
// mission start WP // mission start WP
if (posControl.loadedMultiMissionWPCount == 0) { if (posControl.loadedMultiMissionWPCount == 0) {
posControl.loadedMultiMissionWPCount = 1; // start marker only, value here unimportant (but not 0) posControl.loadedMultiMissionWPCount = 1; // start marker only, value here unimportant (but not 0)
posControl.loadedMultiMissionStartWP = i; wpMissionStartIndex = i; // CR74
loadedMultiMissionGeoWPCount = posControl.geoWaypointCount; loadedMultiMissionGeoWPCount = posControl.geoWaypointCount;
} }
// 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 - posControl.loadedMultiMissionStartWP + 1; posControl.loadedMultiMissionWPCount = i - wpMissionStartIndex + 1; // CR74
loadedMultiMissionGeoWPCount = posControl.geoWaypointCount - loadedMultiMissionGeoWPCount + 1; loadedMultiMissionGeoWPCount = posControl.geoWaypointCount - loadedMultiMissionGeoWPCount + 1;
} }
} }
@ -3265,7 +3306,6 @@ bool loadNonVolatileWaypointList(bool clearIfLoaded)
break; break;
} }
} }
posControl.geoWaypointCount = loadedMultiMissionGeoWPCount; posControl.geoWaypointCount = loadedMultiMissionGeoWPCount;
posControl.loadedMultiMissionIndex = posControl.multiMissionCount ? navConfig()->general.waypoint_multi_mission_index : 0; posControl.loadedMultiMissionIndex = posControl.multiMissionCount ? navConfig()->general.waypoint_multi_mission_index : 0;
@ -3369,8 +3409,8 @@ static void calculateAndSetActiveWaypoint(const navWaypoint_t * waypoint)
/* Checks if active waypoint is last in mission */ /* Checks if active waypoint is last in mission */
bool isLastMissionWaypoint(void) bool isLastMissionWaypoint(void)
{ {
return FLIGHT_MODE(NAV_WP_MODE) && (posControl.activeWaypointIndex >= (posControl.waypointCount - 1) || return FLIGHT_MODE(NAV_WP_MODE) && (posControl.activeWaypointIndex >= (wpMissionStartIndex + posControl.waypointCount - 1) ||
(posControl.waypointList[posControl.activeWaypointIndex].flag == NAV_WP_FLAG_LAST)); (posControl.waypointList[posControl.activeWaypointIndex].flag == NAV_WP_FLAG_LAST)); // CR74
} }
/* Checks if Nav hold position is active */ /* Checks if Nav hold position is active */
@ -3481,7 +3521,7 @@ 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 = 0; posControl.activeWaypointIndex = wpMissionStartIndex; // CR74
// Reset RTH trackback // Reset RTH trackback
posControl.activeRthTBPointIndex = -1; posControl.activeRthTBPointIndex = -1;
posControl.flags.rthTrackbackActive = false; posControl.flags.rthTrackbackActive = false;
@ -3670,8 +3710,14 @@ 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 or BOXCHANGEMISSION mode active CR74
if (IS_RC_MODE_ACTIVE(BOXNAVWP) && !posControl.flags.wpMissionPlannerActive) { // Update WP mission change before activating WP mode // CR74
#ifdef USE_MULTI_MISSION
if (IS_RC_MODE_ACTIVE(BOXNAVWP) && !posControl.flags.wpMissionPlannerActive && !IS_RC_MODE_ACTIVE(BOXCHANGEMISSION)) { // CR74
updateWpMissionChange(); // CR74
#else
if (IS_RC_MODE_ACTIVE(BOXNAVWP) && !posControl.flags.wpMissionPlannerActive) { // CR74
#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;
} }
@ -3782,11 +3828,11 @@ bool navigationTerrainFollowingEnabled(void)
uint32_t distanceToFirstWP(void) uint32_t distanceToFirstWP(void)
{ {
fpVector3_t startingWaypointPos; fpVector3_t startingWaypointPos;
#ifdef USE_MULTI_MISSION // #ifdef USE_MULTI_MISSION
mapWaypointToLocalPosition(&startingWaypointPos, &posControl.waypointList[posControl.loadedMultiMissionStartWP], GEO_ALT_RELATIVE); mapWaypointToLocalPosition(&startingWaypointPos, &posControl.waypointList[wpMissionStartIndex], GEO_ALT_RELATIVE); // CR74
#else // #else
mapWaypointToLocalPosition(&startingWaypointPos, &posControl.waypointList[0], GEO_ALT_RELATIVE); // mapWaypointToLocalPosition(&startingWaypointPos, &posControl.waypointList[0], GEO_ALT_RELATIVE);
#endif // #endif
return calculateDistanceToDestination(&startingWaypointPos); return calculateDistanceToDestination(&startingWaypointPos);
} }
@ -3835,20 +3881,21 @@ 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.waypointCount;
#ifdef USE_MULTI_MISSION #ifdef USE_MULTI_MISSION
// Only perform check when mission loaded at start of posControl.waypointList wpCount = posControl.loadedMultiMissionWPCount;
if (posControl.waypointCount && !posControl.loadedMultiMissionStartWP) {
#else
if (posControl.waypointCount) {
#endif #endif
for (uint8_t wp = 0; wp < posControl.waypointCount; wp++){ if (wpCount) { // 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 == 0) || ((posControl.waypointList[wp].p1 > (wp-2)) && (posControl.waypointList[wp].p1 < (wp+2)) ) || (posControl.waypointList[wp].p1 >= posControl.waypointCount) || (posControl.waypointList[wp].p2 < -1)) { 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; 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)) {
return NAV_ARMING_BLOCKER_JUMP_WAYPOINT_ERROR; return NAV_ARMING_BLOCKER_JUMP_WAYPOINT_ERROR;
} }
} }
@ -3904,7 +3951,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; posControl.activeWaypointIndex = 0; // CR74 ????
resetTimerStart = millis(); resetTimerStart = millis();
} }
} }
@ -4103,9 +4150,9 @@ 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
#ifdef USE_MULTI_MISSION #ifdef USE_MULTI_MISSION
posControl.multiMissionCount = 0; posControl.multiMissionCount = 0;
posControl.loadedMultiMissionStartWP = 0;
#endif #endif
/* Set initial surface invalid */ /* Set initial surface invalid */
posControl.actualState.surfaceMin = -1.0f; posControl.actualState.surfaceMin = -1.0f;
@ -4265,7 +4312,7 @@ bool navigationRTHAllowsLanding(void)
{ {
// WP mission RTH landing setting // WP mission RTH landing setting
if (isWaypointMissionRTHActive() && isWaypointMissionValid()) { if (isWaypointMissionRTHActive() && isWaypointMissionValid()) {
return posControl.waypointList[posControl.waypointCount - 1].p1 > 0; return posControl.waypointList[wpMissionStartIndex + posControl.waypointCount - 1].p1 > 0; // CR74
} }
// normal RTH landing setting // normal RTH landing setting

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

@ -68,6 +68,8 @@ 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
#ifndef NAV_MAX_WAYPOINTS #ifndef NAV_MAX_WAYPOINTS
#define NAV_MAX_WAYPOINTS 15 #define NAV_MAX_WAYPOINTS 15
#endif #endif
@ -515,7 +517,6 @@ bool loadNonVolatileWaypointList(bool clearIfLoaded);
bool saveNonVolatileWaypointList(void); bool saveNonVolatileWaypointList(void);
#ifdef USE_MULTI_MISSION #ifdef USE_MULTI_MISSION
void selectMultiMissionIndex(int8_t increment); void selectMultiMissionIndex(int8_t increment);
void setMultiMissionOnArm(void);
#endif #endif
float getFinalRTHAltitude(void); float getFinalRTHAltitude(void);
int16_t fixedWingPitchToThrottleCorrection(int16_t pitch, timeUs_t currentTimeUs); int16_t fixedWingPitchToThrottleCorrection(int16_t pitch, timeUs_t currentTimeUs);

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

@ -393,7 +393,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 loadedMultiMissionStartWP; // selected multi mission start WP
int8_t loadedMultiMissionWPCount; // number of WPs in 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