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

Reenable old landing procedure as fallback

Browse source
This commit is contained in:
Scavanger 2024-02-12 10:06:44 -03:00
parent 919e934e12
commit c809801e4f
8 changed files with 47 additions and 12 deletions
Download patch file Download diff file Expand all files Collapse all files

8
docs/Fixed Wing Landing.md
View file

@ -18,14 +18,18 @@ Supported are landings at safehome after "Return to Home" or at a defined LAND w
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`.
To activate the automatic landing, the parameter `nav_rth_allow_landing` must be set to `ALWAYS` or `FAILSAFE`.
> [!WARNING]
> If landing is activated and no parameters are set for the landing site (Safehome and/or landing waypoint), the old landing procedure (circling until close to the ground, then hovering out) is performed.
> This is probably not what you want.
The following graphics illustrate the process:
![Approach Drawing Up](/docs/assets/images/Approach-Drawing-Up.png "Approach Drawing Up")
![Approach Drawing Side](/docs/assets/images/Approach-Drawing-Side.png "Approach Drawing Side")
To activate the automatic landing, the parameter `nav_rth_allow_landing` must be set to `ALWAYS` or `FAILSAFE`.
## Landing site parameters
### The following parameters are set for each landing site (Safefome/LAND waypoint):

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) || isFwLandInProgess()) {
if (STATE(ALTITUDE_CONTROL)) {
updateLandingStatus(US2MS(currentTimeUs));
}

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

@ -2836,6 +2836,12 @@ 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
min: -20
max: 20
- name: nav_fw_launch_velocity
description: "Forward velocity threshold for swing-launch detection [cm/s]"
default_value: 300

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

@ -212,6 +212,9 @@ 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)
@ -1135,7 +1138,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
.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,
.stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | 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,
@ -1698,7 +1701,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_LANDING(navigationF
return NAV_FSM_EVENT_SWITCH_TO_MIXERAT;
}
if (STATE(FIXED_WING_LEGACY)) {
if (STATE(AIRPLANE)) {
int8_t missionIdx = -1, shIdx = -1, missionFwLandConfigStartIdx = 8;
#ifdef USE_MULTI_MISSION
missionIdx = posControl.loadedMultiMissionIndex - 1;
@ -1723,8 +1726,6 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_LANDING(navigationF
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;
}
}

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

@ -347,6 +347,7 @@ 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)

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

@ -624,7 +624,12 @@ void applyFixedWingPitchRollThrottleController(navigationFSMStateFlags_t navStat
rcCommand[PITCH] = -pidAngleToRcCommand(pitchCorrection, pidProfile()->max_angle_inclination[FD_PITCH]);
int16_t throttleCorrection = fixedWingPitchToThrottleCorrection(pitchCorrection, currentTimeUs);
if (navStateFlags & NAV_CTL_LAND) {
// During LAND we do not allow to raise THROTTLE when nose is up to reduce speed
throttleCorrection = constrain(throttleCorrection, minThrottleCorrection, 0);
} else {
throttleCorrection = constrain(throttleCorrection, minThrottleCorrection, maxThrottleCorrection);
}
// Speed controller - only apply in POS mode when NOT NAV_CTL_LAND
if ((navStateFlags & NAV_CTL_POS) && !(navStateFlags & NAV_CTL_LAND)) {
@ -649,6 +654,7 @@ void applyFixedWingPitchRollThrottleController(navigationFSMStateFlags_t navStat
rcCommand[THROTTLE] = setDesiredThrottle(correctedThrottleValue, false);
}
// Advanced autoland
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();
@ -662,6 +668,24 @@ void applyFixedWingPitchRollThrottleController(navigationFSMStateFlags_t navStat
rcCommand[PITCH] = pidAngleToRcCommand(-DEGREES_TO_DECIDEGREES(navFwAutolandConfig()->flarePitch), pidProfile()->max_angle_inclination[FD_PITCH]);
}
}
// "Traditional" landing as fallback option
if (navStateFlags & NAV_CTL_LAND) {
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)) {
// Set motor to min. throttle and stop it when MOTOR_STOP feature is enabled
ENABLE_STATE(NAV_MOTOR_STOP_OR_IDLE);
// Stabilize ROLL axis on 0 degrees banking regardless of loiter radius and position
rcCommand[ROLL] = 0;
// 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]);
}
}
}
bool isFixedWingAutoThrottleManuallyIncreased(void)

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

@ -744,7 +744,7 @@ static int logicConditionGetFlightOperandValue(int operand) {
break;
case LOGIC_CONDITION_OPERAND_FLIGHT_IS_LANDING: // 0/1
return (navGetCurrentStateFlags() & NAV_CTL_LAND) ? 1 : 0;
return ((navGetCurrentStateFlags() & NAV_CTL_LAND) || isFwLandInProgess()) ? 1 : 0;
break;
case LOGIC_CONDITION_OPERAND_FLIGHT_IS_FAILSAFE: // 0/1

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

@ -141,7 +141,6 @@
#define USE_POWER_LIMITS
#define USE_SAFE_HOME
#define USE_FW_AUTOLAND
#define USE_AUTOTUNE_FIXED_WING
#define USE_LOG
#define USE_STATS