mirror of
https://github.com/iNavFlight/inav.git
synced 2025-07-19 14:25:16 +03:00
Initial cut of AGL
Optimize and refactor Refactoring
This commit is contained in:
Konstantin Sharlaimov (DigitalEntity)
parent
9e3180143f
commit
2c65d2273e
5 changed files with 218 additions and 124 deletions
|
|
@ -696,11 +696,6 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_ALTHOLD_IN_PROGRESS(nav
|
|||
resetGCSFlags();
|
||||
}
|
||||
|
||||
// If we enable terrain mode and surface offset is not set yet - do it
|
||||
if (posControl.flags.hasValidSurfaceSensor && posControl.flags.isTerrainFollowEnabled && posControl.desiredState.surface < 0) {
|
||||
setDesiredSurfaceOffset(posControl.actualState.surface);
|
||||
}
|
||||
|
||||
return NAV_FSM_EVENT_NONE;
|
||||
}
|
||||
|
||||
|
|
@ -779,11 +774,6 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_POSHOLD_3D_IN_PROGRESS(
|
|||
resetGCSFlags();
|
||||
}
|
||||
|
||||
// If we enable terrain mode and surface offset is not set yet - do it
|
||||
if (posControl.flags.hasValidSurfaceSensor && posControl.flags.isTerrainFollowEnabled && posControl.desiredState.surface < 0) {
|
||||
setDesiredSurfaceOffset(posControl.actualState.surface);
|
||||
}
|
||||
|
||||
return NAV_FSM_EVENT_NONE;
|
||||
}
|
||||
|
||||
|
|
@ -791,7 +781,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_INITIALIZE(navigati
|
|||
{
|
||||
navigationFSMStateFlags_t prevFlags = navGetStateFlags(previousState);
|
||||
|
||||
if (!posControl.flags.hasValidHeadingSensor || !posControl.flags.hasValidAltitudeSensor || !STATE(GPS_FIX_HOME)) {
|
||||
if ((posControl.flags.estHeadingStatus == EST_NONE) || (posControl.flags.estAltStatus == EST_NONE) || !STATE(GPS_FIX_HOME)) {
|
||||
// Heading sensor, altitude sensor and HOME fix are mandatory for RTH. If not satisfied - switch to emergency landing
|
||||
return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING;
|
||||
}
|
||||
|
|
@ -802,7 +792,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_INITIALIZE(navigati
|
|||
}
|
||||
|
||||
// If we have valid position sensor or configured to ignore it's loss at initial stage - continue
|
||||
if (posControl.flags.hasValidPositionSensor || navConfig()->general.flags.rth_climb_ignore_emerg) {
|
||||
if ((posControl.flags.estPosStatue >= EST_USABLE) || navConfig()->general.flags.rth_climb_ignore_emerg) {
|
||||
// Reset altitude and position controllers if necessary
|
||||
if ((prevFlags & NAV_CTL_POS) == 0) {
|
||||
resetPositionController();
|
||||
|
|
@ -854,12 +844,12 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_CLIMB_TO_SAFE_ALT(n
|
|||
{
|
||||
UNUSED(previousState);
|
||||
|
||||
if (!posControl.flags.hasValidHeadingSensor) {
|
||||
if ((posControl.flags.estHeadingStatus == EST_NONE)) {
|
||||
return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING;
|
||||
}
|
||||
|
||||
// If we have valid pos sensor OR we are configured to ignore GPS loss
|
||||
if (posControl.flags.hasValidPositionSensor || !checkForPositionSensorTimeout() || navConfig()->general.flags.rth_climb_ignore_emerg) {
|
||||
if ((posControl.flags.estPosStatue >= EST_USABLE) || !checkForPositionSensorTimeout() || navConfig()->general.flags.rth_climb_ignore_emerg) {
|
||||
const float rthAltitudeMargin = STATE(FIXED_WING) ?
|
||||
MAX(100.0f, 0.10f * ABS(posControl.homeWaypointAbove.pos.V.Z - posControl.homePosition.pos.V.Z)) : // Airplane: 10% of target altitude but no less than 1m
|
||||
MAX( 50.0f, 0.05f * ABS(posControl.homeWaypointAbove.pos.V.Z - posControl.homePosition.pos.V.Z)); // Copters: 5% of target altitude but no less than 50cm
|
||||
|
|
@ -906,12 +896,12 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_HEAD_HOME(navigatio
|
|||
{
|
||||
UNUSED(previousState);
|
||||
|
||||
if (!posControl.flags.hasValidHeadingSensor) {
|
||||
if ((posControl.flags.estHeadingStatus == EST_NONE)) {
|
||||
return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING;
|
||||
}
|
||||
|
||||
// If we have position sensor - continue home
|
||||
if (posControl.flags.hasValidPositionSensor) {
|
||||
if ((posControl.flags.estPosStatue >= EST_USABLE)) {
|
||||
if (isWaypointReached(&posControl.homeWaypointAbove, true)) {
|
||||
// Successfully reached position target - update XYZ-position
|
||||
setDesiredPosition(&posControl.homeWaypointAbove.pos, posControl.homeWaypointAbove.yaw, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING);
|
||||
|
|
@ -946,12 +936,12 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_HOVER_PRIOR_TO_LAND
|
|||
{
|
||||
UNUSED(previousState);
|
||||
|
||||
if (!posControl.flags.hasValidHeadingSensor) {
|
||||
if ((posControl.flags.estHeadingStatus == EST_NONE)) {
|
||||
return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING;
|
||||
}
|
||||
|
||||
// If position ok OR within valid timeout - continue
|
||||
if (posControl.flags.hasValidPositionSensor || !checkForPositionSensorTimeout()) {
|
||||
if ((posControl.flags.estPosStatue >= EST_USABLE) || !checkForPositionSensorTimeout()) {
|
||||
// Wait until target heading is reached (with 15 deg margin for error)
|
||||
if (STATE(FIXED_WING)) {
|
||||
resetLandingDetector();
|
||||
|
|
@ -997,7 +987,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_LANDING(navigationF
|
|||
float descentVelLimited = 0;
|
||||
|
||||
// A safeguard - if surface altitude sensors is available and it is reading < 50cm altitude - drop to low descend speed
|
||||
if (posControl.flags.hasValidSurfaceSensor && posControl.actualState.surface < 50.0f) {
|
||||
if ((posControl.flags.estSurfaceStatus == EST_TRUSTED) && posControl.actualState.surface < 50.0f) {
|
||||
// land_descent_rate == 200 : descend speed = 30 cm/s, gentle touchdown
|
||||
// Do not allow descent velocity slower than -30cm/s so the landing detector works.
|
||||
descentVelLimited = MIN(-0.15f * navConfig()->general.land_descent_rate, -30.0f);
|
||||
|
|
@ -1080,7 +1070,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_IN_PROGRESS(na
|
|||
UNUSED(previousState);
|
||||
|
||||
// If no position sensor available - land immediately
|
||||
if (posControl.flags.hasValidPositionSensor && posControl.flags.hasValidHeadingSensor) {
|
||||
if ((posControl.flags.estPosStatue >= EST_USABLE) && (posControl.flags.estHeadingStatus >= EST_USABLE)) {
|
||||
const bool isDoingRTH = (posControl.waypointList[posControl.activeWaypointIndex].action == NAV_WP_ACTION_RTH);
|
||||
|
||||
switch (posControl.waypointList[posControl.activeWaypointIndex].action) {
|
||||
|
|
@ -1164,7 +1154,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_FINISHED(navig
|
|||
UNUSED(previousState);
|
||||
|
||||
// If no position sensor available - land immediately
|
||||
if (posControl.flags.hasValidPositionSensor && posControl.flags.hasValidHeadingSensor) {
|
||||
if ((posControl.flags.estPosStatue >= EST_USABLE) && (posControl.flags.estHeadingStatus >= EST_USABLE)) {
|
||||
return NAV_FSM_EVENT_NONE;
|
||||
}
|
||||
/* No pos sensor available for NAV_WAIT_FOR_GPS_TIMEOUT_MS - land */
|
||||
|
|
@ -1435,7 +1425,7 @@ bool isThrustFacingDownwards(void)
|
|||
bool checkForPositionSensorTimeout(void)
|
||||
{
|
||||
if (navConfig()->general.pos_failure_timeout) {
|
||||
if (!posControl.flags.hasValidPositionSensor && ((millis() - posControl.lastValidPositionTimeMs) > (1000 * navConfig()->general.pos_failure_timeout))) {
|
||||
if ((posControl.flags.estPosStatue == EST_NONE) && ((millis() - posControl.lastValidPositionTimeMs) > (1000 * navConfig()->general.pos_failure_timeout))) {
|
||||
return true;
|
||||
}
|
||||
else {
|
||||
|
|
@ -1451,7 +1441,7 @@ bool checkForPositionSensorTimeout(void)
|
|||
/*-----------------------------------------------------------
|
||||
* Processes an update to XY-position and velocity
|
||||
*-----------------------------------------------------------*/
|
||||
void updateActualHorizontalPositionAndVelocity(bool hasValidSensor, float newX, float newY, float newVelX, float newVelY)
|
||||
void updateActualHorizontalPositionAndVelocity(bool estimateValid, float newX, float newY, float newVelX, float newVelY)
|
||||
{
|
||||
posControl.actualState.pos.V.X = newX;
|
||||
posControl.actualState.pos.V.Y = newY;
|
||||
|
|
@ -1460,17 +1450,18 @@ void updateActualHorizontalPositionAndVelocity(bool hasValidSensor, float newX,
|
|||
posControl.actualState.vel.V.Y = newVelY;
|
||||
posControl.actualState.velXY = sqrtf(sq(newVelX) + sq(newVelY));
|
||||
|
||||
posControl.flags.hasValidPositionSensor = hasValidSensor;
|
||||
posControl.flags.hasValidHeadingSensor = isImuHeadingValid();
|
||||
|
||||
if (hasValidSensor) {
|
||||
if (estimateValid) {
|
||||
posControl.flags.estPosStatue = EST_TRUSTED;
|
||||
posControl.flags.horizontalPositionDataNew = 1;
|
||||
posControl.lastValidPositionTimeMs = millis();
|
||||
}
|
||||
else {
|
||||
posControl.flags.estPosStatue = EST_NONE;
|
||||
posControl.flags.horizontalPositionDataNew = 0;
|
||||
}
|
||||
|
||||
posControl.flags.estHeadingStatus = isImuHeadingValid() ? EST_TRUSTED : EST_NONE;
|
||||
|
||||
#if defined(NAV_BLACKBOX)
|
||||
navLatestActualPosition[X] = newX;
|
||||
navLatestActualPosition[Y] = newY;
|
||||
|
|
@ -1482,20 +1473,20 @@ void updateActualHorizontalPositionAndVelocity(bool hasValidSensor, float newX,
|
|||
/*-----------------------------------------------------------
|
||||
* Processes an update to Z-position and velocity
|
||||
*-----------------------------------------------------------*/
|
||||
void updateActualAltitudeAndClimbRate(bool hasValidSensor, float newAltitude, float newVelocity)
|
||||
void updateActualAltitudeAndClimbRate(bool estimateValid, float newAltitude, float newVelocity)
|
||||
{
|
||||
posControl.actualState.pos.V.Z = newAltitude;
|
||||
posControl.actualState.vel.V.Z = newVelocity;
|
||||
|
||||
posControl.flags.hasValidAltitudeSensor = hasValidSensor;
|
||||
|
||||
// Update altitude that would be used when executing RTH
|
||||
if (hasValidSensor) {
|
||||
if (estimateValid) {
|
||||
updateDesiredRTHAltitude();
|
||||
posControl.flags.estAltStatus = EST_TRUSTED;
|
||||
posControl.flags.verticalPositionDataNew = 1;
|
||||
posControl.lastValidAltitudeTimeMs = millis();
|
||||
}
|
||||
else {
|
||||
posControl.flags.estAltStatus = EST_NONE;
|
||||
posControl.flags.verticalPositionDataNew = 0;
|
||||
}
|
||||
|
||||
|
|
@ -1508,25 +1499,24 @@ void updateActualAltitudeAndClimbRate(bool hasValidSensor, float newAltitude, fl
|
|||
/*-----------------------------------------------------------
|
||||
* Processes an update to surface distance
|
||||
*-----------------------------------------------------------*/
|
||||
void updateActualSurfaceDistance(bool hasValidSensor, float surfaceDistance, float surfaceVelocity)
|
||||
void updateActualAGLAndClimgRate(bool estimateValid, bool estimateReliable, float surfaceDistance, float surfaceVelocity)
|
||||
{
|
||||
posControl.actualState.surface = surfaceDistance;
|
||||
posControl.actualState.surfaceVel = surfaceVelocity;
|
||||
|
||||
// Update validity
|
||||
// Update validity
|
||||
if (hasValidSensor) {
|
||||
posControl.flags.hasValidSurfaceSensor = true;
|
||||
if (estimateValid) {
|
||||
posControl.flags.estSurfaceStatus = estimateReliable ? EST_TRUSTED : EST_USABLE;
|
||||
posControl.flags.surfaceDistanceDataNew = 1;
|
||||
}
|
||||
else {
|
||||
posControl.flags.hasValidSurfaceSensor = false;
|
||||
posControl.flags.estSurfaceStatus = EST_NONE;
|
||||
posControl.flags.surfaceDistanceDataNew = 0;
|
||||
}
|
||||
|
||||
// Update minimum surface distance (landing detection threshold)
|
||||
if (ARMING_FLAG(ARMED)) {
|
||||
if (hasValidSensor && posControl.actualState.surface > 0) {
|
||||
if ((posControl.flags.estSurfaceStatus == EST_TRUSTED) && posControl.actualState.surface > 0) {
|
||||
if (posControl.actualState.surfaceMin > 0) {
|
||||
posControl.actualState.surfaceMin = MIN(posControl.actualState.surfaceMin, posControl.actualState.surface);
|
||||
}
|
||||
|
|
@ -1723,7 +1713,7 @@ void updateHomePosition(void)
|
|||
{
|
||||
// Disarmed and have a valid position, constantly update home
|
||||
if (!ARMING_FLAG(ARMED)) {
|
||||
if (posControl.flags.hasValidPositionSensor) {
|
||||
if ((posControl.flags.estPosStatue >= EST_USABLE)) {
|
||||
setHomePosition(&posControl.actualState.pos, posControl.actualState.yaw, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING );
|
||||
}
|
||||
}
|
||||
|
|
@ -1732,7 +1722,7 @@ void updateHomePosition(void)
|
|||
|
||||
// If pilot so desires he may reset home position to current position
|
||||
if (IS_RC_MODE_ACTIVE(BOXHOMERESET)) {
|
||||
if (isHomeResetAllowed && !FLIGHT_MODE(NAV_RTH_MODE) && !FLIGHT_MODE(NAV_WP_MODE) && posControl.flags.hasValidPositionSensor) {
|
||||
if (isHomeResetAllowed && !FLIGHT_MODE(NAV_RTH_MODE) && !FLIGHT_MODE(NAV_WP_MODE) && (posControl.flags.estPosStatue >= EST_USABLE)) {
|
||||
const navSetWaypointFlags_t homeUpdateFlags = STATE(GPS_FIX_HOME) ? (NAV_POS_UPDATE_XY | NAV_POS_UPDATE_HEADING) : (NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING);
|
||||
setHomePosition(&posControl.actualState.pos, posControl.actualState.yaw, homeUpdateFlags);
|
||||
isHomeResetAllowed = false;
|
||||
|
|
@ -1771,23 +1761,6 @@ int32_t getTotalTravelDistance(void)
|
|||
return lrintf(posControl.totalTripDistance);
|
||||
}
|
||||
|
||||
/*-----------------------------------------------------------
|
||||
* Set surface tracking target
|
||||
*-----------------------------------------------------------*/
|
||||
void setDesiredSurfaceOffset(float surfaceOffset)
|
||||
{
|
||||
if (surfaceOffset > 0) {
|
||||
posControl.desiredState.surface = constrainf(surfaceOffset, 1.0f, INAV_SURFACE_MAX_DISTANCE);
|
||||
}
|
||||
else {
|
||||
posControl.desiredState.surface = -1;
|
||||
}
|
||||
|
||||
#if defined(NAV_BLACKBOX)
|
||||
navTargetSurface = constrain(lrintf(posControl.desiredState.surface), -32678, 32767);
|
||||
#endif
|
||||
}
|
||||
|
||||
/*-----------------------------------------------------------
|
||||
* Calculate platform-specific hold position (account for deceleration)
|
||||
*-----------------------------------------------------------*/
|
||||
|
|
@ -2040,7 +2013,7 @@ void setWaypoint(uint8_t wpNumber, const navWaypoint_t * wpData)
|
|||
wpLLH.alt = wpData->alt;
|
||||
|
||||
// WP #0 - special waypoint - HOME
|
||||
if ((wpNumber == 0) && ARMING_FLAG(ARMED) && posControl.flags.hasValidPositionSensor && posControl.gpsOrigin.valid && posControl.flags.isGCSAssistedNavigationEnabled) {
|
||||
if ((wpNumber == 0) && ARMING_FLAG(ARMED) && (posControl.flags.estPosStatue >= EST_USABLE) && posControl.gpsOrigin.valid && posControl.flags.isGCSAssistedNavigationEnabled) {
|
||||
// Forcibly set home position. Note that this is only valid if already armed, otherwise home will be reset instantly
|
||||
geoConvertGeodeticToLocal(&posControl.gpsOrigin, &wpLLH, &wpPos.pos, GEO_ALT_RELATIVE);
|
||||
setHomePosition(&wpPos.pos, 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING);
|
||||
|
|
@ -2048,7 +2021,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.hasValidPositionSensor && posControl.gpsOrigin.valid && posControl.flags.isGCSAssistedNavigationEnabled &&
|
||||
ARMING_FLAG(ARMED) && (posControl.flags.estPosStatue >= EST_USABLE) && posControl.gpsOrigin.valid && posControl.flags.isGCSAssistedNavigationEnabled &&
|
||||
(posControl.navState == NAV_STATE_POSHOLD_2D_IN_PROGRESS || posControl.navState == NAV_STATE_POSHOLD_3D_IN_PROGRESS)) {
|
||||
// Convert to local coordinates
|
||||
geoConvertGeodeticToLocal(&posControl.gpsOrigin, &wpLLH, &wpPos.pos, GEO_ALT_RELATIVE);
|
||||
|
|
@ -2265,14 +2238,13 @@ void applyWaypointNavigationAndAltitudeHold(void)
|
|||
|
||||
#if defined(NAV_BLACKBOX)
|
||||
navFlags = 0;
|
||||
if (posControl.flags.hasValidAltitudeSensor) navFlags |= (1 << 0);
|
||||
if (posControl.flags.hasValidSurfaceSensor) navFlags |= (1 << 1);
|
||||
if (posControl.flags.hasValidPositionSensor) navFlags |= (1 << 2);
|
||||
//if (STATE(GPS_FIX)) navFlags |= (1 << 3);
|
||||
if (posControl.flags.estAltStatus == EST_TRUSTED) navFlags |= (1 << 0);
|
||||
if (posControl.flags.estSurfaceStatus == EST_TRUSTED) navFlags |= (1 << 1);
|
||||
if (posControl.flags.estPosStatue == EST_TRUSTED) navFlags |= (1 << 2);
|
||||
#if defined(NAV_GPS_GLITCH_DETECTION)
|
||||
if (isGPSGlitchDetected()) navFlags |= (1 << 4);
|
||||
if (isGPSGlitchDetected()) navFlags |= (1 << 4);
|
||||
#endif
|
||||
if (posControl.flags.hasValidHeadingSensor) navFlags |= (1 << 5);
|
||||
if (posControl.flags.estHeadingStatus == EST_TRUSTED) navFlags |= (1 << 5);
|
||||
#endif
|
||||
|
||||
// Reset all navigation requests - NAV controllers will set them if necessary
|
||||
|
|
@ -2334,12 +2306,12 @@ void swithNavigationFlightModes(void)
|
|||
*-----------------------------------------------------------*/
|
||||
static bool canActivateAltHoldMode(void)
|
||||
{
|
||||
return posControl.flags.hasValidAltitudeSensor;
|
||||
return (posControl.flags.estAltStatus >= EST_USABLE);
|
||||
}
|
||||
|
||||
static bool canActivatePosHoldMode(void)
|
||||
{
|
||||
return posControl.flags.hasValidPositionSensor && posControl.flags.hasValidHeadingSensor;
|
||||
return (posControl.flags.estPosStatue >= EST_USABLE) && (posControl.flags.estHeadingStatus >= EST_USABLE);
|
||||
}
|
||||
|
||||
static navigationFSMEvent_t selectNavEventFromBoxModeInput(void)
|
||||
|
|
@ -2487,7 +2459,7 @@ bool navigationBlockArming(void)
|
|||
return false;
|
||||
|
||||
// Apply extra arming safety only if pilot has any of GPS modes configured
|
||||
if ((isUsingNavigationModes() || failsafeMayRequireNavigationMode()) && !(posControl.flags.hasValidPositionSensor && STATE(GPS_FIX_HOME))) {
|
||||
if ((isUsingNavigationModes() || failsafeMayRequireNavigationMode()) && !((posControl.flags.estPosStatue >= EST_USABLE) && STATE(GPS_FIX_HOME))) {
|
||||
shouldBlockArming = true;
|
||||
}
|
||||
|
||||
|
|
@ -2514,7 +2486,7 @@ bool navigationBlockArming(void)
|
|||
|
||||
bool navigationPositionEstimateIsHealthy(void)
|
||||
{
|
||||
return posControl.flags.hasValidPositionSensor && STATE(GPS_FIX_HOME);
|
||||
return (posControl.flags.estPosStatue >= EST_USABLE) && STATE(GPS_FIX_HOME);
|
||||
}
|
||||
|
||||
/**
|
||||
|
|
@ -2627,10 +2599,10 @@ void navigationInit(void)
|
|||
posControl.flags.surfaceDistanceDataNew = 0;
|
||||
posControl.flags.headingDataNew = 0;
|
||||
|
||||
posControl.flags.hasValidAltitudeSensor = 0;
|
||||
posControl.flags.hasValidPositionSensor = 0;
|
||||
posControl.flags.hasValidSurfaceSensor = 0;
|
||||
posControl.flags.hasValidHeadingSensor = 0;
|
||||
posControl.flags.estAltStatus = EST_NONE;
|
||||
posControl.flags.estPosStatue = EST_NONE;
|
||||
posControl.flags.estHeadingStatus = EST_NONE;
|
||||
posControl.flags.estSurfaceStatus = EST_NONE;
|
||||
|
||||
posControl.flags.forcedRTHActivated = 0;
|
||||
posControl.waypointCount = 0;
|
||||
|
|
|
|||
|
|
@ -151,7 +151,7 @@ void applyFixedWingAltitudeAndThrottleController(timeUs_t currentTimeUs)
|
|||
return;
|
||||
}
|
||||
|
||||
if (posControl.flags.hasValidAltitudeSensor) {
|
||||
if ((posControl.flags.estAltStatus >= EST_USABLE)) {
|
||||
if (posControl.flags.verticalPositionDataNew) {
|
||||
const timeDelta_t deltaMicrosPositionUpdate = currentTimeUs - previousTimePositionUpdate;
|
||||
previousTimePositionUpdate = currentTimeUs;
|
||||
|
|
@ -330,7 +330,7 @@ void applyFixedWingPositionController(timeUs_t currentTimeUs)
|
|||
}
|
||||
|
||||
// Apply controller only if position source is valid. In absence of valid pos sensor (GPS loss), we'd stick in forced ANGLE mode
|
||||
if (posControl.flags.hasValidPositionSensor) {
|
||||
if ((posControl.flags.estPosStatue >= EST_USABLE)) {
|
||||
// If we have new position - update velocity and acceleration controllers
|
||||
if (posControl.flags.horizontalPositionDataNew) {
|
||||
const timeDelta_t deltaMicrosPositionUpdate = currentTimeUs - previousTimePositionUpdate;
|
||||
|
|
@ -378,7 +378,7 @@ int16_t applyFixedWingMinSpeedController(timeUs_t currentTimeUs)
|
|||
}
|
||||
|
||||
// Apply controller only if position source is valid
|
||||
if (posControl.flags.hasValidPositionSensor) {
|
||||
if ((posControl.flags.estPosStatue >= EST_USABLE)) {
|
||||
// If we have new position - update velocity and acceleration controllers
|
||||
if (posControl.flags.horizontalPositionDataNew) {
|
||||
const timeDelta_t deltaMicrosPositionUpdate = currentTimeUs - previousTimePositionUpdate;
|
||||
|
|
@ -473,7 +473,8 @@ void applyFixedWingPitchRollThrottleController(navigationFSMStateFlags_t navStat
|
|||
* TODO refactor conditions in this metod if logic is proven to be correct
|
||||
*/
|
||||
if (navStateFlags & NAV_CTL_LAND) {
|
||||
if (posControl.flags.hasValidAltitudeSensor && posControl.actualState.pos.V.Z < navConfig()->general.land_slowdown_minalt) {
|
||||
if ( ((posControl.flags.estAltStatus >= EST_USABLE) && (posControl.actualState.pos.V.Z <= navConfig()->general.land_slowdown_minalt)) ||
|
||||
((posControl.flags.estSurfaceStatus == EST_TRUSTED) && (posControl.actualState.surface <= navConfig()->general.land_slowdown_minalt)) ) {
|
||||
/*
|
||||
* Set motor to min. throttle and stop it when MOTOR_STOP feature is enabled
|
||||
*/
|
||||
|
|
@ -481,7 +482,7 @@ void applyFixedWingPitchRollThrottleController(navigationFSMStateFlags_t navStat
|
|||
ENABLE_STATE(NAV_MOTOR_STOP_OR_IDLE);
|
||||
|
||||
/*
|
||||
* Stabilize ROLL axis on 0 degress banking regardless of loiter radius and position
|
||||
* Stabilize ROLL axis on 0 degrees banking regardless of loiter radius and position
|
||||
*/
|
||||
rcCommand[ROLL] = 0;
|
||||
|
||||
|
|
|
|||
|
|
@ -422,7 +422,7 @@ static void applyMulticopterPositionController(timeUs_t currentTimeUs)
|
|||
|
||||
// Apply controller only if position source is valid. In absence of valid pos sensor (GPS loss), we'd stick in forced ANGLE mode
|
||||
// and pilots input would be passed thru to PID controller
|
||||
if (posControl.flags.hasValidPositionSensor) {
|
||||
if ((posControl.flags.estPosStatue >= EST_USABLE)) {
|
||||
// If we have new position - update velocity and acceleration controllers
|
||||
if (posControl.flags.horizontalPositionDataNew) {
|
||||
const timeDelta_t deltaMicrosPositionUpdate = currentTimeUs - previousTimePositionUpdate;
|
||||
|
|
@ -509,7 +509,7 @@ bool isMulticopterLandingDetected(void)
|
|||
}
|
||||
|
||||
// If we have surface sensor (for example sonar) - use it to detect touchdown
|
||||
if (posControl.flags.hasValidSurfaceSensor && posControl.actualState.surfaceMin >= 0) {
|
||||
if ((posControl.flags.estSurfaceStatus == EST_TRUSTED) && (posControl.actualState.surfaceMin >= 0)) {
|
||||
// TODO: Come up with a clever way to let sonar increase detection performance, not just add extra safety.
|
||||
// TODO: Out of range sonar may give reading that looks like we landed, find a way to check if sonar is healthy.
|
||||
// surfaceMin is our ground reference. If we are less than 5cm above the ground - we are likely landed
|
||||
|
|
@ -540,9 +540,7 @@ static void applyMulticopterEmergencyLandingController(timeUs_t currentTimeUs)
|
|||
rcCommand[PITCH] = 0;
|
||||
rcCommand[YAW] = 0;
|
||||
|
||||
if (posControl.flags.hasValidAltitudeSensor) {
|
||||
/* We have an altitude reference, apply AH-based landing controller */
|
||||
|
||||
if ((posControl.flags.estAltStatus >= EST_USABLE)) {
|
||||
// If last position update was too long in the past - ignore it (likely restarting altitude controller)
|
||||
if (deltaMicros > HZ2US(MIN_POSITION_UPDATE_RATE_HZ)) {
|
||||
previousTimeUpdate = currentTimeUs;
|
||||
|
|
|
|||
|
|
@ -109,9 +109,16 @@ typedef struct {
|
|||
float airspeed; // airspeed (cm/s)
|
||||
} navPositionEstimatorPITOT_t;
|
||||
|
||||
typedef enum {
|
||||
SURFACE_QUAL_LOW, // Surface sensor signal lost long ago - most likely surface distance is incorrect
|
||||
SURFACE_QUAL_MID, // Surface sensor is not available but we can somewhat trust the estimate
|
||||
SURFACE_QUAL_HIGH // All good
|
||||
} navAGLEstimateQuality_e;
|
||||
|
||||
typedef struct {
|
||||
timeUs_t lastUpdateTime; // Last update time (us)
|
||||
float alt; // Raw altitude measurement (cm)
|
||||
float reliability;
|
||||
} navPositionEstimatorSURFACE_t;
|
||||
|
||||
typedef struct {
|
||||
|
|
@ -124,15 +131,18 @@ typedef struct {
|
|||
|
||||
typedef struct {
|
||||
timeUs_t lastUpdateTime; // Last update time (us)
|
||||
|
||||
// 3D position, velocity and confidence
|
||||
t_fp_vector pos;
|
||||
t_fp_vector vel;
|
||||
float eph;
|
||||
float epv;
|
||||
// Surface offset
|
||||
float surface;
|
||||
float surfaceVel;
|
||||
bool surfaceValid;
|
||||
|
||||
// AGL
|
||||
navAGLEstimateQuality_e aglQual;
|
||||
float aglOffset; // Offset between surface and pos.Z
|
||||
float aglAlt;
|
||||
float aglVel;
|
||||
} navPositionEstimatorESTIMATE_t;
|
||||
|
||||
typedef struct {
|
||||
|
|
@ -484,11 +494,39 @@ static void updatePitotTopic(timeUs_t currentTimeUs)
|
|||
* Read surface and update alt/vel topic
|
||||
* Function is called from TASK_RANGEFINDER at arbitrary rate - as soon as new measurements are available
|
||||
*/
|
||||
#define RANGEFINDER_RELIABILITY_RC_CONSTANT (0.47802f)
|
||||
#define RANGEFINDER_RELIABILITY_LIGHT_THRESHOLD (0.15f)
|
||||
#define RANGEFINDER_RELIABILITY_LOW_THRESHOLD (0.33f)
|
||||
#define RANGEFINDER_RELIABILITY_HIGH_THRESHOLD (0.75f)
|
||||
|
||||
void updatePositionEstimator_SurfaceTopic(timeUs_t currentTimeUs, float newSurfaceAlt)
|
||||
{
|
||||
if (newSurfaceAlt > 0 && newSurfaceAlt <= positionEstimationConfig()->max_surface_altitude) {
|
||||
posEstimator.surface.alt = newSurfaceAlt;
|
||||
posEstimator.surface.lastUpdateTime = currentTimeUs;
|
||||
const float dt = US2S(currentTimeUs - posEstimator.surface.lastUpdateTime);
|
||||
float newReliabilityMeasurement = 0;
|
||||
|
||||
posEstimator.surface.lastUpdateTime = currentTimeUs;
|
||||
|
||||
if (newSurfaceAlt >= 0) {
|
||||
if (newSurfaceAlt <= positionEstimationConfig()->max_surface_altitude) {
|
||||
newReliabilityMeasurement = 1.0f;
|
||||
posEstimator.surface.alt = newSurfaceAlt;
|
||||
}
|
||||
else {
|
||||
newReliabilityMeasurement = 0.0f;
|
||||
}
|
||||
}
|
||||
else {
|
||||
// Negative values - out of range or failed hardware
|
||||
newReliabilityMeasurement = 0.0f;
|
||||
}
|
||||
|
||||
/* Reliability is a measure of confidence of rangefinder measurement. It's increased with each valid sample and decreased with each invalid sample */
|
||||
if (dt > 0.5f) {
|
||||
posEstimator.surface.reliability = 0.0f;
|
||||
}
|
||||
else {
|
||||
const float relAlpha = dt / (dt + RANGEFINDER_RELIABILITY_RC_CONSTANT);
|
||||
posEstimator.surface.reliability = posEstimator.surface.reliability * (1.0f - relAlpha) + newReliabilityMeasurement * relAlpha;
|
||||
}
|
||||
}
|
||||
#endif
|
||||
|
|
@ -831,32 +869,111 @@ static void updateEstimatedTopic(timeUs_t currentTimeUs)
|
|||
}
|
||||
}
|
||||
|
||||
/* Surface offset */
|
||||
#ifdef USE_RANGEFINDER
|
||||
posEstimator.est.surface = posEstimator.est.surface + posEstimator.est.surfaceVel * dt;
|
||||
|
||||
if (isSurfaceValid) {
|
||||
const float surfaceResidual = posEstimator.surface.alt - posEstimator.est.surface;
|
||||
const float bellCurveScaler = scaleRangef(bellCurve(surfaceResidual, 50.0f), 0.0f, 1.0f, 0.1f, 1.0f);
|
||||
|
||||
posEstimator.est.surface += surfaceResidual * positionEstimationConfig()->w_z_surface_p * bellCurveScaler * dt;
|
||||
posEstimator.est.surfaceVel += surfaceResidual * positionEstimationConfig()->w_z_surface_v * sq(bellCurveScaler) * dt;
|
||||
posEstimator.est.surfaceValid = true;
|
||||
}
|
||||
else {
|
||||
posEstimator.est.surfaceVel = 0; // Zero out velocity to prevent estimate to drift away
|
||||
posEstimator.est.surfaceValid = false;
|
||||
}
|
||||
|
||||
#else
|
||||
posEstimator.est.surface = -1;
|
||||
posEstimator.est.surfaceVel = 0;
|
||||
posEstimator.est.surfaceValid = false;
|
||||
#endif
|
||||
|
||||
/* Update uncertainty */
|
||||
posEstimator.est.eph = newEPH;
|
||||
posEstimator.est.epv = newEPV;
|
||||
|
||||
#ifdef USE_RANGEFINDER
|
||||
/* Surface offset */
|
||||
if (isSurfaceValid) { // If surface topic is updated in timely manner - do something smart
|
||||
navAGLEstimateQuality_e newAglQuality = posEstimator.est.aglQual;
|
||||
bool resetSurfaceEstimate = false;
|
||||
switch (posEstimator.est.aglQual) {
|
||||
case SURFACE_QUAL_LOW:
|
||||
if (posEstimator.surface.reliability >= RANGEFINDER_RELIABILITY_HIGH_THRESHOLD) {
|
||||
newAglQuality = SURFACE_QUAL_HIGH;
|
||||
resetSurfaceEstimate = true;
|
||||
}
|
||||
else if (posEstimator.surface.reliability >= RANGEFINDER_RELIABILITY_LOW_THRESHOLD) {
|
||||
newAglQuality = SURFACE_QUAL_LOW;
|
||||
}
|
||||
else {
|
||||
newAglQuality = SURFACE_QUAL_LOW;
|
||||
}
|
||||
break;
|
||||
|
||||
case SURFACE_QUAL_MID:
|
||||
if (posEstimator.surface.reliability >= RANGEFINDER_RELIABILITY_HIGH_THRESHOLD) {
|
||||
newAglQuality = SURFACE_QUAL_HIGH;
|
||||
}
|
||||
else if (posEstimator.surface.reliability >= RANGEFINDER_RELIABILITY_LOW_THRESHOLD) {
|
||||
newAglQuality = SURFACE_QUAL_MID;
|
||||
}
|
||||
else {
|
||||
newAglQuality = SURFACE_QUAL_LOW;
|
||||
}
|
||||
break;
|
||||
|
||||
case SURFACE_QUAL_HIGH:
|
||||
if (posEstimator.surface.reliability >= RANGEFINDER_RELIABILITY_HIGH_THRESHOLD) {
|
||||
newAglQuality = SURFACE_QUAL_HIGH;
|
||||
}
|
||||
else if (posEstimator.surface.reliability >= RANGEFINDER_RELIABILITY_LOW_THRESHOLD) {
|
||||
newAglQuality = SURFACE_QUAL_MID;
|
||||
}
|
||||
else {
|
||||
newAglQuality = SURFACE_QUAL_LOW;
|
||||
}
|
||||
break;
|
||||
}
|
||||
|
||||
posEstimator.est.aglQual = newAglQuality;
|
||||
|
||||
if (resetSurfaceEstimate) {
|
||||
posEstimator.est.aglAlt = posEstimator.surface.alt;
|
||||
if (posEstimator.est.epv < positionEstimationConfig()->max_eph_epv) {
|
||||
posEstimator.est.aglVel = posEstimator.est.vel.V.Z;
|
||||
posEstimator.est.aglOffset = posEstimator.est.pos.V.Z - posEstimator.surface.alt;
|
||||
}
|
||||
else {
|
||||
posEstimator.est.aglVel = 0;
|
||||
posEstimator.est.aglOffset = 0;
|
||||
}
|
||||
}
|
||||
|
||||
// Update estimate
|
||||
posEstimator.est.aglAlt = posEstimator.est.aglAlt + posEstimator.est.aglVel * dt + posEstimator.imu.accelNEU.V.Z * dt * dt * 0.5f;
|
||||
posEstimator.est.aglVel = posEstimator.est.aglVel + posEstimator.imu.accelNEU.V.Z * dt;
|
||||
|
||||
// Apply correction
|
||||
if (posEstimator.est.aglQual == SURFACE_QUAL_HIGH) {
|
||||
// Correct estimate from rangefinder
|
||||
const float surfaceResidual = posEstimator.surface.alt - posEstimator.est.aglAlt;
|
||||
const float bellCurveScaler = scaleRangef(bellCurve(surfaceResidual, 50.0f), 0.0f, 1.0f, 0.1f, 1.0f);
|
||||
|
||||
posEstimator.est.aglAlt += surfaceResidual * positionEstimationConfig()->w_z_surface_p * bellCurveScaler * posEstimator.surface.reliability * dt;
|
||||
posEstimator.est.aglVel += surfaceResidual * positionEstimationConfig()->w_z_surface_v * sq(bellCurveScaler) * sq(posEstimator.surface.reliability) * dt;
|
||||
|
||||
// Update estimate offset
|
||||
if ((posEstimator.est.aglQual == SURFACE_QUAL_HIGH) && (posEstimator.est.epv < positionEstimationConfig()->max_eph_epv)) {
|
||||
posEstimator.est.aglOffset = posEstimator.est.pos.V.Z - posEstimator.surface.alt;
|
||||
}
|
||||
}
|
||||
else if (posEstimator.est.aglQual == SURFACE_QUAL_MID) {
|
||||
// Correct estimate from altitude fused from rangefinder and global altitude
|
||||
const float estAltResidual = (posEstimator.est.pos.V.Z - posEstimator.est.aglOffset) - posEstimator.est.aglAlt;
|
||||
const float surfaceResidual = posEstimator.surface.alt - posEstimator.est.aglAlt;
|
||||
const float surfaceWeightScaler = scaleRangef(bellCurve(surfaceResidual, 50.0f), 0.0f, 1.0f, 0.1f, 1.0f) * posEstimator.surface.reliability;
|
||||
const float mixedResidual = surfaceResidual * surfaceWeightScaler + estAltResidual * (1.0f - surfaceWeightScaler);
|
||||
|
||||
posEstimator.est.aglAlt += mixedResidual * positionEstimationConfig()->w_z_surface_p * dt;
|
||||
posEstimator.est.aglVel += mixedResidual * positionEstimationConfig()->w_z_surface_v * dt;
|
||||
}
|
||||
else { // SURFACE_QUAL_LOW
|
||||
// In this case rangefinder can't be trusted - simply use global altitude
|
||||
posEstimator.est.aglAlt = posEstimator.est.pos.V.Z - posEstimator.est.aglOffset;
|
||||
posEstimator.est.aglVel = posEstimator.est.vel.V.Z;
|
||||
}
|
||||
}
|
||||
else {
|
||||
posEstimator.est.aglVel = 0;
|
||||
posEstimator.est.aglQual = SURFACE_QUAL_LOW;
|
||||
}
|
||||
#else
|
||||
posEstimator.est.aglAlt = posEstimator.est.pos.V.Z;
|
||||
posEstimator.est.aglVel = posEstimator.est.vel.V.Z;
|
||||
posEstimator.est.aglQual = SURFACE_QUAL_LOW;
|
||||
#endif
|
||||
}
|
||||
|
||||
/**
|
||||
|
|
@ -882,15 +999,15 @@ static void publishEstimatedTopic(timeUs_t currentTimeUs)
|
|||
|
||||
/* Publish altitude update and set altitude validity */
|
||||
if (posEstimator.est.epv < positionEstimationConfig()->max_eph_epv) {
|
||||
const bool aglReliable = (posEstimator.est.aglQual == SURFACE_QUAL_HIGH);
|
||||
updateActualAltitudeAndClimbRate(true, posEstimator.est.pos.V.Z, posEstimator.est.vel.V.Z);
|
||||
updateActualAGLAndClimgRate(true, aglReliable, posEstimator.est.aglAlt, posEstimator.est.aglVel);
|
||||
}
|
||||
else {
|
||||
updateActualAltitudeAndClimbRate(false, posEstimator.est.pos.V.Z, 0);
|
||||
updateActualAGLAndClimgRate(false, false, posEstimator.est.aglAlt, 0);
|
||||
}
|
||||
|
||||
/* Publish surface distance */
|
||||
updateActualSurfaceDistance(posEstimator.est.surfaceValid, posEstimator.est.surface, posEstimator.est.surfaceVel);
|
||||
|
||||
/* Store history data */
|
||||
posEstimator.history.pos[posEstimator.history.index] = posEstimator.est.pos;
|
||||
posEstimator.history.vel[posEstimator.history.index] = posEstimator.est.vel;
|
||||
|
|
@ -928,8 +1045,8 @@ void initializePositionEstimator(void)
|
|||
posEstimator.baro.lastUpdateTime = 0;
|
||||
posEstimator.surface.lastUpdateTime = 0;
|
||||
|
||||
posEstimator.est.surface = 0;
|
||||
posEstimator.est.surfaceVel = 0;
|
||||
posEstimator.est.aglAlt = 0;
|
||||
posEstimator.est.aglVel = 0;
|
||||
|
||||
posEstimator.history.index = 0;
|
||||
|
||||
|
|
|
|||
|
|
@ -56,6 +56,12 @@ typedef enum {
|
|||
ROC_TO_ALT_NORMAL
|
||||
} climbRateToAltitudeControllerMode_e;
|
||||
|
||||
typedef enum {
|
||||
EST_NONE = 0, // No valid sensor present
|
||||
EST_USABLE = 1, // Estimate is usable but may be inaccurate
|
||||
EST_TRUSTED = 2 // Estimate is usable and based on actual sensor data
|
||||
} navigationEstimateStatus_e;
|
||||
|
||||
typedef struct navigationFlags_s {
|
||||
bool horizontalPositionDataNew;
|
||||
bool verticalPositionDataNew;
|
||||
|
|
@ -65,10 +71,10 @@ typedef struct navigationFlags_s {
|
|||
bool horizontalPositionDataConsumed;
|
||||
bool verticalPositionDataConsumed;
|
||||
|
||||
bool hasValidAltitudeSensor; // Indicates that we have a working altitude sensor (got at least one valid reading from it)
|
||||
bool hasValidPositionSensor; // Indicates that GPS is working (or not)
|
||||
bool hasValidSurfaceSensor;
|
||||
bool hasValidHeadingSensor; // Indicate valid heading - wither mag or GPS at certain speed on airplane
|
||||
navigationEstimateStatus_e estAltStatus; // Indicates that we have a working altitude sensor (got at least one valid reading from it)
|
||||
navigationEstimateStatus_e estPosStatue; // Indicates that GPS is working (or not)
|
||||
navigationEstimateStatus_e estHeadingStatus; // Indicate valid heading - wither mag or GPS at certain speed on airplane
|
||||
navigationEstimateStatus_e estSurfaceStatus;
|
||||
|
||||
bool isAdjustingPosition;
|
||||
bool isAdjustingAltitude;
|
||||
|
|
@ -321,9 +327,9 @@ bool isApproachingLastWaypoint(void);
|
|||
float getActiveWaypointSpeed(void);
|
||||
|
||||
void updateActualHeading(int32_t newHeading);
|
||||
void updateActualHorizontalPositionAndVelocity(bool hasValidSensor, float newX, float newY, float newVelX, float newVelY);
|
||||
void updateActualAltitudeAndClimbRate(bool hasValidSensor, float newAltitude, float newVelocity);
|
||||
void updateActualSurfaceDistance(bool hasValidSensor, float surfaceDistance, float surfaceVelocity);
|
||||
void updateActualHorizontalPositionAndVelocity(bool estimateValid, float newX, float newY, float newVelX, float newVelY);
|
||||
void updateActualAltitudeAndClimbRate(bool estimateValid, float newAltitude, float newVelocity);
|
||||
void updateActualAGLAndClimgRate(bool estimateValid, bool estimateReliable, float surfaceDistance, float surfaceVelocity);
|
||||
|
||||
bool checkForPositionSensorTimeout(void);
|
||||
|
||||
|
|
|
|||
Loading…
Add table
Add a link
Reference in a new issue