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

ArduPlane: convert ALT_HOLD_RTL and ALT_HOLD_FBWCM

This commit is contained in:
Andrew Tridgell 2024-01-18 16:58:09 +11:00
parent 54f38b7570
commit 8151647e04
8 changed files with 27 additions and 27 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

20
ArduPlane/Parameters.cpp
View File

@ -652,19 +652,19 @@ const AP_Param::Info Plane::var_info[] = {
// @User: Standard
GSCALAR(pitch_trim, "TRIM_PITCH_DEG", 0.0f),
// @Param: ALT_HOLD_RTL
// @Param: RTL_ALTITUDE
// @DisplayName: RTL altitude
// @Description: Target altitude above home for RTL mode. Maintains current altitude if set to -1. Rally point altitudes are used if plane does not return to home.
// @Units: cm
// @Units: m
// @User: Standard
GSCALAR(RTL_altitude_cm, "ALT_HOLD_RTL", ALT_HOLD_HOME_CM),
GSCALAR(RTL_altitude, "RTL_ALTITUDE", ALT_HOLD_HOME),
// @Param: ALT_HOLD_FBWCM
// @DisplayName: Minimum altitude for FBWB mode
// @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. It will also force a climb to this altitude if below in these modes. A value of zero means no limit.
// @Units: cm
// @Param: ALT_CRUISE_MIN
// @DisplayName: Minimum altitude for FBWB and CRUISE mode
// @Description: This is the minimum altitude in meters (above home) that FBWB and CRUISE modes will allow. If you attempt to descend below this altitude then the plane will level off. It will also force a climb to this altitude if below in these modes. A value of zero means no limit.
// @Units: m
// @User: Standard
GSCALAR(FBWB_min_altitude_cm, "ALT_HOLD_FBWCM", ALT_HOLD_FBW_CM),
GSCALAR(FBWB_min_altitude, "ALT_CRUISE_MIN", ALT_CRUISE_MIN),
// @Param: FLAP_1_PERCNT
// @DisplayName: Flap 1 percentage
@ -1090,7 +1090,7 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = {
// @Bitmask: 1: Use centered throttle in Cruise or FBWB to indicate trim airspeed
// @Bitmask: 2: Disable attitude check for takeoff arming
// @Bitmask: 3: Force target airspeed to trim airspeed in Cruise or FBWB
// @Bitmask: 4: Climb to ALT_HOLD_RTL before turning for RTL
// @Bitmask: 4: Climb to RTL_ALTITUDE before turning for RTL
// @Bitmask: 5: Enable yaw damper in acro mode
// @Bitmask: 6: Supress speed scaling during auto takeoffs to be 1 or less to prevent oscillations without airspeed sensor.
// @Bitmask: 7: EnableDefaultAirspeed for takeoff
@ -1550,6 +1550,8 @@ void Plane::load_parameters(void)
g.pitch_trim.convert_centi_parameter(AP_PARAM_INT16);
aparm.airspeed_cruise.convert_centi_parameter(AP_PARAM_INT32);
aparm.min_groundspeed.convert_centi_parameter(AP_PARAM_INT32);
g.RTL_altitude.convert_centi_parameter(AP_PARAM_INT32);
g.FBWB_min_altitude.convert_centi_parameter(AP_PARAM_INT16);
hal.console->printf("load_all took %uus\n", (unsigned)(micros() - before));
}

8
ArduPlane/Parameters.h
View File

@ -174,7 +174,7 @@ public:
//
k_param_airspeed_min = 120,
k_param_airspeed_max,
k_param_FBWB_min_altitude_cm, // 0=disabled, minimum value for altitude in cm (for first time try 30 meters = 3000 cm)
k_param_FBWB_min_altitude, // 0=disabled, minimum value for altitude in cm (for first time try 30 meters = 3000 cm)
k_param_flybywire_elev_reverse,
k_param_alt_control_algorithm, // unused
k_param_flybywire_climb_rate,
@ -217,7 +217,7 @@ public:
k_param_pitch_limit_max_cd,
k_param_pitch_limit_min_cd,
k_param_airspeed_cruise,
k_param_RTL_altitude_cm,
k_param_RTL_altitude,
k_param_inverted_flight_ch_unused, // unused
k_param_min_groundspeed,
k_param_crosstrack_use_wind, // unused
@ -436,9 +436,9 @@ public:
AP_Int16 mixing_offset;
AP_Int16 dspoiler_rud_rate;
AP_Int32 log_bitmask;
AP_Int32 RTL_altitude_cm;
AP_Float RTL_altitude;
AP_Float pitch_trim;
AP_Int16 FBWB_min_altitude_cm;
AP_Float FBWB_min_altitude;
AP_Int8 flap_1_percent;
AP_Int8 flap_1_speed;

10
ArduPlane/altitude.cpp
View File

@ -103,10 +103,10 @@ void Plane::setup_glide_slope(void)
*/
int32_t Plane::get_RTL_altitude_cm() const
{
if (g.RTL_altitude_cm < 0) {
if (g.RTL_altitude < 0) {
return current_loc.alt;
}
return g.RTL_altitude_cm + home.alt;
return g.RTL_altitude*100 + home.alt;
}
/*
@ -337,7 +337,7 @@ int32_t Plane::calc_altitude_error_cm(void)
}
/*
check for FBWB_min_altitude_cm and fence min/max altitude
check for FBWB_min_altitude and fence min/max altitude
*/
void Plane::check_fbwb_altitude(void)
{
@ -359,9 +359,9 @@ void Plane::check_fbwb_altitude(void)
}
#endif
if (g.FBWB_min_altitude_cm != 0) {
if (g.FBWB_min_altitude > 0) {
// FBWB min altitude exists
min_alt_cm = MAX(min_alt_cm, plane.g.FBWB_min_altitude_cm);
min_alt_cm = MAX(min_alt_cm, plane.g.FBWB_min_altitude*100.0);
should_check_min = true;
}

4
ArduPlane/avoidance_adsb.cpp
View File

@ -180,9 +180,9 @@ bool AP_Avoidance_Plane::handle_avoidance_vertical(const AP_Avoidance::Obstacle
new_loc.alt = plane.current_loc.alt + 1000; // set alt demand to be 10m above us, climb rate will be TECS_CLMB_MAX
return true;
} else if (plane.current_loc.alt > plane.g.RTL_altitude_cm) {
} else if (plane.current_loc.alt > plane.g.RTL_altitude*100) {
// should descend while above RTL alt
// TODO: consider using a lower altitude than RTL_altitude_cm since it's default (100m) is quite high
// TODO: consider using a lower altitude than RTL_altitude since it's default (100m) is quite high
new_loc.alt = plane.current_loc.alt - 1000; // set alt demand to be 10m below us, sink rate will be TECS_SINK_MAX
return true;
}

6
ArduPlane/config.h
View File

@ -135,10 +135,9 @@
# define AIRSPEED_FBW_MAX 22
#endif
#ifndef ALT_HOLD_FBW
# define ALT_HOLD_FBW 0
#ifndef ALT_CRUISE_MIN
# define ALT_CRUISE_MIN 0
#endif
#define ALT_HOLD_FBW_CM ALT_HOLD_FBW*100
//////////////////////////////////////////////////////////////////////////////
@ -202,7 +201,6 @@
#ifndef ALT_HOLD_HOME
# define ALT_HOLD_HOME 100
#endif
#define ALT_HOLD_HOME_CM ALT_HOLD_HOME*100
//////////////////////////////////////////////////////////////////////////////
// Developer Items

2
ArduPlane/fence.cpp
View File

@ -83,7 +83,7 @@ void Plane::fence_check()
loc.alt = home.alt + 100.0f * fence.get_return_altitude();
} else if (fence.get_safe_alt_min() >= fence.get_safe_alt_max()) {
// invalid min/max, use RTL_altitude
loc.alt = home.alt + g.RTL_altitude_cm;
loc.alt = home.alt + g.RTL_altitude*100;
} else {
// fly to the return point, with an altitude half way between
// min and max

2
ArduPlane/mode_qrtl.cpp
View File

@ -199,7 +199,7 @@ void ModeQRTL::update_target_altitude()
giving time to lose speed before we transition
*/
const float radius = MAX(fabsf(float(plane.aparm.loiter_radius)), fabsf(float(plane.g.rtl_radius)));
const float rtl_alt_delta = MAX(0, plane.g.RTL_altitude_cm*0.01 - plane.quadplane.qrtl_alt);
const float rtl_alt_delta = MAX(0, plane.g.RTL_altitude - plane.quadplane.qrtl_alt);
const float sink_time = rtl_alt_delta / MAX(0.6*plane.TECS_controller.get_max_sinkrate(), 1);
const float sink_dist = plane.aparm.airspeed_cruise * sink_time;
const float dist = plane.auto_state.wp_distance;

2
ArduPlane/mode_rtl.cpp
View File

@ -48,7 +48,7 @@ void ModeRTL::update()
bool alt_threshold_reached = false;
if (plane.flight_option_enabled(FlightOptions::CLIMB_BEFORE_TURN)) {
// Climb to ALT_HOLD_RTL before turning. This overrides RTL_CLIMB_MIN.
// Climb to RTL_ALTITUDE before turning. This overrides RTL_CLIMB_MIN.
alt_threshold_reached = plane.current_loc.alt > plane.next_WP_loc.alt;
} else if (plane.g2.rtl_climb_min > 0) {
/*