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 // @User: Standard
GSCALAR(pitch_trim, "TRIM_PITCH_DEG", 0.0f), GSCALAR(pitch_trim, "TRIM_PITCH_DEG", 0.0f),
// @Param: ALT_HOLD_RTL // @Param: RTL_ALTITUDE
// @DisplayName: 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. // @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 // @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 // @Param: ALT_CRUISE_MIN
// @DisplayName: Minimum altitude for FBWB mode // @DisplayName: Minimum altitude for FBWB and CRUISE 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. // @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: cm // @Units: m
// @User: Standard // @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 // @Param: FLAP_1_PERCNT
// @DisplayName: Flap 1 percentage // @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: 1: Use centered throttle in Cruise or FBWB to indicate trim airspeed
// @Bitmask: 2: Disable attitude check for takeoff arming // @Bitmask: 2: Disable attitude check for takeoff arming
// @Bitmask: 3: Force target airspeed to trim airspeed in Cruise or FBWB // @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: 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: 6: Supress speed scaling during auto takeoffs to be 1 or less to prevent oscillations without airspeed sensor.
// @Bitmask: 7: EnableDefaultAirspeed for takeoff // @Bitmask: 7: EnableDefaultAirspeed for takeoff
@ -1550,6 +1550,8 @@ void Plane::load_parameters(void)
g.pitch_trim.convert_centi_parameter(AP_PARAM_INT16); g.pitch_trim.convert_centi_parameter(AP_PARAM_INT16);
aparm.airspeed_cruise.convert_centi_parameter(AP_PARAM_INT32); aparm.airspeed_cruise.convert_centi_parameter(AP_PARAM_INT32);
aparm.min_groundspeed.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)); 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_min = 120,
k_param_airspeed_max, 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_flybywire_elev_reverse,
k_param_alt_control_algorithm, // unused k_param_alt_control_algorithm, // unused
k_param_flybywire_climb_rate, k_param_flybywire_climb_rate,
@ -217,7 +217,7 @@ public:
k_param_pitch_limit_max_cd, k_param_pitch_limit_max_cd,
k_param_pitch_limit_min_cd, k_param_pitch_limit_min_cd,
k_param_airspeed_cruise, k_param_airspeed_cruise,
k_param_RTL_altitude_cm, k_param_RTL_altitude,
k_param_inverted_flight_ch_unused, // unused k_param_inverted_flight_ch_unused, // unused
k_param_min_groundspeed, k_param_min_groundspeed,
k_param_crosstrack_use_wind, // unused k_param_crosstrack_use_wind, // unused
@ -436,9 +436,9 @@ public:
AP_Int16 mixing_offset; AP_Int16 mixing_offset;
AP_Int16 dspoiler_rud_rate; AP_Int16 dspoiler_rud_rate;
AP_Int32 log_bitmask; AP_Int32 log_bitmask;
AP_Int32 RTL_altitude_cm; AP_Float RTL_altitude;
AP_Float pitch_trim; AP_Float pitch_trim;
AP_Int16 FBWB_min_altitude_cm; AP_Float FBWB_min_altitude;
AP_Int8 flap_1_percent; AP_Int8 flap_1_percent;
AP_Int8 flap_1_speed; 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 int32_t Plane::get_RTL_altitude_cm() const
{ {
if (g.RTL_altitude_cm < 0) { if (g.RTL_altitude < 0) {
return current_loc.alt; 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) void Plane::check_fbwb_altitude(void)
{ {
@ -359,9 +359,9 @@ void Plane::check_fbwb_altitude(void)
} }
#endif #endif
if (g.FBWB_min_altitude_cm != 0) { if (g.FBWB_min_altitude > 0) {
// FBWB min altitude exists // 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; 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 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; 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 // 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 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; return true;
} }

6
ArduPlane/config.h
View File

@ -135,10 +135,9 @@
# define AIRSPEED_FBW_MAX 22 # define AIRSPEED_FBW_MAX 22
#endif #endif
#ifndef ALT_HOLD_FBW #ifndef ALT_CRUISE_MIN
# define ALT_HOLD_FBW 0 # define ALT_CRUISE_MIN 0
#endif #endif
#define ALT_HOLD_FBW_CM ALT_HOLD_FBW*100
////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////
@ -202,7 +201,6 @@
#ifndef ALT_HOLD_HOME #ifndef ALT_HOLD_HOME
# define ALT_HOLD_HOME 100 # define ALT_HOLD_HOME 100
#endif #endif
#define ALT_HOLD_HOME_CM ALT_HOLD_HOME*100
////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////
// Developer Items // 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(); loc.alt = home.alt + 100.0f * fence.get_return_altitude();
} else if (fence.get_safe_alt_min() >= fence.get_safe_alt_max()) { } else if (fence.get_safe_alt_min() >= fence.get_safe_alt_max()) {
// invalid min/max, use RTL_altitude // invalid min/max, use RTL_altitude
loc.alt = home.alt + g.RTL_altitude_cm; loc.alt = home.alt + g.RTL_altitude*100;
} else { } else {
// fly to the return point, with an altitude half way between // fly to the return point, with an altitude half way between
// min and max // 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 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 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_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 sink_dist = plane.aparm.airspeed_cruise * sink_time;
const float dist = plane.auto_state.wp_distance; 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; bool alt_threshold_reached = false;
if (plane.flight_option_enabled(FlightOptions::CLIMB_BEFORE_TURN)) { 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; alt_threshold_reached = plane.current_loc.alt > plane.next_WP_loc.alt;
} else if (plane.g2.rtl_climb_min > 0) { } else if (plane.g2.rtl_climb_min > 0) {
/* /*