mirror of
https://github.com/iNavFlight/inav.git
synced 2025-07-26 01:35:35 +03:00
Merge remote-tracking branch 'upstream/master' into abo_fw_alt_vel_control
This commit is contained in:
breadoven
commit
512604378e
152 changed files with 4165 additions and 1374 deletions
|
|
@ -65,6 +65,7 @@
|
|||
#include "sensors/boardalignment.h"
|
||||
#include "sensors/battery.h"
|
||||
#include "sensors/gyro.h"
|
||||
#include "sensors/diagnostics.h"
|
||||
|
||||
#include "programming/global_variables.h"
|
||||
#include "sensors/rangefinder.h"
|
||||
|
|
@ -174,7 +175,7 @@ PG_RESET_TEMPLATE(navConfig_t, navConfig,
|
|||
.land_detect_sensitivity = SETTING_NAV_LAND_DETECT_SENSITIVITY_DEFAULT, // Changes sensitivity of landing detection
|
||||
.auto_disarm_delay = SETTING_NAV_AUTO_DISARM_DELAY_DEFAULT, // 2000 ms - time delay to disarm when auto disarm after landing enabled
|
||||
.rth_linear_descent_start_distance = SETTING_NAV_RTH_LINEAR_DESCENT_START_DISTANCE_DEFAULT,
|
||||
.cruise_yaw_rate = SETTING_NAV_CRUISE_YAW_RATE_DEFAULT, // 20dps
|
||||
.cruise_yaw_rate = SETTING_NAV_CRUISE_YAW_RATE_DEFAULT, // 20dps
|
||||
.rth_fs_landing_delay = SETTING_NAV_RTH_FS_LANDING_DELAY_DEFAULT, // Delay before landing in FS. 0 = immedate landing
|
||||
},
|
||||
|
||||
|
|
@ -203,45 +204,45 @@ PG_RESET_TEMPLATE(navConfig_t, navConfig,
|
|||
|
||||
// Fixed wing
|
||||
.fw = {
|
||||
.max_bank_angle = SETTING_NAV_FW_BANK_ANGLE_DEFAULT, // degrees
|
||||
.max_auto_climb_rate = SETTING_NAV_FW_AUTO_CLIMB_RATE_DEFAULT, // 5 m/s
|
||||
.max_manual_climb_rate = SETTING_NAV_FW_MANUAL_CLIMB_RATE_DEFAULT, // 3 m/s
|
||||
.max_climb_angle = SETTING_NAV_FW_CLIMB_ANGLE_DEFAULT, // degrees
|
||||
.max_dive_angle = SETTING_NAV_FW_DIVE_ANGLE_DEFAULT, // degrees
|
||||
.cruise_speed = SETTING_NAV_FW_CRUISE_SPEED_DEFAULT, // cm/s
|
||||
.max_bank_angle = SETTING_NAV_FW_BANK_ANGLE_DEFAULT, // degrees
|
||||
.max_auto_climb_rate = SETTING_NAV_FW_AUTO_CLIMB_RATE_DEFAULT, // 5 m/s
|
||||
.max_manual_climb_rate = SETTING_NAV_FW_MANUAL_CLIMB_RATE_DEFAULT, // 3 m/s
|
||||
.max_climb_angle = SETTING_NAV_FW_CLIMB_ANGLE_DEFAULT, // degrees
|
||||
.max_dive_angle = SETTING_NAV_FW_DIVE_ANGLE_DEFAULT, // degrees
|
||||
.cruise_speed = SETTING_NAV_FW_CRUISE_SPEED_DEFAULT, // cm/s
|
||||
.control_smoothness = SETTING_NAV_FW_CONTROL_SMOOTHNESS_DEFAULT,
|
||||
.pitch_to_throttle_smooth = SETTING_NAV_FW_PITCH2THR_SMOOTHING_DEFAULT,
|
||||
.pitch_to_throttle_thresh = SETTING_NAV_FW_PITCH2THR_THRESHOLD_DEFAULT,
|
||||
.minThrottleDownPitchAngle = SETTING_FW_MIN_THROTTLE_DOWN_PITCH_DEFAULT,
|
||||
.loiter_radius = SETTING_NAV_FW_LOITER_RADIUS_DEFAULT, // 75m
|
||||
.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
|
||||
.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)
|
||||
.launch_time_thresh = SETTING_NAV_FW_LAUNCH_DETECT_TIME_DEFAULT, // 40ms
|
||||
.launch_motor_timer = SETTING_NAV_FW_LAUNCH_MOTOR_DELAY_DEFAULT, // ms
|
||||
.launch_idle_motor_timer = SETTING_NAV_FW_LAUNCH_IDLE_MOTOR_DELAY_DEFAULT, // ms
|
||||
.launch_motor_spinup_time = SETTING_NAV_FW_LAUNCH_SPINUP_TIME_DEFAULT, // ms, time to gredually increase throttle from idle to launch
|
||||
.launch_end_time = SETTING_NAV_FW_LAUNCH_END_TIME_DEFAULT, // ms, time to gradually decrease/increase throttle and decrease pitch angle from launch to the current flight mode
|
||||
.launch_min_time = SETTING_NAV_FW_LAUNCH_MIN_TIME_DEFAULT, // ms, min time in launch mode
|
||||
.launch_timeout = SETTING_NAV_FW_LAUNCH_TIMEOUT_DEFAULT, // ms, timeout for launch procedure
|
||||
.launch_max_altitude = SETTING_NAV_FW_LAUNCH_MAX_ALTITUDE_DEFAULT, // cm, altitude where to consider launch ended
|
||||
.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_land_abort_deadband = SETTING_NAV_FW_LAUNCH_LAND_ABORT_DEADBAND_DEFAULT, // 100 us
|
||||
|
||||
.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)
|
||||
.launch_time_thresh = SETTING_NAV_FW_LAUNCH_DETECT_TIME_DEFAULT, // 40ms
|
||||
.launch_motor_timer = SETTING_NAV_FW_LAUNCH_MOTOR_DELAY_DEFAULT, // ms
|
||||
.launch_idle_motor_timer = SETTING_NAV_FW_LAUNCH_IDLE_MOTOR_DELAY_DEFAULT, // ms
|
||||
.launch_wiggle_wake_idle = SETTING_NAV_FW_LAUNCH_WIGGLE_TO_WAKE_IDLE_DEFAULT, // uint8_t
|
||||
.launch_motor_spinup_time = SETTING_NAV_FW_LAUNCH_SPINUP_TIME_DEFAULT, // ms, time to greaually increase throttle from idle to launch
|
||||
.launch_end_time = SETTING_NAV_FW_LAUNCH_END_TIME_DEFAULT, // ms, time to gradually decrease/increase throttle and decrease pitch angle from launch to the current flight mode
|
||||
.launch_min_time = SETTING_NAV_FW_LAUNCH_MIN_TIME_DEFAULT, // ms, min time in launch mode
|
||||
.launch_timeout = SETTING_NAV_FW_LAUNCH_TIMEOUT_DEFAULT, // ms, timeout for launch procedure
|
||||
.launch_max_altitude = SETTING_NAV_FW_LAUNCH_MAX_ALTITUDE_DEFAULT, // cm, altitude where to consider launch ended
|
||||
.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_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,
|
||||
.yawControlDeadband = SETTING_NAV_FW_YAW_DEADBAND_DEFAULT,
|
||||
.soaring_pitch_deadband = SETTING_NAV_FW_SOARING_PITCH_DEADBAND_DEFAULT,// pitch angle mode deadband when Saoring mode enabled
|
||||
.wp_tracking_accuracy = SETTING_NAV_FW_WP_TRACKING_ACCURACY_DEFAULT, // 0, improves course tracking accuracy during FW WP missions
|
||||
.wp_tracking_max_angle = SETTING_NAV_FW_WP_TRACKING_MAX_ANGLE_DEFAULT, // 60 degs
|
||||
.wp_turn_smoothing = SETTING_NAV_FW_WP_TURN_SMOOTHING_DEFAULT, // 0, smooths turns during FW WP mode missions
|
||||
.soaring_pitch_deadband = SETTING_NAV_FW_SOARING_PITCH_DEADBAND_DEFAULT, // pitch angle mode deadband when Saoring mode enabled
|
||||
.wp_tracking_accuracy = SETTING_NAV_FW_WP_TRACKING_ACCURACY_DEFAULT, // 0, improves course tracking accuracy during FW WP missions
|
||||
.wp_tracking_max_angle = SETTING_NAV_FW_WP_TRACKING_MAX_ANGLE_DEFAULT, // 60 degs
|
||||
.wp_turn_smoothing = SETTING_NAV_FW_WP_TURN_SMOOTHING_DEFAULT, // 0, smooths turns during FW WP mode missions
|
||||
}
|
||||
);
|
||||
|
||||
|
|
@ -1045,13 +1046,14 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
|
|||
}
|
||||
},
|
||||
|
||||
/** Advanced Fixed Wing Autoland **/
|
||||
#ifdef USE_FW_AUTOLAND
|
||||
[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,
|
||||
.stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_AUTO_RTH,
|
||||
.mapToFlightModes = NAV_FW_AUTOLAND,
|
||||
.mwState = MW_NAV_STATE_LAND_IN_PROGRESS,
|
||||
.mwError = MW_NAV_ERROR_NONE,
|
||||
.onEvent = {
|
||||
|
|
@ -1071,8 +1073,8 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
|
|||
.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,
|
||||
.stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_AUTO_RTH,
|
||||
.mapToFlightModes = NAV_FW_AUTOLAND,
|
||||
.mwState = MW_NAV_STATE_LAND_IN_PROGRESS,
|
||||
.mwError = MW_NAV_ERROR_NONE,
|
||||
.onEvent = {
|
||||
|
|
@ -1092,8 +1094,8 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
|
|||
.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,
|
||||
.stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_AUTO_WP,
|
||||
.mapToFlightModes = NAV_FW_AUTOLAND,
|
||||
.mwState = MW_NAV_STATE_LAND_IN_PROGRESS,
|
||||
.mwError = MW_NAV_ERROR_NONE,
|
||||
.onEvent = {
|
||||
|
|
@ -1114,7 +1116,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
|
|||
.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,
|
||||
.mapToFlightModes = NAV_FW_AUTOLAND,
|
||||
.mwState = MW_NAV_STATE_LAND_IN_PROGRESS,
|
||||
.mwError = MW_NAV_ERROR_NONE,
|
||||
.onEvent = {
|
||||
|
|
@ -1135,7 +1137,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
|
|||
.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,
|
||||
.mapToFlightModes = NAV_FW_AUTOLAND,
|
||||
.mwState = MW_NAV_STATE_LAND_IN_PROGRESS,
|
||||
.mwError = MW_NAV_ERROR_NONE,
|
||||
.onEvent = {
|
||||
|
|
@ -1148,8 +1150,8 @@ 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_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_RTH | NAV_RC_POS | NAV_RC_YAW,
|
||||
.mapToFlightModes = NAV_WP_MODE | NAV_ALTHOLD_MODE,
|
||||
.stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_AUTO_RTH | NAV_RC_POS | NAV_RC_YAW,
|
||||
.mapToFlightModes = NAV_FW_AUTOLAND,
|
||||
.mwState = MW_NAV_STATE_LAND_IN_PROGRESS,
|
||||
.mwError = MW_NAV_ERROR_NONE,
|
||||
.onEvent = {
|
||||
|
|
@ -1678,7 +1680,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_LANDING(navigationF
|
|||
|
||||
/* If position sensors unavailable - land immediately (wait for timeout on GPS)
|
||||
* Continue to check for RTH sanity during landing */
|
||||
if (posControl.flags.estHeadingStatus == EST_NONE || checkForPositionSensorTimeout() || !validateRTHSanityChecker()) {
|
||||
if (posControl.flags.estHeadingStatus == EST_NONE || checkForPositionSensorTimeout() || (previousState != NAV_STATE_WAYPOINT_REACHED && !validateRTHSanityChecker())) {
|
||||
return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING;
|
||||
}
|
||||
|
||||
|
|
@ -1688,7 +1690,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_LANDING(navigationF
|
|||
|
||||
#ifdef USE_FW_AUTOLAND
|
||||
if (STATE(AIRPLANE)) {
|
||||
int8_t missionIdx = -1, shIdx = -1, missionFwLandConfigStartIdx = 8;
|
||||
int8_t missionIdx = -1, shIdx = -1, missionFwLandConfigStartIdx = 8, approachSettingIdx = -1;
|
||||
#ifdef USE_MULTI_MISSION
|
||||
missionIdx = posControl.loadedMultiMissionIndex - 1;
|
||||
#endif
|
||||
|
|
@ -1697,18 +1699,23 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_LANDING(navigationF
|
|||
shIdx = posControl.safehomeState.index;
|
||||
missionFwLandConfigStartIdx = MAX_SAFE_HOMES;
|
||||
#endif
|
||||
if (!posControl.fwLandState.landAborted && (shIdx >= 0 || missionIdx >= 0) && (fwAutolandApproachConfig(shIdx)->landApproachHeading1 != 0 || fwAutolandApproachConfig(shIdx)->landApproachHeading2 != 0)) {
|
||||
if (previousState == NAV_STATE_WAYPOINT_REACHED && missionIdx >= 0) {
|
||||
approachSettingIdx = missionFwLandConfigStartIdx + missionIdx;
|
||||
} else if (shIdx >= 0) {
|
||||
approachSettingIdx = shIdx;
|
||||
}
|
||||
|
||||
if (!posControl.fwLandState.landAborted && approachSettingIdx >= 0 && (fwAutolandApproachConfig(approachSettingIdx)->landApproachHeading1 != 0 || fwAutolandApproachConfig(approachSettingIdx)->landApproachHeading2 != 0)) {
|
||||
|
||||
if (previousState == NAV_STATE_WAYPOINT_REACHED) {
|
||||
posControl.fwLandState.landPos = posControl.activeWaypoint.pos;
|
||||
posControl.fwLandState.approachSettingIdx = missionFwLandConfigStartIdx + missionIdx;
|
||||
posControl.fwLandState.landWp = true;
|
||||
} else {
|
||||
posControl.fwLandState.landPos = posControl.safehomeState.nearestSafeHome;
|
||||
posControl.fwLandState.approachSettingIdx = shIdx;
|
||||
posControl.fwLandState.landWp = false;
|
||||
}
|
||||
|
||||
posControl.fwLandState.approachSettingIdx = approachSettingIdx;
|
||||
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;
|
||||
|
|
@ -1780,6 +1787,12 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_INITIALIZE(nav
|
|||
resetPositionController();
|
||||
resetAltitudeController(false); // Make sure surface tracking is not enabled - WP uses global altitude, not AGL
|
||||
|
||||
#ifdef USE_FW_AUTOLAND
|
||||
if (previousState != NAV_STATE_FW_LANDING_ABORT) {
|
||||
posControl.fwLandState.landAborted = false;
|
||||
}
|
||||
#endif
|
||||
|
||||
if (posControl.activeWaypointIndex == posControl.startWpIndex || posControl.wpMissionRestart) {
|
||||
/* Use p3 as the volatile jump counter, allowing embedded, rearmed jumps
|
||||
Using p3 minimises the risk of saving an invalid counter if a mission is aborted */
|
||||
|
|
@ -1999,11 +2012,20 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_RTH_LAND(navig
|
|||
{
|
||||
UNUSED(previousState);
|
||||
|
||||
#ifdef USE_FW_AUTOLAND
|
||||
if (posControl.fwLandState.landAborted) {
|
||||
return NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_FINISHED;
|
||||
}
|
||||
#endif
|
||||
|
||||
const navigationFSMEvent_t landEvent = navOnEnteringState_NAV_STATE_RTH_LANDING(previousState);
|
||||
|
||||
#ifdef USE_FW_AUTOLAND
|
||||
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) {
|
||||
} else
|
||||
#endif
|
||||
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
|
||||
|
|
@ -2333,7 +2355,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_FW_LANDING_APPROACH(nav
|
|||
|
||||
if (isLandingDetected()) {
|
||||
posControl.fwLandState.landState = FW_AUTOLAND_STATE_IDLE;
|
||||
disarm(DISARM_LANDING);
|
||||
//disarm(DISARM_LANDING);
|
||||
return NAV_FSM_EVENT_SWITCH_TO_IDLE;
|
||||
}
|
||||
|
||||
|
|
@ -2377,14 +2399,14 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_FW_LANDING_GLIDE(naviga
|
|||
return NAV_FSM_EVENT_SWITCH_TO_NAV_STATE_FW_LANDING_ABORT;
|
||||
}
|
||||
|
||||
if (getLandAltitude() <= posControl.fwLandState.landAltAgl + navFwAutolandConfig()->flareAltitude) {
|
||||
if (getHwRangefinderStatus() == HW_SENSOR_OK && getLandAltitude() <= posControl.fwLandState.landAltAgl + navFwAutolandConfig()->flareAltitude) {
|
||||
posControl.fwLandState.landState = FW_AUTOLAND_STATE_FLARE;
|
||||
return NAV_FSM_EVENT_SUCCESS;
|
||||
}
|
||||
|
||||
if (isLandingDetected()) {
|
||||
posControl.fwLandState.landState = FW_AUTOLAND_STATE_IDLE;
|
||||
disarm(DISARM_LANDING);
|
||||
//disarm(DISARM_LANDING);
|
||||
return NAV_FSM_EVENT_SWITCH_TO_IDLE;
|
||||
}
|
||||
|
||||
|
|
@ -2398,7 +2420,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_FW_LANDING_FLARE(naviga
|
|||
|
||||
if (isLandingDetected()) {
|
||||
posControl.fwLandState.landState = FW_AUTOLAND_STATE_IDLE;
|
||||
disarm(DISARM_LANDING);
|
||||
//disarm(DISARM_LANDING);
|
||||
return NAV_FSM_EVENT_SUCCESS;
|
||||
}
|
||||
setDesiredPosition(NULL, posControl.cruise.course, NAV_POS_UPDATE_HEADING);
|
||||
|
|
@ -3004,7 +3026,7 @@ void setHomePosition(const fpVector3_t * pos, int32_t heading, navSetWaypointFla
|
|||
updateDesiredRTHAltitude();
|
||||
|
||||
// Reset RTH sanity checker for new home position if RTH active
|
||||
if (FLIGHT_MODE(NAV_RTH_MODE)) {
|
||||
if (FLIGHT_MODE(NAV_RTH_MODE) || FLIGHT_MODE(NAV_FW_AUTOLAND) ) {
|
||||
initializeRTHSanityChecker();
|
||||
}
|
||||
|
||||
|
|
@ -3126,7 +3148,7 @@ void updateHomePosition(void)
|
|||
static bool isHomeResetAllowed = false;
|
||||
// If pilot so desires he may reset home position to current position
|
||||
if (IS_RC_MODE_ACTIVE(BOXHOMERESET)) {
|
||||
if (isHomeResetAllowed && !FLIGHT_MODE(FAILSAFE_MODE) && !FLIGHT_MODE(NAV_RTH_MODE) && !FLIGHT_MODE(NAV_WP_MODE) && (posControl.flags.estPosStatus >= EST_USABLE)) {
|
||||
if (isHomeResetAllowed && !FLIGHT_MODE(FAILSAFE_MODE) && !FLIGHT_MODE(NAV_RTH_MODE) && !FLIGHT_MODE(NAV_FW_AUTOLAND) && !FLIGHT_MODE(NAV_WP_MODE) && (posControl.flags.estPosStatus >= EST_USABLE)) {
|
||||
homeUpdateFlags = 0;
|
||||
homeUpdateFlags = STATE(GPS_FIX_HOME) ? (NAV_POS_UPDATE_XY | NAV_POS_UPDATE_HEADING) : (NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING);
|
||||
setHome = true;
|
||||
|
|
@ -3949,6 +3971,7 @@ bool isNavHoldPositionActive(void)
|
|||
// Also hold position during emergency landing if position valid
|
||||
return (FLIGHT_MODE(NAV_RTH_MODE) && !posControl.flags.rthTrackbackActive) ||
|
||||
FLIGHT_MODE(NAV_POSHOLD_MODE) ||
|
||||
(posControl.navState == NAV_STATE_FW_LANDING_CLIMB_TO_LOITER || posControl.navState == NAV_STATE_FW_LANDING_LOITER) ||
|
||||
navigationIsExecutingAnEmergencyLanding();
|
||||
}
|
||||
|
||||
|
|
@ -3988,7 +4011,9 @@ bool isWaypointNavTrackingActive(void)
|
|||
// is set from current position not previous WP. Works for WP Restart intermediate WP as well as first mission WP.
|
||||
// (NAV_WP_MODE flag isn't set until WP initialisation is finished, i.e. after calculateAndSetActiveWaypoint called)
|
||||
|
||||
return FLIGHT_MODE(NAV_WP_MODE) || (posControl.flags.rthTrackbackActive && rth_trackback.activePointIndex != rth_trackback.lastSavedIndex);
|
||||
return FLIGHT_MODE(NAV_WP_MODE)
|
||||
|| posControl.navState == NAV_STATE_FW_LANDING_APPROACH
|
||||
|| (posControl.flags.rthTrackbackActive && rth_trackback.activePointIndex != rth_trackback.lastSavedIndex);
|
||||
}
|
||||
|
||||
/*-----------------------------------------------------------
|
||||
|
|
@ -4053,7 +4078,7 @@ void applyWaypointNavigationAndAltitudeHold(void)
|
|||
posControl.flags.verticalPositionDataConsumed = false;
|
||||
|
||||
#ifdef USE_FW_AUTOLAND
|
||||
if (!isFwLandInProgess()) {
|
||||
if (!FLIGHT_MODE(NAV_FW_AUTOLAND)) {
|
||||
posControl.fwLandState.landState = FW_AUTOLAND_STATE_IDLE;
|
||||
}
|
||||
#endif
|
||||
|
|
@ -4094,7 +4119,7 @@ void applyWaypointNavigationAndAltitudeHold(void)
|
|||
void switchNavigationFlightModes(void)
|
||||
{
|
||||
const flightModeFlags_e enabledNavFlightModes = navGetMappedFlightModes(posControl.navState);
|
||||
const flightModeFlags_e disabledFlightModes = (NAV_ALTHOLD_MODE | NAV_RTH_MODE | NAV_POSHOLD_MODE | NAV_WP_MODE | NAV_LAUNCH_MODE | NAV_COURSE_HOLD_MODE) & (~enabledNavFlightModes);
|
||||
const flightModeFlags_e disabledFlightModes = (NAV_ALTHOLD_MODE | NAV_RTH_MODE | NAV_FW_AUTOLAND | NAV_POSHOLD_MODE | NAV_WP_MODE | NAV_LAUNCH_MODE | NAV_COURSE_HOLD_MODE) & (~enabledNavFlightModes);
|
||||
DISABLE_FLIGHT_MODE(disabledFlightModes);
|
||||
ENABLE_FLIGHT_MODE(enabledNavFlightModes);
|
||||
}
|
||||
|
|
@ -4206,22 +4231,6 @@ static navigationFSMEvent_t selectNavEventFromBoxModeInput(void)
|
|||
}
|
||||
posControl.rthSanityChecker.rthSanityOK = true;
|
||||
|
||||
/* WP mission activation control:
|
||||
* canActivateWaypoint & waypointWasActivated are used to prevent WP mission
|
||||
* auto restarting after interruption by Manual or RTH modes.
|
||||
* WP mode must be deselected before it can be reactivated again. */
|
||||
static bool waypointWasActivated = false;
|
||||
const bool isWpMissionLoaded = isWaypointMissionValid();
|
||||
bool canActivateWaypoint = isWpMissionLoaded && !posControl.flags.wpMissionPlannerActive; // Block activation if using WP Mission Planner
|
||||
|
||||
if (waypointWasActivated && !FLIGHT_MODE(NAV_WP_MODE)) {
|
||||
canActivateWaypoint = false;
|
||||
if (!IS_RC_MODE_ACTIVE(BOXNAVWP)) {
|
||||
canActivateWaypoint = true;
|
||||
waypointWasActivated = false;
|
||||
}
|
||||
}
|
||||
|
||||
/* Airplane specific modes */
|
||||
if (STATE(AIRPLANE)) {
|
||||
// LAUNCH mode has priority over any other NAV mode
|
||||
|
|
@ -4261,14 +4270,36 @@ static navigationFSMEvent_t selectNavEventFromBoxModeInput(void)
|
|||
return NAV_FSM_EVENT_SWITCH_TO_RTH;
|
||||
}
|
||||
|
||||
/* Pilot-triggered RTH, also fall-back for WP if there is no mission loaded.
|
||||
* WP prevented from falling back to RTH if WP mission planner is active */
|
||||
const bool wpRthFallbackIsActive = IS_RC_MODE_ACTIVE(BOXNAVWP) && !isWpMissionLoaded && !posControl.flags.wpMissionPlannerActive;
|
||||
/* WP mission activation control:
|
||||
* canActivateWaypoint & waypointWasActivated are used to prevent WP mission
|
||||
* auto restarting after interruption by Manual or RTH modes.
|
||||
* WP mode must be deselected before it can be reactivated again
|
||||
* WP Mode also inhibited when Mission Planner is active */
|
||||
static bool waypointWasActivated = false;
|
||||
bool canActivateWaypoint = isWaypointMissionValid();
|
||||
bool wpRthFallbackIsActive = false;
|
||||
|
||||
if (IS_RC_MODE_ACTIVE(BOXMANUAL) || posControl.flags.wpMissionPlannerActive) {
|
||||
canActivateWaypoint = false;
|
||||
} else {
|
||||
if (waypointWasActivated && !FLIGHT_MODE(NAV_WP_MODE)) {
|
||||
canActivateWaypoint = false;
|
||||
|
||||
if (!IS_RC_MODE_ACTIVE(BOXNAVWP)) {
|
||||
canActivateWaypoint = true;
|
||||
waypointWasActivated = false;
|
||||
}
|
||||
}
|
||||
|
||||
wpRthFallbackIsActive = IS_RC_MODE_ACTIVE(BOXNAVWP) && !canActivateWaypoint;
|
||||
}
|
||||
|
||||
/* Pilot-triggered RTH, also fall-back for WP if no mission is loaded.
|
||||
* Check for isExecutingRTH to prevent switching our from RTH in case of a brief GPS loss
|
||||
* Without this loss of any of the canActivateNavigation && canActivateAltHold
|
||||
* will kick us out of RTH state machine via NAV_FSM_EVENT_SWITCH_TO_IDLE and will prevent any of the fall-back
|
||||
* logic kicking in (waiting for GPS on airplanes, switch to emergency landing etc) */
|
||||
if (IS_RC_MODE_ACTIVE(BOXNAVRTH) || wpRthFallbackIsActive) {
|
||||
// Check for isExecutingRTH to prevent switching our from RTH in case of a brief GPS loss
|
||||
// Without this loss of any of the canActivateNavigation && canActivateAltHold
|
||||
// will kick us out of RTH state machine via NAV_FSM_EVENT_SWITCH_TO_IDLE and will prevent any of the fall-back
|
||||
// logic kicking in (waiting for GPS on airplanes, switch to emergency landing etc)
|
||||
if (isExecutingRTH || (canActivateNavigation && canActivateAltHold && STATE(GPS_FIX_HOME))) {
|
||||
return NAV_FSM_EVENT_SWITCH_TO_RTH;
|
||||
}
|
||||
|
|
@ -4282,11 +4313,11 @@ static navigationFSMEvent_t selectNavEventFromBoxModeInput(void)
|
|||
// Pilot-activated waypoint mission. Fall-back to RTH if no mission loaded.
|
||||
// Also check multimission mission change status before activating WP mode.
|
||||
#ifdef USE_MULTI_MISSION
|
||||
if (updateWpMissionChange() && IS_RC_MODE_ACTIVE(BOXNAVWP) && canActivateWaypoint) {
|
||||
if (updateWpMissionChange() && IS_RC_MODE_ACTIVE(BOXNAVWP)) {
|
||||
#else
|
||||
if (IS_RC_MODE_ACTIVE(BOXNAVWP) && canActivateWaypoint) {
|
||||
if (IS_RC_MODE_ACTIVE(BOXNAVWP)) {
|
||||
#endif
|
||||
if (FLIGHT_MODE(NAV_WP_MODE) || (canActivateNavigation && canActivateAltHold && STATE(GPS_FIX_HOME))) {
|
||||
if (FLIGHT_MODE(NAV_WP_MODE) || (canActivateWaypoint && canActivateNavigation && canActivateAltHold && STATE(GPS_FIX_HOME))) {
|
||||
waypointWasActivated = true;
|
||||
return NAV_FSM_EVENT_SWITCH_TO_WAYPOINT;
|
||||
}
|
||||
|
|
@ -4781,7 +4812,7 @@ void abortForcedRTH(void)
|
|||
rthState_e getStateOfForcedRTH(void)
|
||||
{
|
||||
/* If forced RTH activated and in AUTO_RTH or EMERG state */
|
||||
if (posControl.flags.forcedRTHActivated && (navGetStateFlags(posControl.navState) & (NAV_AUTO_RTH | NAV_CTL_EMERG | NAV_MIXERAT))) {
|
||||
if (posControl.flags.forcedRTHActivated && ((navGetStateFlags(posControl.navState) & (NAV_AUTO_RTH | NAV_CTL_EMERG | NAV_MIXERAT)) || FLIGHT_MODE(NAV_FW_AUTOLAND))) {
|
||||
if (posControl.navState == NAV_STATE_RTH_FINISHED || posControl.navState == NAV_STATE_EMERGENCY_LANDING_FINISHED) {
|
||||
return RTH_HAS_LANDED;
|
||||
}
|
||||
|
|
@ -4863,6 +4894,12 @@ bool navigationIsFlyingAutonomousMode(void)
|
|||
|
||||
bool navigationRTHAllowsLanding(void)
|
||||
{
|
||||
#ifdef USE_FW_AUTOLAND
|
||||
if (posControl.fwLandState.landAborted) {
|
||||
return false;
|
||||
}
|
||||
#endif
|
||||
|
||||
// WP mission RTH landing setting
|
||||
if (isWaypointMissionRTHActive() && isWaypointMissionValid()) {
|
||||
return posControl.waypointList[posControl.startWpIndex + posControl.waypointCount - 1].p1 > 0;
|
||||
|
|
@ -5020,15 +5057,6 @@ void resetFwAutolandApproach(int8_t idx)
|
|||
}
|
||||
}
|
||||
|
||||
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
|
||||
|
|
|
|||
Loading…
Add table
Add a link
Reference in a new issue