mirror of
https://github.com/iNavFlight/inav.git
synced 2025-07-23 16:25:26 +03:00
first build
This commit is contained in:
breadoven
parent
e3e52c49c6
commit
f357482966
8 changed files with 120 additions and 68 deletions
|
|
@ -265,7 +265,7 @@ static void updateArmingStatus(void)
|
|||
|
||||
/* CHECK: */
|
||||
if (
|
||||
sensors(SENSOR_ACC) &&
|
||||
sensors(SENSOR_ACC) &&
|
||||
!STATE(ACCELEROMETER_CALIBRATED) &&
|
||||
// Require ACC calibration only if any of the setting might require it
|
||||
(
|
||||
|
|
@ -525,9 +525,6 @@ void releaseSharedTelemetryPorts(void) {
|
|||
|
||||
void tryArm(void)
|
||||
{
|
||||
#ifdef USE_MULTI_MISSION
|
||||
setMultiMissionOnArm();
|
||||
#endif
|
||||
updateArmingStatus();
|
||||
|
||||
#ifdef USE_DSHOT
|
||||
|
|
|
|||
|
|
@ -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
|
||||
navWaypoint_t msp_wp;
|
||||
getWaypoint(msp_wp_no, &msp_wp);
|
||||
sbufWriteU8(dst, msp_wp_no); // wp_no
|
||||
getWaypoint(msp_wp_no + wpMissionStartIndex, &msp_wp); // CR74
|
||||
sbufWriteU8(dst, msp_wp_no); // wp_no
|
||||
sbufWriteU8(dst, msp_wp.action); // action (WAYPOINT)
|
||||
sbufWriteU32(dst, msp_wp.lat); // lat
|
||||
sbufWriteU32(dst, msp_wp.lon); // lon
|
||||
|
|
|
|||
|
|
@ -95,6 +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 = CHECKBOX_ITEM_COUNT, .boxName = NULL, .permanentId = 0xFF }
|
||||
};
|
||||
|
||||
|
|
@ -227,6 +228,9 @@ 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
|
||||
#endif
|
||||
}
|
||||
|
||||
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(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
|
||||
#endif
|
||||
|
||||
memset(mspBoxModeFlags, 0, sizeof(boxBitmask_t));
|
||||
for (uint32_t i = 0; i < activeBoxIdCount; i++) {
|
||||
|
|
|
|||
|
|
@ -75,6 +75,7 @@ typedef enum {
|
|||
BOXPLANWPMISSION = 46,
|
||||
BOXSOARING = 47,
|
||||
BOXUSER3 = 48,
|
||||
BOXCHANGEMISSION = 49, // CR74
|
||||
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 = 0; i <= waypointIndex; i++) {
|
||||
for (uint8_t i = wpMissionStartIndex; i <= waypointIndex; i++) { // CR74
|
||||
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 + 1;
|
||||
return geoWaypointIndex - wpMissionStartIndex + 1; // CR74
|
||||
}
|
||||
|
||||
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 > 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;
|
||||
}
|
||||
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)){
|
||||
if (ARMING_FLAG(ARMED) && !IS_RC_MODE_ACTIVE(BOXCHANGEMISSION)){ // CR74
|
||||
// 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){
|
||||
|
|
|
|||
|
|
@ -91,6 +91,8 @@ PG_REGISTER_ARRAY(navSafeHome_t, MAX_SAFE_HOMES, safeHomeConfig, PG_SAFE_HOME_CO
|
|||
|
||||
#endif
|
||||
|
||||
uint8_t wpMissionStartIndex = 0; // WP mode start waypoint index CR74
|
||||
|
||||
// waypoint 254, 255 are special waypoints
|
||||
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
|
||||
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();
|
||||
posControl.activeWaypointIndex = 0;
|
||||
posControl.activeWaypointIndex = wpMissionStartIndex; // CR74
|
||||
wpHeadingControl.mode = NAV_WP_HEAD_MODE_NONE;
|
||||
}
|
||||
|
||||
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 {
|
||||
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)
|
||||
{
|
||||
/* 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 (isLastWaypoint) {
|
||||
// non-geo state is the last waypoint, switch to finish.
|
||||
/* simple helper for non-geographical states that just set other data */
|
||||
if (isLastMissionWaypoint()) { // non-geo state is the last waypoint, switch to finish. CR74
|
||||
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++;
|
||||
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)
|
||||
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){
|
||||
resetJumpCounter();
|
||||
return nextForNonGeoStates();
|
||||
|
|
@ -1584,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;
|
||||
posControl.activeWaypointIndex = posControl.waypointList[posControl.activeWaypointIndex].p1 + wpMissionStartIndex; // CR74
|
||||
return NAV_FSM_EVENT_NONE; // re-process the state passing to the next WP
|
||||
|
||||
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.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;
|
||||
if ((posControl.activeWaypointIndex >= 0) && (posControl.activeWaypointIndex < NAV_MAX_WAYPOINTS)) {
|
||||
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 (posControl.waypointList[posControl.activeWaypointIndex + 1].p3 != 0 ||
|
||||
posControl.waypointList[posControl.activeWaypointIndex + 1].p2 == -1) {
|
||||
nextWpIndex = posControl.waypointList[posControl.activeWaypointIndex + 1].p1;
|
||||
} else if (posControl.activeWaypointIndex + 2 <= posControl.waypointCount - 1) {
|
||||
nextWpIndex = posControl.waypointList[posControl.activeWaypointIndex + 1].p1 + wpMissionStartIndex; // CR74
|
||||
} else if (posControl.activeWaypointIndex + 2 <= wpMissionStartIndex + posControl.waypointCount - 1) { // CR74
|
||||
if (posControl.waypointList[posControl.activeWaypointIndex + 2].action != NAV_WP_ACTION_JUMP) {
|
||||
nextWpIndex++;
|
||||
} else {
|
||||
|
|
@ -2942,7 +2940,7 @@ static bool adjustAltitudeFromRCInput(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){
|
||||
posControl.waypointList[wp].p3 = posControl.waypointList[wp].p2;
|
||||
}
|
||||
|
|
@ -2952,13 +2950,12 @@ static void setupJumpCounters(void)
|
|||
static void resetJumpCounter(void)
|
||||
{
|
||||
// reset the volatile counter from the set / static value
|
||||
posControl.waypointList[posControl.activeWaypointIndex].p3 =
|
||||
posControl.waypointList[posControl.activeWaypointIndex].p2;
|
||||
posControl.waypointList[posControl.activeWaypointIndex].p3 = posControl.waypointList[posControl.activeWaypointIndex].p2;
|
||||
}
|
||||
|
||||
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) {
|
||||
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
|
||||
else if ((wpNumber >= 1) && (wpNumber <= NAV_MAX_WAYPOINTS)) {
|
||||
if (wpNumber <= posControl.waypointCount) {
|
||||
if (wpNumber <= posControl.waypointCount + wpMissionStartIndex) { // CR74
|
||||
*wpData = posControl.waypointList[wpNumber - 1];
|
||||
if(wpData->action == NAV_WP_ACTION_JUMP) {
|
||||
wpData->p1 += 1; // make WP # (vice index)
|
||||
}
|
||||
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
@ -3160,9 +3156,9 @@ void resetWaypointList(void)
|
|||
posControl.waypointCount = 0;
|
||||
posControl.waypointListValid = false;
|
||||
posControl.geoWaypointCount = 0;
|
||||
wpMissionStartIndex = 0; // CR74
|
||||
#ifdef USE_MULTI_MISSION
|
||||
posControl.loadedMultiMissionIndex = 0;
|
||||
posControl.loadedMultiMissionStartWP = 0;
|
||||
posControl.loadedMultiMissionWPCount = 0;
|
||||
#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);
|
||||
}
|
||||
}
|
||||
|
||||
void setMultiMissionOnArm(void)
|
||||
// CR74
|
||||
void loadMultiMission(uint8_t missionIndex)
|
||||
{
|
||||
if (posControl.multiMissionCount > 1 && posControl.loadedMultiMissionWPCount) {
|
||||
posControl.waypointCount = posControl.loadedMultiMissionWPCount;
|
||||
uint8_t missionCount = 1;
|
||||
posControl.loadedMultiMissionWPCount = 0;
|
||||
posControl.geoWaypointCount = 0;
|
||||
|
||||
for (int8_t i = 0; i < NAV_MAX_WAYPOINTS; i++) {
|
||||
posControl.waypointList[i] = posControl.waypointList[i + posControl.loadedMultiMissionStartWP];
|
||||
if (i == posControl.waypointCount - 1) {
|
||||
for (int i = 0; i < NAV_MAX_WAYPOINTS; i++) {
|
||||
if ((missionCount == missionIndex)) {
|
||||
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;
|
||||
}
|
||||
} 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)
|
||||
{
|
||||
if (nonVolatileWaypointList(waypoint)->flag == NAV_WP_FLAG_LAST) {
|
||||
|
|
@ -3250,12 +3291,12 @@ bool loadNonVolatileWaypointList(bool clearIfLoaded)
|
|||
// mission start WP
|
||||
if (posControl.loadedMultiMissionWPCount == 0) {
|
||||
posControl.loadedMultiMissionWPCount = 1; // start marker only, value here unimportant (but not 0)
|
||||
posControl.loadedMultiMissionStartWP = i;
|
||||
wpMissionStartIndex = i; // CR74
|
||||
loadedMultiMissionGeoWPCount = posControl.geoWaypointCount;
|
||||
}
|
||||
// mission end WP
|
||||
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;
|
||||
}
|
||||
}
|
||||
|
|
@ -3265,7 +3306,6 @@ bool loadNonVolatileWaypointList(bool clearIfLoaded)
|
|||
break;
|
||||
}
|
||||
}
|
||||
|
||||
posControl.geoWaypointCount = loadedMultiMissionGeoWPCount;
|
||||
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 */
|
||||
bool isLastMissionWaypoint(void)
|
||||
{
|
||||
return FLIGHT_MODE(NAV_WP_MODE) && (posControl.activeWaypointIndex >= (posControl.waypointCount - 1) ||
|
||||
(posControl.waypointList[posControl.activeWaypointIndex].flag == NAV_WP_FLAG_LAST));
|
||||
return FLIGHT_MODE(NAV_WP_MODE) && (posControl.activeWaypointIndex >= (wpMissionStartIndex + posControl.waypointCount - 1) ||
|
||||
(posControl.waypointList[posControl.activeWaypointIndex].flag == NAV_WP_FLAG_LAST)); // CR74
|
||||
}
|
||||
|
||||
/* Checks if Nav hold position is active */
|
||||
|
|
@ -3481,7 +3521,7 @@ void applyWaypointNavigationAndAltitudeHold(void)
|
|||
posControl.flags.forcedRTHActivated = false;
|
||||
posControl.flags.forcedEmergLandingActivated = false;
|
||||
// ensure WP missions always restart from first waypoint after disarm
|
||||
posControl.activeWaypointIndex = 0;
|
||||
posControl.activeWaypointIndex = wpMissionStartIndex; // CR74
|
||||
// Reset RTH trackback
|
||||
posControl.activeRthTBPointIndex = -1;
|
||||
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
|
||||
// Block activation if using WP Mission Planner
|
||||
if (IS_RC_MODE_ACTIVE(BOXNAVWP) && !posControl.flags.wpMissionPlannerActive) {
|
||||
// Block activation if using WP Mission Planner or BOXCHANGEMISSION mode active CR74
|
||||
// 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)))
|
||||
return NAV_FSM_EVENT_SWITCH_TO_WAYPOINT;
|
||||
}
|
||||
|
|
@ -3782,11 +3828,11 @@ bool navigationTerrainFollowingEnabled(void)
|
|||
uint32_t distanceToFirstWP(void)
|
||||
{
|
||||
fpVector3_t startingWaypointPos;
|
||||
#ifdef USE_MULTI_MISSION
|
||||
mapWaypointToLocalPosition(&startingWaypointPos, &posControl.waypointList[posControl.loadedMultiMissionStartWP], GEO_ALT_RELATIVE);
|
||||
#else
|
||||
mapWaypointToLocalPosition(&startingWaypointPos, &posControl.waypointList[0], GEO_ALT_RELATIVE);
|
||||
#endif
|
||||
// #ifdef USE_MULTI_MISSION
|
||||
mapWaypointToLocalPosition(&startingWaypointPos, &posControl.waypointList[wpMissionStartIndex], GEO_ALT_RELATIVE); // CR74
|
||||
// #else
|
||||
// mapWaypointToLocalPosition(&startingWaypointPos, &posControl.waypointList[0], GEO_ALT_RELATIVE);
|
||||
// #endif
|
||||
return calculateDistanceToDestination(&startingWaypointPos);
|
||||
}
|
||||
|
||||
|
|
@ -3835,20 +3881,21 @@ navArmingBlocker_e navigationIsBlockingArming(bool *usedBypass)
|
|||
* Can't jump beyond WP list
|
||||
* Only jump to geo-referenced WP types
|
||||
*/
|
||||
// CR74
|
||||
uint8_t wpCount = posControl.waypointCount;
|
||||
#ifdef USE_MULTI_MISSION
|
||||
// Only perform check when mission loaded at start of posControl.waypointList
|
||||
if (posControl.waypointCount && !posControl.loadedMultiMissionStartWP) {
|
||||
#else
|
||||
if (posControl.waypointCount) {
|
||||
wpCount = posControl.loadedMultiMissionWPCount;
|
||||
#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((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;
|
||||
}
|
||||
// CR74
|
||||
/* check for target geo-ref sanity */
|
||||
uint16_t target = posControl.waypointList[wp].p1;
|
||||
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)) {
|
||||
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)) {
|
||||
return NAV_ARMING_BLOCKER_JUMP_WAYPOINT_ERROR;
|
||||
}
|
||||
}
|
||||
|
|
@ -3904,7 +3951,7 @@ void updateWpMissionPlanner(void)
|
|||
}
|
||||
} else if (posControl.flags.wpMissionPlannerActive) {
|
||||
posControl.flags.wpMissionPlannerActive = false;
|
||||
posControl.activeWaypointIndex = 0;
|
||||
posControl.activeWaypointIndex = 0; // CR74 ????
|
||||
resetTimerStart = millis();
|
||||
}
|
||||
}
|
||||
|
|
@ -4103,9 +4150,9 @@ void navigationInit(void)
|
|||
posControl.waypointListValid = false;
|
||||
posControl.wpPlannerActiveWPIndex = 0;
|
||||
posControl.flags.wpMissionPlannerActive = false;
|
||||
wpMissionStartIndex = 0; // CR74
|
||||
#ifdef USE_MULTI_MISSION
|
||||
posControl.multiMissionCount = 0;
|
||||
posControl.loadedMultiMissionStartWP = 0;
|
||||
#endif
|
||||
/* Set initial surface invalid */
|
||||
posControl.actualState.surfaceMin = -1.0f;
|
||||
|
|
@ -4265,7 +4312,7 @@ bool navigationRTHAllowsLanding(void)
|
|||
{
|
||||
// WP mission RTH landing setting
|
||||
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
|
||||
|
|
|
|||
|
|
@ -68,6 +68,8 @@ bool findNearestSafeHome(void); // Find nearest safehome
|
|||
|
||||
#endif // defined(USE_SAFE_HOME)
|
||||
|
||||
extern uint8_t wpMissionStartIndex; // first waypoint index in mission CR74
|
||||
|
||||
#ifndef NAV_MAX_WAYPOINTS
|
||||
#define NAV_MAX_WAYPOINTS 15
|
||||
#endif
|
||||
|
|
@ -515,7 +517,6 @@ bool loadNonVolatileWaypointList(bool clearIfLoaded);
|
|||
bool saveNonVolatileWaypointList(void);
|
||||
#ifdef USE_MULTI_MISSION
|
||||
void selectMultiMissionIndex(int8_t increment);
|
||||
void setMultiMissionOnArm(void);
|
||||
#endif
|
||||
float getFinalRTHAltitude(void);
|
||||
int16_t fixedWingPitchToThrottleCorrection(int16_t pitch, timeUs_t currentTimeUs);
|
||||
|
|
|
|||
|
|
@ -393,7 +393,6 @@ typedef struct {
|
|||
/* Multi Missions */
|
||||
int8_t multiMissionCount; // number of missions in multi mission entry
|
||||
int8_t loadedMultiMissionIndex; // index of selected multi mission
|
||||
int8_t loadedMultiMissionStartWP; // selected multi mission start WP
|
||||
int8_t loadedMultiMissionWPCount; // number of WPs in selected multi mission
|
||||
#endif
|
||||
navWaypointPosition_t activeWaypoint; // Local position, current bearing and turn angle to next WP, filled on waypoint activation
|
||||
|
|
|
|||
Loading…
Add table
Add a link
Reference in a new issue