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

Plane: seperate kinimatic shaping from pid limit setting

This commit is contained in:
Leonard Hall 2021-07-08 13:46:41 +09:30 committed by Randy Mackay
parent 48a99df2b3
commit 7db568c85a
2 changed files with 11 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
ArduPlane/qautotune.cpp
View File

@ -45,6 +45,9 @@ void QAutoTune::init_z_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_velocity_z_max_up,
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.pilot_velocity_z_max_up,
plane.quadplane.pilot_accel_z);
} }

8
ArduPlane/quadplane.cpp
View File

@ -1134,6 +1134,7 @@ void QuadPlane::init_hover(void)
{ {
// 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_velocity_z_max_up, pilot_accel_z);
pos_control->set_correction_speed_accel_z(-get_pilot_velocity_z_max_dn(), pilot_velocity_z_max_up, pilot_accel_z);
set_climb_rate_cms(0, false); set_climb_rate_cms(0, false);
init_throttle_wait(); init_throttle_wait();
@ -1320,6 +1321,7 @@ void QuadPlane::init_loiter(void)
// 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_velocity_z_max_up, pilot_accel_z);
pos_control->set_correction_speed_accel_z(-get_pilot_velocity_z_max_dn(), pilot_velocity_z_max_up, pilot_accel_z);
init_throttle_wait(); init_throttle_wait();
@ -2562,6 +2564,7 @@ void QuadPlane::run_xy_controller(void)
{ {
if (!pos_control->is_active_xy()) { if (!pos_control->is_active_xy()) {
pos_control->set_max_speed_accel_xy(wp_nav->get_default_speed_xy(), wp_nav->get_wp_acceleration()); pos_control->set_max_speed_accel_xy(wp_nav->get_default_speed_xy(), wp_nav->get_wp_acceleration());
pos_control->set_correction_speed_accel_xy(wp_nav->get_default_speed_xy(), wp_nav->get_wp_acceleration());
pos_control->init_xy_controller(); pos_control->init_xy_controller();
} }
pos_control->update_xy_controller(); pos_control->update_xy_controller();
@ -2600,6 +2603,8 @@ void QuadPlane::PosControlState::set_state(enum position_control_state s)
// back to normal // back to normal
qp.pos_control->set_max_speed_accel_xy(qp.wp_nav->get_default_speed_xy(), qp.pos_control->set_max_speed_accel_xy(qp.wp_nav->get_default_speed_xy(),
qp.wp_nav->get_wp_acceleration()); qp.wp_nav->get_wp_acceleration());
qp.pos_control->set_correction_speed_accel_xy(qp.wp_nav->get_default_speed_xy(),
qp.wp_nav->get_wp_acceleration());
} else if (s == QPOS_AIRBRAKE) { } else if (s == QPOS_AIRBRAKE) {
// start with zero integrator on vertical throttle // start with zero integrator on vertical throttle
qp.pos_control->get_accel_z_pid().set_integrator(0); qp.pos_control->get_accel_z_pid().set_integrator(0);
@ -2907,6 +2912,7 @@ void QuadPlane::vtol_position_controller(void)
const float scaled_wp_speed = get_scaled_wp_speed(degrees(diff_wp.angle())); const float scaled_wp_speed = get_scaled_wp_speed(degrees(diff_wp.angle()));
pos_control->set_max_speed_accel_xy(scaled_wp_speed*100, wp_nav->get_wp_acceleration()); pos_control->set_max_speed_accel_xy(scaled_wp_speed*100, wp_nav->get_wp_acceleration());
pos_control->set_correction_speed_accel_xy(scaled_wp_speed*100, wp_nav->get_wp_acceleration());
run_xy_controller(); run_xy_controller();
@ -3098,6 +3104,7 @@ void QuadPlane::setup_target_position(void)
// 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_velocity_z_max_up, pilot_accel_z);
pos_control->set_correction_speed_accel_z(-get_pilot_velocity_z_max_dn(), pilot_velocity_z_max_up, pilot_accel_z);
} }
/* /*
@ -3275,6 +3282,7 @@ bool QuadPlane::do_vtol_takeoff(const AP_Mission::Mission_Command& cmd)
// 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_velocity_z_max_up, pilot_accel_z);
pos_control->set_correction_speed_accel_z(-get_pilot_velocity_z_max_dn(), pilot_velocity_z_max_up, pilot_accel_z);
// initialise the vertical position controller // initialise the vertical position controller
pos_control->init_z_controller(); pos_control->init_z_controller();