Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-08 17:08:28 -04:00
Code Issues Packages Projects Releases Wiki Activity

Plane: improvements to POSITION1 controller

Browse Source 
this improves 4 things in the POSITION1 controller based on logs from
4.2.0beta2. The changes are designed to increase the tolerance to
an incorrect value for Q_TRANS_DECEL, reducing landing overshoot

1) we fix the initialisation of the acceleration. The
   init_xy_controller() function assumes zero accel, so we need to
   call set_accel_desired_xy_cmss() just after that init to get the
   correct accel. Thanks to Leonard for this fix

2) if we decel more than expected due to too low Q_TRANS_DECEL we
   need to reduce the target speed, rather than putting the nose down

3) lower the default Q_P_JERK_XY to a value more appropriate for most
   quadplanes (Leonard suggested a value of 2)

4) fixed the pitch envelope from Q_BACKTRANS_MS to start after the
   airbrake phase is complete
This commit is contained in:
Andrew Tridgell 2022-03-14 13:52:06 +11:00
parent 3772029450
commit cc6fc2b130
2 changed files with 33 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

36
ArduPlane/quadplane.cpp
View File

@ -470,6 +470,7 @@ static const struct AP_Param::defaults_table_struct defaults_table[] = {
{ "Q_LOIT_SPEED", 500 },
{ "Q_WP_SPEED", 500 },
{ "Q_WP_ACCEL", 100 },
{ "Q_P_JERK_XY", 2 },
};
/*
@ -2175,7 +2176,8 @@ void QuadPlane::PosControlState::set_state(enum position_control_state s)
// never do a rate reset, if attitude control is not active it will be automaticaly reset before running, see: last_att_control_ms
// if it is active then the rate control should not be reset at all
qp.attitude_control->reset_yaw_target_and_rate(false);
pos1_start_speed = plane.ahrs.groundspeed_vector().length();
pos1_speed_limit = plane.ahrs.groundspeed_vector().length();
done_accel_init = false;
} else if (s == QPOS_AIRBRAKE) {
// start with zero integrator on vertical throttle
qp.pos_control->get_accel_z_pid().set_integrator(0);
@ -2422,11 +2424,13 @@ void QuadPlane::vtol_position_controller(void)
float target_speed = stopping_speed;
// maximum configured VTOL speed
const float wp_speed = wp_nav->get_default_speed_xy() * 0.01;
const float wp_speed = MAX(1.0, wp_nav->get_default_speed_xy() * 0.01);
const float scaled_wp_speed = get_scaled_wp_speed(degrees(diff_wp.angle()));
// limit target speed to initial pos1 speed, but at least twice the Q_WP_SPEED
target_speed = MIN(MAX(poscontrol.pos1_start_speed, 2*wp_speed), target_speed);
// limit target speed to a the pos1 speed limit, which starts out at the initial speed
// but is adjusted if we start putting our nose down. We always allow at least twice
// the WP speed
target_speed = MIN(MAX(poscontrol.pos1_speed_limit, 2*wp_speed), target_speed);
if (poscontrol.reached_wp_speed ||
rel_groundspeed_sq < sq(wp_speed) ||
@ -2452,17 +2456,39 @@ void QuadPlane::vtol_position_controller(void)
target_speed_xy_cms = diff_wp_norm * target_speed * 100;
target_accel_cms = diff_wp_norm * (-target_accel*100);
}
const float target_speed_ms = target_speed_xy_cms.length() * 0.01;
target_speed_xy_cms += landing_velocity * 100;
poscontrol.target_speed = target_speed_xy_cms.length()*0.01;
poscontrol.target_speed = target_speed_ms;
poscontrol.target_accel = target_accel;
if (!poscontrol.reached_wp_speed &&
rel_groundspeed_sq < sq(target_speed_ms) &&
rel_groundspeed_sq > sq(2*wp_speed) &&
plane.nav_pitch_cd < 0) {
// we have slowed down more than expected, likely due to
// drag from the props and we're starting to put our nose
// down as a result. We want to accept the slowdown and
// re-calculate the target speed profile
poscontrol.pos1_speed_limit = sqrtf(rel_groundspeed_sq);
}
// use input shaping and abide by accel and jerk limits
pos_control->input_vel_accel_xy(target_speed_xy_cms, target_accel_cms);
// run horizontal velocity controller
run_xy_controller(MAX(target_accel, transition_decel)*1.5);
if (!poscontrol.done_accel_init) {
/*
the pos controller init assumes zero accel, we need to
override that so that we can start decelerating more
quickly at the start of POSITION1
*/
poscontrol.done_accel_init = true;
pos_control->set_accel_desired_xy_cmss(target_accel_cms);
}
// nav roll and pitch are controller by position controller
plane.nav_roll_cd = pos_control->get_roll_cd();
plane.nav_pitch_cd = pos_control->get_pitch_cd();

3
ArduPlane/quadplane.h
View File

@ -447,7 +447,8 @@ private:
uint32_t last_log_ms;
bool reached_wp_speed;
uint32_t last_run_ms;
float pos1_start_speed;
float pos1_speed_limit;
bool done_accel_init;
Vector2f velocity_match;
uint32_t last_velocity_match_ms;
float target_speed;