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

Set max allowed altitude for both FW and MR

Browse source
This commit is contained in:
Pawel Spychalski (DzikuVx) 2019-03-03 11:15:45 +01:00
parent 471849b33b
commit c66241d9ac
3 changed files with 18 additions and 1 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -1307,6 +1307,9 @@ groups:
- name: nav_max_terrain_follow_alt
field: general.max_terrain_follow_altitude
max: 1000
- name: nav_max_altitude
field: general.max_altitude
max: 65000
- name: nav_rth_altitude
field: general.rth_altitude
max: 65000

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

@ -77,7 +77,7 @@ PG_DECLARE_ARRAY(navWaypoint_t, NAV_MAX_WAYPOINTS, nonVolatileWaypointList);
PG_REGISTER_ARRAY(navWaypoint_t, NAV_MAX_WAYPOINTS, nonVolatileWaypointList, PG_WAYPOINT_MISSION_STORAGE, 0);
#endif
PG_REGISTER_WITH_RESET_TEMPLATE(navConfig_t, navConfig, PG_NAV_CONFIG, 3);
PG_REGISTER_WITH_RESET_TEMPLATE(navConfig_t, navConfig, PG_NAV_CONFIG, 4);
PG_RESET_TEMPLATE(navConfig_t, navConfig,
.general = {
@ -112,6 +112,7 @@ PG_RESET_TEMPLATE(navConfig_t, navConfig,
.rth_home_altitude = 0,
.rth_abort_threshold = 50000, // 500m - should be safe for all aircraft
.max_terrain_follow_altitude = 100, // max 1m altitude in terrain following mode
.max_altitude = 0, // Altitude limit disabled by default
},
// MC-specific
@ -2289,6 +2290,18 @@ void updateClimbRateToAltitudeController(float desiredClimbRate, climbRateToAlti
posControl.desiredState.pos.z = altitudeToUse;
}
else {
/*
* If max altitude is set, reset climb rate if altitude is reached and climb rate is > 0
* In other words, when altitude is reached, allow it only to shrink
*/
if (navConfig()->general.max_altitude > 0 &&
altitudeToUse >= navConfig()->general.max_altitude &&
desiredClimbRate > 0
) {
desiredClimbRate = 0;
}
if (STATE(FIXED_WING)) {
// Fixed wing climb rate controller is open-loop. We simply move the known altitude target
float timeDelta = US2S(currentTimeUs - lastUpdateTimeUs);

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

@ -153,6 +153,7 @@ typedef struct navConfig_s {
uint16_t min_rth_distance; // 0 Disables. Minimal distance for RTH in cm, otherwise it will just autoland
uint16_t rth_abort_threshold; // Initiate emergency landing if during RTH we get this much [cm] away from home
uint16_t max_terrain_follow_altitude; // Max altitude to be used in SURFACE TRACKING mode
uint16_t max_altitude; // Max altitude when in AltHold mode (not Surface Following)
} general;
struct {