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

Merge branch 'development' into dzikuvx-nav-yaw-adjustments

Browse source
This commit is contained in:
Pawel Spychalski (DzikuVx) 2020-03-04 13:37:35 +01:00
commit b5c87169b3
74 changed files with 1005 additions and 2502 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -619,12 +619,13 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
[NAV_STATE_WAYPOINT_PRE_ACTION] = {
.persistentId = NAV_PERSISTENT_ID_WAYPOINT_PRE_ACTION,
.onEntry = navOnEnteringState_NAV_STATE_WAYPOINT_PRE_ACTION,
.timeoutMs = 0,
.timeoutMs = 10,
.stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_WP,
.mapToFlightModes = NAV_WP_MODE | NAV_ALTHOLD_MODE,
.mwState = MW_NAV_STATE_PROCESS_NEXT,
.mwError = MW_NAV_ERROR_NONE,
.onEvent = {
[NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_WAYPOINT_PRE_ACTION, // re-process the state (for JUMP)
[NAV_FSM_EVENT_SUCCESS] = NAV_STATE_WAYPOINT_IN_PROGRESS,
[NAV_FSM_EVENT_ERROR] = NAV_STATE_IDLE,
[NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE,
@ -1369,20 +1370,46 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_PRE_ACTION(nav
/* A helper function to do waypoint-specific action */
UNUSED(previousState);
switch (posControl.waypointList[posControl.activeWaypointIndex].action) {
switch ((navWaypointActions_e)posControl.waypointList[posControl.activeWaypointIndex].action) {
case NAV_WP_ACTION_WAYPOINT:
calculateAndSetActiveWaypoint(&posControl.waypointList[posControl.activeWaypointIndex]);
posControl.wpInitialDistance = calculateDistanceToDestination(&posControl.activeWaypoint.pos);
posControl.wpInitialAltitude = posControl.actualState.abs.pos.z;
return NAV_FSM_EVENT_SUCCESS; // will switch to NAV_STATE_WAYPOINT_IN_PROGRESS
case NAV_WP_ACTION_JUMP:
if(posControl.waypointList[posControl.activeWaypointIndex].p2 != -1){
if(posControl.waypointList[posControl.activeWaypointIndex].p2 == 0){
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
}
}
else
{
posControl.waypointList[posControl.activeWaypointIndex].p2--;
}
}
posControl.activeWaypointIndex = posControl.waypointList[posControl.activeWaypointIndex].p1 - 1;
return NAV_FSM_EVENT_NONE; // re-process the state passing to the next WP
case NAV_WP_ACTION_RTH:
default:
posControl.rthState.rthInitialDistance = posControl.homeDistance;
initializeRTHSanityChecker(&navGetCurrentActualPositionAndVelocity()->pos);
calculateAndSetActiveWaypointToLocalPosition(rthGetHomeTargetPosition(RTH_HOME_ENROUTE_INITIAL));
return NAV_FSM_EVENT_SUCCESS; // will switch to NAV_STATE_WAYPOINT_IN_PROGRESS
};
UNREACHABLE();
}
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_IN_PROGRESS(navigationFSMState_t previousState)
@ -1391,9 +1418,8 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_IN_PROGRESS(na
// If no position sensor available - land immediately
if ((posControl.flags.estPosStatus >= EST_USABLE) && (posControl.flags.estHeadingStatus >= EST_USABLE)) {
switch (posControl.waypointList[posControl.activeWaypointIndex].action) {
switch ((navWaypointActions_e)posControl.waypointList[posControl.activeWaypointIndex].action) {
case NAV_WP_ACTION_WAYPOINT:
default:
if (isWaypointReached(&posControl.activeWaypoint, false) || isWaypointMissed(&posControl.activeWaypoint)) {
return NAV_FSM_EVENT_SUCCESS; // will switch to NAV_STATE_WAYPOINT_REACHED
}
@ -1409,6 +1435,9 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_IN_PROGRESS(na
}
break;
case NAV_WP_ACTION_JUMP:
UNREACHABLE();
case NAV_WP_ACTION_RTH:
if (isWaypointReached(&posControl.activeWaypoint, true) || isWaypointMissed(&posControl.activeWaypoint)) {
return NAV_FSM_EVENT_SUCCESS; // will switch to NAV_STATE_WAYPOINT_REACHED
@ -1428,13 +1457,21 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_IN_PROGRESS(na
else {
return NAV_FSM_EVENT_NONE; // will re-process state in >10ms
}
UNREACHABLE();
}
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_REACHED(navigationFSMState_t previousState)
{
UNUSED(previousState);
switch (posControl.waypointList[posControl.activeWaypointIndex].action) {
switch ((navWaypointActions_e)posControl.waypointList[posControl.activeWaypointIndex].action) {
case NAV_WP_ACTION_WAYPOINT:
return NAV_FSM_EVENT_SUCCESS; // NAV_STATE_WAYPOINT_NEXT
case NAV_WP_ACTION_JUMP:
UNREACHABLE();
case NAV_WP_ACTION_RTH:
if (posControl.waypointList[posControl.activeWaypointIndex].p1 != 0) {
return NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_RTH_LAND;
@ -1442,10 +1479,9 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_REACHED(naviga
else {
return NAV_FSM_EVENT_SUCCESS; // NAV_STATE_WAYPOINT_NEXT
}
default:
return NAV_FSM_EVENT_SUCCESS; // NAV_STATE_WAYPOINT_NEXT
}
UNREACHABLE();
}
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_RTH_LAND(navigationFSMState_t previousState)
@ -2603,9 +2639,9 @@ void setWaypoint(uint8_t wpNumber, const navWaypoint_t * wpData)
setDesiredPosition(&wpPos.pos, DEGREES_TO_CENTIDEGREES(wpData->p1), waypointUpdateFlags);
}
// WP #1 - #15 - common waypoints - pre-programmed mission
// 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_RTH) {
if (wpData->action == NAV_WP_ACTION_WAYPOINT || wpData->action == NAV_WP_ACTION_JUMP || wpData->action == NAV_WP_ACTION_RTH) {
// 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;
@ -3079,6 +3115,17 @@ navArmingBlocker_e navigationIsBlockingArming(bool *usedBypass)
}
}
// Don't allow arming if any of JUMP waypoint has invalid settings
if (posControl.waypointCount > 0) {
for (uint8_t wp = 0; wp < posControl.waypointCount ; wp++){
if (posControl.waypointList[wp].action == NAV_WP_ACTION_JUMP){
if((posControl.waypointList[wp].p1 > posControl.waypointCount) || (posControl.waypointList[wp].p2 < -1)){
return NAV_ARMING_BLOCKER_JUMP_WAYPOINT_ERROR;
}
}
}
}
return NAV_ARMING_BLOCKER_NONE;
}