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

altitude tolerance fix

Browse source
This commit is contained in:
breadoven 2022-08-01 16:16:39 +01:00
parent f835bcecef
commit f7e10fe44e
5 changed files with 17 additions and 18 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -61,8 +61,6 @@
#include "sensors/boardalignment.h"
#include "sensors/battery.h"
#define WP_ALTITUDE_MARGIN_CM 100 // WP enforce altitude tolerance, used when WP altitude setting enforced when WP reached
// 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)
@ -100,7 +98,7 @@ STATIC_ASSERT(NAV_MAX_WAYPOINTS < 254, NAV_MAX_WAYPOINTS_exceeded_allowable_rang
PG_REGISTER_ARRAY(navWaypoint_t, NAV_MAX_WAYPOINTS, nonVolatileWaypointList, PG_WAYPOINT_MISSION_STORAGE, 2);
#endif
PG_REGISTER_WITH_RESET_TEMPLATE(navConfig_t, navConfig, PG_NAV_CONFIG, 1);
PG_REGISTER_WITH_RESET_TEMPLATE(navConfig_t, navConfig, PG_NAV_CONFIG, 2);
PG_RESET_TEMPLATE(navConfig_t, navConfig,
.general = {
@ -122,7 +120,6 @@ PG_RESET_TEMPLATE(navConfig_t, navConfig,
.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
.waypoint_enforce_altitude = SETTING_NAV_WP_ENFORCE_ALTITUDE_DEFAULT, // Forces set wp altitude to be achieved
.rth_trackback_mode = SETTING_NAV_RTH_TRACKBACK_MODE_DEFAULT // RTH trackback useage mode
},
@ -153,6 +150,7 @@ PG_RESET_TEMPLATE(navConfig_t, navConfig,
.safehome_max_distance = SETTING_SAFEHOME_MAX_DISTANCE_DEFAULT, // Max distance that a safehome is from the arming point
.max_altitude = SETTING_NAV_MAX_ALTITUDE_DEFAULT,
.rth_trackback_distance = SETTING_NAV_RTH_TRACKBACK_DISTANCE_DEFAULT, // Max distance allowed for RTH trackback
.waypoint_enforce_altitude = SETTING_NAV_WP_ENFORCE_ALTITUDE_DEFAULT, // Forces set wp altitude to be achieved
},
// MC-specific
@ -1670,13 +1668,13 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_REACHED(naviga
{
UNUSED(previousState);
if (navConfig()->general.flags.waypoint_enforce_altitude) {
if (navConfig()->general.waypoint_enforce_altitude) {
posControl.wpAltitudeReached = isWaypointAltitudeReached();
}
switch ((navWaypointActions_e)posControl.waypointList[posControl.activeWaypointIndex].action) {
case NAV_WP_ACTION_WAYPOINT:
if (navConfig()->general.flags.waypoint_enforce_altitude && !posControl.wpAltitudeReached) {
if (navConfig()->general.waypoint_enforce_altitude && !posControl.wpAltitudeReached) {
return NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_HOLD_TIME;
} else {
return NAV_FSM_EVENT_SUCCESS; // NAV_STATE_WAYPOINT_NEXT
@ -1709,7 +1707,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_HOLD_TIME(navi
return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING;
}
if (navConfig()->general.flags.waypoint_enforce_altitude && !posControl.wpAltitudeReached) {
if (navConfig()->general.waypoint_enforce_altitude && !posControl.wpAltitudeReached) {
// Adjust altitude to waypoint setting
if (STATE(AIRPLANE)) {
int8_t altitudeChangeDirection = posControl.activeWaypoint.pos.z > navGetCurrentActualPositionAndVelocity()->pos.z ? 1 : -1;
@ -2243,7 +2241,7 @@ bool isWaypointReached(const navWaypointPosition_t * waypoint, const bool isWayp
bool isWaypointAltitudeReached(void)
{
return ABS(navGetCurrentActualPositionAndVelocity()->pos.z - posControl.activeWaypoint.pos.z) < WP_ALTITUDE_MARGIN_CM;
return ABS(navGetCurrentActualPositionAndVelocity()->pos.z - posControl.activeWaypoint.pos.z) < navConfig()->general.waypoint_enforce_altitude;
}
static void updateHomePositionCompatibility(void)