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

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 <pb-gh@barker.dropbear.id.au>
This commit is contained in:
Hwurzburg 2021-03-16 16:54:03 -05:00 committed by Peter Barker
parent 46fb88e5a0
commit 8aabf7c22a
3 changed files with 40 additions and 16 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
ArduPlane/qautotune.cpp
View File

@ -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);
}

45
ArduPlane/quadplane.cpp
View File

@ -39,13 +39,22 @@ 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),
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
@ -335,6 +344,8 @@ const AP_Param::GroupInfo QuadPlane::var_info[] = {
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;

5
ArduPlane/quadplane.h
View File

@ -155,6 +155,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;
uint64_t time_us;
@ -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;