Skip to content

Commit

Permalink
Update navigation.c
Browse files Browse the repository at this point in the history
  • Loading branch information
@breadoven
breadoven committed Feb 17, 2024
1 parent 4695d6a commit fe89694
Showing 1 changed file with 60 additions and 61 deletions.
121 changes: 60 additions & 61 deletions src/main/navigation/navigation.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -233,7 +233,7 @@ PG_RESET_TEMPLATE(navConfig_t, navConfig,
.launch_land_abort_deadband = SETTING_NAV_FW_LAUNCH_LAND_ABORT_DEADBAND_DEFAULT, // 100 us

.allow_manual_thr_increase = SETTING_NAV_FW_ALLOW_MANUAL_THR_INCREASE_DEFAULT,
.useFwNavYawControl = SETTING_NAV_USE_FW_YAW_CONTROL_DEFAULT,
.useFwNavYawControl = SETTING_NAV_USE_FW_YAW_CONTROL_DEFAULT,
.yawControlDeadband = SETTING_NAV_FW_YAW_DEADBAND_DEFAULT,
.soaring_pitch_deadband = SETTING_NAV_FW_SOARING_PITCH_DEADBAND_DEFAULT,// pitch angle mode deadband when Saoring mode enabled
.wp_tracking_accuracy = SETTING_NAV_FW_WP_TRACKING_ACCURACY_DEFAULT, // 0, improves course tracking accuracy during FW WP missions
Expand Down Expand Up @@ -1065,13 +1065,13 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
[NAV_FSM_EVENT_SWITCH_TO_CRUISE] = NAV_STATE_CRUISE_INITIALIZE,
[NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_FW_LANDING_ABORT] = NAV_STATE_FW_LANDING_ABORT,
}
},
},

[NAV_STATE_FW_LANDING_LOITER] = {
.persistentId = NAV_PERSISTENT_ID_FW_LANDING_LOITER,
.onEntry = navOnEnteringState_NAV_STATE_FW_LANDING_LOITER,
.timeoutMs = 10,
.stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_RTH,
.stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_RTH,
.mapToFlightModes = NAV_RTH_MODE | NAV_ALTHOLD_MODE,
.mwState = MW_NAV_STATE_LAND_IN_PROGRESS,
.mwError = MW_NAV_ERROR_NONE,
Expand All @@ -1086,7 +1086,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
[NAV_FSM_EVENT_SWITCH_TO_CRUISE] = NAV_STATE_CRUISE_INITIALIZE,
[NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_FW_LANDING_ABORT] = NAV_STATE_FW_LANDING_ABORT,
}
},
},

[NAV_STATE_FW_LANDING_APPROACH] = {
.persistentId = NAV_PERSISTENT_ID_FW_LANDING_APPROACH,
Expand All @@ -1107,7 +1107,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
[NAV_FSM_EVENT_SWITCH_TO_CRUISE] = NAV_STATE_CRUISE_INITIALIZE,
[NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_FW_LANDING_ABORT] = NAV_STATE_FW_LANDING_ABORT,
}
},
},

[NAV_STATE_FW_LANDING_GLIDE] = {
.persistentId = NAV_PERSISTENT_ID_FW_LANDING_GLIDE,
Expand All @@ -1118,7 +1118,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
.mwState = MW_NAV_STATE_LAND_IN_PROGRESS,
.mwError = MW_NAV_ERROR_NONE,
.onEvent = {
[NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_FW_LANDING_GLIDE,
[NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_FW_LANDING_GLIDE,
[NAV_FSM_EVENT_SUCCESS] = NAV_STATE_FW_LANDING_FLARE,
[NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE,
[NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_ALTHOLD_INITIALIZE,
Expand All @@ -1128,7 +1128,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
[NAV_FSM_EVENT_SWITCH_TO_CRUISE] = NAV_STATE_CRUISE_INITIALIZE,
[NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_FW_LANDING_ABORT] = NAV_STATE_FW_LANDING_ABORT,
}
},
},

[NAV_STATE_FW_LANDING_FLARE] = {
.persistentId = NAV_PERSISTENT_ID_FW_LANDING_FLARE,
Expand Down Expand Up @@ -1159,7 +1159,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
[NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE,
[NAV_FSM_EVENT_SWITCH_TO_WAYPOINT] = NAV_STATE_WAYPOINT_INITIALIZE,
}
},
},
#endif
};

Expand Down Expand Up @@ -1404,7 +1404,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_INITIALIZE(navigati
}

if (previousState != NAV_STATE_FW_LANDING_ABORT) {
#ifdef USE_FW_AUTOLAND
#ifdef USE_FW_AUTOLAND
posControl.fwLandState.landAborted = false;
#endif
if (STATE(FIXED_WING_LEGACY) && (posControl.homeDistance < navConfig()->general.min_rth_distance) && !posControl.flags.forcedRTHActivated) {
Expand Down Expand Up @@ -1721,17 +1721,17 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_LANDING(navigationF

#ifdef USE_FW_AUTOLAND
if (STATE(AIRPLANE)) {
int8_t missionIdx = -1, shIdx = -1, missionFwLandConfigStartIdx = 8;
int8_t missionIdx = -1, shIdx = -1, missionFwLandConfigStartIdx = 8;
#ifdef USE_MULTI_MISSION
missionIdx = posControl.loadedMultiMissionIndex - 1;
#endif
#endif

#ifdef USE_SAFE_HOME
shIdx = posControl.safehomeState.index;
missionFwLandConfigStartIdx = MAX_SAFE_HOMES;
#endif
if (!posControl.fwLandState.landAborted && (shIdx >= 0 || missionIdx >= 0) && (fwAutolandApproachConfig(shIdx)->landApproachHeading1 != 0 || fwAutolandApproachConfig(shIdx)->landApproachHeading2 != 0)) {

if (previousState == NAV_STATE_WAYPOINT_REACHED) {
posControl.fwLandState.landPos = posControl.activeWaypoint.pos;
posControl.fwLandState.approachSettingIdx = missionFwLandConfigStartIdx + missionIdx;
Expand All @@ -1741,30 +1741,29 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_LANDING(navigationF
posControl.fwLandState.approachSettingIdx = shIdx;
posControl.fwLandState.landWp = false;
}

posControl.fwLandState.landAltAgl = fwAutolandApproachConfig(posControl.fwLandState.approachSettingIdx)->isSeaLevelRef ? fwAutolandApproachConfig(posControl.fwLandState.approachSettingIdx)->landAlt - GPS_home.alt : fwAutolandApproachConfig(posControl.fwLandState.approachSettingIdx)->landAlt;
posControl.fwLandState.landAproachAltAgl = fwAutolandApproachConfig(posControl.fwLandState.approachSettingIdx)->isSeaLevelRef ? fwAutolandApproachConfig(posControl.fwLandState.approachSettingIdx)->approachAlt - GPS_home.alt : fwAutolandApproachConfig(posControl.fwLandState.approachSettingIdx)->approachAlt;
return NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_FW_LANDING;
}
}
}
#endif

float descentVelLimited = 0;

int32_t landingElevation = posControl.rthState.homeTmpWaypoint.z;
fpVector3_t tmpHomePos = posControl.rthState.homeTmpWaypoint;
uint32_t remaning_distance = calculateDistanceToDestination(&tmpHomePos);
uint32_t remainingDistance = calculateDistanceToDestination(&tmpHomePos);

int32_t landingElevation = posControl.rthState.homeTmpWaypoint.z;
if(STATE(MULTIROTOR) && (remaning_distance>MR_RTH_LAND_MARGIN_CM)){
descentVelLimited = navConfig()->general.land_minalt_vspd;
}
// A safeguard - if surface altitude sensors is available and it is reading < 50cm altitude - drop to low descend speed
else if ((posControl.flags.estAglStatus == EST_TRUSTED) && posControl.actualState.agl.pos.z < 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.
// A safeguard - if surface altitude sensor is available and is reading < 50cm altitude - drop to min descend speed.
// Also slow down to min descent speed during RTH MR landing if MR drifted too far away from home position.
bool minDescentSpeedRequired = (posControl.flags.estAglStatus == EST_TRUSTED && posControl.actualState.agl.pos.z < 50.0f) ||
(FLIGHT_MODE(NAV_RTH_MODE) && STATE(MULTIROTOR) && remainingDistance > MR_RTH_LAND_MARGIN_CM);

// Do not allow descent velocity slower than -30cm/s so the landing detector works (limited by land_minalt_vspd).
if (minDescentSpeedRequired) {
descentVelLimited = navConfig()->general.land_minalt_vspd;
} else {
// Ramp down descent velocity from 100% at maxAlt altitude to 25% from minAlt to 0cm.
// Ramp down descent velocity from max speed at maxAlt altitude to min speed from minAlt to 0cm.
float descentVelScaled = scaleRangef(navGetCurrentActualPositionAndVelocity()->pos.z,
navConfig()->general.land_slowdown_minalt + landingElevation,
navConfig()->general.land_slowdown_maxalt + landingElevation,
Expand Down Expand Up @@ -2237,17 +2236,17 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_FW_LANDING_CLIMB_TO_LOI
if (posControl.fwLandState.loiterStartTime == 0) {
posControl.fwLandState.loiterStartTime = micros();
}

if (ABS(getEstimatedActualPosition(Z) - posControl.fwLandState.landAproachAltAgl) < (navConfig()->general.waypoint_enforce_altitude > 0 ? navConfig()->general.waypoint_enforce_altitude : FW_LAND_LOITER_ALT_TOLERANCE)) {
updateClimbRateToAltitudeController(0, 0, ROC_TO_ALT_RESET);
posControl.fwLandState.landState = FW_AUTOLAND_STATE_LOITER;
return NAV_FSM_EVENT_SUCCESS;
}

fpVector3_t tmpHomePos = posControl.rthState.homePosition.pos;
tmpHomePos.z = posControl.fwLandState.landAproachAltAgl;
setDesiredPosition(&tmpHomePos, 0, NAV_POS_UPDATE_Z);

return NAV_FSM_EVENT_NONE;
}

Expand All @@ -2258,13 +2257,13 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_FW_LANDING_LOITER(navig
if ((posControl.flags.estHeadingStatus == EST_NONE) || checkForPositionSensorTimeout()) {
return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING;
}

if (isRollPitchStickDeflected(navConfig()->fw.launch_land_abort_deadband)) {
return NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_FW_LANDING_ABORT;
}

if (micros() - posControl.fwLandState.loiterStartTime > FW_LAND_LOITER_MIN_TIME) {
if (isEstimatedWindSpeedValid()) {
if (isEstimatedWindSpeedValid()) {

uint16_t windAngle = 0;
int32_t approachHeading = -1;
Expand All @@ -2277,7 +2276,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_FW_LANDING_LOITER(navig
approachHeading = posControl.fwLandState.landingDirection = ABS(DEGREES_TO_CENTIDEGREES(fwAutolandApproachConfig(posControl.fwLandState.approachSettingIdx)->landApproachHeading1));
} else if ((fwAutolandApproachConfig(posControl.fwLandState.approachSettingIdx)->landApproachHeading2 != 0) ) {
approachHeading = posControl.fwLandState.landingDirection = ABS(DEGREES_TO_CENTIDEGREES(fwAutolandApproachConfig(posControl.fwLandState.approachSettingIdx)->landApproachHeading2));
}
}
} else {
int32_t heading1 = calcFinalApproachHeading(DEGREES_TO_CENTIDEGREES(fwAutolandApproachConfig(posControl.fwLandState.approachSettingIdx)->landApproachHeading1), windAngle);
int32_t heading2 = calcFinalApproachHeading(DEGREES_TO_CENTIDEGREES(fwAutolandApproachConfig(posControl.fwLandState.approachSettingIdx)->landApproachHeading2), windAngle);
Expand Down Expand Up @@ -2305,23 +2304,23 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_FW_LANDING_LOITER(navig

if (posControl.fwLandState.landingDirection >= 0) {
fpVector3_t tmpPos;
int32_t finalApproachAlt = posControl.fwLandState.landAproachAltAgl / 3 * 2;

int32_t finalApproachAlt = posControl.fwLandState.landAproachAltAgl / 3 * 2;
int32_t dir = 0;
if (fwAutolandApproachConfig(posControl.fwLandState.approachSettingIdx)->approachDirection == FW_AUTOLAND_APPROACH_DIRECTION_LEFT) {
dir = wrap_36000(ABS(approachHeading) - 9000);
} else {
dir = wrap_36000(ABS(approachHeading) + 9000);
}

calculateFarAwayPos(&tmpPos, &posControl.fwLandState.landPos, posControl.fwLandState.landingDirection, navFwAutolandConfig()->approachLength);
tmpPos.z = posControl.fwLandState.landAltAgl - finalApproachAlt;
tmpPos.z = posControl.fwLandState.landAltAgl - finalApproachAlt;
posControl.fwLandState.landWaypoints[FW_AUTOLAND_WP_LAND] = tmpPos;

calculateFarAwayPos(&tmpPos, &posControl.fwLandState.landPos, wrap_36000(posControl.fwLandState.landingDirection + 18000), navFwAutolandConfig()->approachLength);
tmpPos.z = finalApproachAlt;
posControl.fwLandState.landWaypoints[FW_AUTOLAND_WP_FINAL_APPROACH] = tmpPos;

calculateFarAwayPos(&tmpPos, &posControl.fwLandState.landWaypoints[FW_AUTOLAND_WP_FINAL_APPROACH], dir, MAX((uint32_t)navConfig()->fw.loiter_radius * 4, navFwAutolandConfig()->approachLength / 2));
tmpPos.z = posControl.fwLandState.landAproachAltAgl;
posControl.fwLandState.landWaypoints[FW_AUTOLAND_WP_TURN] = tmpPos;
Expand All @@ -2342,7 +2341,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_FW_LANDING_LOITER(navig
fpVector3_t tmpPoint = posControl.fwLandState.landPos;
tmpPoint.z = posControl.fwLandState.landAproachAltAgl;
setDesiredPosition(&tmpPoint, posControl.fwLandState.landPosHeading, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING);

return NAV_FSM_EVENT_NONE;
}
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_FW_LANDING_APPROACH(navigationFSMState_t previousState)
Expand Down Expand Up @@ -2381,17 +2380,17 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_FW_LANDING_APPROACH(nav
posControl.fwLandState.landCurrentWp = FW_AUTOLAND_WP_LAND;
posControl.fwLandState.landState = FW_AUTOLAND_STATE_FINAL_APPROACH;
return NAV_FSM_EVENT_NONE;
}
}
}
}

fpVector3_t tmpWaypoint;
tmpWaypoint.x = posControl.activeWaypoint.pos.x;
tmpWaypoint.y = posControl.activeWaypoint.pos.y;
tmpWaypoint.z = scaleRangef(constrainf(posControl.wpDistance, posControl.wpInitialDistance / 10.0f, posControl.wpInitialDistance),
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);
setDesiredPosition(&tmpWaypoint, 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_BEARING);

return NAV_FSM_EVENT_NONE;
}

Expand All @@ -2407,32 +2406,32 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_FW_LANDING_GLIDE(naviga
posControl.fwLandState.landState = FW_AUTOLAND_STATE_FLARE;
return NAV_FSM_EVENT_SUCCESS;
}

if (isLandingDetected()) {
posControl.fwLandState.landState = FW_AUTOLAND_STATE_IDLE;
disarm(DISARM_LANDING);
return NAV_FSM_EVENT_SWITCH_TO_IDLE;
}

setDesiredPosition(NULL, posControl.cruise.course, NAV_POS_UPDATE_HEADING);
return NAV_FSM_EVENT_NONE;
}

static navigationFSMEvent_t navOnEnteringState_NAV_STATE_FW_LANDING_FLARE(navigationFSMState_t previousState)
{
UNUSED(previousState);

if (isLandingDetected()) {
posControl.fwLandState.landState = FW_AUTOLAND_STATE_IDLE;
disarm(DISARM_LANDING);
return NAV_FSM_EVENT_SUCCESS;
}
setDesiredPosition(NULL, posControl.cruise.course, NAV_POS_UPDATE_HEADING);

return NAV_FSM_EVENT_NONE;
}

static navigationFSMEvent_t navOnEnteringState_NAV_STATE_FW_LANDING_ABORT(navigationFSMState_t previousState)
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_FW_LANDING_ABORT(navigationFSMState_t previousState)
{
UNUSED(previousState);
posControl.fwLandState.landAborted = true;
Expand Down Expand Up @@ -5060,7 +5059,7 @@ uint8_t getActiveWpNumber(void)

static void resetFwAutoland(void)
{
posControl.fwLandState.landAltAgl = 0;
posControl.fwLandState.landAltAgl = 0;
posControl.fwLandState.landAproachAltAgl = 0;
posControl.fwLandState.loiterStartTime = 0;
posControl.fwLandState.approachSettingIdx = 0;
Expand All @@ -5074,21 +5073,21 @@ static int32_t calcFinalApproachHeading(int32_t approachHeading, int32_t windAng
if (approachHeading == 0) {
return -1;
}

int32_t windRelToRunway = wrap_36000(windAngle - ABS(approachHeading));
// Headwind?
if (windRelToRunway >= 27000 || windRelToRunway <= 9000) {
return wrap_36000(ABS(approachHeading));
}
}

if (approachHeading > 0) {
return wrap_36000(approachHeading + 18000);
}

return -1;
}

static float getLandAltitude(void)
static float getLandAltitude(void)
{
float altitude = -1;
#ifdef USE_RANGEFINDER
Expand All @@ -5110,7 +5109,7 @@ static int32_t calcWindDiff(int32_t heading, int32_t windHeading)
return ABS(wrap_18000(windHeading - heading));
}

static void setLandWaypoint(const fpVector3_t *pos, const fpVector3_t *nextWpPos)
static void setLandWaypoint(const fpVector3_t *pos, const fpVector3_t *nextWpPos)
{
calculateAndSetActiveWaypointToLocalPosition(pos);

Expand All @@ -5120,7 +5119,7 @@ static void setLandWaypoint(const fpVector3_t *pos, const fpVector3_t *nextWpPos
} else {
posControl.activeWaypoint.nextTurnAngle = -1;
}

posControl.wpInitialDistance = calculateDistanceToDestination(&posControl.activeWaypoint.pos);
posControl.wpInitialAltitude = posControl.actualState.abs.pos.z;
posControl.wpAltitudeReached = false;
Expand All @@ -5135,19 +5134,19 @@ void resetFwAutolandApproach(int8_t idx)
}
}

bool isFwLandInProgess(void)
bool isFwLandInProgess(void)
{
return posControl.navState == NAV_STATE_FW_LANDING_CLIMB_TO_LOITER
|| posControl.navState == NAV_STATE_FW_LANDING_LOITER
return posControl.navState == NAV_STATE_FW_LANDING_CLIMB_TO_LOITER
|| posControl.navState == NAV_STATE_FW_LANDING_LOITER
|| posControl.navState == NAV_STATE_FW_LANDING_APPROACH
|| posControl.navState == NAV_STATE_FW_LANDING_GLIDE
|| posControl.navState == NAV_STATE_FW_LANDING_FLARE;
}

bool canFwLandCanceld(void)
bool canFwLandCanceld(void)
{
return posControl.navState == NAV_STATE_FW_LANDING_CLIMB_TO_LOITER
|| posControl.navState == NAV_STATE_FW_LANDING_LOITER
return posControl.navState == NAV_STATE_FW_LANDING_CLIMB_TO_LOITER
|| posControl.navState == NAV_STATE_FW_LANDING_LOITER
|| posControl.navState == NAV_STATE_FW_LANDING_APPROACH
|| posControl.navState == NAV_STATE_FW_LANDING_GLIDE;
}
Expand Down

0 comments on commit fe89694

Please sign in to comment.