Mirror/inav
1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-24 00:35:34 +03:00
Code Issues Wiki Activity

Improvements

Browse source
This commit is contained in:
Scavanger 2023-11-24 08:34:43 -03:00
parent 02806b17f7
commit 3a3f507a94
10 changed files with 200 additions and 32 deletions
Download patch file Download diff file Expand all files Collapse all files

91
docs/Fixed Wing Landing.md Normal file
View file

@ -0,0 +1,91 @@
# Fixed Wing Landing
## Introducion
INAV supports advanced automatic landings for fixed wing aircraft from version 8.
The procedure is based on landings for man-carrying aircraft, so that safe landings at a specific location are possible.
Supported are landings at the safehome after "Return to Home" or at a defined LAND waypoint for missions.
## General procedure:
1. After reaching Safehome/LAND Waypoint the altitude is corrected to "Approach Altitude".
2. The aircraft circles for at least 30 seconds to determine the wind direction and strength.
3. The landing direction and the approach waypoints are calculated on the basis of the measured wind parameters. If no headwind landing is possible or the wind strength is greater than "Max. tailwind" (see Global Parameters), return to point 2.
4. The landing is initiated. The aircraft flies the downwind course, "Approach Altitude" is held.
5. Base Leg: the altitude is reduced from 2/3 of "Approach Altitude".
6. Final Appraoch: The engine power is reduced using "Pitch2throttle modifier" to reduce speed and the altitude is reduced to "Land Altitude".
7. Glide: When "Glide Altitude" is reached, the motor is switched off and the pitch angle of "Glide Pitch" is held.
7. Flare: Only if a LIDAR/Rangefinder sensor is present: the motor remains switched off and the pitch angle of "Flare Pitch" is held
8. Landing: As soon as INAV has detected the landing, it is automatically disarmed, see setting nav_disarm_on_landing
The following graphics illustrate the process:
## Landing site parameters:
### The following parameters are set for each landing site (Safefome/LAND waypoint):
All settings can also be conveniently made in the Configurator via Missionplanner
CLI command fwapproach:
fwapproach <index> <Apprach direction> <Approach altitude> <Land altitude> <approach heading 1> <approach heading 2> <sea level>
fwapprach has 17 slots in which landing parameters can be stored. In slot 0-7 the landing parameters for Safehome are stored, in 8 - 16 the parameters for waypoint missions. Only one landing point per mission can be saved.
- index: 0 - 17, 0 - 7 Safehome, 8 - 16 Mission
- Approach direction: 0 - Left, 1 - Right. Always seen from the primary landing direction (positive value), i.e. whether the aircraft flies left or right turns on approach.
- Approach Altitude: Initial altitude of the approach, the altitude at which the wind direction is determined and the downwind approach, in cm
- Land Altitude: Altitude of the landing site, in cm
- Approach heading 1 and 2: Two landing directions can be set, values: 0 - +/-360. 0 = landing direction is deactivated.
A positive value means that you can approach in both directions, a negative value means that this direction is exclusive.
Example: 90 degrees: It is possible to land in 90 degrees as well as in 270 degrees. -90 means that you can only land in a 90 degree direction.
This means that practically 4 landing directions can be saved.
- Sea Level: 0 - Deactivated, 1 - Activated. If activated, approach and land altitude refer to normal zero (sea level), otherwise relative altitude to the altitude during first GPS fix.
WARNING: The Configuator automatically determines the ground altitude based on databases on the Internet, which may be inaccurate. Please always compare with the measured GPS altitude at the landing site to avoid crashes.
### Global paramters
All settings are available via “Advanced Tuning” in the Configurator.
- nav_fw_land_approach_length: Length of the final approach, measured from the land site (Safehome/Waypoint) to the last turning point.
In cm. Max: 100000, Min: 100, Default: 35000
- nav_fw_land_final_approach_pitch2throttle_mod: Modifier for pitch to throttle ratio at final approach. This parameter can be used to reduce speed during the final approach.
Example: If the parameter is set to 200% and Pitch To Throttle Ratio is set to 15, Pitch To Throttle Ratio is set to 30 on the final approach. This causes a reduction in engine power on approach when the nose is pointing downwards.
In Percent. Min: 100, Max: 400, Default: 100
- nav_fw_land_glide_alt: Initial altitude of the glide phase. The altitude refers to "Landing Altitude", see above under "Landing site parameters"
In cm. Min: 100, Max: 5000, Default: 200
- nav_fw_land_flare_alt: Initial altitude of the flare phase. The altitude refers to "Landing Altitude", see above under "Landing site parameters"
In cm. Min: 0, Max: 5000, Default: 200
- nav_fw_land_glide_pitch: Pitch value for glide phase.
In degrees. Min: 0, Max: 45, Default: 0
- nav_fw_land_flare_pitch: Pitch value for flare phase.
In degrees. Min: 0, Max: 45, Default: 8
- nav_fw_land_max_tailwind: Max. tailwind if no landing direction with downwind is available. Wind strengths below this value are ignored (error tolerance in the wind measurement). Landing then takes place in the main direction. If, for example, 90 degrees is configured, landing takes place in this direction, NOT in 270 degrees (see above).
In cm/s. Min: 0; Max: 3000, Default: 140
## Waypoint missions
Only one landing waypoint per mission can be active and saved and the landing waypoint must be the last waypoint of the mission.
If the altitude of the waypoint and the "Approach Altitude" are different, the altitude of the waypoint is approached first and then the altitude is corrected to "Approach Altitude".
## Logic Conditions
The current landing state can be retrieved via ID 38 in "Flight" (FW Land State). This allows additional actions to be executed according to the landing phases, e.g. deplyoment of the landing flaps.
### The returned values:
0: Idle/Inactive
1: Downwind
2: Base Leg
3: Final Approach
4: Glide
5: Flare
## Tuning Tipps:
To do

1
docs/Programming Framework.md
View file

@ -156,6 +156,7 @@ IPF can be edited using INAV Configurator user interface, or via CLI. To use COn
| 38 | ACTIVE_MIXER_PROFILE | Which mixers are currently active (for vtol etc) |
| 39 | MIXER_TRANSITION_ACTIVE | Currently switching between mixers (quad to plane etc) |
| 40 | ATTITUDE_YAW | current heading (yaw) in `degrees` |
| 38 | FW Land Sate | integer `1` - `5`, indicates the status of the FW landing, 0 Idle, 1 Downwind, 2 Base Leg, 3 Final Approach, 4 Glide, 5 Flare |
#### FLIGHT_MODE

4
src/main/fc/cli.c
View file

@ -1311,7 +1311,7 @@ static void cliTempSensor(char *cmdline)
static void printFwAutolandApproach(uint8_t dumpMask, const navFwAutolandApproach_t *navFwAutolandApproach, const navFwAutolandApproach_t *defaultFwAutolandApproach)
{
const char *format = "fwapproach %u %d %d %d %u %d %d %u";
const char *format = "fwapproach %u %d %d %u %d %d %u";
for (uint8_t i = 0; i < MAX_FW_LAND_APPOACH_SETTINGS; i++) {
bool equalsDefault = false;
if (defaultFwAutolandApproach) {
@ -1334,7 +1334,7 @@ static void cliFwAutolandApproach(char * cmdline)
if (isEmpty(cmdline)) {
printFwAutolandApproach(DUMP_MASTER, fwAutolandApproachConfig(0), NULL);
} else if (sl_strcasecmp(cmdline, "reset") == 0) {
resetFwAutolandApproach();
resetFwAutolandApproach(-1);
} else {
int32_t approachAlt = 0, heading1 = 0, heading2 = 0, landDirection = 0, landAlt = 0;
bool isSeaLevelRef = false;

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

@ -457,7 +457,6 @@ void disarm(disarmReason_t disarmReason)
#ifdef USE_PROGRAMMING_FRAMEWORK
programmingPidReset();
#endif
beeper(BEEPER_DISARMING); // emit disarm tone
prearmWasReset = false;

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

@ -2668,6 +2668,7 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
case MSP_SET_WP:
if (dataSize == 21) {
static uint8_t mmIdx = 0;
const uint8_t msp_wp_no = sbufReadU8(src); // get the waypoint number
navWaypoint_t msp_wp;
msp_wp.action = sbufReadU8(src); // action
@ -2679,6 +2680,17 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
msp_wp.p3 = sbufReadU16(src); // P3
msp_wp.flag = sbufReadU8(src); // future: to set nav flag
setWaypoint(msp_wp_no, &msp_wp);
uint8_t fwAppraochStartIdx = 8;
#ifdef USE_SAFE_HOME
fwAppraochStartIdx = MAX_SAFE_HOMES;
#endif
if (msp_wp_no == 0) {
mmIdx = 0;
} else if (msp_wp.flag == NAV_WP_FLAG_LAST) {
mmIdx++;
}
resetFwAutolandApproach(fwAppraochStartIdx + mmIdx);
} else
return MSP_RESULT_ERROR;
break;
@ -3137,6 +3149,7 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
safeHomeConfigMutable(i)->enabled = sbufReadU8(src);
safeHomeConfigMutable(i)->lat = sbufReadU32(src);
safeHomeConfigMutable(i)->lon = sbufReadU32(src);
resetFwAutolandApproach(i);
} else {
return MSP_RESULT_ERROR;
}

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

@ -4090,8 +4090,8 @@ groups:
condition: USE_SAFE_HOME
members:
- name: nav_fw_land_approach_length
description: "Angle of the final approach"
default_value: 50000
description: "Length of the final approach"
default_value: 35000
field: approachLength
min: 100
max: 100000
@ -4103,13 +4103,13 @@ groups:
max: 400
- name: nav_fw_land_glide_alt
description: "Initial altitude of the glide phase"
default_value: 500
default_value: 200
field: glideAltitude
min: 100
max: 5000
- name: nav_fw_land_flare_alt
description: "Initial altitude of the flare phase"
default_value: 2000
default_value: 150
field: flareAltitude
min: 0
max: 10000
@ -4121,7 +4121,7 @@ groups:
max: 45
- name: nav_fw_land_flare_pitch
description: "Pitch value for flare phase. In degrees"
default_value: 2
default_value: 8
field: flarePitch
min: 0
max: 45

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 ((NAV_Status.state != MW_NAV_STATE_HOVER_ABOVE_HOME && safehome_applied) || isFwLandInProgess()) {
if (!posControl.fwLandWp && (NAV_Status.state != MW_NAV_STATE_HOVER_ABOVE_HOME && safehome_applied)) {
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()) {
if (isWaypointMissionRTHActive() && !posControl.fwLandWp) {
// 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);
}

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

@ -66,6 +66,7 @@
#include "sensors/gyro.h"
#include "programming/global_variables.h"
#include "sensors/rangefinder.h"
// Multirotors:
#define MR_RTH_CLIMB_OVERSHOOT_CM 100 // target this amount of cm *above* the target altitude to ensure it is actually reached (Vz > 0 at target alt)
@ -98,6 +99,7 @@ PG_RESET_TEMPLATE(navFwAutolandConfig_t, navFwAutolandConfig,
.approachLength = SETTING_NAV_FW_LAND_APPROACH_LENGTH_DEFAULT,
.finalApproachPitchToThrottleMod = SETTING_NAV_FW_LAND_FINAL_APPROACH_PITCH2THROTTLE_MOD_DEFAULT,
.flareAltitude = SETTING_NAV_FW_LAND_FLARE_ALT_DEFAULT,
.glideAltitude = SETTING_NAV_FW_LAND_GLIDE_ALT_DEFAULT,
.flarePitch = SETTING_NAV_FW_LAND_FLARE_PITCH_DEFAULT,
.maxTailwind = SETTING_NAV_FW_LAND_MAX_TAILWIND_DEFAULT,
.glidePitch = SETTING_NAV_FW_LAND_GLIDE_PITCH_DEFAULT,
@ -847,6 +849,8 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
[NAV_FSM_EVENT_SWITCH_TO_COURSE_HOLD] = NAV_STATE_COURSE_HOLD_INITIALIZE,
[NAV_FSM_EVENT_SWITCH_TO_CRUISE] = NAV_STATE_CRUISE_INITIALIZE,
[NAV_FSM_EVENT_SWITCH_TO_MIXERAT] = NAV_STATE_MIXERAT_INITIALIZE,
[NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_FW_LANDING] = NAV_STATE_FW_LANDING_CLIMB_TO_LOITER,
[NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_FINISHED] = NAV_STATE_WAYPOINT_FINISHED,
}
},
@ -1140,6 +1144,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
[NAV_FSM_EVENT_SUCCESS] = NAV_STATE_IDLE,
[NAV_FSM_EVENT_SWITCH_TO_RTH] = NAV_STATE_RTH_INITIALIZE,
[NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE,
[NAV_FSM_EVENT_SWITCH_TO_WAYPOINT] = NAV_STATE_WAYPOINT_INITIALIZE,
}
},
};
@ -1673,7 +1678,6 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_HOVER_ABOVE_HOME(na
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_LANDING(navigationFSMState_t previousState)
{
UNUSED(previousState);
//On ROVER and BOAT we immediately switch to the next event
if (!STATE(ALTITUDE_CONTROL)) {
@ -1695,13 +1699,30 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_LANDING(navigationF
}
if (STATE(FIXED_WING_LEGACY)) {
// FW Autoland configuured?
if (!posControl.fwLandAborted && safehome_index >= 0 && (fwAutolandApproachConfig(safehome_index)->landApproachHeading1 != 0 || fwAutolandApproachConfig(safehome_index)->landApproachHeading2 != 0)) {
// FW Autoland configured?
int8_t missionIdx = -1, shIdx = -1, missionFwLandConfigStartIdx = 8;
#ifdef USE_MULTI_MISSION
missionIdx = posControl.loadedMultiMissionIndex - 1;
#endif
#ifdef USE_SAFE_HOME
shIdx = safehome_index;
missionFwLandConfigStartIdx = MAX_SAFE_HOMES;
#endif
if (!posControl.fwLandAborted && (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;
} else {
posControl.fwLandPos = posControl.rthState.homePosition.pos;
posControl.fwApproachSettingIdx = safehome_index;
posControl.fwApproachSettingIdx = shIdx;
posControl.fwLandWp = 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;
return NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_FW_LANDING;
} else {
return NAV_FSM_EVENT_SWITCH_TO_RTH_HOVER_ABOVE_HOME;
@ -1986,7 +2007,12 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_RTH_LAND(navig
UNUSED(previousState);
const navigationFSMEvent_t landEvent = navOnEnteringState_NAV_STATE_RTH_LANDING(previousState);
if (landEvent == NAV_FSM_EVENT_SUCCESS) {
if (landEvent == NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_FW_LANDING) {
return NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_FW_LANDING;
} else if (landEvent == NAV_FSM_EVENT_SWITCH_TO_RTH_HOVER_ABOVE_HOME) {
return NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_FINISHED;
} else if (landEvent == NAV_FSM_EVENT_SUCCESS) {
// Landing controller returned success - invoke RTH finishing state and finish the waypoint
navOnEnteringState_NAV_STATE_RTH_FINISHING(previousState);
return NAV_FSM_EVENT_SUCCESS;
@ -2029,6 +2055,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_EMERGENCY_LANDING_INITI
{
UNUSED(previousState);
posControl.fwLandState = FW_AUTOLAND_STATE_IDLE;
if ((posControl.flags.estPosStatus >= EST_USABLE)) {
resetPositionController();
setDesiredPosition(&navGetCurrentActualPositionAndVelocity()->pos, 0, NAV_POS_UPDATE_XY);
@ -2283,7 +2310,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_FW_LANDING_LOITER(navig
posControl.fwLandWaypoint[FW_AUTOLAND_WP_FINAL_APPROACH] = tmpPos;
calculateFarAwayPos(&tmpPos, &posControl.fwLandWaypoint[FW_AUTOLAND_WP_FINAL_APPROACH], dir, navFwAutolandConfig()->approachLength / 2);
tmpPos.z = posControl.fwLandAltAgl;
tmpPos.z = posControl.fwLandAproachAltAgl;
posControl.fwLandWaypoint[FW_AUTOLAND_WP_TURN] = tmpPos;
setLandWaypoint(&posControl.fwLandWaypoint[FW_AUTOLAND_WP_TURN], &posControl.fwLandWaypoint[FW_AUTOLAND_WP_FINAL_APPROACH]);
@ -2319,7 +2346,26 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_FW_LANDING_APPROACH(nav
return NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_FW_LANDING_ABORT;
}
if (getEstimatedActualPosition(Z) <= fwAutolandApproachConfig(posControl.fwApproachSettingIdx)->landAlt + navFwAutolandConfig()->glideAltitude - (fwAutolandApproachConfig(posControl.fwApproachSettingIdx)->isSeaLevelRef ? GPS_home.alt : 0)) {
if (isLandingDetected()) {
posControl.fwLandState = FW_AUTOLAND_STATE_IDLE;
disarm(DISARM_LANDING);
return NAV_FSM_EVENT_SWITCH_TO_IDLE;
}
float altitude;
#ifdef USE_RANGEFINDER
if (rangefinderIsHealthy() && rangefinderGetLatestAltitude() > RANGEFINDER_OUT_OF_RANGE) {
altitude = rangefinderGetLatestAltitude();
}
else
#endif
if (posControl.flags.estAglStatus >= EST_USABLE) {
altitude = posControl.actualState.agl.pos.z;
} else {
altitude = posControl.actualState.abs.pos.z;
}
if (altitude <= fwAutolandApproachConfig(posControl.fwApproachSettingIdx)->landAlt + navFwAutolandConfig()->glideAltitude - (fwAutolandApproachConfig(posControl.fwApproachSettingIdx)->isSeaLevelRef ? GPS_home.alt : 0)) {
resetPositionController();
posControl.fwLandState = FW_AUTOLAND_STATE_GLIDE;
return NAV_FSM_EVENT_SUCCESS;
@ -2356,9 +2402,15 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_FW_LANDING_GLIDE(naviga
return NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_FW_LANDING_ABORT;
}
float altitude = -1;
#ifdef USE_RANGEFINDER
if (rangefinderIsHealthy() && rangefinderGetLatestAltitude() > RANGEFINDER_OUT_OF_RANGE) {
altitude = rangefinderGetLatestAltitude();
}
#endif
// Surface sensor present?
if (posControl.flags.estAglStatus == EST_TRUSTED) {
if (getEstimatedActualPosition(Z) <= posControl.fwLandAltAgl + navFwAutolandConfig()->flareAltitude) {
if (altitude >= 0) {
if (altitude <= posControl.fwLandAltAgl + navFwAutolandConfig()->flareAltitude) {
posControl.cruise.course = posControl.fwLandingDirection;
posControl.cruise.previousCourse = posControl.cruise.course;
posControl.cruise.lastCourseAdjustmentTime = 0;
@ -2366,6 +2418,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_FW_LANDING_GLIDE(naviga
return NAV_FSM_EVENT_SUCCESS;
}
} else if (isLandingDetected()) {
posControl.fwLandState = FW_AUTOLAND_STATE_IDLE;
return NAV_FSM_EVENT_SWITCH_TO_IDLE;
}
@ -2389,6 +2442,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_FW_LANDING_FLARE(naviga
UNUSED(previousState);
if (isLandingDetected()) {
posControl.fwLandState = FW_AUTOLAND_STATE_IDLE;
return NAV_FSM_EVENT_SUCCESS;
}
setDesiredPosition(NULL, posControl.cruise.course, NAV_POS_UPDATE_HEADING);
@ -2400,7 +2454,9 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_FW_LANDING_ABORT(naviga
{
UNUSED(previousState);
posControl.fwLandAborted = true;
return NAV_FSM_EVENT_SWITCH_TO_RTH;
posControl.fwLandState = FW_AUTOLAND_STATE_IDLE;
return posControl.fwLandWp ? NAV_FSM_EVENT_SWITCH_TO_WAYPOINT : NAV_FSM_EVENT_SWITCH_TO_RTH;
}
static navigationFSMState_t navSetNewFSMState(navigationFSMState_t newState)
@ -4134,7 +4190,7 @@ void applyWaypointNavigationAndAltitudeHold(void)
posControl.flags.verticalPositionDataConsumed = false;
if (!isFwLandInProgess()) {
posControl.fwLandState = FW_AUTOLAND_LC_STATE_IDLE;
posControl.fwLandState = FW_AUTOLAND_STATE_IDLE;
}
/* Process controllers */
@ -5001,6 +5057,8 @@ static void resetFwAutoland(void)
posControl.fwLandWpReached = false;
posControl.fwApproachSettingIdx = 0;
posControl.fwLandPosHeading = -1;
posControl.fwLandState = FW_AUTOLAND_STATE_IDLE;
posControl.fwLandWp = false;
}
static int32_t calcFinalApproachHeading(int32_t approachHeading, int32_t windAngle)
@ -5043,9 +5101,14 @@ static void setLandWaypoint(const fpVector3_t *pos, const fpVector3_t *nextWpPos
posControl.wpAltitudeReached = false;
}
void resetFwAutolandApproach(void)
void resetFwAutolandApproach(int8_t idx)
{
if (idx >= 0 && idx < MAX_FW_LAND_APPOACH_SETTINGS)
{
memset(fwAutolandApproachConfigMutable(idx), 0, sizeof(navFwAutolandApproach_t));
} else {
memset(fwAutolandApproachConfigMutable(0), 0, sizeof(navFwAutolandApproach_t) * MAX_FW_LAND_APPOACH_SETTINGS);
}
}
bool isFwLandInProgess(void)

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

@ -59,7 +59,7 @@ typedef enum {
} fwAutolandApproachDirection_e;
typedef enum {
FW_AUTOLAND_LC_STATE_IDLE,
FW_AUTOLAND_STATE_IDLE,
FW_AUTOLAND_STATE_CROSSWIND,
FW_AUTOLAND_STATE_BASE_LEG,
FW_AUTOLAND_STATE_FINAL_APPROACH,
@ -103,7 +103,7 @@ extern int8_t safehome_index; // -1 if no safehome, 0 to MAX_
extern uint32_t safehome_distance; // distance to the nearest safehome
extern bool safehome_applied; // whether the safehome has been applied to home.
void resetFwAutolandApproach(void);
void resetFwAutolandApproach(int8_t idx);
void resetSafeHomes(void); // remove all safehomes
bool findNearestSafeHome(void); // Find nearest safehome

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

@ -467,6 +467,7 @@ typedef struct {
bool fwLandWpReached;
fwAutolandWayppoints_t fwLandCurrentWp;
bool fwLandAborted;
bool fwLandWp;
fwAutolandState_t fwLandState;
/* Internals & statistics */