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

Merge remote-tracking branch 'upstream/master' into abo_multi_function_utility

Browse source
This commit is contained in:
breadoven 2023-04-28 10:19:48 +01:00
commit cfca116a14
425 changed files with 152438 additions and 1937 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -63,6 +63,8 @@
#include "sensors/boardalignment.h"
#include "sensors/battery.h"
#include "programming/global_variables.h"
// Multirotors:
#define MR_RTH_CLIMB_OVERSHOOT_CM 100 // target this amount of cm *above* the target altitude to ensure it is actually reached (Vz > 0 at target alt)
#define MR_RTH_CLIMB_MARGIN_MIN_CM 100 // start cruising home this amount of cm *before* reaching the cruise altitude (while continuing the ascend)
@ -112,10 +114,11 @@ PG_RESET_TEMPLATE(navConfig_t, navConfig,
.rth_alt_control_override = SETTING_NAV_RTH_ALT_CONTROL_OVERRIDE_DEFAULT, // Override RTH Altitude and Climb First using Pitch and Roll stick
.nav_overrides_motor_stop = SETTING_NAV_OVERRIDES_MOTOR_STOP_DEFAULT,
.safehome_usage_mode = SETTING_SAFEHOME_USAGE_MODE_DEFAULT,
.mission_planner_reset = SETTING_NAV_MISSION_PLANNER_RESET_DEFAULT, // Allow mode switch toggle to reset Mission Planner WPs
.waypoint_mission_restart = SETTING_NAV_WP_MISSION_RESTART_DEFAULT, // WP mission restart action
.soaring_motor_stop = SETTING_NAV_FW_SOARING_MOTOR_STOP_DEFAULT, // stops motor when Saoring mode enabled
.rth_trackback_mode = SETTING_NAV_RTH_TRACKBACK_MODE_DEFAULT // RTH trackback useage mode
.mission_planner_reset = SETTING_NAV_MISSION_PLANNER_RESET_DEFAULT, // Allow mode switch toggle to reset Mission Planner WPs
.waypoint_mission_restart = SETTING_NAV_WP_MISSION_RESTART_DEFAULT, // WP mission restart action
.soaring_motor_stop = SETTING_NAV_FW_SOARING_MOTOR_STOP_DEFAULT, // stops motor when Saoring mode enabled
.rth_trackback_mode = SETTING_NAV_RTH_TRACKBACK_MODE_DEFAULT, // RTH trackback useage mode
.rth_use_linear_descent = SETTING_NAV_RTH_USE_LINEAR_DESCENT_DEFAULT, // Use linear descent during RTH
},
// General navigation parameters
@ -148,6 +151,7 @@ PG_RESET_TEMPLATE(navConfig_t, navConfig,
.waypoint_enforce_altitude = SETTING_NAV_WP_ENFORCE_ALTITUDE_DEFAULT, // Forces set wp altitude to be achieved
.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,
},
// MC-specific
@ -1351,6 +1355,15 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_HEAD_HOME(navigatio
return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING;
}
if (navConfig()->general.flags.rth_use_linear_descent && navConfig()->general.rth_home_altitude > 0) {
// Check linear descent status
uint32_t homeDistance = calculateDistanceToDestination(&posControl.rthState.homePosition.pos);
if (homeDistance <= METERS_TO_CENTIMETERS(navConfig()->general.rth_linear_descent_start_distance)) {
posControl.rthState.rthFinalAltitude = posControl.rthState.homePosition.pos.z + navConfig()->general.rth_home_altitude;
}
}
// If we have position sensor - continue home
if ((posControl.flags.estPosStatus >= EST_USABLE)) {
fpVector3_t * tmpHomePos = rthGetHomeTargetPosition(RTH_HOME_ENROUTE_PROPORTIONAL);
@ -2334,16 +2347,15 @@ static void updateDesiredRTHAltitude(void)
posControl.rthState.rthFinalAltitude = posControl.rthState.rthInitialAltitude;
break;
case NAV_RTH_AT_LEAST_ALT_LINEAR_DESCENT:
posControl.rthState.rthInitialAltitude = MAX(posControl.rthState.homePosition.pos.z + navConfig()->general.rth_altitude, posControl.actualState.abs.pos.z);
posControl.rthState.rthFinalAltitude = posControl.rthState.homePosition.pos.z + navConfig()->general.rth_altitude;
break;
case NAV_RTH_CONST_ALT: // Climb/descend to predefined altitude above home
default:
posControl.rthState.rthInitialAltitude = posControl.rthState.homePosition.pos.z + navConfig()->general.rth_altitude;
posControl.rthState.rthFinalAltitude = posControl.rthState.rthInitialAltitude;
}
if ((navConfig()->general.flags.rth_use_linear_descent) && (navConfig()->general.rth_home_altitude > 0) && (navConfig()->general.rth_linear_descent_start_distance == 0) ) {
posControl.rthState.rthFinalAltitude = posControl.rthState.homePosition.pos.z + navConfig()->general.rth_home_altitude;
}
}
} else {
posControl.rthState.rthClimbStageAltitude = posControl.actualState.abs.pos.z;