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

Bugfix

Browse source
This commit is contained in:
Scavanger 2023-12-02 18:04:52 -03:00
parent 3a3f507a94
commit b8dcf95a75
13 changed files with 127 additions and 156 deletions
Download patch file Download diff file Expand all files Collapse all files

3
.vscode/settings.json vendored
View file

@ -11,5 +11,6 @@
"editor.insertSpaces": true, "editor.insertSpaces": true,
"editor.detectIndentation": false, "editor.detectIndentation": false,
"editor.expandTabs": true, "editor.expandTabs": true,
"C_Cpp.clang_format_fallbackStyle": "{ BasedOnStyle: Google, IndentWidth: 4, BreakBeforeBraces: Mozilla }" "C_Cpp.clang_format_fallbackStyle": "{ BasedOnStyle: Google, IndentWidth: 4, BreakBeforeBraces: Mozilla }",
"cmake.configureOnOpen": false
} }

3
docs/development/IDE - Visual Studio Code with Windows 10.md
View file

@ -39,8 +39,7 @@ Edit file `./.vscode/c_cpp_properties.json` to setup enabled `defines`
"intelliSenseMode": "msvc-x64", "intelliSenseMode": "msvc-x64",
"cStandard": "c11", "cStandard": "c11",
"cppStandard": "c++17", "cppStandard": "c++17",
"defines": [ "defines": [,
"NAV_FIXED_WING_LANDING",
"USE_OSD", "USE_OSD",
"USE_GYRO_NOTCH_1", "USE_GYRO_NOTCH_1",
"USE_GYRO_NOTCH_2", "USE_GYRO_NOTCH_2",

1
docs/development/Windows 11 - VS Code - WSL2 - Hardware Debugging.md
View file

@ -235,7 +235,6 @@ sudo udevadm control --reload-rules
"cStandard": "c11", "cStandard": "c11",
"cppStandard": "c++17", "cppStandard": "c++17",
"defines": [ "defines": [
"NAV_FIXED_WING_LANDING",
"USE_OSD", "USE_OSD",
"USE_GYRO_NOTCH_1", "USE_GYRO_NOTCH_1",
"USE_GYRO_NOTCH_2", "USE_GYRO_NOTCH_2",

2
src/main/fc/fc_core.c
View file

@ -974,7 +974,7 @@ void taskMainPidLoop(timeUs_t currentTimeUs)
} }
#endif #endif
// Check if landed, FW and MR // Check if landed, FW and MR
if (STATE(ALTITUDE_CONTROL)) { if (STATE(ALTITUDE_CONTROL) || isFwLandInProgess()) {
updateLandingStatus(US2MS(currentTimeUs)); updateLandingStatus(US2MS(currentTimeUs));
} }

1
src/main/fc/fc_msp.c
View file

@ -3178,7 +3178,6 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
return MSP_RESULT_ERROR; return MSP_RESULT_ERROR;
} }
break; break;
#endif
#ifdef USE_EZ_TUNE #ifdef USE_EZ_TUNE

4
src/main/fc/settings.yaml
View file

@ -4117,13 +4117,13 @@ groups:
description: "Pitch value for glide phase. In degrees." description: "Pitch value for glide phase. In degrees."
default_value: 0 default_value: 0
field: glidePitch field: glidePitch
min: 0 min: -15
max: 45 max: 45
- name: nav_fw_land_flare_pitch - name: nav_fw_land_flare_pitch
description: "Pitch value for flare phase. In degrees" description: "Pitch value for flare phase. In degrees"
default_value: 8 default_value: 8
field: flarePitch field: flarePitch
min: 0 min: -15
max: 45 max: 45
- name: nav_fw_land_max_tailwind - name: nav_fw_land_max_tailwind
description: "Max. tailwind (in cm/s) if no landing direction with downwind is available" description: "Max. tailwind (in cm/s) if no landing direction with downwind is available"

4
src/main/io/osd.c
View file

@ -1007,7 +1007,7 @@ static const char * osdFailsafeInfoMessage(void)
#if defined(USE_SAFE_HOME) #if defined(USE_SAFE_HOME)
static const char * divertingToSafehomeMessage(void) static const char * divertingToSafehomeMessage(void)
{ {
if (!posControl.fwLandWp && (NAV_Status.state != MW_NAV_STATE_HOVER_ABOVE_HOME && safehome_applied)) { if (!posControl.fwLandState.landWp && (NAV_Status.state != MW_NAV_STATE_HOVER_ABOVE_HOME && posControl.safehomeState.isApplied)) {
return OSD_MESSAGE_STR(OSD_MSG_DIVERT_SAFEHOME); return OSD_MESSAGE_STR(OSD_MSG_DIVERT_SAFEHOME);
} }
return NULL; return NULL;
@ -5135,7 +5135,7 @@ textAttributes_t osdGetSystemMessage(char *buff, size_t buff_size, bool isCenter
if (ARMING_FLAG(ARMED)) { if (ARMING_FLAG(ARMED)) {
if (FLIGHT_MODE(FAILSAFE_MODE) || FLIGHT_MODE(NAV_RTH_MODE) || FLIGHT_MODE(NAV_WP_MODE) || navigationIsExecutingAnEmergencyLanding() || isFwLandInProgess()) { if (FLIGHT_MODE(FAILSAFE_MODE) || FLIGHT_MODE(NAV_RTH_MODE) || FLIGHT_MODE(NAV_WP_MODE) || navigationIsExecutingAnEmergencyLanding() || isFwLandInProgess()) {
if (isWaypointMissionRTHActive() && !posControl.fwLandWp) { if (isWaypointMissionRTHActive() && !posControl.fwLandState.landWp) {
// if RTH activated whilst WP mode selected, remind pilot to cancel WP mode to exit RTH // if RTH activated whilst WP mode selected, remind pilot to cancel WP mode to exit RTH
messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_WP_RTH_CANCEL); messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_WP_RTH_CANCEL);
} }

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

@ -1031,7 +1031,6 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
} }
}, },
},
[NAV_STATE_FW_LANDING_CLIMB_TO_LOITER] = { [NAV_STATE_FW_LANDING_CLIMB_TO_LOITER] = {
.persistentId = NAV_PERSISTENT_ID_FW_LANDING_CLIMB_TO_LOITER, .persistentId = NAV_PERSISTENT_ID_FW_LANDING_CLIMB_TO_LOITER,
@ -1388,7 +1387,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_INITIALIZE(navigati
} }
if (previousState != NAV_STATE_FW_LANDING_ABORT) { if (previousState != NAV_STATE_FW_LANDING_ABORT) {
posControl.fwLandAborted = false; posControl.fwLandState.landAborted = false;
if (STATE(FIXED_WING_LEGACY) && (posControl.homeDistance < navConfig()->general.min_rth_distance) && !posControl.flags.forcedRTHActivated) { if (STATE(FIXED_WING_LEGACY) && (posControl.homeDistance < navConfig()->general.min_rth_distance) && !posControl.flags.forcedRTHActivated) {
// Prevent RTH from activating on airplanes if too close to home unless it's a failsafe RTH // Prevent RTH from activating on airplanes if too close to home unless it's a failsafe RTH
return NAV_FSM_EVENT_SWITCH_TO_IDLE; return NAV_FSM_EVENT_SWITCH_TO_IDLE;
@ -1706,23 +1705,23 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_LANDING(navigationF
#endif #endif
#ifdef USE_SAFE_HOME #ifdef USE_SAFE_HOME
shIdx = safehome_index; shIdx = posControl.safehomeState.index;
missionFwLandConfigStartIdx = MAX_SAFE_HOMES; missionFwLandConfigStartIdx = MAX_SAFE_HOMES;
#endif #endif
if (!posControl.fwLandAborted && (shIdx >= 0 || missionIdx >= 0) && (fwAutolandApproachConfig(shIdx)->landApproachHeading1 != 0 || fwAutolandApproachConfig(shIdx)->landApproachHeading2 != 0)) { if (!posControl.fwLandState.landAborted && (shIdx >= 0 || missionIdx >= 0) && (fwAutolandApproachConfig(shIdx)->landApproachHeading1 != 0 || fwAutolandApproachConfig(shIdx)->landApproachHeading2 != 0)) {
if (previousState == NAV_STATE_WAYPOINT_REACHED) { if (previousState == NAV_STATE_WAYPOINT_REACHED) {
posControl.fwLandPos = posControl.activeWaypoint.pos; posControl.fwLandState.landPos = posControl.activeWaypoint.pos;
posControl.fwApproachSettingIdx = missionFwLandConfigStartIdx + missionIdx; posControl.fwLandState.approachSettingIdx = missionFwLandConfigStartIdx + missionIdx;
posControl.fwLandWp = true; posControl.fwLandState.landWp = true;
} else { } else {
posControl.fwLandPos = posControl.rthState.homePosition.pos; posControl.fwLandState.landPos = posControl.rthState.homePosition.pos;
posControl.fwApproachSettingIdx = shIdx; posControl.fwLandState.approachSettingIdx = shIdx;
posControl.fwLandWp = false; posControl.fwLandState.landWp = false;
} }
posControl.fwLandAltAgl = fwAutolandApproachConfig(posControl.fwApproachSettingIdx)->isSeaLevelRef ? fwAutolandApproachConfig(posControl.fwApproachSettingIdx)->landAlt - GPS_home.alt : fwAutolandApproachConfig(posControl.fwApproachSettingIdx)->landAlt; posControl.fwLandState.landAltAgl = fwAutolandApproachConfig(posControl.fwLandState.approachSettingIdx)->isSeaLevelRef ? fwAutolandApproachConfig(posControl.fwLandState.approachSettingIdx)->landAlt - GPS_home.alt : fwAutolandApproachConfig(posControl.fwLandState.approachSettingIdx)->landAlt;
posControl.fwLandAproachAltAgl = fwAutolandApproachConfig(posControl.fwApproachSettingIdx)->isSeaLevelRef ? fwAutolandApproachConfig(posControl.fwApproachSettingIdx)->approachAlt - GPS_home.alt : fwAutolandApproachConfig(posControl.fwApproachSettingIdx)->approachAlt; 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; return NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_FW_LANDING;
} else { } else {
return NAV_FSM_EVENT_SWITCH_TO_RTH_HOVER_ABOVE_HOME; return NAV_FSM_EVENT_SWITCH_TO_RTH_HOVER_ABOVE_HOME;
@ -1740,7 +1739,6 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_LANDING(navigationF
} }
// A safeguard - if surface altitude sensors is available and it is reading < 50cm altitude - drop to low descend speed // 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) { else if ((posControl.flags.estAglStatus == EST_TRUSTED) && posControl.actualState.agl.pos.z < 50.0f) {
if ((posControl.flags.estAglStatus ) && posControl.actualState.agl.pos.z < 50.0f) {
// land_descent_rate == 200 : descend speed = 30 cm/s, gentle touchdown // 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. // Do not allow descent velocity slower than -30cm/s so the landing detector works.
descentVelLimited = navConfig()->general.land_minalt_vspd; descentVelLimited = navConfig()->general.land_minalt_vspd;
@ -2055,7 +2053,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_EMERGENCY_LANDING_INITI
{ {
UNUSED(previousState); UNUSED(previousState);
posControl.fwLandState = FW_AUTOLAND_STATE_IDLE; posControl.fwLandState.landState = FW_AUTOLAND_STATE_IDLE;
if ((posControl.flags.estPosStatus >= EST_USABLE)) { if ((posControl.flags.estPosStatus >= EST_USABLE)) {
resetPositionController(); resetPositionController();
setDesiredPosition(&navGetCurrentActualPositionAndVelocity()->pos, 0, NAV_POS_UPDATE_XY); setDesiredPosition(&navGetCurrentActualPositionAndVelocity()->pos, 0, NAV_POS_UPDATE_XY);
@ -2211,27 +2209,18 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_FW_LANDING_CLIMB_TO_LOI
return NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_FW_LANDING_ABORT; return NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_FW_LANDING_ABORT;
} }
if (posControl.fwLandLoiterStartTime == 0) { if (posControl.fwLandState.loiterStartTime == 0) {
posControl.fwLandLoiterStartTime = micros(); posControl.fwLandState.loiterStartTime = micros();
} }
if (ABS(getEstimatedActualPosition(Z) - posControl.fwLandAproachAltAgl) < (navConfig()->general.waypoint_enforce_altitude > 0 ? navConfig()->general.waypoint_enforce_altitude : FW_LAND_LOITER_ALT_TOLERANCE)) { 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, ROC_TO_ALT_RESET); updateClimbRateToAltitudeController(0, 0, ROC_TO_ALT_RESET);
return NAV_FSM_EVENT_SUCCESS; return NAV_FSM_EVENT_SUCCESS;
} }
fpVector3_t tmpHomePos = posControl.rthState.homePosition.pos; fpVector3_t tmpHomePos = posControl.rthState.homePosition.pos;
tmpHomePos.z = posControl.fwLandAproachAltAgl; tmpHomePos.z = posControl.fwLandState.landAproachAltAgl;
float timeToReachHomeAltitude = fabsf(tmpHomePos.z - navGetCurrentActualPositionAndVelocity()->pos.z) / navConfig()->general.max_auto_climb_rate;
if (timeToReachHomeAltitude < 1) {
// we almost reached the target home altitude so set the desired altitude to the desired home altitude
setDesiredPosition(&tmpHomePos, 0, NAV_POS_UPDATE_Z); setDesiredPosition(&tmpHomePos, 0, NAV_POS_UPDATE_Z);
} else {
float altitudeChangeDirection = tmpHomePos.z > navGetCurrentActualPositionAndVelocity()->pos.z ? 1 : -1;
updateClimbRateToAltitudeController(altitudeChangeDirection * navConfig()->general.max_auto_climb_rate, ROC_TO_ALT_NORMAL);
}
return NAV_FSM_EVENT_NONE; return NAV_FSM_EVENT_NONE;
} }
@ -2248,7 +2237,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_FW_LANDING_LOITER(navig
return NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_FW_LANDING_ABORT; return NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_FW_LANDING_ABORT;
} }
if (micros() - posControl.fwLandLoiterStartTime > FW_LAND_LOITER_MIN_TIME) { if (micros() - posControl.fwLandState.loiterStartTime > FW_LAND_LOITER_MIN_TIME) {
if (isEstimatedWindSpeedValid()) { if (isEstimatedWindSpeedValid()) {
uint16_t windAngle = 0; uint16_t windAngle = 0;
@ -2258,77 +2247,75 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_FW_LANDING_LOITER(navig
// Ignore low wind speed, could be the error of the wind estimator // Ignore low wind speed, could be the error of the wind estimator
if (windSpeed < navFwAutolandConfig()->maxTailwind) { if (windSpeed < navFwAutolandConfig()->maxTailwind) {
if (fwAutolandApproachConfig(posControl.fwApproachSettingIdx)->landApproachHeading1 != 0) { if (fwAutolandApproachConfig(posControl.fwLandState.approachSettingIdx)->landApproachHeading1 != 0) {
approachHeading = posControl.fwLandingDirection = ABS(DEGREES_TO_CENTIDEGREES(fwAutolandApproachConfig(posControl.fwApproachSettingIdx)->landApproachHeading1)); approachHeading = posControl.fwLandState.landingDirection = ABS(DEGREES_TO_CENTIDEGREES(fwAutolandApproachConfig(posControl.fwLandState.approachSettingIdx)->landApproachHeading1));
} else if ((fwAutolandApproachConfig(posControl.fwApproachSettingIdx)->landApproachHeading2 != 0) ) { } else if ((fwAutolandApproachConfig(posControl.fwLandState.approachSettingIdx)->landApproachHeading2 != 0) ) {
approachHeading = posControl.fwLandingDirection = ABS(DEGREES_TO_CENTIDEGREES(fwAutolandApproachConfig(posControl.fwApproachSettingIdx)->landApproachHeading2)); approachHeading = posControl.fwLandState.landingDirection = ABS(DEGREES_TO_CENTIDEGREES(fwAutolandApproachConfig(posControl.fwLandState.approachSettingIdx)->landApproachHeading2));
} else {
approachHeading = posControl.fwLandingDirection = -1;
} }
} else { } else {
int32_t heading1 = calcFinalApproachHeading(DEGREES_TO_CENTIDEGREES(fwAutolandApproachConfig(posControl.fwApproachSettingIdx)->landApproachHeading1), windAngle); int32_t heading1 = calcFinalApproachHeading(DEGREES_TO_CENTIDEGREES(fwAutolandApproachConfig(posControl.fwLandState.approachSettingIdx)->landApproachHeading1), windAngle);
int32_t heading2 = calcFinalApproachHeading(DEGREES_TO_CENTIDEGREES(fwAutolandApproachConfig(posControl.fwApproachSettingIdx)->landApproachHeading2), windAngle); int32_t heading2 = calcFinalApproachHeading(DEGREES_TO_CENTIDEGREES(fwAutolandApproachConfig(posControl.fwLandState.approachSettingIdx)->landApproachHeading2), windAngle);
if (heading1 == heading2 || heading1 == wrap_36000(heading2 + 18000)) { if (heading1 == heading2 || heading1 == wrap_36000(heading2 + 18000)) {
heading2 = -1; heading2 = -1;
} }
if (heading1 == -1 && heading2 >= 0) { if (heading1 == -1 && heading2 >= 0) {
posControl.fwLandingDirection = heading2; posControl.fwLandState.landingDirection = heading2;
approachHeading = DEGREES_TO_CENTIDEGREES(fwAutolandApproachConfig(posControl.fwApproachSettingIdx)->landApproachHeading2); approachHeading = DEGREES_TO_CENTIDEGREES(fwAutolandApproachConfig(posControl.fwLandState.approachSettingIdx)->landApproachHeading2);
} else if (heading1 >= 0 && heading2 == -1) { } else if (heading1 >= 0 && heading2 == -1) {
posControl.fwLandingDirection = heading1; posControl.fwLandState.landingDirection = heading1;
approachHeading = DEGREES_TO_CENTIDEGREES(fwAutolandApproachConfig(posControl.fwApproachSettingIdx)->landApproachHeading1); approachHeading = DEGREES_TO_CENTIDEGREES(fwAutolandApproachConfig(posControl.fwLandState.approachSettingIdx)->landApproachHeading1);
} else { } else {
if (calcWindDiff(heading1, windAngle) < calcWindDiff(heading2, windAngle)) { if (calcWindDiff(heading1, windAngle) < calcWindDiff(heading2, windAngle)) {
posControl.fwLandingDirection = heading1; posControl.fwLandState.landingDirection = heading1;
approachHeading = DEGREES_TO_CENTIDEGREES(fwAutolandApproachConfig(posControl.fwApproachSettingIdx)->landApproachHeading1); approachHeading = DEGREES_TO_CENTIDEGREES(fwAutolandApproachConfig(posControl.fwLandState.approachSettingIdx)->landApproachHeading1);
} else { } else {
posControl.fwLandingDirection = heading2; posControl.fwLandState.landingDirection = heading2;
approachHeading = DEGREES_TO_CENTIDEGREES(fwAutolandApproachConfig(posControl.fwApproachSettingIdx)->landApproachHeading2); approachHeading = DEGREES_TO_CENTIDEGREES(fwAutolandApproachConfig(posControl.fwLandState.approachSettingIdx)->landApproachHeading2);
} }
} }
} }
if (posControl.fwLandingDirection >= 0) { if (posControl.fwLandState.landingDirection >= 0) {
fpVector3_t tmpPos; fpVector3_t tmpPos;
int32_t finalApproachAlt = posControl.fwLandAproachAltAgl / 3 * 2; int32_t finalApproachAlt = posControl.fwLandState.landAproachAltAgl / 3 * 2;
int32_t dir = 0; int32_t dir = 0;
if (fwAutolandApproachConfig(posControl.fwApproachSettingIdx)->approachDirection == FW_AUTOLAND_APPROACH_DIRECTION_LEFT) { if (fwAutolandApproachConfig(posControl.fwLandState.approachSettingIdx)->approachDirection == FW_AUTOLAND_APPROACH_DIRECTION_LEFT) {
dir = wrap_36000(ABS(approachHeading) - 9000); dir = wrap_36000(ABS(approachHeading) - 9000);
} else { } else {
dir = wrap_36000(ABS(approachHeading) + 9000); dir = wrap_36000(ABS(approachHeading) + 9000);
} }
calculateFarAwayPos(&tmpPos, &posControl.fwLandPos, posControl.fwLandingDirection, navFwAutolandConfig()->approachLength); calculateFarAwayPos(&tmpPos, &posControl.fwLandState.landPos, posControl.fwLandState.landingDirection, navFwAutolandConfig()->approachLength);
tmpPos.z = posControl.fwLandAltAgl - finalApproachAlt; tmpPos.z = posControl.fwLandState.landAltAgl - finalApproachAlt;
posControl.fwLandWaypoint[FW_AUTOLAND_WP_LAND] = tmpPos; posControl.fwLandState.landWaypoints[FW_AUTOLAND_WP_LAND] = tmpPos;
calculateFarAwayPos(&tmpPos, &posControl.fwLandPos, wrap_36000(posControl.fwLandingDirection + 18000), navFwAutolandConfig()->approachLength); calculateFarAwayPos(&tmpPos, &posControl.fwLandState.landPos, wrap_36000(posControl.fwLandState.landingDirection + 18000), navFwAutolandConfig()->approachLength);
tmpPos.z = finalApproachAlt; tmpPos.z = finalApproachAlt;
posControl.fwLandWaypoint[FW_AUTOLAND_WP_FINAL_APPROACH] = tmpPos; posControl.fwLandState.landWaypoints[FW_AUTOLAND_WP_FINAL_APPROACH] = tmpPos;
calculateFarAwayPos(&tmpPos, &posControl.fwLandWaypoint[FW_AUTOLAND_WP_FINAL_APPROACH], dir, navFwAutolandConfig()->approachLength / 2); calculateFarAwayPos(&tmpPos, &posControl.fwLandState.landWaypoints[FW_AUTOLAND_WP_FINAL_APPROACH], dir, navFwAutolandConfig()->approachLength / 2);
tmpPos.z = posControl.fwLandAproachAltAgl; tmpPos.z = posControl.fwLandState.landAproachAltAgl;
posControl.fwLandWaypoint[FW_AUTOLAND_WP_TURN] = tmpPos; posControl.fwLandState.landWaypoints[FW_AUTOLAND_WP_TURN] = tmpPos;
setLandWaypoint(&posControl.fwLandWaypoint[FW_AUTOLAND_WP_TURN], &posControl.fwLandWaypoint[FW_AUTOLAND_WP_FINAL_APPROACH]); setLandWaypoint(&posControl.fwLandState.landWaypoints[FW_AUTOLAND_WP_TURN], &posControl.fwLandState.landWaypoints[FW_AUTOLAND_WP_FINAL_APPROACH]);
posControl.fwLandCurrentWp = FW_AUTOLAND_WP_TURN; posControl.fwLandState.landCurrentWp = FW_AUTOLAND_WP_TURN;
posControl.fwLandState = FW_AUTOLAND_STATE_CROSSWIND; posControl.fwLandState.landState = FW_AUTOLAND_STATE_DOWNWIND;
return NAV_FSM_EVENT_SUCCESS; return NAV_FSM_EVENT_SUCCESS;
} else { } else {
posControl.fwLandLoiterStartTime = micros(); posControl.fwLandState.loiterStartTime = micros();
} }
} else { } else {
posControl.fwLandLoiterStartTime = micros(); posControl.fwLandState.loiterStartTime = micros();
} }
} }
fpVector3_t tmpPoint = posControl.fwLandPos; fpVector3_t tmpPoint = posControl.fwLandState.landPos;
tmpPoint.z = posControl.fwLandAproachAltAgl; tmpPoint.z = posControl.fwLandState.landAproachAltAgl;
setDesiredPosition(&tmpPoint, posControl.fwLandPosHeading, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING); setDesiredPosition(&tmpPoint, posControl.fwLandState.landPosHeading, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING);
return NAV_FSM_EVENT_NONE; return NAV_FSM_EVENT_NONE;
} }
@ -2337,7 +2324,6 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_FW_LANDING_APPROACH(nav
{ {
UNUSED(previousState); UNUSED(previousState);
/* If position sensors unavailable - land immediately (wait for timeout on GPS) */
if ((posControl.flags.estHeadingStatus == EST_NONE) || checkForPositionSensorTimeout()) { if ((posControl.flags.estHeadingStatus == EST_NONE) || checkForPositionSensorTimeout()) {
return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING; return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING;
} }
@ -2347,7 +2333,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_FW_LANDING_APPROACH(nav
} }
if (isLandingDetected()) { if (isLandingDetected()) {
posControl.fwLandState = FW_AUTOLAND_STATE_IDLE; posControl.fwLandState.landState = FW_AUTOLAND_STATE_IDLE;
disarm(DISARM_LANDING); disarm(DISARM_LANDING);
return NAV_FSM_EVENT_SWITCH_TO_IDLE; return NAV_FSM_EVENT_SWITCH_TO_IDLE;
} }
@ -2365,20 +2351,20 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_FW_LANDING_APPROACH(nav
altitude = posControl.actualState.abs.pos.z; altitude = posControl.actualState.abs.pos.z;
} }
if (altitude <= fwAutolandApproachConfig(posControl.fwApproachSettingIdx)->landAlt + navFwAutolandConfig()->glideAltitude - (fwAutolandApproachConfig(posControl.fwApproachSettingIdx)->isSeaLevelRef ? GPS_home.alt : 0)) { if (altitude <= fwAutolandApproachConfig(posControl.fwLandState.approachSettingIdx)->landAlt + navFwAutolandConfig()->glideAltitude - (fwAutolandApproachConfig(posControl.fwLandState.approachSettingIdx)->isSeaLevelRef ? GPS_home.alt : 0)) {
resetPositionController(); resetPositionController();
posControl.fwLandState = FW_AUTOLAND_STATE_GLIDE; posControl.fwLandState.landState = FW_AUTOLAND_STATE_GLIDE;
return NAV_FSM_EVENT_SUCCESS; return NAV_FSM_EVENT_SUCCESS;
} else if (isWaypointReached(&posControl.fwLandWaypoint[posControl.fwLandCurrentWp], &posControl.activeWaypoint.bearing)) { } else if (isWaypointReached(&posControl.fwLandState.landWaypoints[posControl.fwLandState.landCurrentWp], &posControl.activeWaypoint.bearing)) {
if (posControl.fwLandCurrentWp == FW_AUTOLAND_WP_TURN) { if (posControl.fwLandState.landCurrentWp == FW_AUTOLAND_WP_TURN) {
setLandWaypoint(&posControl.fwLandWaypoint[FW_AUTOLAND_WP_FINAL_APPROACH], &posControl.fwLandWaypoint[FW_AUTOLAND_WP_LAND]); setLandWaypoint(&posControl.fwLandState.landWaypoints[FW_AUTOLAND_WP_FINAL_APPROACH], &posControl.fwLandState.landWaypoints[FW_AUTOLAND_WP_LAND]);
posControl.fwLandCurrentWp = FW_AUTOLAND_WP_FINAL_APPROACH; posControl.fwLandState.landCurrentWp = FW_AUTOLAND_WP_FINAL_APPROACH;
posControl.fwLandState = FW_AUTOLAND_STATE_BASE_LEG; posControl.fwLandState.landState = FW_AUTOLAND_STATE_BASE_LEG;
return NAV_FSM_EVENT_NONE; return NAV_FSM_EVENT_NONE;
} else if (posControl.fwLandCurrentWp == FW_AUTOLAND_WP_FINAL_APPROACH) { } else if (posControl.fwLandState.landCurrentWp == FW_AUTOLAND_WP_FINAL_APPROACH) {
setLandWaypoint(&posControl.fwLandWaypoint[FW_AUTOLAND_WP_LAND], NULL); setLandWaypoint(&posControl.fwLandState.landWaypoints[FW_AUTOLAND_WP_LAND], NULL);
posControl.fwLandCurrentWp = FW_AUTOLAND_WP_LAND; posControl.fwLandState.landCurrentWp = FW_AUTOLAND_WP_LAND;
posControl.fwLandState = FW_AUTOLAND_STATE_FINAL_APPROACH; posControl.fwLandState.landState = FW_AUTOLAND_STATE_FINAL_APPROACH;
return NAV_FSM_EVENT_NONE; return NAV_FSM_EVENT_NONE;
} }
} }
@ -2410,30 +2396,20 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_FW_LANDING_GLIDE(naviga
#endif #endif
// Surface sensor present? // Surface sensor present?
if (altitude >= 0) { if (altitude >= 0) {
if (altitude <= posControl.fwLandAltAgl + navFwAutolandConfig()->flareAltitude) { if (altitude <= posControl.fwLandState.landAltAgl + navFwAutolandConfig()->flareAltitude) {
posControl.cruise.course = posControl.fwLandingDirection; posControl.cruise.course = posControl.fwLandState.landingDirection;
posControl.cruise.previousCourse = posControl.cruise.course; posControl.cruise.previousCourse = posControl.cruise.course;
posControl.cruise.lastCourseAdjustmentTime = 0; posControl.cruise.lastCourseAdjustmentTime = 0;
posControl.fwLandState = FW_AUTOLAND_STATE_FLARE; posControl.fwLandState.landState = FW_AUTOLAND_STATE_FLARE;
return NAV_FSM_EVENT_SUCCESS; return NAV_FSM_EVENT_SUCCESS;
} }
} else if (isLandingDetected()) { } else if (isLandingDetected()) {
posControl.fwLandState = FW_AUTOLAND_STATE_IDLE; posControl.fwLandState.landState = FW_AUTOLAND_STATE_IDLE;
disarm(DISARM_LANDING);
return NAV_FSM_EVENT_SWITCH_TO_IDLE; return NAV_FSM_EVENT_SWITCH_TO_IDLE;
} }
if (!posControl.fwLandWpReached) {
if (calculateDistanceToDestination(&posControl.fwLandWaypoint[FW_AUTOLAND_WP_LAND]) > navConfig()->general.waypoint_radius / 2) {
posControl.cruise.course = calculateBearingToDestination(&posControl.fwLandWaypoint[FW_AUTOLAND_WP_LAND]);
posControl.cruise.previousCourse = posControl.cruise.course;
posControl.cruise.lastCourseAdjustmentTime = 0;
} else {
posControl.fwLandWpReached = true;
}
}
setDesiredPosition(NULL, posControl.cruise.course, NAV_POS_UPDATE_HEADING); setDesiredPosition(NULL, posControl.cruise.course, NAV_POS_UPDATE_HEADING);
ENABLE_STATE(NAV_MOTOR_STOP_OR_IDLE);
return NAV_FSM_EVENT_NONE; return NAV_FSM_EVENT_NONE;
} }
@ -2442,7 +2418,8 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_FW_LANDING_FLARE(naviga
UNUSED(previousState); UNUSED(previousState);
if (isLandingDetected()) { if (isLandingDetected()) {
posControl.fwLandState = FW_AUTOLAND_STATE_IDLE; posControl.fwLandState.landState = FW_AUTOLAND_STATE_IDLE;
disarm(DISARM_LANDING);
return NAV_FSM_EVENT_SUCCESS; return NAV_FSM_EVENT_SUCCESS;
} }
setDesiredPosition(NULL, posControl.cruise.course, NAV_POS_UPDATE_HEADING); setDesiredPosition(NULL, posControl.cruise.course, NAV_POS_UPDATE_HEADING);
@ -2453,10 +2430,10 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_FW_LANDING_FLARE(naviga
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); UNUSED(previousState);
posControl.fwLandAborted = true; posControl.fwLandState.landAborted = true;
posControl.fwLandState = FW_AUTOLAND_STATE_IDLE; posControl.fwLandState.landState = FW_AUTOLAND_STATE_IDLE;
return posControl.fwLandWp ? NAV_FSM_EVENT_SWITCH_TO_WAYPOINT : NAV_FSM_EVENT_SWITCH_TO_RTH; return posControl.fwLandState.landWp ? NAV_FSM_EVENT_SWITCH_TO_WAYPOINT : NAV_FSM_EVENT_SWITCH_TO_RTH;
} }
static navigationFSMState_t navSetNewFSMState(navigationFSMState_t newState) static navigationFSMState_t navSetNewFSMState(navigationFSMState_t newState)
@ -4180,8 +4157,8 @@ void applyWaypointNavigationAndAltitudeHold(void)
posControl.activeRthTBPointIndex = -1; posControl.activeRthTBPointIndex = -1;
posControl.flags.rthTrackbackActive = false; posControl.flags.rthTrackbackActive = false;
posControl.rthTBWrapAroundCounter = -1; posControl.rthTBWrapAroundCounter = -1;
posControl.fwLandAborted = false; posControl.fwLandState.landAborted = false;
posControl.fwApproachSettingIdx = 0; posControl.fwLandState.approachSettingIdx = 0;
return; return;
} }
@ -4190,7 +4167,7 @@ void applyWaypointNavigationAndAltitudeHold(void)
posControl.flags.verticalPositionDataConsumed = false; posControl.flags.verticalPositionDataConsumed = false;
if (!isFwLandInProgess()) { if (!isFwLandInProgess()) {
posControl.fwLandState = FW_AUTOLAND_STATE_IDLE; posControl.fwLandState.landState = FW_AUTOLAND_STATE_IDLE;
} }
/* Process controllers */ /* Process controllers */
@ -5051,14 +5028,13 @@ int32_t navigationGetHeadingError(void)
static void resetFwAutoland(void) static void resetFwAutoland(void)
{ {
posControl.fwLandAltAgl = 0; posControl.fwLandState.landAltAgl = 0;
posControl.fwLandAproachAltAgl = 0; posControl.fwLandState.landAproachAltAgl = 0;
posControl.fwLandLoiterStartTime = 0; posControl.fwLandState.loiterStartTime = 0;
posControl.fwLandWpReached = false; posControl.fwLandState.approachSettingIdx = 0;
posControl.fwApproachSettingIdx = 0; posControl.fwLandState.landPosHeading = -1;
posControl.fwLandPosHeading = -1; posControl.fwLandState.landState = FW_AUTOLAND_STATE_IDLE;
posControl.fwLandState = FW_AUTOLAND_STATE_IDLE; posControl.fwLandState.landWp = false;
posControl.fwLandWp = false;
} }
static int32_t calcFinalApproachHeading(int32_t approachHeading, int32_t windAngle) static int32_t calcFinalApproachHeading(int32_t approachHeading, int32_t windAngle)

9
src/main/navigation/navigation.h
View file

@ -60,7 +60,7 @@ typedef enum {
typedef enum { typedef enum {
FW_AUTOLAND_STATE_IDLE, FW_AUTOLAND_STATE_IDLE,
FW_AUTOLAND_STATE_CROSSWIND, FW_AUTOLAND_STATE_DOWNWIND,
FW_AUTOLAND_STATE_BASE_LEG, FW_AUTOLAND_STATE_BASE_LEG,
FW_AUTOLAND_STATE_FINAL_APPROACH, FW_AUTOLAND_STATE_FINAL_APPROACH,
FW_AUTOLAND_STATE_GLIDE, FW_AUTOLAND_STATE_GLIDE,
@ -94,19 +94,16 @@ typedef struct navFwAutolandConfig_s
uint16_t flareAltitude; uint16_t flareAltitude;
uint8_t flarePitch; uint8_t flarePitch;
uint16_t maxTailwind; uint16_t maxTailwind;
uint8_t glidePitch; int8_t glidePitch;
} navFwAutolandConfig_t; } navFwAutolandConfig_t;
PG_DECLARE(navFwAutolandConfig_t, navFwAutolandConfig); PG_DECLARE(navFwAutolandConfig_t, navFwAutolandConfig);
extern int8_t safehome_index; // -1 if no safehome, 0 to MAX_SAFEHOMES -1 otherwise
extern uint32_t safehome_distance; // distance to the nearest safehome
extern bool safehome_applied; // whether the safehome has been applied to home.
void resetFwAutolandApproach(int8_t idx); void resetFwAutolandApproach(int8_t idx);
void resetSafeHomes(void); // remove all safehomes void resetSafeHomes(void); // remove all safehomes
bool findNearestSafeHome(void); // Find nearest safehome bool findNearestSafeHome(void); // Find nearest safehome
#endif // defined(USE_SAFE_HOME) #endif // defined(USE_SAFE_HOME)
#ifndef NAV_MAX_WAYPOINTS #ifndef NAV_MAX_WAYPOINTS

11
src/main/navigation/navigation_fixedwing.c
View file

@ -589,7 +589,7 @@ int16_t fixedWingPitchToThrottleCorrection(int16_t pitch, timeUs_t currentTimeUs
int16_t filteredPitch = (int16_t)pt1FilterApply4(&pitchToThrFilterState, pitch, getPitchToThrottleSmoothnessCutoffFreq(NAV_FW_BASE_PITCH_CUTOFF_FREQUENCY_HZ), US2S(deltaMicrosPitchToThrCorr)); int16_t filteredPitch = (int16_t)pt1FilterApply4(&pitchToThrFilterState, pitch, getPitchToThrottleSmoothnessCutoffFreq(NAV_FW_BASE_PITCH_CUTOFF_FREQUENCY_HZ), US2S(deltaMicrosPitchToThrCorr));
int16_t pitchToThrottle = currentBatteryProfile->nav.fw.pitch_to_throttle; int16_t pitchToThrottle = currentBatteryProfile->nav.fw.pitch_to_throttle;
if (pitch < 0 && posControl.fwLandState == FW_AUTOLAND_STATE_FINAL_APPROACH) { if (pitch < 0 && posControl.fwLandState.landState == FW_AUTOLAND_STATE_FINAL_APPROACH) {
pitchToThrottle *= navFwAutolandConfig()->finalApproachPitchToThrottleMod / 100.0f; pitchToThrottle *= navFwAutolandConfig()->finalApproachPitchToThrottleMod / 100.0f;
} }
@ -635,7 +635,7 @@ void applyFixedWingPitchRollThrottleController(navigationFSMStateFlags_t navStat
uint16_t correctedThrottleValue = constrain(currentBatteryProfile->nav.fw.cruise_throttle + throttleCorrection, currentBatteryProfile->nav.fw.min_throttle, currentBatteryProfile->nav.fw.max_throttle); uint16_t correctedThrottleValue = constrain(currentBatteryProfile->nav.fw.cruise_throttle + throttleCorrection, currentBatteryProfile->nav.fw.min_throttle, currentBatteryProfile->nav.fw.max_throttle);
// Manual throttle increase // Manual throttle increase
if (navConfig()->fw.allow_manual_thr_increase && !FLIGHT_MODE(FAILSAFE_MODE)) { if (navConfig()->fw.allow_manual_thr_increase && !FLIGHT_MODE(FAILSAFE_MODE) && !isFwLandInProgess()) {
if (rcCommand[THROTTLE] < PWM_RANGE_MIN + (PWM_RANGE_MAX - PWM_RANGE_MIN) * 0.95){ if (rcCommand[THROTTLE] < PWM_RANGE_MIN + (PWM_RANGE_MAX - PWM_RANGE_MIN) * 0.95){
correctedThrottleValue += MAX(0, rcCommand[THROTTLE] - currentBatteryProfile->nav.fw.cruise_throttle); correctedThrottleValue += MAX(0, rcCommand[THROTTLE] - currentBatteryProfile->nav.fw.cruise_throttle);
} else { } else {
@ -649,22 +649,19 @@ void applyFixedWingPitchRollThrottleController(navigationFSMStateFlags_t navStat
rcCommand[THROTTLE] = setDesiredThrottle(correctedThrottleValue, false); rcCommand[THROTTLE] = setDesiredThrottle(correctedThrottleValue, false);
} }
#ifdef NAV_FIXED_WING_LANDING
if (posControl.navState == NAV_STATE_FW_LANDING_GLIDE || posControl.navState == NAV_STATE_FW_LANDING_FLARE || STATE(LANDING_DETECTED)) { if (posControl.navState == NAV_STATE_FW_LANDING_GLIDE || posControl.navState == NAV_STATE_FW_LANDING_FLARE || STATE(LANDING_DETECTED)) {
// Set motor to min. throttle and stop it when MOTOR_STOP feature is enabled // Set motor to min. throttle and stop it when MOTOR_STOP feature is enabled
rcCommand[THROTTLE] = getThrottleIdleValue(); rcCommand[THROTTLE] = getThrottleIdleValue();
ENABLE_STATE(NAV_MOTOR_STOP_OR_IDLE); ENABLE_STATE(NAV_MOTOR_STOP_OR_IDLE);
if (posControl.navState == NAV_STATE_FW_LANDING_GLIDE) { if (posControl.navState == NAV_STATE_FW_LANDING_GLIDE) {
rcCommand[PITCH] = pidAngleToRcCommand(DEGREES_TO_DECIDEGREES(navFwAutolandConfig()->glidePitch), pidProfile()->max_angle_inclination[FD_PITCH]); rcCommand[PITCH] = pidAngleToRcCommand(-DEGREES_TO_DECIDEGREES(navFwAutolandConfig()->glidePitch), pidProfile()->max_angle_inclination[FD_PITCH]);
} }
if (posControl.navState == NAV_STATE_FW_LANDING_FLARE) { if (posControl.navState == NAV_STATE_FW_LANDING_FLARE) {
rcCommand[PITCH] = pidAngleToRcCommand(DEGREES_TO_DECIDEGREES(navFwAutolandConfig()->flarePitch), pidProfile()->max_angle_inclination[FD_PITCH]); rcCommand[PITCH] = pidAngleToRcCommand(-DEGREES_TO_DECIDEGREES(navFwAutolandConfig()->flarePitch), pidProfile()->max_angle_inclination[FD_PITCH]);
} }
} }
#endif
} }
bool isFixedWingAutoThrottleManuallyIncreased(void) bool isFixedWingAutoThrottleManuallyIncreased(void)

42
src/main/navigation/navigation_private.h
View file

@ -368,6 +368,28 @@ typedef struct {
bool rthLinearDescentActive; // Activation status of Linear Descent bool rthLinearDescentActive; // Activation status of Linear Descent
} rthState_t; } rthState_t;
typedef enum {
FW_AUTOLAND_WP_TURN,
FW_AUTOLAND_WP_FINAL_APPROACH,
FW_AUTOLAND_WP_LAND,
FW_AUTOLAND_WP_COUNT,
} fwAutolandWaypoint_t;
typedef struct {
timeUs_t loiterStartTime;
fpVector3_t landWaypoints[FW_AUTOLAND_WP_COUNT];
fpVector3_t landPos;
int32_t landPosHeading;
int32_t landingDirection;
int32_t landAproachAltAgl;
int32_t landAltAgl;
uint8_t approachSettingIdx;
fwAutolandWaypoint_t landCurrentWp;
bool landAborted;
bool landWp;
fwAutolandState_t landState;
} fwLandState_t;
typedef enum { typedef enum {
RTH_HOME_ENROUTE_INITIAL, // Initial position for RTH approach RTH_HOME_ENROUTE_INITIAL, // Initial position for RTH approach
RTH_HOME_ENROUTE_PROPORTIONAL, // Prorpotional position for RTH approach RTH_HOME_ENROUTE_PROPORTIONAL, // Prorpotional position for RTH approach
@ -383,12 +405,6 @@ typedef struct {
bool isApplied; // whether the safehome has been applied to home bool isApplied; // whether the safehome has been applied to home
} safehomeState_t; } safehomeState_t;
typedef enum {
FW_AUTOLAND_WP_TURN,
FW_AUTOLAND_WP_FINAL_APPROACH,
FW_AUTOLAND_WP_LAND,
FW_AUTOLAND_WP_COUNT,
} fwAutolandWayppoints_t;
typedef struct { typedef struct {
/* Flags and navigation system state */ /* Flags and navigation system state */
navigationFSMState_t navState; navigationFSMState_t navState;
@ -456,19 +472,7 @@ typedef struct {
int8_t rthTBWrapAroundCounter; // stores trackpoint array overwrite index position int8_t rthTBWrapAroundCounter; // stores trackpoint array overwrite index position
/* Fixedwing autoland */ /* Fixedwing autoland */
timeUs_t fwLandLoiterStartTime; fwLandState_t fwLandState;
fpVector3_t fwLandWaypoint[FW_AUTOLAND_WP_COUNT];
fpVector3_t fwLandPos;
int32_t fwLandPosHeading;
int32_t fwLandingDirection;
int32_t fwLandAproachAltAgl;
int32_t fwLandAltAgl;
uint8_t fwApproachSettingIdx;
bool fwLandWpReached;
fwAutolandWayppoints_t fwLandCurrentWp;
bool fwLandAborted;
bool fwLandWp;
fwAutolandState_t fwLandState;
/* Internals & statistics */ /* Internals & statistics */
int16_t rcAdjustment[4]; int16_t rcAdjustment[4];

2
src/main/programming/logic_condition.c
View file

@ -811,7 +811,7 @@ static int logicConditionGetFlightOperandValue(int operand) {
break; break;
case LOGIC_CONDITION_OPERAND_FLIGHT_FW_LAND_STATE: case LOGIC_CONDITION_OPERAND_FLIGHT_FW_LAND_STATE:
return posControl.fwLandState; return posControl.fwLandState.landState;
break; break;
default: default:

1
src/main/target/common.h
View file

@ -140,7 +140,6 @@
#define USE_POWER_LIMITS #define USE_POWER_LIMITS
#define NAV_FIXED_WING_LANDING
#define USE_SAFE_HOME #define USE_SAFE_HOME
#define USE_FW_AUTOLAND #define USE_FW_AUTOLAND
#define USE_AUTOTUNE_FIXED_WING #define USE_AUTOTUNE_FIXED_WING