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

Merge branch 'master' into dzikuvx-nav-cruise-improvements

Browse source
This commit is contained in:
Pawel Spychalski (DzikuVx) 2020-07-03 13:48:25 +02:00
commit 6f4e5bef0a
46 changed files with 791 additions and 86 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -172,8 +172,9 @@ PG_RESET_TEMPLATE(navConfig_t, navConfig,
}
);
EXTENDED_FASTRAM navigationPosControl_t posControl;
EXTENDED_FASTRAM navSystemStatus_t NAV_Status;
static navWapointHeading_t wpHeadingControl;
navigationPosControl_t posControl;
navSystemStatus_t NAV_Status;
EXTENDED_FASTRAM multicopterPosXyCoefficients_t multicopterPosXyCoefficients;
#if defined(NAV_BLACKBOX)
@ -209,6 +210,8 @@ void calculateInitialHoldPosition(fpVector3_t * pos);
void calculateFarAwayTarget(fpVector3_t * farAwayPos, int32_t yaw, int32_t distance);
void calculateNewCruiseTarget(fpVector3_t * origin, int32_t yaw, int32_t distance);
static bool isWaypointPositionReached(const fpVector3_t * pos, const bool isWaypointHome);
static void mapWaypointToLocalPosition(fpVector3_t * localPos, const navWaypoint_t * waypoint);
static navigationFSMEvent_t nextForNonGeoStates(void);
void initializeRTHSanityChecker(const fpVector3_t * pos);
bool validateRTHSanityChecker(void);
@ -1416,6 +1419,21 @@ 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.
return NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_FINISHED;
} else {
// Finished non-geo, move to next WP
posControl.activeWaypointIndex++;
return NAV_FSM_EVENT_NONE; // re-process the state passing to the next WP
}
}
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_PRE_ACTION(navigationFSMState_t previousState)
{
/* A helper function to do waypoint-specific action */
@ -1435,28 +1453,36 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_PRE_ACTION(nav
if(posControl.waypointList[posControl.activeWaypointIndex].p3 != -1){
if(posControl.waypointList[posControl.activeWaypointIndex].p3 == 0){
resetJumpCounter();
const bool isLastWaypoint = (posControl.waypointList[posControl.activeWaypointIndex].flag == NAV_WP_FLAG_LAST) ||
(posControl.activeWaypointIndex >= (posControl.waypointCount - 1));
if (isLastWaypoint) {
// JUMP is the last waypoint and we reached the last jump, switch to finish.
return NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_FINISHED;
} else {
// Finished JUMP, move to next WP
posControl.activeWaypointIndex++;
return NAV_FSM_EVENT_NONE; // re-process the state passing to the next WP
}
return nextForNonGeoStates();
}
else
{
posControl.waypointList[posControl.activeWaypointIndex].p3--;
}
}
posControl.activeWaypointIndex = posControl.waypointList[posControl.activeWaypointIndex].p1;
return NAV_FSM_EVENT_NONE; // re-process the state passing to the next WP
case NAV_WP_ACTION_SET_POI:
if (STATE(MULTIROTOR)) {
wpHeadingControl.mode = NAV_WP_HEAD_MODE_POI;
mapWaypointToLocalPosition(&wpHeadingControl.poi_pos,
&posControl.waypointList[posControl.activeWaypointIndex]);
}
return nextForNonGeoStates();
case NAV_WP_ACTION_SET_HEAD:
if (STATE(MULTIROTOR)) {
if (posControl.waypointList[posControl.activeWaypointIndex].p1 < 0 ||
posControl.waypointList[posControl.activeWaypointIndex].p1 > 359) {
wpHeadingControl.mode = NAV_WP_HEAD_MODE_NONE;
} else {
wpHeadingControl.mode = NAV_WP_HEAD_MODE_FIXED;
wpHeadingControl.heading = DEGREES_TO_CENTIDEGREES(posControl.waypointList[posControl.activeWaypointIndex].p1);
}
}
return nextForNonGeoStates();
case NAV_WP_ACTION_RTH:
posControl.rthState.rthInitialDistance = posControl.homeDistance;
initializeRTHSanityChecker(&navGetCurrentActualPositionAndVelocity()->pos);
@ -1488,11 +1514,25 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_IN_PROGRESS(na
posControl.wpInitialDistance, posControl.wpInitialDistance / 10.0f,
posControl.wpInitialAltitude, posControl.activeWaypoint.pos.z);
setDesiredPosition(&tmpWaypoint, 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_BEARING);
if(STATE(MULTIROTOR)) {
switch (wpHeadingControl.mode) {
case NAV_WP_HEAD_MODE_NONE:
break;
case NAV_WP_HEAD_MODE_FIXED:
setDesiredPosition(NULL, wpHeadingControl.heading, NAV_POS_UPDATE_HEADING);
break;
case NAV_WP_HEAD_MODE_POI:
setDesiredPosition(&wpHeadingControl.poi_pos, 0, NAV_POS_UPDATE_BEARING);
break;
}
}
return NAV_FSM_EVENT_NONE; // will re-process state in >10ms
}
break;
case NAV_WP_ACTION_JUMP:
case NAV_WP_ACTION_SET_HEAD:
case NAV_WP_ACTION_SET_POI:
UNREACHABLE();
case NAV_WP_ACTION_RTH:
@ -1530,6 +1570,8 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_REACHED(naviga
return NAV_FSM_EVENT_SUCCESS; // NAV_STATE_WAYPOINT_NEXT
case NAV_WP_ACTION_JUMP:
case NAV_WP_ACTION_SET_HEAD:
case NAV_WP_ACTION_SET_POI:
UNREACHABLE();
case NAV_WP_ACTION_RTH:
@ -2765,7 +2807,7 @@ void setWaypoint(uint8_t wpNumber, const navWaypoint_t * wpData)
}
// WP #1 - #NAV_MAX_WAYPOINTS - common waypoints - pre-programmed mission
else if ((wpNumber >= 1) && (wpNumber <= NAV_MAX_WAYPOINTS) && !ARMING_FLAG(ARMED)) {
if (wpData->action == NAV_WP_ACTION_WAYPOINT || wpData->action == NAV_WP_ACTION_JUMP || wpData->action == NAV_WP_ACTION_RTH || wpData->action == NAV_WP_ACTION_HOLD_TIME || wpData->action == NAV_WP_ACTION_LAND) {
if (wpData->action == NAV_WP_ACTION_WAYPOINT || wpData->action == NAV_WP_ACTION_JUMP || wpData->action == NAV_WP_ACTION_RTH || wpData->action == NAV_WP_ACTION_HOLD_TIME || wpData->action == NAV_WP_ACTION_LAND || wpData->action == NAV_WP_ACTION_SET_POI || wpData->action == NAV_WP_ACTION_SET_HEAD ) {
// Only allow upload next waypoint (continue upload mission) or first waypoint (new mission)
if (wpNumber == (posControl.waypointCount + 1) || wpNumber == 1) {
posControl.waypointList[wpNumber - 1] = *wpData;