Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Plane: Max altitude for FBWB using Fence altitude

This commit is contained in:
khanasif786 2022-01-28 21:01:33 +05:30 committed by Andrew Tridgell
parent 1421fccdac
commit 1fe6d77eeb
4 changed files with 48 additions and 17 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
ArduPlane/Parameters.cpp
View File

@ -662,7 +662,7 @@ const AP_Param::Info Plane::var_info[] = {
// @Param: ALT_HOLD_FBWCM
// @DisplayName: Minimum altitude for FBWB mode
// @Description: This is the minimum altitude in centimeters that FBWB and CRUISE modes will allow. If you attempt to descend below this altitude then the plane will level off. A value of zero means no limit.
// @Description: This is the minimum altitude in centimeters (above home) that FBWB and CRUISE modes will allow. If you attempt to descend below this altitude then the plane will level off. A value of zero means no limit.
// @Units: cm
// @User: Standard
GSCALAR(FBWB_min_altitude_cm, "ALT_HOLD_FBWCM", ALT_HOLD_FBW_CM),

2
ArduPlane/Plane.h
View File

@ -833,7 +833,7 @@ private:
void set_target_altitude_proportion(const Location &loc, float proportion);
void constrain_target_altitude_location(const Location &loc1, const Location &loc2);
int32_t calc_altitude_error_cm(void);
void check_fbwb_minimum_altitude(void);
void check_fbwb_altitude(void);
void reset_offset_altitude(void);
void set_offset_altitude_location(const Location &start_loc, const Location &destination_loc);
bool above_location_current(const Location &loc);

59
ArduPlane/altitude.cpp
View File

@ -361,26 +361,57 @@ int32_t Plane::calc_altitude_error_cm(void)
}
/*
check for FBWB_min_altitude_cm violation
check for FBWB_min_altitude_cm and fence min/max altitude
*/
void Plane::check_fbwb_minimum_altitude(void)
void Plane::check_fbwb_altitude(void)
{
if (g.FBWB_min_altitude_cm == 0) {
return;
}
float max_alt_cm = 0.0;
float min_alt_cm = 0.0;
bool should_check_max = false;
bool should_check_min = false;
#if AP_TERRAIN_AVAILABLE
if (target_altitude.terrain_following) {
// set our target terrain height to be at least the min set
if (target_altitude.terrain_alt_cm < g.FBWB_min_altitude_cm) {
target_altitude.terrain_alt_cm = g.FBWB_min_altitude_cm;
}
return;
#if AP_FENCE_ENABLED
// taking fence max and min altitude (with margin)
const uint8_t enabled_fences = plane.fence.get_enabled_fences();
if ((enabled_fences & AC_FENCE_TYPE_ALT_MIN) != 0) {
min_alt_cm = plane.fence.get_safe_alt_min()*100.0;
should_check_min = true;
}
if ((enabled_fences & AC_FENCE_TYPE_ALT_MAX) != 0) {
max_alt_cm = plane.fence.get_safe_alt_max()*100.0;
should_check_max = true;
}
#endif
if (target_altitude.amsl_cm < home.alt + g.FBWB_min_altitude_cm) {
target_altitude.amsl_cm = home.alt + g.FBWB_min_altitude_cm;
if (g.FBWB_min_altitude_cm != 0) {
// FBWB min altitude exists
min_alt_cm = MAX(min_alt_cm, plane.g.FBWB_min_altitude_cm);
should_check_min = true;
}
if (!should_check_min && !should_check_max) {
return;
}
//check if terrain following (min and max)
#if AP_TERRAIN_AVAILABLE
if (target_altitude.terrain_following) {
// set our target terrain height to be at least the min set
if (should_check_max) {
target_altitude.terrain_alt_cm = MIN(target_altitude.terrain_alt_cm, max_alt_cm);
}
if (should_check_min) {
target_altitude.terrain_alt_cm = MAX(target_altitude.terrain_alt_cm, min_alt_cm);
}
return;
}
#endif
if (should_check_max) {
target_altitude.amsl_cm = MIN(target_altitude.amsl_cm, home.alt + max_alt_cm);
}
if (should_check_min) {
target_altitude.amsl_cm = MAX(target_altitude.amsl_cm, home.alt + min_alt_cm);
}
}

2
ArduPlane/navigation.cpp
View File

@ -411,7 +411,7 @@ void Plane::update_fbwb_speed_height(void)
target_altitude.last_elevator_input = elevator_input;
}
check_fbwb_minimum_altitude();
check_fbwb_altitude();
altitude_error_cm = calc_altitude_error_cm();