mirror of
https://github.com/iNavFlight/inav.git
synced 2025-07-25 17:25:18 +03:00
fixes + flags
This commit is contained in:
breadoven
parent
a670d1d216
commit
09626ea0c5
7 changed files with 63 additions and 55 deletions
|
|
@ -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
|
||||
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.action); // action (WAYPOINT)
|
||||
sbufWriteU32(dst, msp_wp.lat); // lat
|
||||
|
|
|
|||
|
|
@ -95,7 +95,7 @@ static const box_t boxes[CHECKBOX_ITEM_COUNT + 1] = {
|
|||
{ .boxId = BOXAUTOLEVEL, .boxName = "AUTO LEVEL", .permanentId = 54 },
|
||||
{ .boxId = BOXPLANWPMISSION, .boxName = "WP PLANNER", .permanentId = 55 },
|
||||
{ .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 }
|
||||
};
|
||||
|
||||
|
|
@ -228,8 +228,8 @@ void initActiveBoxIds(void)
|
|||
ADD_ACTIVE_BOX(BOXHOMERESET);
|
||||
ADD_ACTIVE_BOX(BOXGCSNAV);
|
||||
ADD_ACTIVE_BOX(BOXPLANWPMISSION);
|
||||
#ifdef USE_MULTI_MISSION // CR74
|
||||
ADD_ACTIVE_BOX(BOXCHANGEMISSION); // CR74
|
||||
#ifdef USE_MULTI_MISSION
|
||||
ADD_ACTIVE_BOX(BOXCHANGEMISSION);
|
||||
#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(BOXPLANWPMISSION)), BOXPLANWPMISSION);
|
||||
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
|
||||
#ifdef USE_MULTI_MISSION
|
||||
CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXCHANGEMISSION)), BOXCHANGEMISSION);
|
||||
#endif
|
||||
|
||||
memset(mspBoxModeFlags, 0, sizeof(boxBitmask_t));
|
||||
|
|
|
|||
|
|
@ -75,7 +75,7 @@ typedef enum {
|
|||
BOXPLANWPMISSION = 46,
|
||||
BOXSOARING = 47,
|
||||
BOXUSER3 = 48,
|
||||
BOXCHANGEMISSION = 49, // CR74
|
||||
BOXCHANGEMISSION = 49,
|
||||
CHECKBOX_ITEM_COUNT
|
||||
} boxId_e;
|
||||
|
||||
|
|
|
|||
|
|
@ -1514,7 +1514,7 @@ int8_t getGeoWaypointNumber(int8_t waypointIndex)
|
|||
|
||||
if (waypointIndex != lastWaypointIndex) {
|
||||
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 ||
|
||||
posControl.waypointList[i].action == NAV_WP_ACTION_SET_HEAD ||
|
||||
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) {
|
||||
|
|
@ -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
|
||||
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;
|
||||
}
|
||||
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
|
||||
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
|
||||
tfp_sprintf(buff, "M%u ", posControl.loadedMultiMissionIndex);
|
||||
} 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);
|
||||
} else {
|
||||
// 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) {
|
||||
tfp_sprintf(buff, "M%u/%u>%2uWP", posControl.loadedMultiMissionIndex, posControl.multiMissionCount, wpCount);
|
||||
} else {
|
||||
|
|
|
|||
|
|
@ -91,7 +91,7 @@ PG_REGISTER_ARRAY(navSafeHome_t, MAX_SAFE_HOMES, safeHomeConfig, PG_SAFE_HOME_CO
|
|||
|
||||
#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
|
||||
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
|
||||
.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_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
|
||||
}
|
||||
);
|
||||
|
|
@ -1528,14 +1528,14 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_INITIALIZE(nav
|
|||
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.
|
||||
*/
|
||||
if (posControl.activeWaypointIndex == wpMissionStartIndex || posControl.wpMissionRestart) { // CR74
|
||||
if (posControl.activeWaypointIndex == wpMissionStartIndex || posControl.wpMissionRestart) {
|
||||
setupJumpCounters();
|
||||
posControl.activeWaypointIndex = wpMissionStartIndex; // CR74
|
||||
posControl.activeWaypointIndex = wpMissionStartIndex;
|
||||
wpHeadingControl.mode = NAV_WP_HEAD_MODE_NONE;
|
||||
}
|
||||
|
||||
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 {
|
||||
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)
|
||||
{
|
||||
/* 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;
|
||||
} else { // Finished non-geo, move to next WP
|
||||
posControl.activeWaypointIndex++;
|
||||
|
|
@ -1582,7 +1582,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_PRE_ACTION(nav
|
|||
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
|
||||
|
||||
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.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;
|
||||
if ((posControl.activeWaypointIndex >= 0) && (posControl.activeWaypointIndex < NAV_MAX_WAYPOINTS)) {
|
||||
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 (posControl.waypointList[posControl.activeWaypointIndex + 1].p3 != 0 ||
|
||||
posControl.waypointList[posControl.activeWaypointIndex + 1].p2 == -1) {
|
||||
nextWpIndex = posControl.waypointList[posControl.activeWaypointIndex + 1].p1 + wpMissionStartIndex; // CR74
|
||||
} else if (posControl.activeWaypointIndex + 2 <= wpMissionStartIndex + posControl.waypointCount - 1) { // CR74
|
||||
nextWpIndex = posControl.waypointList[posControl.activeWaypointIndex + 1].p1 + wpMissionStartIndex;
|
||||
} else if (posControl.activeWaypointIndex + 2 <= wpMissionStartIndex + posControl.waypointCount - 1) {
|
||||
if (posControl.waypointList[posControl.activeWaypointIndex + 2].action != NAV_WP_ACTION_JUMP) {
|
||||
nextWpIndex++;
|
||||
} else {
|
||||
|
|
@ -2940,7 +2940,7 @@ static bool adjustAltitudeFromRCInput(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){
|
||||
posControl.waypointList[wp].p3 = posControl.waypointList[wp].p2;
|
||||
}
|
||||
|
|
@ -2955,7 +2955,7 @@ static void resetJumpCounter(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) {
|
||||
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
|
||||
else if ((wpNumber >= 1) && (wpNumber <= NAV_MAX_WAYPOINTS)) {
|
||||
if (wpNumber <= posControl.waypointCount + wpMissionStartIndex) { // CR74
|
||||
if (wpNumber <= posControl.waypointCount + wpMissionStartIndex) {
|
||||
*wpData = posControl.waypointList[wpNumber - 1];
|
||||
if(wpData->action == NAV_WP_ACTION_JUMP) {
|
||||
wpData->p1 += 1; // make WP # (vice index)
|
||||
|
|
@ -3156,9 +3156,10 @@ void resetWaypointList(void)
|
|||
posControl.waypointCount = 0;
|
||||
posControl.waypointListValid = false;
|
||||
posControl.geoWaypointCount = 0;
|
||||
wpMissionStartIndex = 0; // CR74
|
||||
posControl.loadedMissionWPCount = 0; // CR74
|
||||
wpMissionStartIndex = 0;
|
||||
posControl.loadedMissionWPCount = 0;
|
||||
#ifdef USE_MULTI_MISSION
|
||||
posControl.totalMultiMissionWpCount = 0;
|
||||
posControl.loadedMultiMissionIndex = 0;
|
||||
#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);
|
||||
}
|
||||
}
|
||||
// CR74
|
||||
|
||||
void loadSelectedMultiMission(uint8_t missionIndex)
|
||||
{
|
||||
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.wpMissionRestart = true;
|
||||
}
|
||||
|
||||
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;
|
||||
return true;
|
||||
}
|
||||
|
||||
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);
|
||||
} else if (posControl.waypointCount > posControl.loadedMissionWPCount) {
|
||||
} else if (posControl.waypointCount != posControl.loadedMissionWPCount) {
|
||||
posControl.waypointCount = posControl.loadedMissionWPCount;
|
||||
}
|
||||
return true;
|
||||
|
|
@ -3247,7 +3247,7 @@ bool updateWpMissionChange(void)
|
|||
}
|
||||
return false;
|
||||
}
|
||||
// CR74
|
||||
|
||||
bool checkMissionCount(int8_t waypoint)
|
||||
{
|
||||
if (nonVolatileWaypointList(waypoint)->flag == NAV_WP_FLAG_LAST) {
|
||||
|
|
@ -3295,8 +3295,8 @@ bool loadNonVolatileWaypointList(bool clearIfLoaded)
|
|||
break;
|
||||
}
|
||||
}
|
||||
posControl.totalMultiMissionWpCount = posControl.waypointCount;
|
||||
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
|
||||
* Also reset if no selected mission loaded (shouldn't happen) */
|
||||
|
|
@ -3307,7 +3307,7 @@ bool loadNonVolatileWaypointList(bool clearIfLoaded)
|
|||
break;
|
||||
}
|
||||
}
|
||||
posControl.loadedMissionWPCount = posControl.waypointCount; // CR74
|
||||
posControl.loadedMissionWPCount = posControl.waypointCount;
|
||||
|
||||
// Mission sanity check failed - reset the list
|
||||
if (!posControl.waypointListValid) {
|
||||
|
|
@ -3401,7 +3401,7 @@ static void calculateAndSetActiveWaypoint(const navWaypoint_t * waypoint)
|
|||
bool isLastMissionWaypoint(void)
|
||||
{
|
||||
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 */
|
||||
|
|
@ -3512,11 +3512,18 @@ void applyWaypointNavigationAndAltitudeHold(void)
|
|||
posControl.flags.forcedRTHActivated = false;
|
||||
posControl.flags.forcedEmergLandingActivated = false;
|
||||
// ensure WP missions always restart from first waypoint after disarm
|
||||
posControl.activeWaypointIndex = wpMissionStartIndex; // CR74
|
||||
posControl.activeWaypointIndex = wpMissionStartIndex;
|
||||
// Reset RTH trackback
|
||||
posControl.activeRthTBPointIndex = -1;
|
||||
posControl.flags.rthTrackbackActive = false;
|
||||
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;
|
||||
}
|
||||
|
|
@ -3702,11 +3709,11 @@ static navigationFSMEvent_t selectNavEventFromBoxModeInput(void)
|
|||
|
||||
// Pilot-activated waypoint mission. Fall-back to RTH in case of no mission loaded
|
||||
// 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
|
||||
if (updateWpMissionChange() && IS_RC_MODE_ACTIVE(BOXNAVWP) && !posControl.flags.wpMissionPlannerActive) { // CR74
|
||||
if (updateWpMissionChange() && IS_RC_MODE_ACTIVE(BOXNAVWP) && !posControl.flags.wpMissionPlannerActive) {
|
||||
#else
|
||||
if (IS_RC_MODE_ACTIVE(BOXNAVWP) && !posControl.flags.wpMissionPlannerActive) { // CR74
|
||||
if (IS_RC_MODE_ACTIVE(BOXNAVWP) && !posControl.flags.wpMissionPlannerActive) {
|
||||
#endif
|
||||
if (FLIGHT_MODE(NAV_WP_MODE) || (canActivateWaypoint && canActivateNavigation && canActivateAltHold && STATE(GPS_FIX_HOME)))
|
||||
return NAV_FSM_EVENT_SWITCH_TO_WAYPOINT;
|
||||
|
|
@ -3818,7 +3825,7 @@ bool navigationTerrainFollowingEnabled(void)
|
|||
uint32_t distanceToFirstWP(void)
|
||||
{
|
||||
fpVector3_t startingWaypointPos;
|
||||
mapWaypointToLocalPosition(&startingWaypointPos, &posControl.waypointList[wpMissionStartIndex], GEO_ALT_RELATIVE); // CR74
|
||||
mapWaypointToLocalPosition(&startingWaypointPos, &posControl.waypointList[wpMissionStartIndex], GEO_ALT_RELATIVE);
|
||||
return calculateDistanceToDestination(&startingWaypointPos);
|
||||
}
|
||||
|
||||
|
|
@ -3868,15 +3875,15 @@ navArmingBlocker_e navigationIsBlockingArming(bool *usedBypass)
|
|||
* Only jump to geo-referenced WP types
|
||||
*/
|
||||
uint8_t wpCount = posControl.loadedMissionWPCount;
|
||||
if (wpCount) { // CR74
|
||||
for (uint8_t wp = wpMissionStartIndex; wp < wpCount + wpMissionStartIndex; wp++){ // CR74
|
||||
if (wpCount) {
|
||||
for (uint8_t wp = wpMissionStartIndex; wp < wpCount + wpMissionStartIndex; wp++){
|
||||
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;
|
||||
}
|
||||
|
||||
/* 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)) {
|
||||
return NAV_ARMING_BLOCKER_JUMP_WAYPOINT_ERROR;
|
||||
}
|
||||
|
|
@ -3933,7 +3940,7 @@ void updateWpMissionPlanner(void)
|
|||
}
|
||||
} else if (posControl.flags.wpMissionPlannerActive) {
|
||||
posControl.flags.wpMissionPlannerActive = false;
|
||||
posControl.activeWaypointIndex = 0; // CR74 ????
|
||||
posControl.activeWaypointIndex = 0;
|
||||
resetTimerStart = millis();
|
||||
}
|
||||
}
|
||||
|
|
@ -3964,7 +3971,7 @@ void missionPlannerSetWaypoint(void)
|
|||
posControl.waypointList[posControl.wpPlannerActiveWPIndex].p3 = 1; // use absolute altitude datum
|
||||
posControl.waypointList[posControl.wpPlannerActiveWPIndex].flag = NAV_WP_FLAG_LAST;
|
||||
posControl.waypointListValid = true;
|
||||
wpMissionStartIndex = 0; // CR74
|
||||
wpMissionStartIndex = 0;
|
||||
|
||||
if (posControl.wpPlannerActiveWPIndex) {
|
||||
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.wpPlannerActiveWPIndex = 0;
|
||||
posControl.flags.wpMissionPlannerActive = false;
|
||||
wpMissionStartIndex = 0; // CR74
|
||||
wpMissionStartIndex = 0;
|
||||
#ifdef USE_MULTI_MISSION
|
||||
posControl.multiMissionCount = 0;
|
||||
#endif
|
||||
|
|
@ -4295,7 +4302,7 @@ bool navigationRTHAllowsLanding(void)
|
|||
{
|
||||
// WP mission RTH landing setting
|
||||
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
|
||||
|
|
|
|||
|
|
@ -68,7 +68,7 @@ bool findNearestSafeHome(void); // Find nearest safehome
|
|||
|
||||
#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
|
||||
#define NAV_MAX_WAYPOINTS 15
|
||||
|
|
|
|||
|
|
@ -382,8 +382,8 @@ typedef struct {
|
|||
/* Waypoint list */
|
||||
navWaypoint_t waypointList[NAV_MAX_WAYPOINTS];
|
||||
bool waypointListValid;
|
||||
int8_t waypointCount; // number of WPs loaded from WP source CR74
|
||||
int8_t loadedMissionWPCount; // number of WPs in loaded mission CR74
|
||||
int8_t waypointCount; // number of WPs loaded from WP source
|
||||
int8_t loadedMissionWPCount; // number of WPs in loaded mission
|
||||
int8_t geoWaypointCount; // total geospatial WPs in mission
|
||||
bool wpMissionRestart; // mission restart from first waypoint
|
||||
|
||||
|
|
@ -394,6 +394,7 @@ typedef struct {
|
|||
/* Multi Missions */
|
||||
int8_t multiMissionCount; // number of missions in multi mission entry
|
||||
int8_t loadedMultiMissionIndex; // index of selected multi mission
|
||||
int8_t totalMultiMissionWpCount; // total number of waypoints in all multi missions
|
||||
#endif
|
||||
navWaypointPosition_t activeWaypoint; // Local position, current bearing and turn angle to next WP, filled on waypoint activation
|
||||
int8_t activeWaypointIndex;
|
||||
|
|
|
|||
Loading…
Add table
Add a link
Reference in a new issue