From 108a0b77bdf570b9150fb5ccaaf42094e1cdd9a1 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 19 Jan 2024 13:40:25 +1100 Subject: [PATCH] Plane: Q_ACCEL_Z -> Q_PILOT_ACCEL_Z --- ArduPlane/mode_qhover.cpp | 4 ++-- ArduPlane/mode_qloiter.cpp | 6 +++--- ArduPlane/qautotune.cpp | 4 ++-- ArduPlane/quadplane.cpp | 26 +++++++++++++------------- ArduPlane/quadplane.h | 2 +- 5 files changed, 21 insertions(+), 21 deletions(-) diff --git a/ArduPlane/mode_qhover.cpp b/ArduPlane/mode_qhover.cpp index a11bcd100b..e9e4ede8e6 100644 --- a/ArduPlane/mode_qhover.cpp +++ b/ArduPlane/mode_qhover.cpp @@ -6,8 +6,8 @@ bool ModeQHover::_enter() { // set vertical speed and acceleration limits - pos_control->set_max_speed_accel_z(-quadplane.get_pilot_velocity_z_max_dn(), quadplane.pilot_speed_z_max_up*100, quadplane.pilot_accel_z); - pos_control->set_correction_speed_accel_z(-quadplane.get_pilot_velocity_z_max_dn(), quadplane.pilot_speed_z_max_up*100, quadplane.pilot_accel_z); + pos_control->set_max_speed_accel_z(-quadplane.get_pilot_velocity_z_max_dn(), quadplane.pilot_speed_z_max_up*100, quadplane.pilot_accel_z*100); + pos_control->set_correction_speed_accel_z(-quadplane.get_pilot_velocity_z_max_dn(), quadplane.pilot_speed_z_max_up*100, quadplane.pilot_accel_z*100); quadplane.set_climb_rate_cms(0); quadplane.init_throttle_wait(); diff --git a/ArduPlane/mode_qloiter.cpp b/ArduPlane/mode_qloiter.cpp index 76271644b1..fdadd0e255 100644 --- a/ArduPlane/mode_qloiter.cpp +++ b/ArduPlane/mode_qloiter.cpp @@ -10,8 +10,8 @@ bool ModeQLoiter::_enter() loiter_nav->init_target(); // set vertical speed and acceleration limits - pos_control->set_max_speed_accel_z(-quadplane.get_pilot_velocity_z_max_dn(), quadplane.pilot_speed_z_max_up*100, quadplane.pilot_accel_z); - pos_control->set_correction_speed_accel_z(-quadplane.get_pilot_velocity_z_max_dn(), quadplane.pilot_speed_z_max_up*100, quadplane.pilot_accel_z); + pos_control->set_max_speed_accel_z(-quadplane.get_pilot_velocity_z_max_dn(), quadplane.pilot_speed_z_max_up*100, quadplane.pilot_accel_z*100); + pos_control->set_correction_speed_accel_z(-quadplane.get_pilot_velocity_z_max_dn(), quadplane.pilot_speed_z_max_up*100, quadplane.pilot_accel_z*100); quadplane.init_throttle_wait(); @@ -66,7 +66,7 @@ void ModeQLoiter::run() quadplane.set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED); // set vertical speed and acceleration limits - pos_control->set_max_speed_accel_z(-quadplane.get_pilot_velocity_z_max_dn(), quadplane.pilot_speed_z_max_up*100, quadplane.pilot_accel_z); + pos_control->set_max_speed_accel_z(-quadplane.get_pilot_velocity_z_max_dn(), quadplane.pilot_speed_z_max_up*100, quadplane.pilot_accel_z*100); // process pilot's roll and pitch input float target_roll_cd, target_pitch_cd; diff --git a/ArduPlane/qautotune.cpp b/ArduPlane/qautotune.cpp index 3bd66b80de..67db319a0e 100644 --- a/ArduPlane/qautotune.cpp +++ b/ArduPlane/qautotune.cpp @@ -44,10 +44,10 @@ void QAutoTune::init_z_limits() // set vertical speed and acceleration limits plane.quadplane.pos_control->set_max_speed_accel_z(-plane.quadplane.get_pilot_velocity_z_max_dn(), plane.quadplane.pilot_speed_z_max_up*100, - plane.quadplane.pilot_accel_z); + plane.quadplane.pilot_accel_z*100); plane.quadplane.pos_control->set_correction_speed_accel_z(-plane.quadplane.get_pilot_velocity_z_max_dn(), plane.quadplane.pilot_speed_z_max_up*100, - plane.quadplane.pilot_accel_z); + plane.quadplane.pilot_accel_z*100); } diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp index a370391d99..378b00beb5 100644 --- a/ArduPlane/quadplane.cpp +++ b/ArduPlane/quadplane.cpp @@ -61,14 +61,14 @@ const AP_Param::GroupInfo QuadPlane::var_info[] = { // @User: Standard AP_GROUPINFO("PILOT_SPD_DN", 60, QuadPlane, pilot_speed_z_max_dn, 0), - // @Param: ACCEL_Z + // @Param: PILOT_ACCEL_Z // @DisplayName: Pilot vertical acceleration // @Description: The vertical acceleration used when pilot is controlling the altitude - // @Units: cm/s/s - // @Range: 50 500 - // @Increment: 10 + // @Units: m/s/s + // @Range: 0.5 5 + // @Increment: 0.1 // @User: Standard - AP_GROUPINFO("ACCEL_Z", 19, QuadPlane, pilot_accel_z, 250), + AP_GROUPINFO("PILOT_ACCEL_Z", 19, QuadPlane, pilot_accel_z, 2.5), // @Group: WP_ // @Path: ../libraries/AC_WPNav/AC_WPNav.cpp @@ -827,7 +827,7 @@ bool QuadPlane::setup(void) land_final_speed.convert_centi_parameter(AP_PARAM_INT16); pilot_speed_z_max_up.convert_centi_parameter(AP_PARAM_INT16); pilot_speed_z_max_dn.convert_centi_parameter(AP_PARAM_INT16); - + pilot_accel_z.convert_centi_parameter(AP_PARAM_INT16); tailsitter.setup(); @@ -1024,7 +1024,7 @@ void QuadPlane::run_z_controller(void) } if ((now - last_pidz_active_ms) > 20 || !pos_control->is_active_z()) { // set vertical speed and acceleration limits - pos_control->set_max_speed_accel_z(-get_pilot_velocity_z_max_dn(), pilot_speed_z_max_up*100, pilot_accel_z); + pos_control->set_max_speed_accel_z(-get_pilot_velocity_z_max_dn(), pilot_speed_z_max_up*100, pilot_accel_z*100); // initialise the vertical position controller if (!tailsitter.enabled()) { @@ -1077,7 +1077,7 @@ void QuadPlane::hold_hover(float target_climb_rate_cms) set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED); // set vertical speed and acceleration limits - pos_control->set_max_speed_accel_z(-get_pilot_velocity_z_max_dn(), pilot_speed_z_max_up*100, pilot_accel_z); + pos_control->set_max_speed_accel_z(-get_pilot_velocity_z_max_dn(), pilot_speed_z_max_up*100, pilot_accel_z*100); // call attitude controller multicopter_attitude_rate_update(get_desired_yaw_rate_cds(false)); @@ -3099,8 +3099,8 @@ void QuadPlane::setup_target_position(void) poscontrol.target_cm.z = plane.next_WP_loc.alt - origin.alt; // set vertical speed and acceleration limits - pos_control->set_max_speed_accel_z(-get_pilot_velocity_z_max_dn(), pilot_speed_z_max_up*100, pilot_accel_z); - pos_control->set_correction_speed_accel_z(-get_pilot_velocity_z_max_dn(), pilot_speed_z_max_up*100, pilot_accel_z); + pos_control->set_max_speed_accel_z(-get_pilot_velocity_z_max_dn(), pilot_speed_z_max_up*100, pilot_accel_z*100); + pos_control->set_correction_speed_accel_z(-get_pilot_velocity_z_max_dn(), pilot_speed_z_max_up*100, pilot_accel_z*100); } /* @@ -3349,8 +3349,8 @@ bool QuadPlane::do_vtol_takeoff(const AP_Mission::Mission_Command& cmd) throttle_wait = false; // set vertical speed and acceleration limits - pos_control->set_max_speed_accel_z(-get_pilot_velocity_z_max_dn(), pilot_speed_z_max_up*100, pilot_accel_z); - pos_control->set_correction_speed_accel_z(-get_pilot_velocity_z_max_dn(), pilot_speed_z_max_up*100, pilot_accel_z); + pos_control->set_max_speed_accel_z(-get_pilot_velocity_z_max_dn(), pilot_speed_z_max_up*100, pilot_accel_z*100); + pos_control->set_correction_speed_accel_z(-get_pilot_velocity_z_max_dn(), pilot_speed_z_max_up*100, pilot_accel_z*100); // initialise the vertical position controller pos_control->init_z_controller(); @@ -3367,7 +3367,7 @@ bool QuadPlane::do_vtol_takeoff(const AP_Mission::Mission_Command& cmd) // t_{constant} = \frac{d_{remaining}}{V_z} // 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 accel_m_s_s = MAX(0.1, pilot_accel_z); const float vel_max = MAX(0.1, pilot_speed_z_max_up); const float vel_z = inertial_nav.get_velocity_z_up_cms() * 0.01f; const float t_accel = (vel_max - vel_z) / accel_m_s_s; diff --git a/ArduPlane/quadplane.h b/ArduPlane/quadplane.h index 5fa0a5fee2..63a0cb235a 100644 --- a/ArduPlane/quadplane.h +++ b/ArduPlane/quadplane.h @@ -217,7 +217,7 @@ private: AP_Float pilot_speed_z_max_dn; // vertical acceleration the pilot may request - AP_Int16 pilot_accel_z; + AP_Float pilot_accel_z; // air mode state: OFF, ON, ASSISTED_FLIGHT_ONLY AirMode air_mode;