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.detectIndentation": false,
"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",
"cStandard": "c11",
"cppStandard": "c++17",
"defines": [
"NAV_FIXED_WING_LANDING",
"defines": [,
"USE_OSD",
"USE_GYRO_NOTCH_1",
"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",
"cppStandard": "c++17",
"defines": [
"NAV_FIXED_WING_LANDING",
"USE_OSD",
"USE_GYRO_NOTCH_1",
"USE_GYRO_NOTCH_2",

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

@ -974,7 +974,7 @@ void taskMainPidLoop(timeUs_t currentTimeUs)
}
#endif
// Check if landed, FW and MR
if (STATE(ALTITUDE_CONTROL)) {
if (STATE(ALTITUDE_CONTROL) || isFwLandInProgess()) {
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;
}
break;
#endif
#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."
default_value: 0
field: glidePitch
min: 0
min: -15
max: 45
- name: nav_fw_land_flare_pitch
description: "Pitch value for flare phase. In degrees"
default_value: 8
field: flarePitch
min: 0
min: -15
max: 45
- name: nav_fw_land_max_tailwind
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)
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 NULL;
@ -5135,7 +5135,7 @@ textAttributes_t osdGetSystemMessage(char *buff, size_t buff_size, bool isCenter
if (ARMING_FLAG(ARMED)) {
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
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] = {
.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) {
posControl.fwLandAborted = false;
posControl.fwLandState.landAborted = false;
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
return NAV_FSM_EVENT_SWITCH_TO_IDLE;
@ -1706,23 +1705,23 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_LANDING(navigationF
#endif
#ifdef USE_SAFE_HOME
shIdx = safehome_index;
shIdx = posControl.safehomeState.index;
missionFwLandConfigStartIdx = MAX_SAFE_HOMES;
#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) {
posControl.fwLandPos = posControl.activeWaypoint.pos;
posControl.fwApproachSettingIdx = missionFwLandConfigStartIdx + missionIdx;
posControl.fwLandWp = true;
posControl.fwLandState.landPos = posControl.activeWaypoint.pos;
posControl.fwLandState.approachSettingIdx = missionFwLandConfigStartIdx + missionIdx;
posControl.fwLandState.landWp = true;
} else {
posControl.fwLandPos = posControl.rthState.homePosition.pos;
posControl.fwApproachSettingIdx = shIdx;
posControl.fwLandWp = false;
posControl.fwLandState.landPos = posControl.rthState.homePosition.pos;
posControl.fwLandState.approachSettingIdx = shIdx;
posControl.fwLandState.landWp = false;
}
posControl.fwLandAltAgl = fwAutolandApproachConfig(posControl.fwApproachSettingIdx)->isSeaLevelRef ? fwAutolandApproachConfig(posControl.fwApproachSettingIdx)->landAlt - GPS_home.alt : fwAutolandApproachConfig(posControl.fwApproachSettingIdx)->landAlt;
posControl.fwLandAproachAltAgl = fwAutolandApproachConfig(posControl.fwApproachSettingIdx)->isSeaLevelRef ? fwAutolandApproachConfig(posControl.fwApproachSettingIdx)->approachAlt - GPS_home.alt : fwAutolandApproachConfig(posControl.fwApproachSettingIdx)->approachAlt;
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;
} else {
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
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
// Do not allow descent velocity slower than -30cm/s so the landing detector works.
descentVelLimited = navConfig()->general.land_minalt_vspd;
@ -2055,7 +2053,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_EMERGENCY_LANDING_INITI
{
UNUSED(previousState);
posControl.fwLandState = FW_AUTOLAND_STATE_IDLE;
posControl.fwLandState.landState = FW_AUTOLAND_STATE_IDLE;
if ((posControl.flags.estPosStatus >= EST_USABLE)) {
resetPositionController();
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;
}
if (posControl.fwLandLoiterStartTime == 0) {
posControl.fwLandLoiterStartTime = micros();
if (posControl.fwLandState.loiterStartTime == 0) {
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)) {
updateClimbRateToAltitudeController(0, ROC_TO_ALT_RESET);
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);
return NAV_FSM_EVENT_SUCCESS;
}
fpVector3_t tmpHomePos = posControl.rthState.homePosition.pos;
tmpHomePos.z = posControl.fwLandAproachAltAgl;
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
tmpHomePos.z = posControl.fwLandState.landAproachAltAgl;
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;
}
@ -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;
}
if (micros() - posControl.fwLandLoiterStartTime > FW_LAND_LOITER_MIN_TIME) {
if (micros() - posControl.fwLandState.loiterStartTime > FW_LAND_LOITER_MIN_TIME) {
if (isEstimatedWindSpeedValid()) {
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
if (windSpeed < navFwAutolandConfig()->maxTailwind) {
if (fwAutolandApproachConfig(posControl.fwApproachSettingIdx)->landApproachHeading1 != 0) {
approachHeading = posControl.fwLandingDirection = ABS(DEGREES_TO_CENTIDEGREES(fwAutolandApproachConfig(posControl.fwApproachSettingIdx)->landApproachHeading1));
} else if ((fwAutolandApproachConfig(posControl.fwApproachSettingIdx)->landApproachHeading2 != 0) ) {
approachHeading = posControl.fwLandingDirection = ABS(DEGREES_TO_CENTIDEGREES(fwAutolandApproachConfig(posControl.fwApproachSettingIdx)->landApproachHeading2));
} else {
approachHeading = posControl.fwLandingDirection = -1;
if (fwAutolandApproachConfig(posControl.fwLandState.approachSettingIdx)->landApproachHeading1 != 0) {
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.fwApproachSettingIdx)->landApproachHeading1), windAngle);
int32_t heading2 = calcFinalApproachHeading(DEGREES_TO_CENTIDEGREES(fwAutolandApproachConfig(posControl.fwApproachSettingIdx)->landApproachHeading2), windAngle);
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);
if (heading1 == heading2 || heading1 == wrap_36000(heading2 + 18000)) {
heading2 = -1;
}
if (heading1 == -1 && heading2 >= 0) {
posControl.fwLandingDirection = heading2;
approachHeading = DEGREES_TO_CENTIDEGREES(fwAutolandApproachConfig(posControl.fwApproachSettingIdx)->landApproachHeading2);
posControl.fwLandState.landingDirection = heading2;
approachHeading = DEGREES_TO_CENTIDEGREES(fwAutolandApproachConfig(posControl.fwLandState.approachSettingIdx)->landApproachHeading2);
} else if (heading1 >= 0 && heading2 == -1) {
posControl.fwLandingDirection = heading1;
approachHeading = DEGREES_TO_CENTIDEGREES(fwAutolandApproachConfig(posControl.fwApproachSettingIdx)->landApproachHeading1);
posControl.fwLandState.landingDirection = heading1;
approachHeading = DEGREES_TO_CENTIDEGREES(fwAutolandApproachConfig(posControl.fwLandState.approachSettingIdx)->landApproachHeading1);
} else {
if (calcWindDiff(heading1, windAngle) < calcWindDiff(heading2, windAngle)) {
posControl.fwLandingDirection = heading1;
approachHeading = DEGREES_TO_CENTIDEGREES(fwAutolandApproachConfig(posControl.fwApproachSettingIdx)->landApproachHeading1);
posControl.fwLandState.landingDirection = heading1;
approachHeading = DEGREES_TO_CENTIDEGREES(fwAutolandApproachConfig(posControl.fwLandState.approachSettingIdx)->landApproachHeading1);
} else {
posControl.fwLandingDirection = heading2;
approachHeading = DEGREES_TO_CENTIDEGREES(fwAutolandApproachConfig(posControl.fwApproachSettingIdx)->landApproachHeading2);
posControl.fwLandState.landingDirection = heading2;
approachHeading = DEGREES_TO_CENTIDEGREES(fwAutolandApproachConfig(posControl.fwLandState.approachSettingIdx)->landApproachHeading2);
}
}
}
if (posControl.fwLandingDirection >= 0) {
if (posControl.fwLandState.landingDirection >= 0) {
fpVector3_t tmpPos;
int32_t finalApproachAlt = posControl.fwLandAproachAltAgl / 3 * 2;
int32_t finalApproachAlt = posControl.fwLandState.landAproachAltAgl / 3 * 2;
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);
} else {
dir = wrap_36000(ABS(approachHeading) + 9000);
}
calculateFarAwayPos(&tmpPos, &posControl.fwLandPos, posControl.fwLandingDirection, navFwAutolandConfig()->approachLength);
tmpPos.z = posControl.fwLandAltAgl - finalApproachAlt;
posControl.fwLandWaypoint[FW_AUTOLAND_WP_LAND] = tmpPos;
calculateFarAwayPos(&tmpPos, &posControl.fwLandState.landPos, posControl.fwLandState.landingDirection, navFwAutolandConfig()->approachLength);
tmpPos.z = posControl.fwLandState.landAltAgl - finalApproachAlt;
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;
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);
tmpPos.z = posControl.fwLandAproachAltAgl;
posControl.fwLandWaypoint[FW_AUTOLAND_WP_TURN] = tmpPos;
calculateFarAwayPos(&tmpPos, &posControl.fwLandState.landWaypoints[FW_AUTOLAND_WP_FINAL_APPROACH], dir, navFwAutolandConfig()->approachLength / 2);
tmpPos.z = posControl.fwLandState.landAproachAltAgl;
posControl.fwLandState.landWaypoints[FW_AUTOLAND_WP_TURN] = tmpPos;
setLandWaypoint(&posControl.fwLandWaypoint[FW_AUTOLAND_WP_TURN], &posControl.fwLandWaypoint[FW_AUTOLAND_WP_FINAL_APPROACH]);
posControl.fwLandCurrentWp = FW_AUTOLAND_WP_TURN;
posControl.fwLandState = FW_AUTOLAND_STATE_CROSSWIND;
setLandWaypoint(&posControl.fwLandState.landWaypoints[FW_AUTOLAND_WP_TURN], &posControl.fwLandState.landWaypoints[FW_AUTOLAND_WP_FINAL_APPROACH]);
posControl.fwLandState.landCurrentWp = FW_AUTOLAND_WP_TURN;
posControl.fwLandState.landState = FW_AUTOLAND_STATE_DOWNWIND;
return NAV_FSM_EVENT_SUCCESS;
} else {
posControl.fwLandLoiterStartTime = micros();
posControl.fwLandState.loiterStartTime = micros();
}
} else {
posControl.fwLandLoiterStartTime = micros();
posControl.fwLandState.loiterStartTime = micros();
}
}
fpVector3_t tmpPoint = posControl.fwLandPos;
tmpPoint.z = posControl.fwLandAproachAltAgl;
setDesiredPosition(&tmpPoint, posControl.fwLandPosHeading, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING);
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;
}
@ -2337,7 +2324,6 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_FW_LANDING_APPROACH(nav
{
UNUSED(previousState);
/* If position sensors unavailable - land immediately (wait for timeout on GPS) */
if ((posControl.flags.estHeadingStatus == EST_NONE) || checkForPositionSensorTimeout()) {
return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING;
}
@ -2347,7 +2333,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_FW_LANDING_APPROACH(nav
}
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;
}
@ -2365,20 +2351,20 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_FW_LANDING_APPROACH(nav
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();
posControl.fwLandState = FW_AUTOLAND_STATE_GLIDE;
posControl.fwLandState.landState = FW_AUTOLAND_STATE_GLIDE;
return NAV_FSM_EVENT_SUCCESS;
} else if (isWaypointReached(&posControl.fwLandWaypoint[posControl.fwLandCurrentWp], &posControl.activeWaypoint.bearing)) {
if (posControl.fwLandCurrentWp == FW_AUTOLAND_WP_TURN) {
setLandWaypoint(&posControl.fwLandWaypoint[FW_AUTOLAND_WP_FINAL_APPROACH], &posControl.fwLandWaypoint[FW_AUTOLAND_WP_LAND]);
posControl.fwLandCurrentWp = FW_AUTOLAND_WP_FINAL_APPROACH;
posControl.fwLandState = FW_AUTOLAND_STATE_BASE_LEG;
} else if (isWaypointReached(&posControl.fwLandState.landWaypoints[posControl.fwLandState.landCurrentWp], &posControl.activeWaypoint.bearing)) {
if (posControl.fwLandState.landCurrentWp == FW_AUTOLAND_WP_TURN) {
setLandWaypoint(&posControl.fwLandState.landWaypoints[FW_AUTOLAND_WP_FINAL_APPROACH], &posControl.fwLandState.landWaypoints[FW_AUTOLAND_WP_LAND]);
posControl.fwLandState.landCurrentWp = FW_AUTOLAND_WP_FINAL_APPROACH;
posControl.fwLandState.landState = FW_AUTOLAND_STATE_BASE_LEG;
return NAV_FSM_EVENT_NONE;
} else if (posControl.fwLandCurrentWp == FW_AUTOLAND_WP_FINAL_APPROACH) {
setLandWaypoint(&posControl.fwLandWaypoint[FW_AUTOLAND_WP_LAND], NULL);
posControl.fwLandCurrentWp = FW_AUTOLAND_WP_LAND;
posControl.fwLandState = FW_AUTOLAND_STATE_FINAL_APPROACH;
} else if (posControl.fwLandState.landCurrentWp == FW_AUTOLAND_WP_FINAL_APPROACH) {
setLandWaypoint(&posControl.fwLandState.landWaypoints[FW_AUTOLAND_WP_LAND], NULL);
posControl.fwLandState.landCurrentWp = FW_AUTOLAND_WP_LAND;
posControl.fwLandState.landState = FW_AUTOLAND_STATE_FINAL_APPROACH;
return NAV_FSM_EVENT_NONE;
}
}
@ -2410,30 +2396,20 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_FW_LANDING_GLIDE(naviga
#endif
// Surface sensor present?
if (altitude >= 0) {
if (altitude <= posControl.fwLandAltAgl + navFwAutolandConfig()->flareAltitude) {
posControl.cruise.course = posControl.fwLandingDirection;
if (altitude <= posControl.fwLandState.landAltAgl + navFwAutolandConfig()->flareAltitude) {
posControl.cruise.course = posControl.fwLandState.landingDirection;
posControl.cruise.previousCourse = posControl.cruise.course;
posControl.cruise.lastCourseAdjustmentTime = 0;
posControl.fwLandState = FW_AUTOLAND_STATE_FLARE;
posControl.fwLandState.landState = FW_AUTOLAND_STATE_FLARE;
return NAV_FSM_EVENT_SUCCESS;
}
} 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;
}
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);
ENABLE_STATE(NAV_MOTOR_STOP_OR_IDLE);
return NAV_FSM_EVENT_NONE;
}
@ -2442,7 +2418,8 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_FW_LANDING_FLARE(naviga
UNUSED(previousState);
if (isLandingDetected()) {
posControl.fwLandState = FW_AUTOLAND_STATE_IDLE;
posControl.fwLandState.landState = FW_AUTOLAND_STATE_IDLE;
disarm(DISARM_LANDING);
return NAV_FSM_EVENT_SUCCESS;
}
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)
{
UNUSED(previousState);
posControl.fwLandAborted = true;
posControl.fwLandState = FW_AUTOLAND_STATE_IDLE;
posControl.fwLandState.landAborted = true;
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)
@ -4180,8 +4157,8 @@ void applyWaypointNavigationAndAltitudeHold(void)
posControl.activeRthTBPointIndex = -1;
posControl.flags.rthTrackbackActive = false;
posControl.rthTBWrapAroundCounter = -1;
posControl.fwLandAborted = false;
posControl.fwApproachSettingIdx = 0;
posControl.fwLandState.landAborted = false;
posControl.fwLandState.approachSettingIdx = 0;
return;
}
@ -4190,7 +4167,7 @@ void applyWaypointNavigationAndAltitudeHold(void)
posControl.flags.verticalPositionDataConsumed = false;
if (!isFwLandInProgess()) {
posControl.fwLandState = FW_AUTOLAND_STATE_IDLE;
posControl.fwLandState.landState = FW_AUTOLAND_STATE_IDLE;
}
/* Process controllers */
@ -5051,14 +5028,13 @@ int32_t navigationGetHeadingError(void)
static void resetFwAutoland(void)
{
posControl.fwLandAltAgl = 0;
posControl.fwLandAproachAltAgl = 0;
posControl.fwLandLoiterStartTime = 0;
posControl.fwLandWpReached = false;
posControl.fwApproachSettingIdx = 0;
posControl.fwLandPosHeading = -1;
posControl.fwLandState = FW_AUTOLAND_STATE_IDLE;
posControl.fwLandWp = false;
posControl.fwLandState.landAltAgl = 0;
posControl.fwLandState.landAproachAltAgl = 0;
posControl.fwLandState.loiterStartTime = 0;
posControl.fwLandState.approachSettingIdx = 0;
posControl.fwLandState.landPosHeading = -1;
posControl.fwLandState.landState = FW_AUTOLAND_STATE_IDLE;
posControl.fwLandState.landWp = false;
}
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 {
FW_AUTOLAND_STATE_IDLE,
FW_AUTOLAND_STATE_CROSSWIND,
FW_AUTOLAND_STATE_DOWNWIND,
FW_AUTOLAND_STATE_BASE_LEG,
FW_AUTOLAND_STATE_FINAL_APPROACH,
FW_AUTOLAND_STATE_GLIDE,
@ -94,19 +94,16 @@ typedef struct navFwAutolandConfig_s
uint16_t flareAltitude;
uint8_t flarePitch;
uint16_t maxTailwind;
uint8_t glidePitch;
int8_t glidePitch;
} navFwAutolandConfig_t;
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 resetSafeHomes(void); // remove all safehomes
bool findNearestSafeHome(void); // Find nearest safehome
#endif // defined(USE_SAFE_HOME)
#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 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;
}
@ -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);
// 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){
correctedThrottleValue += MAX(0, rcCommand[THROTTLE] - currentBatteryProfile->nav.fw.cruise_throttle);
} else {
@ -649,22 +649,19 @@ void applyFixedWingPitchRollThrottleController(navigationFSMStateFlags_t navStat
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)) {
// Set motor to min. throttle and stop it when MOTOR_STOP feature is enabled
rcCommand[THROTTLE] = getThrottleIdleValue();
ENABLE_STATE(NAV_MOTOR_STOP_OR_IDLE);
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) {
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)

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

@ -368,6 +368,28 @@ typedef struct {
bool rthLinearDescentActive; // Activation status of Linear Descent
} 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 {
RTH_HOME_ENROUTE_INITIAL, // Initial 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
} 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 {
/* Flags and navigation system state */
navigationFSMState_t navState;
@ -456,19 +472,7 @@ typedef struct {
int8_t rthTBWrapAroundCounter; // stores trackpoint array overwrite index position
/* Fixedwing autoland */
timeUs_t fwLandLoiterStartTime;
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;
fwLandState_t fwLandState;
/* Internals & statistics */
int16_t rcAdjustment[4];

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

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

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

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