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 'development' into dzikuvx-mr-cruise-experiments

Browse source
This commit is contained in:
Pawel Spychalski (DzikuVx) 2018-07-04 17:08:56 +02:00
commit 1231df9654
160 changed files with 6506 additions and 2781 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -41,6 +41,7 @@
#include "fc/runtime_config.h"
#include "flight/imu.h"
#include "flight/mixer.h"
#include "flight/pid.h"
#include "io/beeper.h"
@ -107,6 +108,7 @@ PG_RESET_TEMPLATE(navConfig_t, navConfig,
.emerg_descent_rate = 500, // 5 m/s
.min_rth_distance = 500, // If closer than 5m - land immediately
.rth_altitude = 1000, // 10m
.rth_home_altitude = 0,
.rth_abort_threshold = 50000, // 500m - should be safe for all aircraft
.max_terrain_follow_altitude = 100, // max 1m altitude in terrain following mode
},
@ -132,6 +134,7 @@ PG_RESET_TEMPLATE(navConfig_t, navConfig,
.max_climb_angle = 20,
.max_dive_angle = 15,
.cruise_throttle = 1400,
.cruise_speed = 0, // cm/s
.max_throttle = 1700,
.min_throttle = 1200,
.pitch_to_throttle = 10, // pwm units per degree of pitch (10pwm units ~ 1% throttle)
@ -152,7 +155,9 @@ PG_RESET_TEMPLATE(navConfig_t, navConfig,
.launch_timeout = 5000, // ms, timeout for launch procedure
.launch_max_altitude = 0, // cm, altitude where to consider launch ended
.launch_climb_angle = 18, // 18 degrees
.launch_max_angle = 45 // 45 deg
.launch_max_angle = 45, // 45 deg
.cruise_yaw_rate = 20, // 20dps
.allow_manual_thr_increase = false
}
);
@ -181,11 +186,11 @@ static void setupAltitudeController(void);
static void resetHeadingController(void);
void resetGCSFlags(void);
static bool posEstimationHasGlobalReference(void);
static void calculateAndSetActiveWaypoint(const navWaypoint_t * waypoint);
static void calculateAndSetActiveWaypointToLocalPosition(const fpVector3_t * pos);
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);
void initializeRTHSanityChecker(const fpVector3_t * pos);
bool validateRTHSanityChecker(void);
@ -196,6 +201,12 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_ALTHOLD_INITIALIZE(navi
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_ALTHOLD_IN_PROGRESS(navigationFSMState_t previousState);
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_POSHOLD_3D_INITIALIZE(navigationFSMState_t previousState);
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_POSHOLD_3D_IN_PROGRESS(navigationFSMState_t previousState);
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_CRUISE_2D_INITIALIZE(navigationFSMState_t previousState);
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_CRUISE_2D_IN_PROGRESS(navigationFSMState_t previousState);
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_CRUISE_2D_ADJUSTING(navigationFSMState_t previousState);
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_CRUISE_3D_INITIALIZE(navigationFSMState_t previousState);
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_CRUISE_3D_IN_PROGRESS(navigationFSMState_t previousState);
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_CRUISE_3D_ADJUSTING(navigationFSMState_t previousState);
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_INITIALIZE(navigationFSMState_t previousState);
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_CLIMB_TO_SAFE_ALT(navigationFSMState_t previousState);
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_HEAD_HOME(navigationFSMState_t previousState);
@ -235,6 +246,8 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
[NAV_FSM_EVENT_SWITCH_TO_WAYPOINT] = NAV_STATE_WAYPOINT_INITIALIZE,
[NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE,
[NAV_FSM_EVENT_SWITCH_TO_LAUNCH] = NAV_STATE_LAUNCH_INITIALIZE,
[NAV_FSM_EVENT_SWITCH_TO_CRUISE_2D] = NAV_STATE_CRUISE_2D_INITIALIZE,
[NAV_FSM_EVENT_SWITCH_TO_CRUISE_3D] = NAV_STATE_CRUISE_3D_INITIALIZE,
}
},
@ -269,6 +282,8 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
[NAV_FSM_EVENT_SWITCH_TO_RTH] = NAV_STATE_RTH_INITIALIZE,
[NAV_FSM_EVENT_SWITCH_TO_WAYPOINT] = NAV_STATE_WAYPOINT_INITIALIZE,
[NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE,
[NAV_FSM_EVENT_SWITCH_TO_CRUISE_2D] = NAV_STATE_CRUISE_2D_INITIALIZE,
[NAV_FSM_EVENT_SWITCH_TO_CRUISE_3D] = NAV_STATE_CRUISE_3D_INITIALIZE,
}
},
@ -303,6 +318,112 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
[NAV_FSM_EVENT_SWITCH_TO_RTH] = NAV_STATE_RTH_INITIALIZE,
[NAV_FSM_EVENT_SWITCH_TO_WAYPOINT] = NAV_STATE_WAYPOINT_INITIALIZE,
[NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE,
[NAV_FSM_EVENT_SWITCH_TO_CRUISE_2D] = NAV_STATE_CRUISE_2D_INITIALIZE,
[NAV_FSM_EVENT_SWITCH_TO_CRUISE_3D] = NAV_STATE_CRUISE_3D_INITIALIZE,
}
},
/** CRUISE_2D mode ************************************************/
[NAV_STATE_CRUISE_2D_INITIALIZE] = {
.persistentId = NAV_PERSISTENT_ID_CRUISE_2D_INITIALIZE,
.onEntry = navOnEnteringState_NAV_STATE_CRUISE_2D_INITIALIZE,
.timeoutMs = 0,
.stateFlags = NAV_REQUIRE_ANGLE,
.mapToFlightModes = NAV_CRUISE_MODE,
.mwState = MW_NAV_STATE_NONE,
.mwError = MW_NAV_ERROR_NONE,
.onEvent = {
[NAV_FSM_EVENT_SUCCESS] = NAV_STATE_CRUISE_2D_IN_PROGRESS,
[NAV_FSM_EVENT_ERROR] = NAV_STATE_IDLE,
[NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE,
}
},
[NAV_STATE_CRUISE_2D_IN_PROGRESS] = {
.persistentId = NAV_PERSISTENT_ID_CRUISE_2D_IN_PROGRESS,
.onEntry = navOnEnteringState_NAV_STATE_CRUISE_2D_IN_PROGRESS,
.timeoutMs = 10,
.stateFlags = NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_RC_POS | NAV_RC_YAW,
.mapToFlightModes = NAV_CRUISE_MODE,
.mwState = MW_NAV_STATE_NONE,
.mwError = MW_NAV_ERROR_NONE,
.onEvent = {
[NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_CRUISE_2D_IN_PROGRESS, // re-process the state
[NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE,
[NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_ALTHOLD_INITIALIZE,
[NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D] = NAV_STATE_POSHOLD_3D_INITIALIZE,
[NAV_FSM_EVENT_SWITCH_TO_CRUISE_3D] = NAV_STATE_CRUISE_3D_INITIALIZE,
[NAV_FSM_EVENT_SWITCH_TO_CRUISE_ADJ] = NAV_STATE_CRUISE_2D_ADJUSTING,
[NAV_FSM_EVENT_SWITCH_TO_RTH] = NAV_STATE_RTH_INITIALIZE,
[NAV_FSM_EVENT_SWITCH_TO_WAYPOINT] = NAV_STATE_WAYPOINT_INITIALIZE,
[NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE,
}
},
[NAV_STATE_CRUISE_2D_ADJUSTING] = {
.persistentId = NAV_PERSISTENT_ID_CRUISE_2D_ADJUSTING,
.onEntry = navOnEnteringState_NAV_STATE_CRUISE_2D_ADJUSTING,
.timeoutMs = 10,
.stateFlags = NAV_REQUIRE_ANGLE | NAV_RC_POS,
.mapToFlightModes = NAV_CRUISE_MODE,
.mwState = MW_NAV_STATE_NONE,
.mwError = MW_NAV_ERROR_NONE,
.onEvent = {
[NAV_FSM_EVENT_SUCCESS] = NAV_STATE_CRUISE_2D_IN_PROGRESS,
[NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_CRUISE_2D_ADJUSTING,
[NAV_FSM_EVENT_ERROR] = NAV_STATE_IDLE,
[NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE,
}
},
/** CRUISE_3D mode ************************************************/
[NAV_STATE_CRUISE_3D_INITIALIZE] = {
.persistentId = NAV_PERSISTENT_ID_CRUISE_3D_INITIALIZE,
.onEntry = navOnEnteringState_NAV_STATE_CRUISE_3D_INITIALIZE,
.timeoutMs = 0,
.stateFlags = NAV_REQUIRE_ANGLE,
.mapToFlightModes = NAV_ALTHOLD_MODE | NAV_CRUISE_MODE,
.mwState = MW_NAV_STATE_NONE,
.mwError = MW_NAV_ERROR_NONE,
.onEvent = {
[NAV_FSM_EVENT_SUCCESS] = NAV_STATE_CRUISE_3D_IN_PROGRESS,
[NAV_FSM_EVENT_ERROR] = NAV_STATE_IDLE,
[NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE,
}
},
[NAV_STATE_CRUISE_3D_IN_PROGRESS] = {
.persistentId = NAV_PERSISTENT_ID_CRUISE_3D_IN_PROGRESS,
.onEntry = navOnEnteringState_NAV_STATE_CRUISE_3D_IN_PROGRESS,
.timeoutMs = 10,
.stateFlags = NAV_CTL_POS | NAV_CTL_YAW | NAV_CTL_ALT | NAV_REQUIRE_ANGLE | NAV_RC_POS | NAV_RC_YAW | NAV_RC_ALT,
.mapToFlightModes = NAV_ALTHOLD_MODE | NAV_CRUISE_MODE,
.mwState = MW_NAV_STATE_NONE,
.mwError = MW_NAV_ERROR_NONE,
.onEvent = {
[NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_CRUISE_3D_IN_PROGRESS, // re-process the state
[NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE,
[NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_ALTHOLD_INITIALIZE,
[NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D] = NAV_STATE_POSHOLD_3D_INITIALIZE,
[NAV_FSM_EVENT_SWITCH_TO_CRUISE_2D] = NAV_STATE_CRUISE_2D_INITIALIZE,
[NAV_FSM_EVENT_SWITCH_TO_CRUISE_ADJ] = NAV_STATE_CRUISE_3D_ADJUSTING,
[NAV_FSM_EVENT_SWITCH_TO_RTH] = NAV_STATE_RTH_INITIALIZE,
[NAV_FSM_EVENT_SWITCH_TO_WAYPOINT] = NAV_STATE_WAYPOINT_INITIALIZE,
[NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE,
}
},
[NAV_STATE_CRUISE_3D_ADJUSTING] = {
.persistentId = NAV_PERSISTENT_ID_CRUISE_3D_ADJUSTING,
.onEntry = navOnEnteringState_NAV_STATE_CRUISE_3D_ADJUSTING,
.timeoutMs = 10,
.stateFlags = NAV_CTL_ALT | NAV_REQUIRE_ANGLE | NAV_RC_POS | NAV_RC_ALT,
.mapToFlightModes = NAV_ALTHOLD_MODE | NAV_CRUISE_MODE,
.mwState = MW_NAV_STATE_NONE,
.mwError = MW_NAV_ERROR_NONE,
.onEvent = {
[NAV_FSM_EVENT_SUCCESS] = NAV_STATE_CRUISE_3D_IN_PROGRESS,
[NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_CRUISE_3D_ADJUSTING,
[NAV_FSM_EVENT_ERROR] = NAV_STATE_IDLE,
[NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE,
}
},
@ -338,6 +459,8 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
[NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE,
[NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_ALTHOLD_INITIALIZE,
[NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D] = NAV_STATE_POSHOLD_3D_INITIALIZE,
[NAV_FSM_EVENT_SWITCH_TO_CRUISE_2D] = NAV_STATE_CRUISE_2D_INITIALIZE,
[NAV_FSM_EVENT_SWITCH_TO_CRUISE_3D] = NAV_STATE_CRUISE_3D_INITIALIZE,
}
},
@ -356,6 +479,8 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
[NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_ALTHOLD_INITIALIZE,
[NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D] = NAV_STATE_POSHOLD_3D_INITIALIZE,
[NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE,
[NAV_FSM_EVENT_SWITCH_TO_CRUISE_2D] = NAV_STATE_CRUISE_2D_INITIALIZE,
[NAV_FSM_EVENT_SWITCH_TO_CRUISE_3D] = NAV_STATE_CRUISE_3D_INITIALIZE,
}
},
@ -375,6 +500,8 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
[NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_ALTHOLD_INITIALIZE,
[NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D] = NAV_STATE_POSHOLD_3D_INITIALIZE,
[NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE,
[NAV_FSM_EVENT_SWITCH_TO_CRUISE_2D] = NAV_STATE_CRUISE_2D_INITIALIZE,
[NAV_FSM_EVENT_SWITCH_TO_CRUISE_3D] = NAV_STATE_CRUISE_3D_INITIALIZE,
}
},
@ -392,6 +519,8 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
[NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_ALTHOLD_INITIALIZE,
[NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D] = NAV_STATE_POSHOLD_3D_INITIALIZE,
[NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE,
[NAV_FSM_EVENT_SWITCH_TO_CRUISE_2D] = NAV_STATE_CRUISE_2D_INITIALIZE,
[NAV_FSM_EVENT_SWITCH_TO_CRUISE_3D] = NAV_STATE_CRUISE_3D_INITIALIZE,
}
},
@ -493,6 +622,8 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
[NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D] = NAV_STATE_POSHOLD_3D_INITIALIZE,
[NAV_FSM_EVENT_SWITCH_TO_RTH] = NAV_STATE_RTH_INITIALIZE,
[NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE,
[NAV_FSM_EVENT_SWITCH_TO_CRUISE_2D] = NAV_STATE_CRUISE_2D_INITIALIZE,
[NAV_FSM_EVENT_SWITCH_TO_CRUISE_3D] = NAV_STATE_CRUISE_3D_INITIALIZE,
}
},
@ -514,6 +645,8 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
[NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D] = NAV_STATE_POSHOLD_3D_INITIALIZE,
[NAV_FSM_EVENT_SWITCH_TO_RTH] = NAV_STATE_RTH_INITIALIZE,
[NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE,
[NAV_FSM_EVENT_SWITCH_TO_CRUISE_2D] = NAV_STATE_CRUISE_2D_INITIALIZE,
[NAV_FSM_EVENT_SWITCH_TO_CRUISE_3D] = NAV_STATE_CRUISE_3D_INITIALIZE,
}
},
@ -533,6 +666,8 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
[NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D] = NAV_STATE_POSHOLD_3D_INITIALIZE,
[NAV_FSM_EVENT_SWITCH_TO_RTH] = NAV_STATE_RTH_INITIALIZE,
[NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE,
[NAV_FSM_EVENT_SWITCH_TO_CRUISE_2D] = NAV_STATE_CRUISE_2D_INITIALIZE,
[NAV_FSM_EVENT_SWITCH_TO_CRUISE_3D] = NAV_STATE_CRUISE_3D_INITIALIZE,
}
},
@ -564,6 +699,8 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
[NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D] = NAV_STATE_POSHOLD_3D_INITIALIZE,
[NAV_FSM_EVENT_SWITCH_TO_RTH] = NAV_STATE_RTH_INITIALIZE,
[NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE,
[NAV_FSM_EVENT_SWITCH_TO_CRUISE_2D] = NAV_STATE_CRUISE_2D_INITIALIZE,
[NAV_FSM_EVENT_SWITCH_TO_CRUISE_3D] = NAV_STATE_CRUISE_3D_INITIALIZE,
}
},
@ -748,7 +885,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_POSHOLD_3D_INITIALIZE(n
setDesiredPosition(&navGetCurrentActualPositionAndVelocity()->pos, posControl.actualState.yaw, NAV_POS_UPDATE_Z); // This will reset surface offset
}
if (((prevFlags & NAV_CTL_POS) == 0) || ((prevFlags & NAV_AUTO_RTH) != 0) || ((prevFlags & NAV_AUTO_WP) != 0)) {
if ((previousState != NAV_STATE_RTH_HOVER_PRIOR_TO_LANDING) && (previousState != NAV_STATE_RTH_HOVER_ABOVE_HOME) && (previousState != NAV_STATE_RTH_LANDING)) {
fpVector3_t targetHoldPos;
calculateInitialHoldPosition(&targetHoldPos);
setDesiredPosition(&targetHoldPos, posControl.actualState.yaw, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_HEADING);
@ -772,12 +909,117 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_POSHOLD_3D_IN_PROGRESS(
return NAV_FSM_EVENT_NONE;
}
/////////////////
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_CRUISE_2D_INITIALIZE(navigationFSMState_t previousState)
{
const navigationFSMStateFlags_t prevFlags = navGetStateFlags(previousState);
if (!STATE(FIXED_WING)) { return NAV_FSM_EVENT_ERROR; } // Only on FW for now
DEBUG_SET(DEBUG_CRUISE, 0, 1);
if (checkForPositionSensorTimeout()) { return NAV_FSM_EVENT_SWITCH_TO_IDLE; } // Switch to IDLE if we do not have an healty position. Try the next iteration.
if (!(prevFlags & NAV_CTL_POS)) {
resetPositionController();
}
posControl.cruise.yaw = posControl.actualState.yaw; // Store the yaw to follow
posControl.cruise.previousYaw = posControl.cruise.yaw;
posControl.cruise.lastYawAdjustmentTime = 0;
return NAV_FSM_EVENT_SUCCESS; // Go to CRUISE_XD_IN_PROGRESS state
}
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_CRUISE_2D_IN_PROGRESS(navigationFSMState_t previousState)
{
const timeMs_t currentTimeMs = millis();
if (checkForPositionSensorTimeout()) { return NAV_FSM_EVENT_SWITCH_TO_IDLE; } // Switch to IDLE if we do not have an healty position. Do the CRUISE init the next iteration.
DEBUG_SET(DEBUG_CRUISE, 0, 2);
DEBUG_SET(DEBUG_CRUISE, 2, 0);
if (posControl.flags.isAdjustingPosition) {
return NAV_FSM_EVENT_SWITCH_TO_CRUISE_ADJ;
}
// User is yawing. We record the desidered yaw and we change the desidered target in the meanwhile
if (posControl.flags.isAdjustingHeading) {
timeMs_t timeDifference = currentTimeMs - posControl.cruise.lastYawAdjustmentTime;
if (timeDifference > 100) timeDifference = 0; // if adjustment was called long time ago, reset the time difference.
float rateTarget = scaleRangef((float)rcCommand[YAW], -500.0f, 500.0f, -DEGREES_TO_CENTIDEGREES(navConfig()->fw.cruise_yaw_rate), DEGREES_TO_CENTIDEGREES(navConfig()->fw.cruise_yaw_rate));
float centidegsPerIteration = rateTarget * timeDifference / 1000.0f;
posControl.cruise.yaw = wrap_36000(posControl.cruise.yaw - centidegsPerIteration);
DEBUG_SET(DEBUG_CRUISE, 1, CENTIDEGREES_TO_DEGREES(posControl.cruise.yaw));
posControl.cruise.lastYawAdjustmentTime = currentTimeMs;
}
if (currentTimeMs - posControl.cruise.lastYawAdjustmentTime > 4000)
posControl.cruise.previousYaw = posControl.cruise.yaw;
uint32_t distance = gpsSol.groundSpeed * 60; // next WP to be reached in 60s [cm]
if ((previousState == NAV_STATE_CRUISE_2D_INITIALIZE) || (previousState == NAV_STATE_CRUISE_2D_ADJUSTING)
|| (previousState == NAV_STATE_CRUISE_3D_INITIALIZE) || (previousState == NAV_STATE_CRUISE_3D_ADJUSTING)
|| posControl.flags.isAdjustingHeading) {
calculateFarAwayTarget(&posControl.cruise.targetPos, posControl.cruise.yaw, distance);
DEBUG_SET(DEBUG_CRUISE, 2, 1);
} else if (calculateDistanceToDestination(&posControl.cruise.targetPos) <= (navConfig()->fw.loiter_radius * 1.10f)) { //10% margin
calculateNewCruiseTarget(&posControl.cruise.targetPos, posControl.cruise.yaw, distance);
DEBUG_SET(DEBUG_CRUISE, 2, 2);
}
setDesiredPosition(&posControl.cruise.targetPos, posControl.cruise.yaw, NAV_POS_UPDATE_XY);
return NAV_FSM_EVENT_NONE;
}
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_CRUISE_2D_ADJUSTING(navigationFSMState_t previousState)
{
UNUSED(previousState);
DEBUG_SET(DEBUG_CRUISE, 0, 3);
// User is rolling, changing manually direction. Wait until it is done and then restore CRUISE
if (posControl.flags.isAdjustingPosition) {
posControl.cruise.yaw = posControl.actualState.yaw; //store current heading
posControl.cruise.lastYawAdjustmentTime = millis();
return NAV_FSM_EVENT_NONE; // reprocess the state
}
resetPositionController();
return NAV_FSM_EVENT_SUCCESS; // go back to the CRUISE_XD_IN_PROGRESS state
}
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_CRUISE_3D_INITIALIZE(navigationFSMState_t previousState)
{
if (!STATE(FIXED_WING)) { return NAV_FSM_EVENT_ERROR; } // only on FW for now
navOnEnteringState_NAV_STATE_ALTHOLD_INITIALIZE(previousState);
return navOnEnteringState_NAV_STATE_CRUISE_2D_INITIALIZE(previousState);
}
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_CRUISE_3D_IN_PROGRESS(navigationFSMState_t previousState)
{
navOnEnteringState_NAV_STATE_ALTHOLD_IN_PROGRESS(previousState);
return navOnEnteringState_NAV_STATE_CRUISE_2D_IN_PROGRESS(previousState);
}
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_CRUISE_3D_ADJUSTING(navigationFSMState_t previousState)
{
navOnEnteringState_NAV_STATE_ALTHOLD_IN_PROGRESS(previousState);
return navOnEnteringState_NAV_STATE_CRUISE_2D_ADJUSTING(previousState);
}
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_INITIALIZE(navigationFSMState_t previousState)
{
navigationFSMStateFlags_t prevFlags = navGetStateFlags(previousState);
if ((posControl.flags.estHeadingStatus == EST_NONE) || (posControl.flags.estAltStatus == EST_NONE) || !STATE(GPS_FIX_HOME) || !posEstimationHasGlobalReference()) {
if ((posControl.flags.estHeadingStatus == EST_NONE) || (posControl.flags.estAltStatus == EST_NONE) || (posControl.flags.estPosStatus != EST_TRUSTED) || !STATE(GPS_FIX_HOME)) {
// Heading sensor, altitude sensor and HOME fix are mandatory for RTH. If not satisfied - switch to emergency landing
// If we are in dead-reckoning mode - also fail, since coordinates may be unreliable
return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING;
@ -949,6 +1191,13 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_HOVER_PRIOR_TO_LAND
// If position ok OR within valid timeout - continue
if ((posControl.flags.estPosStatus >= EST_USABLE) || !checkForPositionSensorTimeout()) {
// set altitude to go to when landing is not allowed
if (navConfig()->general.rth_home_altitude && !navigationRTHAllowsLanding()) {
posControl.homeWaypointAbove.pos.z = navConfig()->general.rth_home_altitude;
setDesiredPosition(&posControl.homeWaypointAbove.pos, 0, NAV_POS_UPDATE_Z);
}
// Wait until target heading is reached (with 15 deg margin for error)
if (STATE(FIXED_WING)) {
resetLandingDetector();
@ -968,8 +1217,8 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_HOVER_PRIOR_TO_LAND
return NAV_FSM_EVENT_NONE;
}
}
}
else {
} else {
return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING;
}
}
@ -1229,10 +1478,10 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_LAUNCH_WAIT(navigationF
return NAV_FSM_EVENT_SUCCESS; // NAV_STATE_LAUNCH_IN_PROGRESS
}
//allow to leave NAV_LAUNCH_MODE if it has being enabled as feature by moving sticks with low throttle.
if(feature(FEATURE_FW_LAUNCH)){
//allow to leave NAV_LAUNCH_MODE if it has being enabled as feature by moving sticks with low throttle.
if (feature(FEATURE_FW_LAUNCH)) {
throttleStatus_e throttleStatus = calculateThrottleStatus();
if ((throttleStatus == THROTTLE_LOW) && (areSticksDeflectedMoreThanPosHoldDeadband())){
if ((throttleStatus == THROTTLE_LOW) && (areSticksDeflectedMoreThanPosHoldDeadband())) {
return NAV_FSM_EVENT_SWITCH_TO_IDLE;
}
}
@ -1384,6 +1633,11 @@ float navPidApply3(pidController_t *pid, const float setpoint, const float measu
const float outVal = newProportional + (pid->integrator * gainScaler) + newDerivative;
const float outValConstrained = constrainf(outVal, outMin, outMax);
pid->proportional = newProportional;
pid->integral = pid->integrator;
pid->derivative = newDerivative;
pid->output_constrained = outValConstrained;
/* Update I-term */
if (!(pidFlags & PID_ZERO_INTEGRATOR)) {
const float newIntegrator = pid->integrator + (error * pid->param.kI * gainScaler * dt) + ((outValConstrained - outVal) * pid->param.kT * dt);
@ -1411,10 +1665,14 @@ float navPidApply2(pidController_t *pid, const float setpoint, const float measu
void navPidReset(pidController_t *pid)
{
pid->reset = true;
pid->proportional = 0.0f;
pid->integral = 0.0f;
pid->derivative = 0.0f;
pid->integrator = 0.0f;
pid->last_input = 0.0f;
pid->dterm_filter_state.state = 0.0f;
pid->dterm_filter_state.RC = 0.0f;
pid->output_constrained = 0.0f;
}
void navPidInit(pidController_t *pid, float _kP, float _kI, float _kD)
@ -1475,7 +1733,7 @@ bool checkForPositionSensorTimeout(void)
/*-----------------------------------------------------------
* Processes an update to XY-position and velocity
*-----------------------------------------------------------*/
void updateActualHorizontalPositionAndVelocity(bool estimateValid, float newX, float newY, float newVelX, float newVelY)
void updateActualHorizontalPositionAndVelocity(bool estPosValid, bool estVelValid, float newX, float newY, float newVelX, float newVelY)
{
posControl.actualState.abs.pos.x = newX;
posControl.actualState.abs.pos.y = newY;
@ -1489,13 +1747,24 @@ void updateActualHorizontalPositionAndVelocity(bool estimateValid, float newX, f
posControl.actualState.velXY = sqrtf(sq(newVelX) + sq(newVelY));
if (estimateValid) {
// CASE 1: POS & VEL valid
if (estPosValid && estVelValid) {
posControl.flags.estPosStatus = EST_TRUSTED;
posControl.flags.estVelStatus = EST_TRUSTED;
posControl.flags.horizontalPositionDataNew = 1;
posControl.lastValidPositionTimeMs = millis();
}
// CASE 1: POS invalid, VEL valid
else if (!estPosValid && estVelValid) {
posControl.flags.estPosStatus = EST_USABLE; // Pos usable, but not trusted
posControl.flags.estVelStatus = EST_TRUSTED;
posControl.flags.horizontalPositionDataNew = 1;
posControl.lastValidPositionTimeMs = millis();
}
// CASE 3: can't use pos/vel data
else {
posControl.flags.estPosStatus = EST_NONE;
posControl.flags.estVelStatus = EST_NONE;
posControl.flags.horizontalPositionDataNew = 0;
}
@ -1657,6 +1926,22 @@ static void updateHomePositionCompatibility(void)
GPS_directionToHome = posControl.homeDirection / 100;
}
float RTHAltitude() {
switch (navConfig()->general.flags.rth_alt_control_mode) {
case NAV_RTH_NO_ALT:
return(posControl.actualState.abs.pos.z);
case NAV_RTH_EXTRA_ALT: // Maintain current altitude + predefined safety margin
return(posControl.actualState.abs.pos.z + navConfig()->general.rth_altitude);
case NAV_RTH_MAX_ALT:
return(MAX(posControl.homeWaypointAbove.pos.z, posControl.actualState.abs.pos.z));
case NAV_RTH_AT_LEAST_ALT: // Climb to at least some predefined altitude above home
return(MAX(posControl.homePosition.pos.z + navConfig()->general.rth_altitude, posControl.actualState.abs.pos.z));
case NAV_RTH_CONST_ALT: // Climb/descend to predefined altitude above home
default:
return(posControl.homePosition.pos.z + navConfig()->general.rth_altitude);
}
}
/*-----------------------------------------------------------
* Reset home position to current position
*-----------------------------------------------------------*/
@ -1664,24 +1949,7 @@ static void updateDesiredRTHAltitude(void)
{
if (ARMING_FLAG(ARMED)) {
if (!(navGetStateFlags(posControl.navState) & NAV_AUTO_RTH)) {
switch (navConfig()->general.flags.rth_alt_control_mode) {
case NAV_RTH_NO_ALT:
posControl.homeWaypointAbove.pos.z = posControl.actualState.abs.pos.z;
break;
case NAV_RTH_EXTRA_ALT: // Maintain current altitude + predefined safety margin
posControl.homeWaypointAbove.pos.z = posControl.actualState.abs.pos.z + navConfig()->general.rth_altitude;
break;
case NAV_RTH_MAX_ALT:
posControl.homeWaypointAbove.pos.z = MAX(posControl.homeWaypointAbove.pos.z, posControl.actualState.abs.pos.z);
break;
case NAV_RTH_AT_LEAST_ALT: // Climb to at least some predefined altitude above home
posControl.homeWaypointAbove.pos.z = MAX(posControl.homePosition.pos.z + navConfig()->general.rth_altitude, posControl.actualState.abs.pos.z);
break;
case NAV_RTH_CONST_ALT: // Climb/descend to predefined altitude above home
default:
posControl.homeWaypointAbove.pos.z = posControl.homePosition.pos.z + navConfig()->general.rth_altitude;
break;
}
posControl.homeWaypointAbove.pos.z = RTHAltitude();
}
}
else {
@ -1911,6 +2179,13 @@ void calculateFarAwayTarget(fpVector3_t * farAwayPos, int32_t yaw, int32_t dista
farAwayPos->z = navGetCurrentActualPositionAndVelocity()->pos.z;
}
void calculateNewCruiseTarget(fpVector3_t * origin, int32_t yaw, int32_t distance)
{
origin->x = origin->x + distance * cos_approx(CENTIDEGREES_TO_RADIANS(yaw));
origin->y = origin->y + distance * sin_approx(CENTIDEGREES_TO_RADIANS(yaw));
origin->z = origin->z;
}
/*-----------------------------------------------------------
* NAV land detector
*-----------------------------------------------------------*/
@ -2208,7 +2483,7 @@ void setWaypoint(uint8_t wpNumber, const navWaypoint_t * wpData)
// WP #255 - special waypoint - directly set desiredPosition
// Only valid when armed and in poshold mode
else if ((wpNumber == 255) && (wpData->action == NAV_WP_ACTION_WAYPOINT) &&
ARMING_FLAG(ARMED) && (posControl.flags.estPosStatus >= EST_USABLE) && posControl.gpsOrigin.valid && posControl.flags.isGCSAssistedNavigationEnabled &&
ARMING_FLAG(ARMED) && (posControl.flags.estPosStatus == EST_TRUSTED) && posControl.gpsOrigin.valid && posControl.flags.isGCSAssistedNavigationEnabled &&
(posControl.navState == NAV_STATE_POSHOLD_3D_IN_PROGRESS)) {
// Convert to local coordinates
geoConvertGeodeticToLocal(&posControl.gpsOrigin, &wpLLH, &wpPos.pos, GEO_ALT_RELATIVE);
@ -2416,7 +2691,7 @@ void applyWaypointNavigationAndAltitudeHold(void)
#if defined(NAV_BLACKBOX)
navFlags = 0;
if (posControl.flags.estAltStatus == EST_TRUSTED) navFlags |= (1 << 0);
if (posControl.flags.estAglStatus == EST_TRUSTED) navFlags |= (1 << 1);
if (posControl.flags.estAglStatus == EST_TRUSTED) navFlags |= (1 << 1);
if (posControl.flags.estPosStatus == EST_TRUSTED) navFlags |= (1 << 2);
#if defined(NAV_GPS_GLITCH_DETECTION)
if (isGPSGlitchDetected()) navFlags |= (1 << 4);
@ -2472,7 +2747,7 @@ void applyWaypointNavigationAndAltitudeHold(void)
void switchNavigationFlightModes(void)
{
const flightModeFlags_e enabledNavFlightModes = navGetMappedFlightModes(posControl.navState);
const flightModeFlags_e disabledFlightModes = (NAV_ALTHOLD_MODE | NAV_RTH_MODE | NAV_POSHOLD_MODE | NAV_WP_MODE | NAV_LAUNCH_MODE) & (~enabledNavFlightModes);
const flightModeFlags_e disabledFlightModes = (NAV_ALTHOLD_MODE | NAV_RTH_MODE | NAV_POSHOLD_MODE | NAV_WP_MODE | NAV_LAUNCH_MODE | NAV_CRUISE_MODE) & (~enabledNavFlightModes);
DISABLE_FLIGHT_MODE(disabledFlightModes);
ENABLE_FLIGHT_MODE(enabledNavFlightModes);
}
@ -2487,12 +2762,12 @@ static bool canActivateAltHoldMode(void)
static bool canActivatePosHoldMode(void)
{
return (posControl.flags.estPosStatus >= EST_USABLE) && (posControl.flags.estHeadingStatus >= EST_USABLE);
return (posControl.flags.estPosStatus >= EST_USABLE) && (posControl.flags.estVelStatus == EST_TRUSTED) && (posControl.flags.estHeadingStatus >= EST_USABLE);
}
static bool posEstimationHasGlobalReference(void)
static bool canActivateNavigationModes(void)
{
return true; // For now assume that we always have global coordinates available
return (posControl.flags.estPosStatus == EST_TRUSTED) && (posControl.flags.estVelStatus == EST_TRUSTED) && (posControl.flags.estHeadingStatus >= EST_USABLE);
}
static navigationFSMEvent_t selectNavEventFromBoxModeInput(void)
@ -2508,8 +2783,9 @@ static navigationFSMEvent_t selectNavEventFromBoxModeInput(void)
}
// Flags if we can activate certain nav modes (check if we have required sensors and they provide valid data)
bool canActivateAltHold = canActivateAltHoldMode();
bool canActivatePosHold = canActivatePosHoldMode();
bool canActivateAltHold = canActivateAltHoldMode();
bool canActivatePosHold = canActivatePosHoldMode();
bool canActivateNavigation = canActivateNavigationModes();
// LAUNCH mode has priority over any other NAV mode
if (STATE(FIXED_WING)) {
@ -2536,7 +2812,7 @@ static navigationFSMEvent_t selectNavEventFromBoxModeInput(void)
}
// RTH/Failsafe_RTH can override MANUAL
if (posControl.flags.forcedRTHActivated || (IS_RC_MODE_ACTIVE(BOXNAVRTH) && canActivatePosHold && canActivateAltHold && STATE(GPS_FIX_HOME))) {
if (posControl.flags.forcedRTHActivated || (IS_RC_MODE_ACTIVE(BOXNAVRTH) && canActivatePosHold && canActivateNavigation && canActivateAltHold && STATE(GPS_FIX_HOME))) {
// If we request forced RTH - attempt to activate it no matter what
// This might switch to emergency landing controller if GPS is unavailable
canActivateWaypoint = false; // Block WP mode if we switched to RTH for whatever reason
@ -2550,7 +2826,7 @@ static navigationFSMEvent_t selectNavEventFromBoxModeInput(void)
}
if (IS_RC_MODE_ACTIVE(BOXNAVWP)) {
if ((FLIGHT_MODE(NAV_WP_MODE)) || (posEstimationHasGlobalReference() && canActivateWaypoint && canActivatePosHold && canActivateAltHold && STATE(GPS_FIX_HOME) && ARMING_FLAG(ARMED) && posControl.waypointListValid && (posControl.waypointCount > 0)))
if ((FLIGHT_MODE(NAV_WP_MODE)) || (canActivateNavigation && canActivateWaypoint && canActivatePosHold && canActivateAltHold && STATE(GPS_FIX_HOME) && ARMING_FLAG(ARMED) && posControl.waypointListValid && (posControl.waypointCount > 0)))
return NAV_FSM_EVENT_SWITCH_TO_WAYPOINT;
}
else {
@ -2563,6 +2839,18 @@ static navigationFSMEvent_t selectNavEventFromBoxModeInput(void)
return NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D;
}
// PH has priority over CRUISE
// CRUISE has priority on AH
if (IS_RC_MODE_ACTIVE(BOXNAVCRUISE)) {
if (IS_RC_MODE_ACTIVE(BOXNAVALTHOLD) && ((FLIGHT_MODE(NAV_CRUISE_MODE) && FLIGHT_MODE(NAV_ALTHOLD_MODE)) || (canActivatePosHold && canActivateAltHold)))
return NAV_FSM_EVENT_SWITCH_TO_CRUISE_3D;
if (FLIGHT_MODE(NAV_CRUISE_MODE) || (canActivatePosHold))
return NAV_FSM_EVENT_SWITCH_TO_CRUISE_2D;
}
if (IS_RC_MODE_ACTIVE(BOXNAVALTHOLD)) {
if ((FLIGHT_MODE(NAV_ALTHOLD_MODE)) || (canActivateAltHold))
return NAV_FSM_EVENT_SWITCH_TO_ALTHOLD;
@ -2603,7 +2891,7 @@ bool navigationRequiresTurnAssistance(void)
const navigationFSMStateFlags_t currentState = navGetStateFlags(posControl.navState);
if (STATE(FIXED_WING)) {
// For airplanes turn assistant is always required when controlling position
return (currentState & NAV_CTL_POS);
return (currentState & (NAV_CTL_POS | NAV_CTL_ALT));
}
else {
return false;
@ -2641,8 +2929,8 @@ bool navigationTerrainFollowingEnabled(void)
bool navigationBlockArming(void)
{
const bool navBoxModesEnabled = IS_RC_MODE_ACTIVE(BOXNAVRTH) || IS_RC_MODE_ACTIVE(BOXNAVWP) || IS_RC_MODE_ACTIVE(BOXNAVPOSHOLD) || (STATE(FIXED_WING) && IS_RC_MODE_ACTIVE(BOXNAVALTHOLD));
const bool navLaunchComboModesEnabled = isNavLaunchEnabled() && (IS_RC_MODE_ACTIVE(BOXNAVRTH) || IS_RC_MODE_ACTIVE(BOXNAVWP) || IS_RC_MODE_ACTIVE(BOXNAVALTHOLD));
const bool navBoxModesEnabled = IS_RC_MODE_ACTIVE(BOXNAVRTH) || IS_RC_MODE_ACTIVE(BOXNAVWP) || IS_RC_MODE_ACTIVE(BOXNAVPOSHOLD) || (STATE(FIXED_WING) && IS_RC_MODE_ACTIVE(BOXNAVALTHOLD)) || (STATE(FIXED_WING) && IS_RC_MODE_ACTIVE(BOXNAVCRUISE));
const bool navLaunchComboModesEnabled = isNavLaunchEnabled() && (IS_RC_MODE_ACTIVE(BOXNAVRTH) || IS_RC_MODE_ACTIVE(BOXNAVWP) || IS_RC_MODE_ACTIVE(BOXNAVALTHOLD) || IS_RC_MODE_ACTIVE(BOXNAVCRUISE));
bool shouldBlockArming = false;
if (!navConfig()->general.flags.extra_arming_safety)
@ -2789,6 +3077,7 @@ void navigationInit(void)
posControl.flags.estAltStatus = EST_NONE;
posControl.flags.estPosStatus = EST_NONE;
posControl.flags.estVelStatus = EST_NONE;
posControl.flags.estHeadingStatus = EST_NONE;
posControl.flags.estAglStatus = EST_NONE;
@ -2854,7 +3143,7 @@ rthState_e getStateOfForcedRTH(void)
bool navigationIsControllingThrottle(void)
{
navigationFSMStateFlags_t stateFlags = navGetCurrentStateFlags();
return (stateFlags & (NAV_CTL_ALT | NAV_CTL_EMERG | NAV_CTL_LAUNCH | NAV_CTL_LAND));
return ((stateFlags & (NAV_CTL_ALT | NAV_CTL_EMERG | NAV_CTL_LAUNCH | NAV_CTL_LAND)) && (getMotorStatus() != MOTOR_STOPPED_USER));
}
bool navigationIsFlyingAutonomousMode(void)
@ -2880,6 +3169,27 @@ int32_t navigationGetHomeHeading(void)
return posControl.homePosition.yaw;
}
// returns m/s
float calculateAverageSpeed() {
return (float)getTotalTravelDistance() / (getFlightTime() * 100);
}
const navigationPIDControllers_t* getNavigationPIDControllers(void) {
return &posControl.pids;
}
bool isAdjustingPosition(void) {
return posControl.flags.isAdjustingPosition;
}
bool isAdjustingHeading(void) {
return posControl.flags.isAdjustingHeading;
}
int32_t getCruiseHeadingAdjustment(void) {
return wrap_18000(posControl.cruise.yaw - posControl.cruise.previousYaw);
}
#else // NAV
#ifdef USE_GPS
@ -2917,7 +3227,7 @@ void onNewGPSData(void)
int32_t dir;
GPS_distance_cm_bearing(gpsSol.llh.lat, gpsSol.llh.lon, GPS_home.lat, GPS_home.lon, &dist, &dir);
GPS_distanceToHome = dist / 100;
GPS_directionToHome = dir / 100;
GPS_directionToHome = lrintf(dir / 100.0f);
} else {
GPS_distanceToHome = 0;
GPS_directionToHome = 0;
@ -2942,6 +3252,7 @@ int32_t getTotalTravelDistance(void)
{
return lrintf(GPS_totalTravelDistance);
}
#endif
#endif // NAV