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

Plane: cope with overshoot in POSITION1 VTOL land state 

when we overshoot run a simple position controller that tries to point
the nose at the landing point and aims for the position2 speed
threshold
This commit is contained in:
Andrew Tridgell 2022-07-30 11:48:32 +10:00
parent 693c9d9128
commit dbc3744faa
2 changed files with 38 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

44
ArduPlane/quadplane.cpp
View File

@ -2194,12 +2194,13 @@ void QuadPlane::poscontrol_init_approach(void)
*/
void QuadPlane::log_QPOS(void)
{
AP::logger().WriteStreaming("QPOS", "TimeUS,State,Dist,TSpd,TAcc", "QBfff",
AP::logger().WriteStreaming("QPOS", "TimeUS,State,Dist,TSpd,TAcc,OShoot", "QBfffB",
AP_HAL::micros64(),
poscontrol.get_state(),
plane.auto_state.wp_distance,
poscontrol.target_speed,
poscontrol.target_accel);
poscontrol.target_accel,
poscontrol.overshoot);
}
/*
@ -2237,6 +2238,7 @@ void QuadPlane::PosControlState::set_state(enum position_control_state s)
state = s;
qp.log_QPOS();
last_log_ms = now;
overshoot = false;
}
last_state_change_ms = now;
@ -2458,6 +2460,7 @@ void QuadPlane::vtol_position_controller(void)
const float distance = diff_wp.length();
const Vector2f rel_groundspeed_vector = landing_closing_velocity();
const float rel_groundspeed_sq = rel_groundspeed_vector.length_squared();
const float closing_groundspeed = rel_groundspeed_vector * diff_wp.normalized();
// calculate speed we should be at to reach the position2
// target speed at the position2 distance threshold, assuming
@ -2493,11 +2496,31 @@ void QuadPlane::vtol_position_controller(void)
Vector2f target_speed_xy_cms;
Vector2f target_accel_cms;
const float target_accel = accel_needed(distance, rel_groundspeed_sq);
bool have_target_yaw = false;
float target_yaw_deg;
const float target_accel = accel_needed(distance, sq(closing_groundspeed));
if (distance > 0.1) {
Vector2f diff_wp_norm = diff_wp.normalized();
target_speed_xy_cms = diff_wp_norm * target_speed * 100;
target_accel_cms = diff_wp_norm * (-target_accel*100);
target_yaw_deg = degrees(diff_wp_norm.angle());
const float yaw_err_deg = wrap_180(target_yaw_deg - degrees(plane.ahrs.yaw));
bool overshoot = (closing_groundspeed < 0 || fabsf(yaw_err_deg) > 60);
if (overshoot && !poscontrol.overshoot) {
gcs().send_text(MAV_SEVERITY_INFO,"VTOL Overshoot d=%.1f cs=%.1f yerr=%.1f",
distance, closing_groundspeed, yaw_err_deg);
poscontrol.overshoot = true;
}
if (poscontrol.overshoot) {
/* we have overshot the landing point or our nose is
off by more than 60 degrees. Zero target accel and
point nose at the landing point. Set target speed
to our position2 threshold speed
*/
target_accel_cms.zero();
target_speed_xy_cms = diff_wp_norm * position2_target_speed * 100;
have_target_yaw = true;
}
}
const float target_speed_ms = target_speed_xy_cms.length() * 0.01;
@ -2541,10 +2564,17 @@ void QuadPlane::vtol_position_controller(void)
}
// call attitude controller
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(plane.nav_roll_cd,
plane.nav_pitch_cd,
desired_auto_yaw_rate_cds() + get_weathervane_yaw_rate_cds());
if ((plane.auto_state.wp_distance < position2_dist_threshold) && tiltrotor.tilt_angle_achieved()) {
if (have_target_yaw) {
attitude_control->input_euler_angle_roll_pitch_yaw(plane.nav_roll_cd,
plane.nav_pitch_cd,
target_yaw_deg*100, true);
} else {
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(plane.nav_roll_cd,
plane.nav_pitch_cd,
desired_auto_yaw_rate_cds() + get_weathervane_yaw_rate_cds());
}
if ((plane.auto_state.wp_distance < position2_dist_threshold) && tiltrotor.tilt_angle_achieved() &&
fabsf(rel_groundspeed_sq) < sq(3*position2_target_speed)) {
// if continuous tiltrotor only advance to position 2 once tilts have finished moving
poscontrol.set_state(QPOS_POSITION2);
poscontrol.pilot_correction_done = false;

1
ArduPlane/quadplane.h
View File

@ -478,6 +478,7 @@ private:
float target_speed;
float target_accel;
uint32_t last_pos_reset_ms;
bool overshoot;
private:
uint32_t last_state_change_ms;
enum position_control_state state;