Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 14:38:30 -04:00
Code Issues Packages Projects Releases Wiki Activity

Plane: Q_VELZ_MAX -> Q_PILOT_SPD_UP and Q_VELZ_MAX_DN -> Q_PILOT_SPD_DN

Browse Source
This commit is contained in:
Andrew Tridgell 2024-01-19 13:35:47 +11:00
parent d9ee8113ee
commit 57e88e2242
5 changed files with 42 additions and 36 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
ArduPlane/mode_qhover.cpp
View File

@ -6,8 +6,8 @@
bool ModeQHover::_enter() bool ModeQHover::_enter()
{ {
// set vertical speed and acceleration limits // set vertical speed and acceleration limits
pos_control->set_max_speed_accel_z(-quadplane.get_pilot_velocity_z_max_dn(), quadplane.pilot_velocity_z_max_up, 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);
pos_control->set_correction_speed_accel_z(-quadplane.get_pilot_velocity_z_max_dn(), quadplane.pilot_velocity_z_max_up, 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);
quadplane.set_climb_rate_cms(0); quadplane.set_climb_rate_cms(0);
quadplane.init_throttle_wait(); quadplane.init_throttle_wait();

6
ArduPlane/mode_qloiter.cpp
View File

@ -10,8 +10,8 @@ bool ModeQLoiter::_enter()
loiter_nav->init_target(); loiter_nav->init_target();
// set vertical speed and acceleration limits // set vertical speed and acceleration limits
pos_control->set_max_speed_accel_z(-quadplane.get_pilot_velocity_z_max_dn(), quadplane.pilot_velocity_z_max_up, 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);
pos_control->set_correction_speed_accel_z(-quadplane.get_pilot_velocity_z_max_dn(), quadplane.pilot_velocity_z_max_up, 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);
quadplane.init_throttle_wait(); quadplane.init_throttle_wait();
@ -66,7 +66,7 @@ void ModeQLoiter::run()
quadplane.set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED); quadplane.set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
// set vertical speed and acceleration limits // set vertical speed and acceleration limits
pos_control->set_max_speed_accel_z(-quadplane.get_pilot_velocity_z_max_dn(), quadplane.pilot_velocity_z_max_up, 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);
// process pilot's roll and pitch input // process pilot's roll and pitch input
float target_roll_cd, target_pitch_cd; float target_roll_cd, target_pitch_cd;

4
ArduPlane/qautotune.cpp
View File

@ -43,10 +43,10 @@ void QAutoTune::init_z_limits()
{ {
// set vertical speed and acceleration 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.pos_control->set_max_speed_accel_z(-plane.quadplane.get_pilot_velocity_z_max_dn(),
plane.quadplane.pilot_velocity_z_max_up, plane.quadplane.pilot_speed_z_max_up*100,
plane.quadplane.pilot_accel_z); plane.quadplane.pilot_accel_z);
plane.quadplane.pos_control->set_correction_speed_accel_z(-plane.quadplane.get_pilot_velocity_z_max_dn(), plane.quadplane.pos_control->set_correction_speed_accel_z(-plane.quadplane.get_pilot_velocity_z_max_dn(),
plane.quadplane.pilot_velocity_z_max_up, plane.quadplane.pilot_speed_z_max_up*100,
plane.quadplane.pilot_accel_z); plane.quadplane.pilot_accel_z);
} }

56
ArduPlane/quadplane.cpp
View File

@ -43,23 +43,23 @@ const AP_Param::GroupInfo QuadPlane::var_info[] = {
// @Path: ../libraries/AC_AttitudeControl/AC_PosControl.cpp // @Path: ../libraries/AC_AttitudeControl/AC_PosControl.cpp
AP_SUBGROUPPTR(pos_control, "P", 17, QuadPlane, AC_PosControl), AP_SUBGROUPPTR(pos_control, "P", 17, QuadPlane, AC_PosControl),
// @Param: VELZ_MAX // @Param: PILOT_SPD_UP
// @DisplayName: Pilot maximum vertical speed up // @DisplayName: Pilot maximum vertical speed up
// @Description: The maximum ascending vertical velocity the pilot may request in cm/s // @Description: The maximum ascending vertical velocity the pilot may request in m/s
// @Units: cm/s // @Units: m/s
// @Range: 50 500 // @Range: 0.5 5
// @Increment: 10 // @Increment: 0.1
// @User: Standard // @User: Standard
AP_GROUPINFO("VELZ_MAX", 18, QuadPlane, pilot_velocity_z_max_up, 250), AP_GROUPINFO("PILOT_SPD_UP", 18, QuadPlane, pilot_speed_z_max_up, 2.50),
// @Param: VELZ_MAX_DN // @Param: PILOT_SPD_DN
// @DisplayName: Pilot maximum vertical speed down // @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. // @Description: The maximum vertical velocity the pilot may request in m/s going down. If 0, uses Q_PILOT_SPD_UP value.
// @Units: cm/s // @Units: m/s
// @Range: 50 500 // @Range: 0.5 5
// @Increment: 10 // @Increment: 0.1
// @User: Standard // @User: Standard
AP_GROUPINFO("VELZ_MAX_DN", 60, QuadPlane, pilot_velocity_z_max_dn, 0), AP_GROUPINFO("PILOT_SPD_DN", 60, QuadPlane, pilot_speed_z_max_dn, 0),
// @Param: ACCEL_Z // @Param: ACCEL_Z
// @DisplayName: Pilot vertical acceleration // @DisplayName: Pilot vertical acceleration
@ -823,8 +823,11 @@ bool QuadPlane::setup(void)
AP_Param::convert_old_parameters(&q_conversion_table[0], ARRAY_SIZE(q_conversion_table)); AP_Param::convert_old_parameters(&q_conversion_table[0], ARRAY_SIZE(q_conversion_table));
// added January 2024 // centi-conversions added January 2024
land_final_speed.convert_centi_parameter(AP_PARAM_INT16); 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);
tailsitter.setup(); tailsitter.setup();
@ -1021,7 +1024,7 @@ void QuadPlane::run_z_controller(void)
} }
if ((now - last_pidz_active_ms) > 20 || !pos_control->is_active_z()) { if ((now - last_pidz_active_ms) > 20 || !pos_control->is_active_z()) {
// set vertical speed and acceleration limits // set vertical speed and acceleration limits
pos_control->set_max_speed_accel_z(-get_pilot_velocity_z_max_dn(), pilot_velocity_z_max_up, 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);
// initialise the vertical position controller // initialise the vertical position controller
if (!tailsitter.enabled()) { if (!tailsitter.enabled()) {
@ -1074,7 +1077,7 @@ void QuadPlane::hold_hover(float target_climb_rate_cms)
set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED); set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
// set vertical speed and acceleration limits // set vertical speed and acceleration limits
pos_control->set_max_speed_accel_z(-get_pilot_velocity_z_max_dn(), pilot_velocity_z_max_up, 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);
// call attitude controller // call attitude controller
multicopter_attitude_rate_update(get_desired_yaw_rate_cds(false)); multicopter_attitude_rate_update(get_desired_yaw_rate_cds(false));
@ -1360,7 +1363,7 @@ float QuadPlane::get_pilot_desired_climb_rate_cms(void) const
uint16_t dead_zone = plane.channel_throttle->get_dead_zone(); 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; uint16_t trim = (plane.channel_throttle->get_radio_max() + plane.channel_throttle->get_radio_min())/2;
const float throttle_request = plane.channel_throttle->pwm_to_angle_dz_trim(dead_zone, trim) *0.01f; 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()); return throttle_request * (throttle_request > 0.0f ? pilot_speed_z_max_up*100 : get_pilot_velocity_z_max_dn());
} }
@ -3096,8 +3099,8 @@ void QuadPlane::setup_target_position(void)
poscontrol.target_cm.z = plane.next_WP_loc.alt - origin.alt; poscontrol.target_cm.z = plane.next_WP_loc.alt - origin.alt;
// set vertical speed and acceleration limits // set vertical speed and acceleration limits
pos_control->set_max_speed_accel_z(-get_pilot_velocity_z_max_dn(), pilot_velocity_z_max_up, 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);
pos_control->set_correction_speed_accel_z(-get_pilot_velocity_z_max_dn(), pilot_velocity_z_max_up, 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);
} }
/* /*
@ -3346,8 +3349,8 @@ bool QuadPlane::do_vtol_takeoff(const AP_Mission::Mission_Command& cmd)
throttle_wait = false; throttle_wait = false;
// set vertical speed and acceleration limits // set vertical speed and acceleration limits
pos_control->set_max_speed_accel_z(-get_pilot_velocity_z_max_dn(), pilot_velocity_z_max_up, 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);
pos_control->set_correction_speed_accel_z(-get_pilot_velocity_z_max_dn(), pilot_velocity_z_max_up, 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);
// initialise the vertical position controller // initialise the vertical position controller
pos_control->init_z_controller(); pos_control->init_z_controller();
@ -3365,7 +3368,7 @@ bool QuadPlane::do_vtol_takeoff(const AP_Mission::Mission_Command& cmd)
// t = max(t_{accel}, 0) + max(t_{constant}, 0) // 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 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(10, pilot_accel_z) * 0.01f;
const float vel_max = MAX(10, pilot_velocity_z_max_up) * 0.01f; 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 vel_z = inertial_nav.get_velocity_z_up_cms() * 0.01f;
const float t_accel = (vel_max - vel_z) / accel_m_s_s; 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); const float d_accel = vel_z * t_accel + 0.5f * accel_m_s_s * sq(t_accel);
@ -4214,13 +4217,16 @@ bool SLT_Transition::show_vtol_view() const
return quadplane.in_vtol_mode(); return quadplane.in_vtol_mode();
} }
// return the PILOT_VELZ_MAX_DN value if non zero, otherwise returns the PILOT_VELZ_MAX value. /*
return the PILOT_VELZ_MAX_DN value if non zero, otherwise returns the PILOT_VELZ_MAX value.
return is in cm/s
*/
uint16_t QuadPlane::get_pilot_velocity_z_max_dn() const uint16_t QuadPlane::get_pilot_velocity_z_max_dn() const
{ {
if (pilot_velocity_z_max_dn == 0) { if (is_zero(pilot_speed_z_max_dn)) {
return abs(pilot_velocity_z_max_up); return abs(pilot_speed_z_max_up*100);
} }
return abs(pilot_velocity_z_max_dn); return abs(pilot_speed_z_max_dn*100);
} }
/* /*

4
ArduPlane/quadplane.h
View File

@ -213,8 +213,8 @@ private:
AC_Loiter *loiter_nav; AC_Loiter *loiter_nav;
// maximum vertical velocity the pilot may request // maximum vertical velocity the pilot may request
AP_Int16 pilot_velocity_z_max_up; AP_Float pilot_speed_z_max_up;
AP_Int16 pilot_velocity_z_max_dn; AP_Float pilot_speed_z_max_dn;
// vertical acceleration the pilot may request // vertical acceleration the pilot may request
AP_Int16 pilot_accel_z; AP_Int16 pilot_accel_z;