Mirror/inav
1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-13 19:40:27 +03:00
Code Issues Wiki Activity

FW-Autoland

Browse source
This commit is contained in:
Scavanger 2023-11-01 09:43:32 -03:00
parent 6fd4a116df
commit 9370fdd49f
19 changed files with 1206 additions and 91 deletions
Download patch file Download diff file Expand all files Collapse all files

1
.gitignore vendored
View file

@ -22,6 +22,7 @@ cov-int*
/downloads/
/debug/
/release/
/*_SITL/
# script-generated files
docs/Manual.pdf

4
docs/Settings.md
View file

@ -2922,9 +2922,9 @@ Dive angle that airplane will use during final landing phase. During dive phase,
---
### nav_fw_launch_abort_deadband
### nav_fw_launch_land_abort_deadband
Launch abort stick deadband in [r/c points], applied after r/c deadband and expo. The Roll/Pitch stick needs to be deflected beyond this deadband to abort the launch.
Launch and landing abort stick deadband in [r/c points], applied after r/c deadband and expo. The Roll/Pitch stick needs to be deflected beyond this deadband to abort the launch or landing.
| Default | Min | Max |
| --- | --- | --- |

4
src/main/config/parameter_group_ids.h
View file

@ -123,8 +123,8 @@
#define PG_EZ_TUNE 1033
#define PG_LEDPIN_CONFIG 1034
#define PG_OSD_JOYSTICK_CONFIG 1035
#define PG_INAV_END PG_OSD_JOYSTICK_CONFIG
#define PG_FW_AUTOLAND_CONFIG 1036
#define PG_INAV_END PG_FW_AUTOLAND_CONFIG
// OSD configuration (subject to change)
//#define PG_OSD_FONT_CONFIG 2047

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

@ -1312,18 +1312,23 @@ static void cliTempSensor(char *cmdline)
#if defined(USE_SAFE_HOME)
static void printSafeHomes(uint8_t dumpMask, const navSafeHome_t *navSafeHome, const navSafeHome_t *defaultSafeHome)
{
const char *format = "safehome %u %u %d %d"; // uint8_t enabled, int32_t lat; int32_t lon
const char *format = "safehome %u %u %d %d %d %d %u %d %d"; // uint8_t enabled, int32_t lat; int32_t lon
for (uint8_t i = 0; i < MAX_SAFE_HOMES; i++) {
bool equalsDefault = false;
if (defaultSafeHome) {
equalsDefault = navSafeHome[i].enabled == defaultSafeHome[i].enabled
&& navSafeHome[i].lat == defaultSafeHome[i].lat
&& navSafeHome[i].lon == defaultSafeHome[i].lon;
&& navSafeHome[i].lon == defaultSafeHome[i].lon
&& navSafeHome[i].approachDirection == defaultSafeHome[i].approachDirection
&& navSafeHome[i].approachAltMSL == defaultSafeHome[i].approachAltMSL
&& navSafeHome[i].landAltMSL == defaultSafeHome[i].landAltMSL
&& navSafeHome[i].landApproachHeading1 == defaultSafeHome[i].landApproachHeading1
&& navSafeHome[i].landApproachHeading2 == defaultSafeHome[i].landApproachHeading2;
cliDefaultPrintLinef(dumpMask, equalsDefault, format, i,
defaultSafeHome[i].enabled, defaultSafeHome[i].lat, defaultSafeHome[i].lon);
defaultSafeHome[i].enabled, defaultSafeHome[i].lat, defaultSafeHome[i].lon, defaultSafeHome[i].approachAltMSL, defaultSafeHome[i].landAltMSL, defaultSafeHome[i].approachDirection, defaultSafeHome[i].landApproachHeading1, defaultSafeHome[i].landApproachHeading2);
}
cliDumpPrintLinef(dumpMask, equalsDefault, format, i,
navSafeHome[i].enabled, navSafeHome[i].lat, navSafeHome[i].lon);
navSafeHome[i].enabled, navSafeHome[i].lat, navSafeHome[i].lon, navSafeHome[i].approachAltMSL, navSafeHome[i].landAltMSL, navSafeHome[i].approachDirection, navSafeHome[i].landApproachHeading1, navSafeHome[i].landApproachHeading2);
}
}
@ -1334,7 +1339,7 @@ static void cliSafeHomes(char *cmdline)
} else if (sl_strcasecmp(cmdline, "reset") == 0) {
resetSafeHomes();
} else {
int32_t lat=0, lon=0;
int32_t lat=0, lon=0, approachAlt = 0, heading1 = 0, heading2 = 0, landDirection = 0, landAlt = 0;
bool enabled=false;
uint8_t validArgumentCount = 0;
const char *ptr = cmdline;
@ -1354,16 +1359,54 @@ static void cliSafeHomes(char *cmdline)
lon = fastA2I(ptr);
validArgumentCount++;
}
if ((ptr = nextArg(ptr))) {
approachAlt = fastA2I(ptr);
validArgumentCount++;
}
if ((ptr = nextArg(ptr))) {
landAlt = fastA2I(ptr);
validArgumentCount++;
}
if ((ptr = nextArg(ptr))) {
landDirection = fastA2I(ptr);
if (landDirection != 0 && landDirection != 1) {
cliShowParseError();
return;
}
validArgumentCount++;
}
if ((ptr = nextArg(ptr))) {
heading1 = fastA2I(ptr);
validArgumentCount++;
}
if ((ptr = nextArg(ptr))) {
heading2 = fastA2I(ptr);
validArgumentCount++;
}
if ((ptr = nextArg(ptr))) {
// check for too many arguments
validArgumentCount++;
}
if (validArgumentCount != 3) {
if (validArgumentCount != 8) {
cliShowParseError();
} else {
safeHomeConfigMutable(i)->enabled = enabled;
safeHomeConfigMutable(i)->lat = lat;
safeHomeConfigMutable(i)->lon = lon;
safeHomeConfigMutable(i)->approachAltMSL = approachAlt;
safeHomeConfigMutable(i)->landAltMSL = landAlt;
safeHomeConfigMutable(i)->approachDirection = (fwAutolandApproachDirection_e)landDirection;
safeHomeConfigMutable(i)->landApproachHeading1 = (int16_t)heading1;
safeHomeConfigMutable(i)->landApproachHeading2 = (int16_t)heading2;
}
}
}

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

@ -2836,15 +2836,6 @@ groups:
field: fw.control_smoothness
min: 0
max: 9
- name: nav_fw_land_dive_angle
description: "Dive angle that airplane will use during final landing phase. During dive phase, motor is stopped or IDLE and roll control is locked to 0 degrees"
default_value: 2
field: fw.land_dive_angle
condition: NAV_FIXED_WING_LANDING
min: -20
max: 20
- name: nav_fw_launch_velocity
description: "Forward velocity threshold for swing-launch detection [cm/s]"
default_value: 300
@ -2921,10 +2912,10 @@ groups:
default_value: OFF
field: fw.launch_manual_throttle
type: bool
- name: nav_fw_launch_abort_deadband
description: "Launch abort stick deadband in [r/c points], applied after r/c deadband and expo. The Roll/Pitch stick needs to be deflected beyond this deadband to abort the launch."
- name: nav_fw_launch_land_abort_deadband
description: "Launch and landing abort stick deadband in [r/c points], applied after r/c deadband and expo. The Roll/Pitch stick needs to be deflected beyond this deadband to abort the launch or landing."
default_value: 100
field: fw.launch_abort_deadband
field: fw.launch_land_abort_deadband
min: 2
max: 250
- name: nav_fw_allow_manual_thr_increase
@ -4092,3 +4083,51 @@ groups:
field: osd_joystick_enter
min: 0
max: 100
- name: PG_FW_AUTOLAND_CONFIG
type: navFwAutolandConfig_t
headers: ["navigation/navigation.h"]
condition: USE_SAFE_HOME
members:
- name: nav_fw_land_approach_length
description: "Angle of the final approach"
default_value: 50000
field: approachLength
min: 100
max: 100000
- name: nav_fw_land_final_approach_pitch2throttle_mod
description: "Modifier for pitch to throttle at final approach. In Percent."
default_value: 100
field: finalApproachPitchToThrottleMod
min: 100
max: 400
- name: nav_fw_land_glide_alt
description: "Initial altitude of the glide phase"
default_value: 500
field: glideAltitude
min: 100
max: 5000
- name: nav_fw_land_flare_alt
description: "Initial altitude of the flare"
default_value: 2000
field: flareAltitude
min: 0
max: 10000
- name: nav_fw_land_glide_pitch
description: "Pitch value for glide phase. In degrees."
default_value: 0
field: glidePitch
min: 0
max: 45
- name: nav_fw_land_flare_pitch
description: "Pitch value for flare phase. In degrees"
default_value: 2
field: flarePitch
min: 0
max: 45
- name: nav_fw_land_max_tailwind
description: "Max. tailwind (in cm/s) if no landing direction with downwind is available"
default_value: 140
field: maxTailwind
min: 0
max: 3000

8
src/main/flight/wind_estimator.c
View file

@ -38,6 +38,8 @@
#include "flight/imu.h"
#include "navigation/navigation_pos_estimator_private.h"
#include "io/gps.h"
@ -105,9 +107,9 @@ void updateWindEstimator(timeUs_t currentTimeUs)
// Get current 3D velocity from GPS in cm/s
// relative to earth frame
groundVelocity[X] = gpsSol.velNED[X];
groundVelocity[Y] = gpsSol.velNED[Y];
groundVelocity[Z] = gpsSol.velNED[Z];
groundVelocity[X] = posEstimator.gps.vel.x;
groundVelocity[Y] = posEstimator.gps.vel.y;
groundVelocity[Z] = posEstimator.gps.vel.z;
// Fuselage direction in earth frame
fuselageDirection[X] = HeadVecEFFiltered.x;

13
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 && posControl.safehomeState.isApplied) {
if ((NAV_Status.state != MW_NAV_STATE_HOVER_ABOVE_HOME && safehome_applied) || isFwLandInProgess()) {
return OSD_MESSAGE_STR(OSD_MSG_DIVERT_SAFEHOME);
}
return NULL;
@ -2269,8 +2269,9 @@ static bool osdDrawSingleElement(uint8_t item)
case OSD_FLYMODE:
{
char *p = "ACRO";
if (FLIGHT_MODE(FAILSAFE_MODE))
if (isFwLandInProgess())
p = "LAND";
else if (FLIGHT_MODE(FAILSAFE_MODE))
p = "!FS!";
else if (FLIGHT_MODE(MANUAL_MODE))
p = "MANU";
@ -5133,7 +5134,7 @@ textAttributes_t osdGetSystemMessage(char *buff, size_t buff_size, bool isCenter
const char *invertedInfoMessage = NULL;
if (ARMING_FLAG(ARMED)) {
if (FLIGHT_MODE(FAILSAFE_MODE) || FLIGHT_MODE(NAV_RTH_MODE) || FLIGHT_MODE(NAV_WP_MODE) || navigationIsExecutingAnEmergencyLanding()) {
if (FLIGHT_MODE(FAILSAFE_MODE) || FLIGHT_MODE(NAV_RTH_MODE) || FLIGHT_MODE(NAV_WP_MODE) || navigationIsExecutingAnEmergencyLanding() || isFwLandInProgess()) {
if (isWaypointMissionRTHActive()) {
// 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);
@ -5165,11 +5166,15 @@ textAttributes_t osdGetSystemMessage(char *buff, size_t buff_size, bool isCenter
messages[messageCount++] = messageBuf;
} else {
if (canFwLandCanceld()) {
messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_MOVE_STICKS);
} else if (!isFwLandInProgess()) {
const char *navStateMessage = navigationStateMessage();
if (navStateMessage) {
messages[messageCount++] = navStateMessage;
}
}
}
#if defined(USE_SAFE_HOME)
const char *safehomeMessage = divertingToSafehomeMessage();
if (safehomeMessage) {

1
src/main/io/osd.h
View file

@ -123,6 +123,7 @@
#define OSD_MSG_ANGLEHOLD_ROLL "(ANGLEHOLD ROLL)"
#define OSD_MSG_ANGLEHOLD_PITCH "(ANGLEHOLD PITCH)"
#define OSD_MSG_ANGLEHOLD_LEVEL "(ANGLEHOLD LEVEL)"
#define OSD_MSG_MOVE_STICKS "MOVE STICKS TO ABORT"
#ifdef USE_DEV_TOOLS
#define OSD_MSG_GRD_TEST_MODE "GRD TEST > MOTORS DISABLED"

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

@ -49,6 +49,7 @@
#include "flight/imu.h"
#include "flight/mixer_profile.h"
#include "flight/pid.h"
#include "flight/wind_estimator.h"
#include "io/beeper.h"
#include "io/gps.h"
@ -76,6 +77,8 @@
#define FW_RTH_CLIMB_OVERSHOOT_CM 100
#define FW_RTH_CLIMB_MARGIN_MIN_CM 100
#define FW_RTH_CLIMB_MARGIN_PERCENT 15
#define FW_LAND_LOITER_MIN_TIME 30000000 // usec (30 sec)
#define FW_LAND_LOITER_ALT_TOLERANCE 150
/*-----------------------------------------------------------
* Compatibility for home position
@ -88,6 +91,18 @@ radar_pois_t radar_pois[RADAR_MAX_POIS];
#if defined(USE_SAFE_HOME)
PG_REGISTER_ARRAY(navSafeHome_t, MAX_SAFE_HOMES, safeHomeConfig, PG_SAFE_HOME_CONFIG , 0);
PG_REGISTER_WITH_RESET_TEMPLATE(navFwAutolandConfig_t, navFwAutolandConfig, PG_FW_AUTOLAND_CONFIG, 0);
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,
.flarePitch = SETTING_NAV_FW_LAND_FLARE_PITCH_DEFAULT,
.maxTailwind = SETTING_NAV_FW_LAND_MAX_TAILWIND_DEFAULT,
.glidePitch = SETTING_NAV_FW_LAND_GLIDE_PITCH_DEFAULT,
);
#endif
// waypoint 254, 255 are special waypoints
@ -193,9 +208,6 @@ PG_RESET_TEMPLATE(navConfig_t, navConfig,
.loiter_radius = SETTING_NAV_FW_LOITER_RADIUS_DEFAULT, // 75m
.loiter_direction = SETTING_FW_LOITER_DIRECTION_DEFAULT,
//Fixed wing landing
.land_dive_angle = SETTING_NAV_FW_LAND_DIVE_ANGLE_DEFAULT, // 2 degrees dive by default
// Fixed wing launch
.launch_velocity_thresh = SETTING_NAV_FW_LAUNCH_VELOCITY_DEFAULT, // 3 m/s
.launch_accel_thresh = SETTING_NAV_FW_LAUNCH_ACCEL_DEFAULT, // cm/s/s (1.9*G)
@ -210,7 +222,7 @@ PG_RESET_TEMPLATE(navConfig_t, navConfig,
.launch_climb_angle = SETTING_NAV_FW_LAUNCH_CLIMB_ANGLE_DEFAULT, // 18 degrees
.launch_max_angle = SETTING_NAV_FW_LAUNCH_MAX_ANGLE_DEFAULT, // 45 deg
.launch_manual_throttle = SETTING_NAV_FW_LAUNCH_MANUAL_THROTTLE_DEFAULT,// OFF
.launch_abort_deadband = SETTING_NAV_FW_LAUNCH_ABORT_DEADBAND_DEFAULT, // 100 us
.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,
@ -251,6 +263,7 @@ static void resetAltitudeController(bool useTerrainFollowing);
static void resetPositionController(void);
static void setupAltitudeController(void);
static void resetHeadingController(void);
static void resetFwAutoland(void);
void resetGCSFlags(void);
static void setupJumpCounters(void);
@ -260,6 +273,7 @@ static void clearJumpCounters(void);
static void calculateAndSetActiveWaypoint(const navWaypoint_t * waypoint);
static void calculateAndSetActiveWaypointToLocalPosition(const fpVector3_t * pos);
void calculateInitialHoldPosition(fpVector3_t * pos);
void calculateFarAwayPos(fpVector3_t * farAwayPos, const fpVector3_t *start, int32_t bearing, int32_t distance);
void calculateFarAwayTarget(fpVector3_t * farAwayPos, int32_t bearing, int32_t distance);
static bool isWaypointReached(const fpVector3_t * waypointPos, const int32_t * waypointBearing);
bool isWaypointAltitudeReached(void);
@ -278,6 +292,11 @@ static bool rthAltControlStickOverrideCheck(unsigned axis);
static void updateRthTrackback(bool forceSaveTrackPoint);
static fpVector3_t * rthGetTrackbackPos(void);
static bool allowFwAutoland(void);
static int32_t calcWindDiff(int32_t heading, int32_t windHeading);
static int32_t calcFinalApproachHeading(int32_t approachHeading, int32_t windAngle);
static void setLandWaypoint(const fpVector3_t *pos, const fpVector3_t *nextWpPos);
/*************************************************************************************************/
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_IDLE(navigationFSMState_t previousState);
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_ALTHOLD_INITIALIZE(navigationFSMState_t previousState);
@ -316,6 +335,12 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_LAUNCH_IN_PROGRESS(navi
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_MIXERAT_INITIALIZE(navigationFSMState_t previousState);
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_MIXERAT_IN_PROGRESS(navigationFSMState_t previousState);
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_MIXERAT_ABORT(navigationFSMState_t previousState);
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_FW_LANDING_CLIMB_TO_LOITER(navigationFSMState_t previousState);
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_FW_LANDING_LOITER(navigationFSMState_t previousState);
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_FW_LANDING_APPROACH(navigationFSMState_t previousState);
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_FW_LANDING_GLIDE(navigationFSMState_t previousState);
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_FW_LANDING_FLARE(navigationFSMState_t previousState);
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_FW_LANDING_ABORT(navigationFSMState_t previousState);
static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
/** Idle state ******************************************************/
@ -665,6 +690,8 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
[NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D] = NAV_STATE_POSHOLD_3D_INITIALIZE,
[NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_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_RTH_HOVER_ABOVE_HOME] = NAV_STATE_RTH_HOVER_ABOVE_HOME,
}
},
@ -999,6 +1026,121 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
}
},
},
[NAV_STATE_FW_LANDING_CLIMB_TO_LOITER] = {
.persistentId = NAV_PERSISTENT_ID_FW_LANDING_CLIMB_TO_LOITER,
.onEntry = navOnEnteringState_NAV_STATE_FW_LANDING_CLIMB_TO_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,
.mapToFlightModes = NAV_RTH_MODE | NAV_ALTHOLD_MODE,
.mwState = MW_NAV_STATE_LAND_IN_PROGRESS,
.mwError = MW_NAV_ERROR_NONE,
.onEvent = {
[NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_FW_LANDING_CLIMB_TO_LOITER,
[NAV_FSM_EVENT_SUCCESS] = NAV_STATE_FW_LANDING_LOITER,
[NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE,
[NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_ALTHOLD_INITIALIZE,
[NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D] = NAV_STATE_POSHOLD_3D_INITIALIZE,
[NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE,
[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_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,
.mapToFlightModes = NAV_RTH_MODE | NAV_ALTHOLD_MODE,
.mwState = MW_NAV_STATE_LAND_IN_PROGRESS,
.mwError = MW_NAV_ERROR_NONE,
.onEvent = {
[NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_FW_LANDING_LOITER,
[NAV_FSM_EVENT_SUCCESS] = NAV_STATE_FW_LANDING_APPROACH,
[NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE,
[NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_ALTHOLD_INITIALIZE,
[NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D] = NAV_STATE_POSHOLD_3D_INITIALIZE,
[NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE,
[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_NAV_STATE_FW_LANDING_ABORT] = NAV_STATE_FW_LANDING_ABORT,
}
},
[NAV_STATE_FW_LANDING_APPROACH] = {
.persistentId = NAV_PERSISTENT_ID_FW_LANDING_APPROACH,
.onEntry = navOnEnteringState_NAV_STATE_FW_LANDING_APPROACH,
.timeoutMs = 10,
.stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_WP,
.mapToFlightModes = NAV_WP_MODE | NAV_ALTHOLD_MODE,
.mwState = MW_NAV_STATE_LAND_IN_PROGRESS,
.mwError = MW_NAV_ERROR_NONE,
.onEvent = {
[NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_FW_LANDING_APPROACH,
[NAV_FSM_EVENT_SUCCESS] = NAV_STATE_FW_LANDING_GLIDE,
[NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE,
[NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_ALTHOLD_INITIALIZE,
[NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D] = NAV_STATE_POSHOLD_3D_INITIALIZE,
[NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE,
[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_NAV_STATE_FW_LANDING_ABORT] = NAV_STATE_FW_LANDING_ABORT,
}
},
[NAV_STATE_FW_LANDING_GLIDE] = {
.persistentId = NAV_PERSISTENT_ID_FW_LANDING_GLIDE,
.onEntry = navOnEnteringState_NAV_STATE_FW_LANDING_GLIDE,
.timeoutMs = 10,
.stateFlags = NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_RC_POS | NAV_RC_YAW,
.mapToFlightModes = NAV_COURSE_HOLD_MODE,
.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_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,
[NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D] = NAV_STATE_POSHOLD_3D_INITIALIZE,
[NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE,
[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_NAV_STATE_FW_LANDING_ABORT] = NAV_STATE_FW_LANDING_ABORT,
}
},
[NAV_STATE_FW_LANDING_FLARE] = {
.persistentId = NAV_PERSISTENT_ID_FW_LANDING_FLARE,
.onEntry = navOnEnteringState_NAV_STATE_FW_LANDING_FLARE,
.timeoutMs = 10,
.stateFlags = NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_RC_POS | NAV_RC_YAW,
.mapToFlightModes = NAV_COURSE_HOLD_MODE,
.mwState = MW_NAV_STATE_LAND_IN_PROGRESS,
.mwError = MW_NAV_ERROR_NONE,
.onEvent = {
[NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_FW_LANDING_FLARE, // re-process the state
[NAV_FSM_EVENT_SUCCESS] = NAV_STATE_IDLE,
[NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE,
}
},
[NAV_STATE_FW_LANDING_ABORT] = {
.persistentId = NAV_PERSISTENT_ID_FW_LANDING_ABORT,
.onEntry = navOnEnteringState_NAV_STATE_FW_LANDING_ABORT,
.timeoutMs = 10,
.stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_CTL_LAND | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_RTH | NAV_RC_POS | NAV_RC_YAW,
.mapToFlightModes = NAV_WP_MODE | NAV_ALTHOLD_MODE,
.mwState = MW_NAV_STATE_LAND_IN_PROGRESS,
.mwError = MW_NAV_ERROR_NONE,
.onEvent = {
[NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_FW_LANDING_ABORT,
[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,
}
},
};
static navigationFSMStateFlags_t navGetStateFlags(navigationFSMState_t state)
@ -1030,6 +1172,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_IDLE(navigationFSMState
resetAltitudeController(false);
resetHeadingController();
resetPositionController();
resetFwAutoland();
return NAV_FSM_EVENT_NONE;
}
@ -1227,7 +1370,6 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_CRUISE_ADJUSTING(naviga
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_INITIALIZE(navigationFSMState_t previousState)
{
UNUSED(previousState);
if (navConfig()->general.flags.rth_use_linear_descent && posControl.rthState.rthLinearDescentActive)
posControl.rthState.rthLinearDescentActive = false;
@ -1239,10 +1381,13 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_INITIALIZE(navigati
return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING;
}
if (previousState != NAV_STATE_FW_LANDING_ABORT) {
posControl.fwLandAborted = 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;
}
}
// If we have valid position sensor or configured to ignore it's loss at initial stage - continue
if ((posControl.flags.estPosStatus >= EST_USABLE) || navConfig()->general.flags.rth_climb_ignore_emerg) {
@ -1548,6 +1693,15 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_LANDING(navigationF
return NAV_FSM_EVENT_SWITCH_TO_MIXERAT;
}
if (STATE(FIXED_WING_LEGACY)) {
if (allowFwAutoland()) {
resetFwAutoland();
return NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_FW_LANDING;
} else {
return NAV_FSM_EVENT_SWITCH_TO_RTH_HOVER_ABOVE_HOME;
}
}
float descentVelLimited = 0;
fpVector3_t tmpHomePos = posControl.rthState.homeTmpWaypoint;
@ -1559,6 +1713,7 @@ 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;
@ -1932,7 +2087,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_LAUNCH_WAIT(navigationF
}
// abort NAV_LAUNCH_MODE by moving sticks with low throttle or throttle stick < launch idle throttle
if (abortLaunchAllowed() && isRollPitchStickDeflected(navConfig()->fw.launch_abort_deadband)) {
if (abortLaunchAllowed() && isRollPitchStickDeflected(navConfig()->fw.launch_land_abort_deadband)) {
abortFixedWingLaunch();
return NAV_FSM_EVENT_SWITCH_TO_IDLE;
}
@ -2015,6 +2170,231 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_MIXERAT_ABORT(navigatio
return NAV_FSM_EVENT_SUCCESS;
}
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_FW_LANDING_CLIMB_TO_LOITER(navigationFSMState_t previousState)
{
UNUSED(previousState);
if (isRollPitchStickDeflected(navConfig()->fw.launch_land_abort_deadband)) {
return NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_FW_LANDING_ABORT;
}
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);
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
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;
}
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_FW_LANDING_LOITER(navigationFSMState_t previousState)
{
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;
}
if (isRollPitchStickDeflected(navConfig()->fw.launch_land_abort_deadband)) {
return NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_FW_LANDING_ABORT;
}
if (posControl.fwLandLoiterStartTime == 0) {
posControl.fwLandLoiterStartTime = micros();
} else if (micros() - posControl.fwLandLoiterStartTime > FW_LAND_LOITER_MIN_TIME) {
if (isEstimatedWindSpeedValid()) {
uint16_t windAngle = 0;
int32_t approachHeading = -1;
float windSpeed = getEstimatedHorizontalWindSpeed(&windAngle);
windAngle = wrap_36000(windAngle + 18000);
// Ignore low wind speed, could be the error of the wind estimator
if (windSpeed < navFwAutolandConfig()->maxTailwind) {
if (safeHomeConfig(safehome_index)->landApproachHeading1 != 0) {
approachHeading = posControl.fwLandingDirection = ABS(DEGREES_TO_CENTIDEGREES(safeHomeConfig(safehome_index)->landApproachHeading1));
} else if ((safeHomeConfig(safehome_index)->landApproachHeading2 != 0) ) {
approachHeading = posControl.fwLandingDirection = ABS(DEGREES_TO_CENTIDEGREES(safeHomeConfig(safehome_index)->landApproachHeading2));
} else {
approachHeading = posControl.fwLandingDirection = -1;
}
} else {
int32_t heading1 = calcFinalApproachHeading(DEGREES_TO_CENTIDEGREES(safeHomeConfig(safehome_index)->landApproachHeading1), windAngle);
int32_t heading2 = calcFinalApproachHeading(DEGREES_TO_CENTIDEGREES(safeHomeConfig(safehome_index)->landApproachHeading2), windAngle);
if (heading1 == heading2 || heading1 == wrap_36000(heading2 + 18000)) {
heading2 = -1;
}
if (heading1 == -1 && heading2 >= 0) {
posControl.fwLandingDirection = heading2;
approachHeading = DEGREES_TO_CENTIDEGREES(safeHomeConfig(safehome_index)->landApproachHeading2);
} else if (heading1 >= 0 && heading2 == -1) {
posControl.fwLandingDirection = heading1;
approachHeading = DEGREES_TO_CENTIDEGREES(safeHomeConfig(safehome_index)->landApproachHeading1);
} else {
if (calcWindDiff(heading1, windAngle) < calcWindDiff(heading2, windAngle)) {
posControl.fwLandingDirection = heading1;
approachHeading = DEGREES_TO_CENTIDEGREES(safeHomeConfig(safehome_index)->landApproachHeading1);
} else {
posControl.fwLandingDirection = heading2;
approachHeading = DEGREES_TO_CENTIDEGREES(safeHomeConfig(safehome_index)->landApproachHeading2);
}
}
}
if (posControl.fwLandingDirection >= 0) {
fpVector3_t tmpPos;
int32_t finalApproachAlt = posControl.fwLandAproachAltAgl / 3 * 2;
int32_t dir = 0;
if (safeHomeConfig(safehome_index)->approachDirection == FW_AUTOLAND_APPROACH_DIRECTION_LEFT) {
dir = wrap_36000(approachHeading - 9000);
} else {
dir = wrap_36000(approachHeading + 9000);
}
tmpPos = posControl.rthState.homePosition.pos;
tmpPos.z = posControl.fwLandAltAgl;
posControl.fwLandWaypoint[FW_AUTOLAND_WP_LAND] = tmpPos;
calculateFarAwayPos(&tmpPos, &posControl.rthState.homePosition.pos, wrap_36000(posControl.fwLandingDirection + 18000), navFwAutolandConfig()->approachLength);
tmpPos.z = finalApproachAlt;
posControl.fwLandWaypoint[FW_AUTOLAND_WP_FINAL_APPROACH] = tmpPos;
calculateFarAwayPos(&tmpPos, &posControl.fwLandWaypoint[FW_AUTOLAND_WP_FINAL_APPROACH], dir, navFwAutolandConfig()->approachLength / 2);
tmpPos.z = finalApproachAlt;
posControl.fwLandWaypoint[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;
return NAV_FSM_EVENT_SUCCESS;
} else {
return NAV_FSM_EVENT_SWITCH_TO_IDLE;
}
} else {
posControl.fwLandLoiterStartTime = micros();
}
}
fpVector3_t tmpPoint = posControl.rthState.homePosition.pos;
tmpPoint.z = posControl.fwLandAproachAltAgl;
setDesiredPosition(&tmpPoint, posControl.rthState.homePosition.heading, 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)
{
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;
}
if (isRollPitchStickDeflected(navConfig()->fw.launch_land_abort_deadband)) {
return NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_FW_LANDING_ABORT;
}
if (getEstimatedActualPosition(Z) <= safeHomeConfig(safehome_index)->landAltMSL + navFwAutolandConfig()->glideAltitude - GPS_home.alt) {
resetPositionController();
posControl.fwLandState = 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;
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;
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);
return NAV_FSM_EVENT_NONE;
}
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_FW_LANDING_GLIDE(navigationFSMState_t previousState)
{
UNUSED(previousState);
if (isRollPitchStickDeflected(navConfig()->fw.launch_land_abort_deadband)) {
return NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_FW_LANDING_ABORT;
}
// Surface sensor present?
if (posControl.flags.estAglStatus == EST_TRUSTED) {
if (getEstimatedActualPosition(Z) <= posControl.fwLandAltAgl + navFwAutolandConfig()->flareAltitude) {
posControl.cruise.course = posControl.fwLandingDirection;
posControl.cruise.previousCourse = posControl.cruise.course;
posControl.cruise.lastCourseAdjustmentTime = 0;
posControl.fwLandState = FW_AUTOLAND_STATE_FLARE;
return NAV_FSM_EVENT_SUCCESS;
}
} else if (isLandingDetected()) {
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;
}
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_FW_LANDING_FLARE(navigationFSMState_t previousState)
{
UNUSED(previousState);
if (isLandingDetected()) {
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)
{
UNUSED(previousState);
posControl.fwLandAborted = true;
return NAV_FSM_EVENT_SWITCH_TO_RTH;
}
static navigationFSMState_t navSetNewFSMState(navigationFSMState_t newState)
{
navigationFSMState_t previousState;
@ -2971,11 +3351,16 @@ void setDesiredPosition(const fpVector3_t * pos, int32_t yaw, navSetWaypointFlag
}
}
void calculateFarAwayPos(fpVector3_t *farAwayPos, const fpVector3_t *start, int32_t bearing, int32_t distance)
{
farAwayPos->x = start->x + distance * cos_approx(CENTIDEGREES_TO_RADIANS(bearing));
farAwayPos->y = start->y + distance * sin_approx(CENTIDEGREES_TO_RADIANS(bearing));
farAwayPos->z = start->z;
}
void calculateFarAwayTarget(fpVector3_t * farAwayPos, int32_t bearing, int32_t distance)
{
farAwayPos->x = navGetCurrentActualPositionAndVelocity()->pos.x + distance * cos_approx(CENTIDEGREES_TO_RADIANS(bearing));
farAwayPos->y = navGetCurrentActualPositionAndVelocity()->pos.y + distance * sin_approx(CENTIDEGREES_TO_RADIANS(bearing));
farAwayPos->z = navGetCurrentActualPositionAndVelocity()->pos.z;
calculateFarAwayPos(farAwayPos, &navGetCurrentActualPositionAndVelocity()->pos, bearing, distance);
}
/*-----------------------------------------------------------
@ -3731,7 +4116,7 @@ void applyWaypointNavigationAndAltitudeHold(void)
posControl.activeRthTBPointIndex = -1;
posControl.flags.rthTrackbackActive = false;
posControl.rthTBWrapAroundCounter = -1;
posControl.fwLandAborted = false;
return;
}
@ -3739,6 +4124,10 @@ void applyWaypointNavigationAndAltitudeHold(void)
posControl.flags.horizontalPositionDataConsumed = false;
posControl.flags.verticalPositionDataConsumed = false;
if (!isFwLandInProgess()) {
posControl.fwLandState = FW_AUTOLAND_LC_STATE_IDLE;
}
/* Process controllers */
navigationFSMStateFlags_t navStateFlags = navGetStateFlags(posControl.navState);
if (STATE(ROVER) || STATE(BOAT)) {
@ -4595,25 +4984,72 @@ int32_t navigationGetHeadingError(void)
return wrap_18000(posControl.desiredState.yaw - posControl.actualState.cog);
}
int8_t navCheckActiveAngleHoldAxis(void)
static void resetFwAutoland(void)
{
int8_t activeAxis = -1;
if (IS_RC_MODE_ACTIVE(BOXANGLEHOLD)) {
navigationFSMStateFlags_t stateFlags = navGetCurrentStateFlags();
bool altholdActive = stateFlags & NAV_REQUIRE_ANGLE_FW && !(stateFlags & NAV_REQUIRE_ANGLE);
if (FLIGHT_MODE(NAV_COURSE_HOLD_MODE) && !FLIGHT_MODE(NAV_ALTHOLD_MODE)) {
activeAxis = FD_PITCH;
} else if (altholdActive) {
activeAxis = FD_ROLL;
}
posControl.fwLandAltAgl = safeHomeConfig(safehome_index)->landAltMSL - GPS_home.alt;
posControl.fwLandAproachAltAgl = safeHomeConfig(safehome_index)->approachAltMSL + posControl.fwLandAltAgl - GPS_home.alt;
posControl.fwLandLoiterStartTime = 0;
posControl.fwLandWpReached = false;
}
return activeAxis;
}
uint8_t getActiveWpNumber(void)
static bool allowFwAutoland(void)
{
return NAV_Status.activeWpNumber;
return !posControl.fwLandAborted && safehome_index >= 0 && (safeHomeConfig(safehome_index)->landApproachHeading1 != 0 || safeHomeConfig(safehome_index)->landApproachHeading2 != 0);
}
static int32_t calcFinalApproachHeading(int32_t approachHeading, int32_t windAngle)
{
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 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)
{
calculateAndSetActiveWaypointToLocalPosition(pos);
if (navConfig()->fw.wp_turn_smoothing && nextWpPos != NULL) {
int32_t bearingToNextWp = calculateBearingBetweenLocalPositions(&posControl.activeWaypoint.pos, nextWpPos);
posControl.activeWaypoint.nextTurnAngle = wrap_18000(bearingToNextWp - posControl.activeWaypoint.bearing);
} else {
posControl.activeWaypoint.nextTurnAngle = -1;
}
posControl.wpInitialDistance = calculateDistanceToDestination(&posControl.activeWaypoint.pos);
posControl.wpInitialAltitude = posControl.actualState.abs.pos.z;
posControl.wpAltitudeReached = false;
}
bool isFwLandInProgess(void)
{
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)
{
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;
}

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

@ -44,10 +44,28 @@ void onNewGPSData(void);
#define MAX_SAFE_HOMES 8
typedef enum {
FW_AUTOLAND_APPROACH_DIRECTION_LEFT,
FW_AUTOLAND_APPROACH_DIRECTION_RIGHT
} fwAutolandApproachDirection_e;
typedef enum {
FW_AUTOLAND_LC_STATE_IDLE,
FW_AUTOLAND_STATE_CROSSWIND,
FW_AUTOLAND_STATE_BASE_LEG,
FW_AUTOLAND_STATE_FINAL_APPROACH,
FW_AUTOLAND_STATE_GLIDE,
FW_AUTOLAND_STATE_FLARE
} fwAutolandState_t;
typedef struct {
uint8_t enabled;
int32_t lat;
int32_t lon;
int32_t approachAltMSL;
int32_t landAltMSL;
fwAutolandApproachDirection_e approachDirection;
int16_t landApproachHeading1;
int16_t landApproachHeading2;
} navSafeHome_t;
typedef enum {
@ -58,8 +76,26 @@ typedef enum {
PG_DECLARE_ARRAY(navSafeHome_t, MAX_SAFE_HOMES, safeHomeConfig);
typedef struct navFwAutolandConfig_s
{
uint32_t approachLength;
uint16_t finalApproachPitchToThrottleMod;
uint16_t glideAltitude;
uint16_t flareAltitude;
uint8_t flarePitch;
uint16_t maxTailwind;
uint8_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 resetSafeHomes(void); // remove all safehomes
bool findNearestSafeHome(void); // Find nearest safehome
#endif // defined(USE_SAFE_HOME)
#ifndef NAV_MAX_WAYPOINTS
@ -302,7 +338,6 @@ typedef struct navConfig_s {
uint16_t minThrottleDownPitchAngle; // Automatic pitch down angle when throttle is at 0 in angle mode. Progressively applied between cruise throttle and zero throttle. [decidegrees]
uint16_t loiter_radius; // Loiter radius when executing PH on a fixed wing
uint8_t loiter_direction; // Direction of loitering center point on right wing (clockwise - as before), or center point on left wing (counterclockwise)
int8_t land_dive_angle;
uint16_t launch_velocity_thresh; // Velocity threshold for swing launch detection
uint16_t launch_accel_thresh; // Acceleration threshold for launch detection (cm/s/s)
uint16_t launch_time_thresh; // Time threshold for launch detection (ms)
@ -316,7 +351,8 @@ typedef struct navConfig_s {
uint8_t launch_climb_angle; // Target climb angle for launch (deg)
uint8_t launch_max_angle; // Max tilt angle (pitch/roll combined) to consider launch successful. Set to 180 to disable completely [deg]
bool launch_manual_throttle; // Allows launch with manual throttle control
uint8_t launch_abort_deadband; // roll/pitch stick movement deadband for launch abort
uint8_t launch_land_abort_deadband; // roll/pitch stick movement deadband for launch abort
uint8_t cruise_yaw_rate; // Max yaw rate (dps) when CRUISE MODE is enabled
bool allow_manual_thr_increase;
bool useFwNavYawControl;
uint8_t yawControlDeadband;
@ -637,6 +673,9 @@ uint8_t getActiveWpNumber(void);
*/
int32_t navigationGetHomeHeading(void);
bool isFwLandInProgess(void);
bool canFwLandCanceld(void);
/* Compatibility data */
extern navSystemStatus_t NAV_Status;

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

@ -588,13 +588,18 @@ int16_t fixedWingPitchToThrottleCorrection(int16_t pitch, timeUs_t currentTimeUs
// Apply low-pass filter to pitch angle to smooth throttle correction
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) {
pitchToThrottle *= navFwAutolandConfig()->finalApproachPitchToThrottleMod / 100.0f;
}
if (ABS(pitch - filteredPitch) > navConfig()->fw.pitch_to_throttle_thresh) {
// Unfiltered throttle correction outside of pitch deadband
return DECIDEGREES_TO_DEGREES(pitch) * currentBatteryProfile->nav.fw.pitch_to_throttle;
return DECIDEGREES_TO_DEGREES(pitch) * pitchToThrottle;
}
else {
// Filtered throttle correction inside of pitch deadband
return DECIDEGREES_TO_DEGREES(filteredPitch) * currentBatteryProfile->nav.fw.pitch_to_throttle;
return DECIDEGREES_TO_DEGREES(filteredPitch) * pitchToThrottle;
}
}
@ -620,7 +625,7 @@ void applyFixedWingPitchRollThrottleController(navigationFSMStateFlags_t navStat
int16_t throttleCorrection = fixedWingPitchToThrottleCorrection(pitchCorrection, currentTimeUs);
#ifdef NAV_FIXED_WING_LANDING
if (navStateFlags & NAV_CTL_LAND) {
if ((navStateFlags & NAV_CTL_LAND) || isFwLandInProgess()) {
// During LAND we do not allow to raise THROTTLE when nose is up to reduce speed
throttleCorrection = constrain(throttleCorrection, minThrottleCorrection, 0);
} else {
@ -654,24 +659,18 @@ void applyFixedWingPitchRollThrottleController(navigationFSMStateFlags_t navStat
}
#ifdef NAV_FIXED_WING_LANDING
/*
* Then altitude is below landing slowdown min. altitude, enable final approach procedure
* TODO refactor conditions in this metod if logic is proven to be correct
*/
if (navStateFlags & NAV_CTL_LAND || STATE(LANDING_DETECTED)) {
int32_t finalAltitude = navConfig()->general.land_slowdown_minalt + posControl.rthState.homeTmpWaypoint.z;
if ((posControl.flags.estAltStatus >= EST_USABLE && navGetCurrentActualPositionAndVelocity()->pos.z <= finalAltitude) ||
(posControl.flags.estAglStatus == EST_TRUSTED && posControl.actualState.agl.pos.z <= navConfig()->general.land_slowdown_minalt)) {
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);
// Stabilize ROLL axis on 0 degrees banking regardless of loiter radius and position
rcCommand[ROLL] = 0;
if (posControl.navState == NAV_STATE_FW_LANDING_GLIDE) {
rcCommand[PITCH] = pidAngleToRcCommand(DEGREES_TO_DECIDEGREES(navFwAutolandConfig()->glidePitch), pidProfile()->max_angle_inclination[FD_PITCH]);
}
// Stabilize PITCH angle into shallow dive as specified by the nav_fw_land_dive_angle setting (default value is 2 - defined in navigation.c).
rcCommand[PITCH] = pidAngleToRcCommand(DEGREES_TO_DECIDEGREES(navConfig()->fw.land_dive_angle), 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]);
}
}
#endif

445
src/main/navigation/navigation_fw_autoland.c Normal file
View file

@ -0,0 +1,445 @@
/*
* This file is part of INAV.
*
* INAV is free software. You can redistribute this software
* and/or modify this software under the terms of the
* GNU General Public License as published by the Free Software
* Foundation, either version 3 of the License, or (at your option)
* any later version.
*
* INAV is distributed in the hope that they will be
* useful, but WITHOUT ANY WARRANTY; without even the implied
* warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
* See the GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this software.
*
* If not, see <http://www.gnu.org/licenses/>.
*/
#include <stdbool.h>
#include <stdint.h>
#include <string.h>
#include <math.h>
#include "platform.h"
#if defined(USE_SAFE_HOME) && defined(USE_FW_AUTOLAND)
#include <navigation/navigation_fw_autoland.h>
#include "common/utils.h"
#include "flight/wind_estimator.h"
#include "fc/rc_controls.h"
#include "fc/settings.h"
#include "rx/rx.h"
#include "flight/mixer.h"
#include "navigation/navigation_private.h"
#include "config/parameter_group_ids.h"
#define LOITER_MIN_TIME 30000000 // usec (30 sec)
#define TARGET_ALT_TOLERANCE 150
#define LAND_WP_TURN 0
#define LAND_WP_FINAL_APPROACH 1
#define LAND_WP_LAND 2
typedef enum {
FW_AUTOLAND_EVENT_NONE,
FW_AUTOLAND_EVENT_SUCCSESS,
FW_AUTOLAND_EVENT_ABORT,
FW_AUTOLAND_EVENT_COUNT
} fwAutolandEvent_t;
typedef struct fixedWingAutolandStateDescriptor_s {
fwAutolandEvent_t (*onEntry)(timeUs_t currentTimeUs);
fwAutolandState_t onEvent[FW_AUTOLAND_EVENT_COUNT];
fwAutolandMessageState_t message;
} fixedWingAutolandStateDescriptor_t;
typedef struct fixedWingAutolandData_s {
timeUs_t currentStateTimeUs;
fwAutolandState_t currentState;
timeUs_t loiterStartTime;
fpVector3_t waypoint[3];
fpVector3_t flareWp;
uint32_t finalApproachLength;
} fixedWingAutolandData_t;
static EXTENDED_FASTRAM fixedWingAutolandData_t fwAutoland;
static fwAutolandEvent_t fwAutolandState_ABOVE_LOITER_ALT(timeUs_t currentTimeUs);
static fwAutolandEvent_t fwAutolandState_LOITER(timeUs_t currentTimeUs);
static fwAutolandEvent_t fwAutolandState_DOWNWIND(timeUs_t currentTimeUs);
static fwAutolandEvent_t fwAutolandState_BASE(timeUs_t currentTimeUs);
static fwAutolandEvent_t fwAutolandState_FINAL_APPROACH(timeUs_t currentTimeUs);
static fwAutolandEvent_t fwAutolandState_GLIDE(timeUs_t currentTimeUs);
static fwAutolandEvent_t fwAutolandState_FLARE(timeUs_t currentTimeUs);
static fwAutolandEvent_t fwAutolandState_ABORT(timeUs_t currentTimeUs);
static const fixedWingAutolandStateDescriptor_t autolandStateMachine[FW_AUTOLAND_STATE_COUNT] = {
[FW_AUTOLAND_STATE_ABOVE_LOITER_ALT] = {
.onEntry = fwAutolandState_ABOVE_LOITER_ALT,
.onEvent = {
[FW_AUTOLAND_EVENT_SUCCSESS] = FW_AUTOLAND_STATE_LOITER,
[FW_AUTOLAND_EVENT_ABORT] = FW_AUTOLAND_STATE_ABORT,
},
.message = FW_AUTOLAND_MESSAGE_ABOVE_LOITER_ALT
},
[FW_AUTOLAND_STATE_LOITER] = {
.onEntry = fwAutolandState_LOITER,
.onEvent = {
[FW_AUTOLAND_EVENT_SUCCSESS] = FW_AUTOLAND_STATE_DOWNWIND,
[FW_AUTOLAND_EVENT_ABORT] = FW_AUTOLAND_STATE_ABORT,
},
.message = FW_AUTOLAND_MESSAGE_LOITER
},
[FW_AUTOLAND_STATE_DOWNWIND] = {
.onEntry = fwAutolandState_DOWNWIND,
.onEvent = {
[FW_AUTOLAND_EVENT_SUCCSESS] = FW_AUTOLAND_STATE_BASE,
[FW_AUTOLAND_EVENT_ABORT] = FW_AUTOLAND_STATE_ABORT,
},
.message = FW_AUTOLAND_MESSAGE_LOITER
},
[FW_AUTOLAND_STATE_BASE] = {
.onEntry = fwAutolandState_BASE,
.onEvent = {
[FW_AUTOLAND_EVENT_SUCCSESS] = FW_AUTOLAND_STATE_FINAL_APPROACH,
[FW_AUTOLAND_EVENT_ABORT] = FW_AUTOLAND_STATE_ABORT,
},
.message = FW_AUTOLAND_MESSAGE_LOITER
},
[FW_AUTOLAND_STATE_FINAL_APPROACH] = {
.onEntry = fwAutolandState_FINAL_APPROACH,
.onEvent = {
[FW_AUTOLAND_EVENT_SUCCSESS] = FW_AUTOLAND_STATE_GLIDE,
[FW_AUTOLAND_EVENT_ABORT] = FW_AUTOLAND_STATE_ABORT,
},
.message = FW_AUTOLAND_MESSAGE_LOITER
},
[FW_AUTOLAND_STATE_GLIDE] = {
.onEntry = fwAutolandState_GLIDE,
.onEvent = {
[FW_AUTOLAND_EVENT_SUCCSESS] = FW_AUTOLAND_STATE_FLARE,
[FW_AUTOLAND_EVENT_ABORT] = FW_AUTOLAND_STATE_ABORT,
},
.message = FW_AUTOLAND_MESSAGE_LOITER
},
[FW_AUTOLAND_STATE_FLARE] = {
.onEntry = fwAutolandState_FLARE,
.onEvent = {
[FW_AUTOLAND_EVENT_SUCCSESS] = FW_AUTOLAND_STATE_LANDED,
[FW_AUTOLAND_EVENT_ABORT] = FW_AUTOLAND_STATE_ABORT,
},
.message = FW_AUTOLAND_MESSAGE_LOITER
}
};
PG_REGISTER_WITH_RESET_TEMPLATE(navFwAutolandConfig_t, navFwAutolandConfig, PG_FW_AUTOLAND_CONFIG, 0);
PG_RESET_TEMPLATE(navFwAutolandConfig_t, navFwAutolandConfig,
.approachAngle = SETTING_NAV_FW_LAND_APPROACH_ANGLE_DEFAULT,
.glideAltitude = SETTING_NAV_FW_LAND_GLIDE_ALT_DEFAULT,
.flareAltitude = SETTING_NAV_FW_LAND_FLARE_ALT_DEFAULT,
.flarePitch = SETTING_NAV_FW_LAND_FLARE_PITCH_DEFAULT,
.maxTailwind = SETTING_NAV_FW_LAND_MAX_TAILWIND_DEFAULT,
);
static uint32_t getFinalApproachHeading(int32_t approachHeading, int32_t windAngle, int32_t *windAngleRelToRunway)
{
if (approachHeading == 0) {
return 0;
}
*windAngleRelToRunway = wrap_36000(windAngle - ABS(approachHeading));
if (*windAngleRelToRunway > 27000 || *windAngleRelToRunway < 9000) {
return approachHeading;
}
if (approachHeading > 0) {
return wrap_36000(approachHeading + 18000);
}
return 0;
}
static int32_t calcApproachLength(int32_t finalApproachAlt, int16_t glidePitch)
{
return finalApproachAlt * 1.0f / sin_approx(DEGREES_TO_RADIANS(glidePitch)) * sin_approx(DEGREES_TO_RADIANS(90 - glidePitch));
}
static void setLandWaypoint(const fpVector3_t *pos, const fpVector3_t *nextWpPos)
{
if (posControl.activeWaypoint.pos.x == 0 && posControl.activeWaypoint.pos.y == 0) {
posControl.activeWaypoint.bearing = calculateBearingToDestination(pos);
} else {
posControl.activeWaypoint.bearing = calculateBearingBetweenLocalPositions(&posControl.activeWaypoint.pos, pos);
}
posControl.activeWaypoint.pos = *pos;
setDesiredPosition(&posControl.activeWaypoint.pos, posControl.activeWaypoint.bearing, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING);
if (navConfig()->fw.wp_turn_smoothing && nextWpPos != NULL) {
int32_t bearingToNextWp = calculateBearingBetweenLocalPositions(pos, nextWpPos);
posControl.activeWaypoint.nextTurnAngle = wrap_18000(bearingToNextWp - posControl.activeWaypoint.bearing);
} else {
posControl.activeWaypoint.nextTurnAngle = -1;
}
posControl.wpInitialDistance = calculateDistanceToDestination(&posControl.activeWaypoint.pos);
posControl.wpInitialAltitude = posControl.actualState.abs.pos.z;
posControl.wpAltitudeReached = false;
}
static void checkLandWpAndUpdateZ(void)
{
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);
}
static bool isLandWpReached(const fpVector3_t * waypointPos, const int32_t * waypointBearing)
{
posControl.wpDistance = calculateDistanceToDestination(waypointPos);
/*
if (getFwAutolandState() == FW_AUTOLAND_STATE_BASE) {
return calculateDistanceToDestination(&fwAutoland.waypoint[LAND_WP_LAND]) <= fwAutoland.finalApproachLength - navConfig()->fw.loiter_radius;
}
*/
if (posControl.flags.wpTurnSmoothingActive) {
posControl.flags.wpTurnSmoothingActive = false;
return true;
}
uint16_t relativeBearing = posControl.flags.wpTurnSmoothingActive ? 6000 : 10000;
if (ABS(wrap_18000(calculateBearingToDestination(waypointPos) - *waypointBearing)) > relativeBearing) {
return true;
}
return posControl.wpDistance <= (navConfig()->general.waypoint_radius);
}
static fwAutolandEvent_t fwAutolandState_ABOVE_LOITER_ALT(timeUs_t currentTimeUs)
{
UNUSED(currentTimeUs);
int32_t approachAltAbs = safeHomeConfig(safehome_index)->approachAltMSL - GPS_home.alt;
if (ABS(getEstimatedActualPosition(Z) - approachAltAbs) < TARGET_ALT_TOLERANCE) {
return FW_AUTOLAND_EVENT_SUCCSESS;
} else {
fpVector3_t tmpPoint = posControl.rthState.homePosition.pos;
tmpPoint.z = approachAltAbs;
setDesiredPosition(&tmpPoint, 0, NAV_POS_UPDATE_Z);
}
return FW_AUTOLAND_EVENT_NONE;
}
static fwAutolandEvent_t fwAutolandState_LOITER(timeUs_t currentTimeUs)
{
#ifndef USE_WIND_ESTIMATOR
return FW_AUTOLAND_EVENT_ABORT;
#endif
int32_t landAltAbs = safeHomeConfig(safehome_index)->landAltMSL - GPS_home.alt;
int32_t approachAltAbs = safeHomeConfig(safehome_index)->approachAltMSL + landAltAbs - GPS_home.alt;
if (fwAutoland.loiterStartTime == 0) {
fwAutoland.loiterStartTime = currentTimeUs;
} else if (micros() - fwAutoland.loiterStartTime > LOITER_MIN_TIME) {
if (isEstimatedWindSpeedValid()) {
uint16_t windAngle = 0;
uint32_t landingDirection = 0;
float windSpeed = getEstimatedHorizontalWindSpeed(&windAngle);
windAngle = wrap_36000(windAngle + 18000);
int32_t windAngel1 = 0, windAngel2 = 0;
int32_t heading1 = getFinalApproachHeading(DEGREES_TO_CENTIDEGREES(safeHomeConfig(safehome_index)->landApproachHeading1), windAngle, &windAngel1);
int32_t heading2 = getFinalApproachHeading(DEGREES_TO_CENTIDEGREES(safeHomeConfig(safehome_index)->landApproachHeading2), windAngle, &windAngel2);
if (heading1 == 0 && heading2 == 0 && windSpeed < navFwAutolandConfig()->maxTailwind) {
heading1 = safeHomeConfig(safehome_index)->landApproachHeading1;
heading2 = safeHomeConfig(safehome_index)->landApproachHeading2;
}
if (heading1 == 0 && heading2 > 0) {
landingDirection = heading2;
} else if (heading1 > 0 && heading2 == 0) {
landingDirection = heading1;
} else {
if (windAngel1 < windAngel2) {
landingDirection = heading1;
}
else {
landingDirection = heading2;
}
}
if (landingDirection != 0) {
fpVector3_t tmpPos;
int32_t finalApproachAlt = approachAltAbs / 3 * 2;
fwAutoland.finalApproachLength = calcApproachLength(finalApproachAlt, navFwAutolandConfig()->approachAngle);
int32_t dir = 0;
if (safeHomeConfig(safehome_index)->approachDirection == FW_AUTOLAND_APPROACH_DIRECTION_LEFT) {
dir = wrap_36000(landingDirection - 9000);
} else {
dir = wrap_36000(landingDirection + 9000);
}
tmpPos = posControl.rthState.homePosition.pos;
tmpPos.z = landAltAbs;
fwAutoland.waypoint[LAND_WP_LAND] = tmpPos;
fpVector3_t tmp2;
calculateFarAwayPos(&tmp2, &posControl.rthState.homePosition.pos, wrap_36000(landingDirection + 18000), fwAutoland.finalApproachLength);
calculateFarAwayPos(&tmpPos, &tmp2, dir, navConfig()->fw.loiter_radius);
tmp2.z = finalApproachAlt;
fwAutoland.waypoint[LAND_WP_FINAL_APPROACH] = tmp2;
calculateFarAwayPos(&tmpPos, &fwAutoland.waypoint[LAND_WP_FINAL_APPROACH], dir, (fwAutoland.finalApproachLength / 2));
tmpPos.z = finalApproachAlt;
fwAutoland.waypoint[LAND_WP_TURN] = tmpPos;
setLandWaypoint(&fwAutoland.waypoint[LAND_WP_TURN], &fwAutoland.waypoint[LAND_WP_FINAL_APPROACH]);
return FW_AUTOLAND_EVENT_SUCCSESS;
} else {
return FW_AUTOLAND_EVENT_ABORT;
}
} else {
fwAutoland.loiterStartTime = currentTimeUs;
}
}
fpVector3_t tmpPoint = posControl.rthState.homePosition.pos;
tmpPoint.z = approachAltAbs;
setDesiredPosition(&tmpPoint, posControl.rthState.homePosition.heading, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING);
return FW_AUTOLAND_EVENT_NONE;
}
static fwAutolandEvent_t fwAutolandState_DOWNWIND(timeUs_t currentTimeUs)
{
UNUSED(currentTimeUs);
if (isLandWpReached(&fwAutoland.waypoint[LAND_WP_TURN], &posControl.activeWaypoint.bearing)) {
setLandWaypoint(&fwAutoland.waypoint[LAND_WP_FINAL_APPROACH], &fwAutoland.waypoint[LAND_WP_LAND]);
return FW_AUTOLAND_EVENT_SUCCSESS;
}
checkLandWpAndUpdateZ();
return FW_AUTOLAND_EVENT_NONE;
}
static fwAutolandEvent_t fwAutolandState_BASE(timeUs_t currentTimeUs)
{
if (isLandWpReached(&fwAutoland.waypoint[LAND_WP_FINAL_APPROACH], &posControl.activeWaypoint.bearing)) {
setLandWaypoint(&fwAutoland.waypoint[LAND_WP_LAND], NULL);
/*
resetGCSFlags();
resetPositionController();
resetAltitudeController(false);
setupAltitudeController();
posControl.cruise.course = calculateBearingToDestination(&fwAutoland.waypoint[LAND_WP_LAND]);
*/
return FW_AUTOLAND_EVENT_SUCCSESS;
}
checkLandWpAndUpdateZ();
return FW_AUTOLAND_EVENT_NONE;
}
static fwAutolandEvent_t fwAutolandState_FINAL_APPROACH(timeUs_t currentTimeUs)
{
if (getEstimatedActualPosition(Z) <= safeHomeConfig(safehome_index)->landAltMSL + navFwAutolandConfig()->glideAltitude - GPS_home.alt) {
setLandWaypoint(&fwAutoland.waypoint[LAND_WP_LAND], NULL);
return FW_AUTOLAND_EVENT_SUCCSESS;
}
posControl.wpDistance = calculateDistanceToDestination(&fwAutoland.waypoint[LAND_WP_LAND]);
checkLandWpAndUpdateZ();
return FW_AUTOLAND_EVENT_NONE;
}
static fwAutolandEvent_t fwAutolandState_GLIDE(timeUs_t currentTimeUs)
{
if (getEstimatedActualPosition(Z) <= safeHomeConfig(safehome_index)->landAltMSL + navFwAutolandConfig()->flareAltitude - GPS_home.alt) {
return FW_AUTOLAND_EVENT_SUCCSESS;
}
posControl.wpDistance = calculateDistanceToDestination(&fwAutoland.waypoint[LAND_WP_LAND]);
checkLandWpAndUpdateZ();
return FW_AUTOLAND_EVENT_NONE;
}
static fwAutolandEvent_t fwAutolandState_FLARE(timeUs_t currentTimeUs)
{
rcCommand[THROTTLE] = getThrottleIdleValue();
ENABLE_STATE(NAV_MOTOR_STOP_OR_IDLE);
rxOverrideRcChannel(PITCH, -navFwAutolandConfig()->flarePitch);
if (isLandingDetected()) {
rxOverrideRcChannel(PITCH, -1);
return FW_AUTOLAND_EVENT_SUCCSESS;
}
return FW_AUTOLAND_EVENT_NONE;
}
static fwAutolandEvent_t fwAutolandState_ABORT(timeUs_t currentTimeUs)
{
UNUSED(currentTimeUs);
return FW_AUTOLAND_EVENT_NONE;
}
static void setCurrentState(fwAutolandState_t nextState, timeUs_t currentTimeUs)
{
fwAutoland.currentState = nextState;
fwAutoland.currentStateTimeUs = currentTimeUs;
}
// Public methods
fwAutolandState_t getFwAutolandState(void)
{
return fwAutoland.currentState;
}
bool isFwAutolandActive(void)
{
return fwAutoland.currentState >= FW_AUTOLAND_STATE_LOITER;
}
bool allowFwAutoland(void)
{
return safehome_index >= 0 && (safeHomeConfig(safehome_index)->landApproachHeading1 != 0 || safeHomeConfig(safehome_index)->landApproachHeading2 != 0);
}
void resetFwAutolandController(timeUs_t currentTimeUs)
{
setCurrentState(FW_AUTOLAND_STATE_ABOVE_LOITER_ALT, currentTimeUs);
rxOverrideRcChannel(PITCH, -1);
fwAutoland.loiterStartTime = 0;
}
void applyFixedWingAutolandController(timeUs_t currentTimeUs)
{
while (autolandStateMachine[fwAutoland.currentState].onEntry) {
fwAutolandEvent_t newEvent = autolandStateMachine[fwAutoland.currentState].onEntry(currentTimeUs);
if (newEvent == FW_AUTOLAND_EVENT_NONE) {
break;
}
setCurrentState(autolandStateMachine[fwAutoland.currentState].onEvent[newEvent], currentTimeUs);
}
}
#endif

66
src/main/navigation/navigation_fw_autoland.h Normal file
View file

@ -0,0 +1,66 @@
/*
* This file is part of INAV.
*
* INAV is free software. You can redistribute this software
* and/or modify this software under the terms of the
* GNU General Public License as published by the Free Software
* Foundation, either version 3 of the License, or (at your option)
* any later version.
*
* INAV is distributed in the hope that they will be
* useful, but WITHOUT ANY WARRANTY; without even the implied
* warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
* See the GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this software.
*
* If not, see <http://www.gnu.org/licenses/>.
*/
#pragma once
#include "drivers/time.h"
#include "config/parameter_group.h"
typedef enum {
FW_AUTOLAND_APPROACH_DIRECTION_LEFT,
FW_AUTOLAND_APPROACH_DIRECTION_RIGHT
} fwAutolandApproachDirection_e;
typedef struct navFwAutolandConfig_s
{
uint8_t approachAngle;
uint16_t glideAltitude;
uint16_t flareAltitude;
uint16_t flarePitch;
uint16_t maxTailwind;
} navFwAutolandConfig_t;
PG_DECLARE(navFwAutolandConfig_t, navFwAutolandConfig);
typedef enum {
FW_AUTOLAND_STATE_ABOVE_LOITER_ALT,
FW_AUTOLAND_STATE_LOITER,
FW_AUTOLAND_STATE_DOWNWIND,
FW_AUTOLAND_STATE_BASE,
FW_AUTOLAND_STATE_FINAL_APPROACH,
FW_AUTOLAND_STATE_GLIDE,
FW_AUTOLAND_STATE_FLARE,
FW_AUTOLAND_STATE_SWITCH_TO_EMERGENCY_LANDING,
FW_AUTOLAND_STATE_ABORT,
FW_AUTOLAND_STATE_LANDED,
FW_AUTOLAND_STATE_COUNT
} fwAutolandState_t;
typedef enum {
FW_AUTOLAND_MESSAGE_ABOVE_LOITER_ALT,
FW_AUTOLAND_MESSAGE_LOITER,
FW_AUTOLAND_MESSAGE_ABORT,
} fwAutolandMessageState_t;
fwAutolandState_t getFwAutolandState(void);
void resetFwAutolandController(timeUs_t currentTimeUs);
bool allowFwAutoland(void);
bool isFwAutolandActive(void);
void applyFixedWingAutolandController(timeUs_t currentTimeUs);

6
src/main/navigation/navigation_fw_launch.c
View file

@ -251,7 +251,7 @@ static inline bool isLaunchMaxAltitudeReached(void)
static inline bool areSticksMoved(timeMs_t initialTime, timeUs_t currentTimeUs)
{
return (initialTime + currentStateElapsedMs(currentTimeUs)) >= navConfig()->fw.launch_min_time &&
isRollPitchStickDeflected(navConfig()->fw.launch_abort_deadband);
isRollPitchStickDeflected(navConfig()->fw.launch_land_abort_deadband);
}
static inline bool isProbablyNotFlying(void)
@ -420,7 +420,7 @@ static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_IN_PROGRESS(timeUs_t
if (throttleStickIsLow()) {
fwLaunch.currentStateTimeUs = currentTimeUs;
fwLaunch.pitchAngle = 0;
if (isRollPitchStickDeflected(navConfig()->fw.launch_abort_deadband)) {
if (isRollPitchStickDeflected(navConfig()->fw.launch_land_abort_deadband)) {
return FW_LAUNCH_EVENT_ABORT;
}
} else {
@ -454,7 +454,7 @@ static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_FINISH(timeUs_t curr
const timeMs_t elapsedTimeMs = currentStateElapsedMs(currentTimeUs);
const timeMs_t endTimeMs = navConfig()->fw.launch_end_time;
if (elapsedTimeMs > endTimeMs || isRollPitchStickDeflected(navConfig()->fw.launch_abort_deadband)) {
if (elapsedTimeMs > endTimeMs || isRollPitchStickDeflected(navConfig()->fw.launch_land_abort_deadband)) {
return FW_LAUNCH_EVENT_SUCCESS; // End launch go to FW_LAUNCH_STATE_FLYING state
}
else {

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

@ -158,6 +158,7 @@ typedef enum {
NAV_FSM_EVENT_SWITCH_TO_CRUISE,
NAV_FSM_EVENT_SWITCH_TO_COURSE_ADJ,
NAV_FSM_EVENT_SWITCH_TO_MIXERAT,
NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_FW_LANDING,
NAV_FSM_EVENT_STATE_SPECIFIC_1, // State-specific event
NAV_FSM_EVENT_STATE_SPECIFIC_2, // State-specific event
@ -173,6 +174,7 @@ typedef enum {
NAV_FSM_EVENT_SWITCH_TO_RTH_HOVER_ABOVE_HOME = NAV_FSM_EVENT_STATE_SPECIFIC_4,
NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_RTH_TRACKBACK = NAV_FSM_EVENT_STATE_SPECIFIC_5,
NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_RTH_INITIALIZE = NAV_FSM_EVENT_STATE_SPECIFIC_6,
NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_FW_LANDING_ABORT = NAV_FSM_EVENT_STATE_SPECIFIC_6,
NAV_FSM_EVENT_COUNT,
} navigationFSMEvent_t;
@ -233,6 +235,12 @@ typedef enum {
NAV_PERSISTENT_ID_MIXERAT_INITIALIZE = 39,
NAV_PERSISTENT_ID_MIXERAT_IN_PROGRESS = 40,
NAV_PERSISTENT_ID_MIXERAT_ABORT = 41,
NAV_PERSISTENT_ID_FW_LANDING_CLIMB_TO_LOITER = 42,
NAV_PERSISTENT_ID_FW_LANDING_LOITER = 43,
NAV_PERSISTENT_ID_FW_LANDING_APPROACH = 44,
NAV_PERSISTENT_ID_FW_LANDING_GLIDE = 45,
NAV_PERSISTENT_ID_FW_LANDING_FLARE = 46,
NAV_PERSISTENT_ID_FW_LANDING_ABORT = 47,
} navigationPersistentId_e;
typedef enum {
@ -280,6 +288,13 @@ typedef enum {
NAV_STATE_CRUISE_IN_PROGRESS,
NAV_STATE_CRUISE_ADJUSTING,
NAV_STATE_FW_LANDING_CLIMB_TO_LOITER,
NAV_STATE_FW_LANDING_LOITER,
NAV_STATE_FW_LANDING_APPROACH,
NAV_STATE_FW_LANDING_GLIDE,
NAV_STATE_FW_LANDING_FLARE,
NAV_STATE_FW_LANDING_ABORT,
NAV_STATE_MIXERAT_INITIALIZE,
NAV_STATE_MIXERAT_IN_PROGRESS,
NAV_STATE_MIXERAT_ABORT,
@ -368,6 +383,12 @@ 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;
@ -434,6 +455,17 @@ typedef struct {
int8_t activeRthTBPointIndex;
int8_t rthTBWrapAroundCounter; // stores trackpoint array overwrite index position
/* Fixedwing autoland */
timeUs_t fwLandLoiterStartTime;
fpVector3_t fwLandWaypoint[FW_AUTOLAND_WP_COUNT];
int32_t fwLandingDirection;
int32_t fwLandAproachAltAgl;
int32_t fwLandAltAgl;
bool fwLandWpReached;
fwAutolandWayppoints_t fwLandCurrentWp;
bool fwLandAborted;
fwAutolandState_t fwLandState;
/* Internals & statistics */
int16_t rcAdjustment[4];
float totalTripDistance;

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

@ -810,6 +810,10 @@ static int logicConditionGetFlightOperandValue(int operand) {
return rangefinderGetLatestRawAltitude();
break;
case LOGIC_CONDITION_OPERAND_FLIGHT_FW_LAND_STATE:
return posControl.fwLandState;
break;
default:
return 0;
break;

1
src/main/programming/logic_condition.h
View file

@ -139,6 +139,7 @@ typedef enum {
LOGIC_CONDITION_OPERAND_FLIGHT_ACTIVE_MIXER_PROFILE, //int // 38
LOGIC_CONDITION_OPERAND_FLIGHT_MIXER_TRANSITION_ACTIVE, //0,1 // 39
LOGIC_CONDITION_OPERAND_FLIGHT_ATTITUDE_YAW, // deg // 40
LOGIC_CONDITION_OPERAND_FLIGHT_FW_LAND_STATE, // 41
} logicFlightOperands_e;
typedef enum {

7
src/main/target/SITL/sim/realFlight.c
View file

@ -58,9 +58,10 @@
#define RF_PORT 18083
#define RF_MAX_CHANNEL_COUNT 12
// RealFlight scenerys doesn't represent real landscapes, so fake some nice coords: Area 51 ;)
#define FAKE_LAT 37.277127f
#define FAKE_LON -115.799669f
// RealFlight scenerys doesn't represent real landscapes, so fake some nice coords
#define FAKE_LAT 0.0f
#define FAKE_LON 0.0f
static uint8_t pwmMapping[RF_MAX_PWM_OUTS];
static uint8_t mappingCount;

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

@ -142,6 +142,7 @@
#define NAV_FIXED_WING_LANDING
#define USE_SAFE_HOME
#define USE_FW_AUTOLAND
#define USE_AUTOTUNE_FIXED_WING
#define USE_LOG
#define USE_STATS