Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-23 08:13:56 -04:00
Code Issues Packages Projects Releases Wiki Activity

Plane: PosControl Update

Browse Source
This commit is contained in:
Leonard Hall 2021-05-04 00:14:05 +09:30 committed by Andrew Tridgell
parent 266bd22df3
commit 57952861d6
3 changed files with 64 additions and 83 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
ArduPlane/GCS_Mavlink.cpp
View File

@ -169,7 +169,7 @@ void GCS_MAVLINK_Plane::send_nav_controller_output() const
targets.z * 1.0e-2f,
wp_nav_valid ? quadplane.wp_nav->get_wp_bearing_to_destination() : 0,
wp_nav_valid ? MIN(quadplane.wp_nav->get_wp_distance_to_destination(), UINT16_MAX) : 0,
(plane.control_mode != &plane.mode_qstabilize) ? quadplane.pos_control->get_alt_error() * 1.0e-2f : 0,
(plane.control_mode != &plane.mode_qstabilize) ? quadplane.pos_control->get_pos_error_z_cm() * 1.0e-2f : 0,
plane.airspeed_error * 100,
wp_nav_valid ? quadplane.wp_nav->crosstrack_error() : 0);
} else {

5
ArduPlane/qautotune.cpp
View File

@ -41,8 +41,9 @@ 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.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);
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_accel_z);
}

138
ArduPlane/quadplane.cpp
View File

@ -779,7 +779,7 @@ bool QuadPlane::setup(void)
}
AP_Param::load_object_from_eeprom(attitude_control, attitude_control->var_info);
pos_control = new AC_PosControl(*ahrs_view, inertial_nav, *motors, *attitude_control);
pos_control = new AC_PosControl(*ahrs_view, inertial_nav, *motors, *attitude_control, loop_delta_t);
if (!pos_control) {
AP_BoardConfig::config_error("Unable to allocate %s", "pos_control");
}
@ -811,7 +811,6 @@ bool QuadPlane::setup(void)
motors->set_throttle_range(thr_min_pwm, thr_max_pwm);
motors->set_update_rate(rc_speed);
motors->set_interlock(true);
pos_control->set_dt(loop_delta_t);
attitude_control->parameter_sanity_check();
wp_nav->wp_and_spline_init();
@ -1081,28 +1080,19 @@ void QuadPlane::control_stabilize(void)
void QuadPlane::run_z_controller(void)
{
const uint32_t now = AP_HAL::millis();
if (now - last_pidz_active_ms > 2000) {
if (!pos_control->is_active_z()) {
// initialize vertical maximum speeds and acceleration
pos_control->set_max_speed_accel_z(-get_pilot_velocity_z_max_dn(), pilot_velocity_z_max_up, pilot_accel_z);
if (!is_tailsitter()) {
// set alt target to current height on transition. This
// starts the Z controller off with the right values
set_alt_target_current();
// initialize the Alt Hold controller with no decent
pos_control->init_z_controller();
} else {
// tailsitters gain lots of vertical speed in transisison, set target to the stopping point
pos_control->set_target_to_stopping_point_z();
// make sure stopping point is above current alt
if (pos_control->get_alt_target() < inertial_nav.get_altitude()) {
set_alt_target_current();
// initialize the Alt Hold controller with no decent
// todo: do we need to add this function just for this?
pos_control->init_z_controller_no_descent();
}
}
gcs().send_text(MAV_SEVERITY_INFO, "Reset alt target to %.1f", (double)pos_control->get_alt_target() * 0.01f);
pos_control->set_desired_velocity_z(inertial_nav.get_velocity_z());
// initialize vertical speeds and leash lengths
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);
last_pidz_init_ms = now;
}
last_pidz_active_ms = now;
pos_control->update_z_controller();
}
@ -1142,13 +1132,11 @@ void QuadPlane::init_qacro(void)
// init quadplane hover mode
void QuadPlane::init_hover(void)
{
// initialize vertical speeds and leash lengths
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);
// initialize vertical maximum speeds and acceleration
pos_control->set_max_speed_accel_z(-get_pilot_velocity_z_max_dn(), pilot_velocity_z_max_up, pilot_accel_z);
// initialise position and desired velocity
set_alt_target_current();
pos_control->set_desired_velocity_z(inertial_nav.get_velocity_z());
// initialize the Alt Hold controller
pos_control->init_z_controller();
init_throttle_wait();
}
@ -1179,14 +1167,13 @@ 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(-get_pilot_velocity_z_max_dn(), pilot_velocity_z_max_up);
pos_control->set_max_accel_z(pilot_accel_z);
pos_control->set_max_speed_accel_z(-get_pilot_velocity_z_max_dn(), pilot_velocity_z_max_up, pilot_accel_z);
// call attitude controller
multicopter_attitude_rate_update(get_desired_yaw_rate_cds());
// call position controller
pos_control->set_alt_target_from_climb_rate_ff(target_climb_rate, plane.G_Dt, false);
pos_control->set_pos_target_z_from_climb_rate_cm(target_climb_rate, false);
run_z_controller();
}
@ -1297,7 +1284,7 @@ void QuadPlane::control_hover(void)
motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE);
attitude_control->set_throttle_out(0, true, 0);
relax_attitude_control();
pos_control->relax_alt_hold_controllers(0);
pos_control->relax_z_controller(0);
} else {
hold_hover(get_pilot_desired_climb_rate_cms());
}
@ -1310,12 +1297,10 @@ void QuadPlane::init_loiter(void)
loiter_nav->init_target();
// initialize vertical speed and acceleration
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);
pos_control->set_max_speed_accel_z(-get_pilot_velocity_z_max_dn(), pilot_velocity_z_max_up, pilot_accel_z);
// initialise position and desired velocity
set_alt_target_current();
pos_control->set_desired_velocity_z(inertial_nav.get_velocity_z());
// initialize the Alt Hold controller with no decent
pos_control->init_z_controller();
init_throttle_wait();
@ -1434,7 +1419,7 @@ void QuadPlane::control_loiter()
motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE);
attitude_control->set_throttle_out(0, true, 0);
relax_attitude_control();
pos_control->relax_alt_hold_controllers(0);
pos_control->relax_z_controller(0);
loiter_nav->clear_pilot_desired_acceleration();
loiter_nav->init_target();
return;
@ -1457,13 +1442,12 @@ 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(-get_pilot_velocity_z_max_dn(), pilot_velocity_z_max_up);
pos_control->set_max_accel_z(pilot_accel_z);
pos_control->set_max_speed_accel_z(-get_pilot_velocity_z_max_dn(), pilot_velocity_z_max_up, pilot_accel_z);
// process pilot's roll and pitch input
float target_roll_cd, target_pitch_cd;
get_pilot_desired_lean_angles(target_roll_cd, target_pitch_cd, loiter_nav->get_angle_max_cd(), attitude_control->get_althold_lean_angle_max());
loiter_nav->set_pilot_desired_acceleration(target_roll_cd, target_pitch_cd, plane.G_Dt);
loiter_nav->set_pilot_desired_acceleration(target_roll_cd, target_pitch_cd);
// run loiter controller
loiter_nav->update();
@ -1499,13 +1483,13 @@ void QuadPlane::control_loiter()
}
float height_above_ground = plane.relative_ground_altitude(plane.g.rangefinder_landing);
float descent_rate = (poscontrol.state == QPOS_LAND_FINAL)? land_speed_cms:landing_descent_rate_cms(height_above_ground);
pos_control->set_alt_target_from_climb_rate(-descent_rate, plane.G_Dt, true);
pos_control->set_pos_target_z_from_climb_rate_cm(-descent_rate, true);
check_land_complete();
} else if (plane.control_mode == &plane.mode_guided && guided_takeoff) {
pos_control->set_alt_target_from_climb_rate_ff(0, plane.G_Dt, false);
pos_control->set_pos_target_z_from_climb_rate_cm(0, false);
} else {
// update altitude target and call position controller
pos_control->set_alt_target_from_climb_rate_ff(get_pilot_desired_climb_rate_cms(), plane.G_Dt, false);
pos_control->set_pos_target_z_from_climb_rate_cm(get_pilot_desired_climb_rate_cms(), false);
}
run_z_controller();
}
@ -2005,7 +1989,8 @@ void QuadPlane::update(void)
// otherwise full relax
attitude_control->relax_attitude_controllers();
}
pos_control->relax_alt_hold_controllers(0);
// todo: do you want to set the throttle at this point?
pos_control->relax_z_controller(0);
}
const uint32_t now = AP_HAL::millis();
@ -2175,7 +2160,7 @@ void QuadPlane::update_throttle_hover()
}
// do not update while climbing or descending
if (!is_zero(pos_control->get_desired_velocity().z)) {
if (!is_zero(pos_control->get_vel_desired_cms().z)) {
return;
}
@ -2517,14 +2502,14 @@ void QuadPlane::vtol_position_controller(void)
} else {
poscontrol.max_speed = target_speed;
}
pos_control->set_desired_velocity_xy(target_speed_xy.x*100,
pos_control->set_vel_desired_xy_cms(target_speed_xy.x*100,
target_speed_xy.y*100);
// reset position controller xy target to current position
// because we only want velocity control (no position control)
const Vector3f& curr_pos = inertial_nav.get_position();
pos_control->set_xy_target(curr_pos.x, curr_pos.y);
pos_control->set_desired_accel_xy(0.0f,0.0f);
pos_control->set_pos_target_xy_cm(curr_pos.x, curr_pos.y);
pos_control->set_accel_desired_xy_cmss(0.0f, 0.0f);
// run horizontal velocity controller
pos_control->update_xy_controller();
@ -2585,14 +2570,14 @@ void QuadPlane::vtol_position_controller(void)
case QPOS_LAND_FINAL:
// set position controller desired velocity and acceleration to zero
pos_control->set_desired_velocity_xy(0.0f,0.0f);
pos_control->set_desired_accel_xy(0.0f,0.0f);
pos_control->set_vel_desired_xy_cms(0.0f,0.0f);
pos_control->set_accel_desired_xy_cmss(0.0f,0.0f);
// set position control target and update
if (should_relax()) {
loiter_nav->soften_for_landing();
} else {
pos_control->set_xy_target(poscontrol.target.x, poscontrol.target.y);
pos_control->set_pos_target_xy_cm(poscontrol.target.x, poscontrol.target.y);
}
pos_control->update_xy_controller();
@ -2650,22 +2635,21 @@ void QuadPlane::vtol_position_controller(void)
target_altitude_cm += MAX(terrain_altitude_offset_cm*100,0);
}
#endif
pos_control->set_alt_target_with_slew(target_altitude_cm, plane.G_Dt);
pos_control->set_alt_target_with_slew(target_altitude_cm);
} else {
pos_control->set_alt_target_from_climb_rate(0, plane.G_Dt, false);
pos_control->set_pos_target_z_from_climb_rate_cm(0, false);
}
break;
}
case QPOS_LAND_DESCEND: {
float height_above_ground = plane.relative_ground_altitude(plane.g.rangefinder_landing);
pos_control->set_alt_target_from_climb_rate(-landing_descent_rate_cms(height_above_ground),
plane.G_Dt, true);
pos_control->set_pos_target_z_from_climb_rate_cm(-landing_descent_rate_cms(height_above_ground), true);
break;
}
case QPOS_LAND_FINAL:
pos_control->set_alt_target_from_climb_rate(-land_speed_cms, plane.G_Dt, true);
pos_control->set_pos_target_z_from_climb_rate_cm(-land_speed_cms, true);
if ((options & OPTION_DISABLE_GROUND_EFFECT_COMP) == 0) {
ahrs.setTouchdownExpected(true);
}
@ -2706,12 +2690,10 @@ void QuadPlane::setup_target_position(void)
last_loiter_ms = now;
// setup vertical speed and acceleration
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);
pos_control->set_max_speed_accel_z(-get_pilot_velocity_z_max_dn(), pilot_velocity_z_max_up, pilot_accel_z);
// setup horizontal speed and acceleration
pos_control->set_max_speed_xy(wp_nav->get_default_speed_xy());
pos_control->set_max_accel_xy(wp_nav->get_wp_acceleration());
pos_control->set_max_speed_accel_xy(wp_nav->get_default_speed_xy(), wp_nav->get_wp_acceleration());
}
/*
@ -2727,11 +2709,11 @@ void QuadPlane::takeoff_controller(void)
setup_target_position();
// set position controller desired velocity and acceleration to zero
pos_control->set_desired_velocity_xy(0.0f,0.0f);
pos_control->set_desired_accel_xy(0.0f,0.0f);
pos_control->set_vel_desired_xy_cms(0.0f,0.0f);
pos_control->set_accel_desired_xy_cmss(0.0f,0.0f);
// set position control target and update
pos_control->set_xy_target(poscontrol.target.x, poscontrol.target.y);
pos_control->set_pos_target_xy_cm(poscontrol.target.x, poscontrol.target.y);
pos_control->update_xy_controller();
// nav roll and pitch are controller by position controller
@ -2742,7 +2724,7 @@ void QuadPlane::takeoff_controller(void)
plane.nav_pitch_cd,
get_pilot_input_yaw_rate_cds() + get_weathervane_yaw_rate_cds());
pos_control->set_alt_target_from_climb_rate(wp_nav->get_default_speed_up(), plane.G_Dt, true);
pos_control->set_pos_target_z_from_climb_rate_cm(wp_nav->get_default_speed_up(), true);
run_z_controller();
}
@ -2771,7 +2753,7 @@ void QuadPlane::waypoint_controller(void)
plane.nav_pitch_cd = wp_nav->get_pitch();
// climb based on altitude error
pos_control->set_alt_target_from_climb_rate_ff(assist_climb_rate_cms(), plane.G_Dt, false);
pos_control->set_pos_target_z_from_climb_rate_cm(assist_climb_rate_cms(), false);
run_z_controller();
}
@ -2836,7 +2818,7 @@ void QuadPlane::init_qrtl(void)
plane.prev_WP_loc = plane.current_loc;
poscontrol.state = QPOS_POSITION1;
poscontrol.speed_scale = 0;
pos_control->set_desired_accel_xy(0.0f, 0.0f);
pos_control->set_accel_desired_xy_cmss(0.0f, 0.0f);
pos_control->init_xy_controller();
int32_t from_alt;
int32_t to_alt;
@ -2879,12 +2861,10 @@ 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(-get_pilot_velocity_z_max_dn(), pilot_velocity_z_max_up);
pos_control->set_max_accel_z(pilot_accel_z);
pos_control->set_max_speed_accel_z(-get_pilot_velocity_z_max_dn(), pilot_velocity_z_max_up, pilot_accel_z);
// initialise position and desired velocity
set_alt_target_current();
pos_control->set_desired_velocity_z(inertial_nav.get_velocity_z());
// initialize the Alt Hold controller with no decent
pos_control->init_z_controller();
// also update nav_controller for status output
plane.nav_controller->update_waypoint(plane.prev_WP_loc, plane.next_WP_loc);
@ -2924,21 +2904,19 @@ bool QuadPlane::do_vtol_land(const AP_Mission::Mission_Command& cmd)
return false;
}
attitude_control->reset_rate_controller_I_terms();
pos_control->get_accel_z_pid().reset_I();
pos_control->get_vel_xy_pid().reset_I();
plane.set_next_WP(cmd.content.location);
// initially aim for current altitude
plane.next_WP_loc.alt = plane.current_loc.alt;
poscontrol.state = QPOS_POSITION1;
poscontrol.speed_scale = 0;
pos_control->set_desired_accel_xy(0.0f, 0.0f);
pos_control->init_xy_controller();
pos_control->init_z_controller();
throttle_wait = false;
landing_detect.lower_limit_start_ms = 0;
landing_detect.land_start_ms = 0;
set_alt_target_current();
plane.crash_state.is_crashed = false;
@ -2986,6 +2964,8 @@ bool QuadPlane::verify_vtol_takeoff(const AP_Mission::Mission_Command &cmd)
}
transition_state = is_tailsitter() ? TRANSITION_ANGLE_WAIT_FW : TRANSITION_AIRSPEED_WAIT;
plane.TECS_controller.set_pitch_max_limit(transition_pitch_max);
// todo: why are you doing this, I want to delete it.
set_alt_target_current();
#if AC_FENCE == ENABLED
@ -3142,8 +3122,8 @@ void QuadPlane::Log_Write_QControl_Tuning()
float des_alt_m = 0.0f;
int16_t target_climb_rate_cms = 0;
if (plane.control_mode != &plane.mode_qstabilize) {
des_alt_m = pos_control->get_alt_target() / 100.0f;
target_climb_rate_cms = pos_control->get_vel_target_z();
des_alt_m = pos_control->get_pos_target_z_cm() / 100.0f;
target_climb_rate_cms = pos_control->get_vel_target_z_cms();
}
struct log_QControl_Tuning pkt = {
@ -3223,7 +3203,7 @@ int8_t QuadPlane::forward_throttle_pct()
vel_forward.last_ms = AP_HAL::millis();
// work out the desired speed in forward direction
const Vector3f &desired_velocity_cms = pos_control->get_desired_velocity();
const Vector3f &desired_velocity_cms = pos_control->get_vel_desired_cms();
Vector3f vel_ned;
if (!plane.ahrs.get_velocity_NED(vel_ned)) {
// we don't know our velocity? EKF must be pretty sick
@ -3405,7 +3385,7 @@ bool QuadPlane::guided_mode_enabled(void)
*/
void QuadPlane::set_alt_target_current(void)
{
pos_control->set_alt_target_to_current_alt();
pos_control->set_pos_target_z_cm(inertial_nav.get_altitude());
}
// user initiated takeoff for guided mode
@ -3552,7 +3532,7 @@ void QuadPlane::update_throttle_mix(void)
bool accel_moving = (throttle_mix_accel_ef_filter.get().length() > LAND_CHECK_ACCEL_MOVING);
// check for requested decent
bool descent_not_demanded = pos_control->get_desired_velocity().z >= 0.0f;
bool descent_not_demanded = pos_control->get_vel_desired_cms().z >= 0.0f;
if (large_angle_request || large_angle_error || accel_moving || descent_not_demanded) {
attitude_control->set_throttle_mix_max(1.0);