From 8aabf7c22a6a8abe97f85148cf13d2c738bf92e8 Mon Sep 17 00:00:00 2001 From: Hwurzburg Date: Tue, 16 Mar 2021 16:54:03 -0500 Subject: [PATCH] Plane: Add VTOL descent rate, convert existing rate to climb rate only Co-authored-by: Reko Merio K9260@student.jamk.fi Co-authored-by: Peter Barker --- ArduPlane/qautotune.cpp | 2 +- ArduPlane/quadplane.cpp | 49 +++++++++++++++++++++++++++++------------ ArduPlane/quadplane.h | 5 ++++- 3 files changed, 40 insertions(+), 16 deletions(-) diff --git a/ArduPlane/qautotune.cpp b/ArduPlane/qautotune.cpp index 9ce2b8d42a..511ceba466 100644 --- a/ArduPlane/qautotune.cpp +++ b/ArduPlane/qautotune.cpp @@ -41,7 +41,7 @@ void QAutoTune::get_pilot_desired_rp_yrate_cd(float &des_roll_cd, float &des_pit void QAutoTune::init_z_limits() { - plane.quadplane.pos_control->set_max_speed_z(-plane.quadplane.pilot_velocity_z_max, plane.quadplane.pilot_velocity_z_max); + plane.quadplane.pos_control->set_max_speed_z(-plane.quadplane.get_pilot_velocity_z_max_dn(), plane.quadplane.pilot_velocity_z_max_up); plane.quadplane.pos_control->set_max_accel_z(plane.quadplane.pilot_accel_z); } diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp index 5e7344a339..49aaf8ee68 100644 --- a/ArduPlane/quadplane.cpp +++ b/ArduPlane/quadplane.cpp @@ -39,15 +39,24 @@ const AP_Param::GroupInfo QuadPlane::var_info[] = { AP_SUBGROUPPTR(pos_control, "P", 17, QuadPlane, AC_PosControl), // @Param: VELZ_MAX - // @DisplayName: Pilot maximum vertical speed - // @Description: The maximum vertical velocity the pilot may request in cm/s + // @DisplayName: Pilot maximum vertical speed up + // @Description: The maximum ascending vertical velocity the pilot may request in cm/s // @Units: cm/s // @Range: 50 500 // @Increment: 10 // @User: Standard - AP_GROUPINFO("VELZ_MAX", 18, QuadPlane, pilot_velocity_z_max, 250), - - // @Param: ACCEL_Z + AP_GROUPINFO("VELZ_MAX", 18, QuadPlane, pilot_velocity_z_max_up, 250), + + // @Param: VELZ_MAX_DN + // @DisplayName: Pilot maximum vertical speed down + // @Description: The maximum vertical velocity the pilot may request in cm/s going down. If 0, uses Q_VELZ_MAX value. + // @Units: cm/s + // @Range: 50 500 + // @Increment: 10 + // @User: Standard + AP_GROUPINFO("VELZ_MAX_DN", 60, QuadPlane, pilot_velocity_z_max_dn, 0), + + // @Param: ACCEL_Z // @DisplayName: Pilot vertical acceleration // @Description: The vertical acceleration used when pilot is controlling the altitude // @Units: cm/s/s @@ -334,6 +343,8 @@ const AP_Param::GroupInfo QuadPlane::var_info[] = { AP_GROUPINFO("OPTIONS", 58, QuadPlane, options, 0), AP_SUBGROUPEXTENSION("",59, QuadPlane, var_info2), + + // 60 is used above for VELZ_MAX_DN AP_GROUPEND }; @@ -1059,7 +1070,7 @@ void QuadPlane::run_z_controller(void) pos_control->set_desired_velocity_z(inertial_nav.get_velocity_z()); // initialize vertical speeds and leash lengths - pos_control->set_max_speed_z(-pilot_velocity_z_max, pilot_velocity_z_max); + pos_control->set_max_speed_z(-get_pilot_velocity_z_max_dn(), pilot_velocity_z_max_up); pos_control->set_max_accel_z(pilot_accel_z); // it has been two seconds since we last ran the Z @@ -1112,7 +1123,7 @@ void QuadPlane::init_qacro(void) void QuadPlane::init_hover(void) { // initialize vertical speeds and leash lengths - pos_control->set_max_speed_z(-pilot_velocity_z_max, pilot_velocity_z_max); + pos_control->set_max_speed_z(-get_pilot_velocity_z_max_dn(), pilot_velocity_z_max_up); pos_control->set_max_accel_z(pilot_accel_z); // initialise position and desired velocity @@ -1148,7 +1159,7 @@ void QuadPlane::hold_hover(float target_climb_rate) motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED); // initialize vertical speeds and acceleration - pos_control->set_max_speed_z(-pilot_velocity_z_max, pilot_velocity_z_max); + pos_control->set_max_speed_z(-get_pilot_velocity_z_max_dn(), pilot_velocity_z_max_up); pos_control->set_max_accel_z(pilot_accel_z); // call attitude controller @@ -1279,7 +1290,7 @@ void QuadPlane::init_loiter(void) loiter_nav->init_target(); // initialize vertical speed and acceleration - pos_control->set_max_speed_z(-pilot_velocity_z_max, pilot_velocity_z_max); + pos_control->set_max_speed_z(-get_pilot_velocity_z_max_dn(), pilot_velocity_z_max_up); pos_control->set_max_accel_z(pilot_accel_z); // initialise position and desired velocity @@ -1426,7 +1437,7 @@ void QuadPlane::control_loiter() motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED); // initialize vertical speed and acceleration - pos_control->set_max_speed_z(-pilot_velocity_z_max, pilot_velocity_z_max); + pos_control->set_max_speed_z(-get_pilot_velocity_z_max_dn(), pilot_velocity_z_max_up); pos_control->set_max_accel_z(pilot_accel_z); // process pilot's roll and pitch input @@ -1542,7 +1553,8 @@ float QuadPlane::get_pilot_desired_climb_rate_cms(void) const } uint16_t dead_zone = plane.channel_throttle->get_dead_zone(); uint16_t trim = (plane.channel_throttle->get_radio_max() + plane.channel_throttle->get_radio_min())/2; - return pilot_velocity_z_max * plane.channel_throttle->pwm_to_angle_dz_trim(dead_zone, trim) / 100.0f; + const float throttle_request = plane.channel_throttle->pwm_to_angle_dz_trim(dead_zone, trim) *0.01f; + return throttle_request * (throttle_request > 0.0f ? pilot_velocity_z_max_up : get_pilot_velocity_z_max_dn()); } @@ -2665,7 +2677,7 @@ void QuadPlane::setup_target_position(void) last_loiter_ms = now; // setup vertical speed and acceleration - pos_control->set_max_speed_z(-pilot_velocity_z_max, pilot_velocity_z_max); + pos_control->set_max_speed_z(-get_pilot_velocity_z_max_dn(), pilot_velocity_z_max_up); pos_control->set_max_accel_z(pilot_accel_z); // setup horizontal speed and acceleration @@ -2831,7 +2843,7 @@ bool QuadPlane::do_vtol_takeoff(const AP_Mission::Mission_Command& cmd) loiter_nav->init_target(); // initialize vertical speed and acceleration - pos_control->set_max_speed_z(-pilot_velocity_z_max, pilot_velocity_z_max); + pos_control->set_max_speed_z(-get_pilot_velocity_z_max_dn(), pilot_velocity_z_max_up); pos_control->set_max_accel_z(pilot_accel_z); // initialise position and desired velocity @@ -2851,7 +2863,7 @@ bool QuadPlane::do_vtol_takeoff(const AP_Mission::Mission_Command& cmd) // t = max(t_{accel}, 0) + max(t_{constant}, 0) const float d_total = (plane.next_WP_loc.alt - plane.current_loc.alt) * 0.01f; const float accel_m_s_s = MAX(10, pilot_accel_z) * 0.01f; - const float vel_max = MAX(10, pilot_velocity_z_max) * 0.01f; + const float vel_max = MAX(10, pilot_velocity_z_max_up) * 0.01f; const float vel_z = inertial_nav.get_velocity_z() * 0.01f; const float t_accel = (vel_max - vel_z) / accel_m_s_s; const float d_accel = vel_z * t_accel + 0.5f * accel_m_s_s * sq(t_accel); @@ -3583,4 +3595,13 @@ bool QuadPlane::show_vtol_view() const return show_vtol; } +// return the PILOT_VELZ_MAX_DN value if non zero, otherwise returns the PILOT_VELZ_MAX value. +uint16_t QuadPlane::get_pilot_velocity_z_max_dn() const +{ + if (pilot_velocity_z_max_dn == 0) { + return abs(pilot_velocity_z_max_up); + } + return abs(pilot_velocity_z_max_dn); +} + QuadPlane *QuadPlane::_singleton = nullptr; diff --git a/ArduPlane/quadplane.h b/ArduPlane/quadplane.h index 4998cdc133..d7f4b02fe2 100644 --- a/ArduPlane/quadplane.h +++ b/ArduPlane/quadplane.h @@ -154,6 +154,8 @@ public: // return true if the user has set ENABLE bool enabled(void) const { return enable != 0; } + + uint16_t get_pilot_velocity_z_max_dn() const; struct PACKED log_QControl_Tuning { LOG_PACKET_HEADER; @@ -200,7 +202,8 @@ private: AC_Loiter *loiter_nav; // maximum vertical velocity the pilot may request - AP_Int16 pilot_velocity_z_max; + AP_Int16 pilot_velocity_z_max_up; + AP_Int16 pilot_velocity_z_max_dn; // vertical acceleration the pilot may request AP_Int16 pilot_accel_z;