diff --git a/ArduPlane/qautotune.cpp b/ArduPlane/qautotune.cpp index 3becb86ed3..3f9a385202 100644 --- a/ArduPlane/qautotune.cpp +++ b/ArduPlane/qautotune.cpp @@ -41,6 +41,7 @@ void QAutoTune::get_pilot_desired_rp_yrate_cd(float &des_roll_cd, float &des_pit void QAutoTune::init_z_limits() { + // set vertical speed and acceleration limits 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); diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp index 729f3ad9f8..d7cabcf9e4 100644 --- a/ArduPlane/quadplane.cpp +++ b/ArduPlane/quadplane.cpp @@ -1080,15 +1080,14 @@ void QuadPlane::run_z_controller(void) { const uint32_t now = AP_HAL::millis(); if (!pos_control->is_active_z()) { - // initialize vertical maximum speeds and acceleration + // 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); + // initialise the vertical position controller if (!is_tailsitter()) { - // initialize the Alt Hold controller with no decent pos_control->init_z_controller(); } else { - // initialize the Alt Hold controller with no decent - // todo: do we need to add this function just for this? + // initialise the vertical position controller with no descent pos_control->init_z_controller_no_descent(); } } @@ -1131,7 +1130,7 @@ void QuadPlane::init_qacro(void) // init quadplane hover mode void QuadPlane::init_hover(void) { - // initialize vertical maximum speeds and acceleration + // 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); set_climb_rate_cms(0, false); @@ -1169,7 +1168,7 @@ void QuadPlane::hold_hover(float target_climb_rate_cms) // motors use full range motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED); - // initialize vertical speeds and acceleration + // 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); // call attitude controller @@ -1318,7 +1317,7 @@ void QuadPlane::init_loiter(void) loiter_nav->clear_pilot_desired_acceleration(); loiter_nav->init_target(); - // initialize vertical speed and acceleration + // 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); init_throttle_wait(); @@ -1437,7 +1436,7 @@ float QuadPlane::landing_descent_rate_cms(float height_above_ground) const if ((options & OPTION_THR_LANDING_CONTROL) != 0) { // allow throttle control for landing speed - float thr_in = get_pilot_land_throttle(); + const float thr_in = get_pilot_land_throttle(); const float dz = 0.1; const float thresh1 = 0.5+dz; const float thresh2 = 0.5-dz; @@ -1488,7 +1487,7 @@ void QuadPlane::control_loiter() // motors use full range motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED); - // initialize vertical speed and acceleration + // 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); // process pilot's roll and pitch input @@ -1513,7 +1512,7 @@ void QuadPlane::control_loiter() last_pidz_init_ms, last_pidz_init_ms+transition_time_ms*2); if (plane.nav_pitch_cd > pitch_limit_cd) { plane.nav_pitch_cd = pitch_limit_cd; - pos_control->set_limit_xy(); + pos_control->set_externally_limited_xy(); } } @@ -2525,7 +2524,7 @@ void QuadPlane::update_land_positioning(void) poscontrol.target_cm += poscontrol.target_vel_cms * dt; - poscontrol.pilot_correction_active = (fabsf(roll_in) > 0 || fabsf(pitch_in) > 0); + poscontrol.pilot_correction_active = (!is_zero(roll_in)|| !is_zero(pitch_in)); if (poscontrol.pilot_correction_active) { poscontrol.pilot_correction_done = true; } @@ -2605,14 +2604,13 @@ void QuadPlane::vtol_position_controller(void) } else { poscontrol.max_speed = target_speed; } - pos_control->set_vel_desired_xy_cms(target_speed_xy.x*100, - target_speed_xy.y*100); + pos_control->set_vel_desired_xy_cms(target_speed_xy * 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_pos_target_xy_cm(curr_pos.x, curr_pos.y); - pos_control->set_accel_desired_xy_cmss(0.0f, 0.0f); + pos_control->set_accel_desired_xy_cmss(Vector2f()); // run horizontal velocity controller run_xy_controller(); @@ -2642,7 +2640,7 @@ void QuadPlane::vtol_position_controller(void) plane.nav_pitch_cd = pitch_limit_cd; // tell the pos controller we have limited the pitch to // stop integrator buildup - pos_control->set_limit_xy(); + pos_control->set_externally_limited_xy(); } // call attitude controller @@ -2665,8 +2663,8 @@ void QuadPlane::vtol_position_controller(void) for final land repositioning and descent we run the position controller */ if (poscontrol.pilot_correction_done) { - // if we have done any repositioning then to handle GPS glitches we need to switch - // to velocity control + // if the pilot has repositioned the vehicle then we switch to velocity control. This prevents the vehicle + // shifting position in the event of GPS glitches. Vector3f zero; pos_control->input_vel_accel_xy(poscontrol.target_vel_cms, zero); } else { @@ -2681,7 +2679,7 @@ void QuadPlane::vtol_position_controller(void) run_xy_controller(); - // nav roll and pitch are controller by position controller + // nav roll and pitch are controlled by position controller plane.nav_roll_cd = pos_control->get_roll_cd(); plane.nav_pitch_cd = pos_control->get_pitch_cd(); @@ -2816,8 +2814,8 @@ void QuadPlane::setup_target_position(void) last_auto_target = loc; } last_loiter_ms = now; - - // setup vertical speed and acceleration + + // 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); } @@ -2834,8 +2832,8 @@ void QuadPlane::takeoff_controller(void) setup_target_position(); // set position controller desired velocity and acceleration to zero - pos_control->set_vel_desired_xy_cms(0.0f,0.0f); - pos_control->set_accel_desired_xy_cmss(0.0f,0.0f); + pos_control->set_vel_desired_xy_cms(Vector2f()); + pos_control->set_accel_desired_xy_cmss(Vector2f()); // set position control target and update Vector3f zero; @@ -2945,7 +2943,7 @@ void QuadPlane::init_qrtl(void) plane.prev_WP_loc = plane.current_loc; poscontrol.state = QPOS_POSITION1; poscontrol.speed_scale = 0; - pos_control->set_accel_desired_xy_cmss(0.0f, 0.0f); + pos_control->set_accel_desired_xy_cmss(Vector2f()); pos_control->init_xy_controller(); int32_t from_alt; int32_t to_alt; @@ -2983,10 +2981,10 @@ bool QuadPlane::do_vtol_takeoff(const AP_Mission::Mission_Command& cmd) } throttle_wait = false; - // initialize vertical speed and acceleration + // 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); - - // initialize the Alt Hold controller with no decent + + // initialise the vertical position controller pos_control->init_z_controller(); // also update nav_controller for status output @@ -3034,6 +3032,7 @@ bool QuadPlane::do_vtol_land(const AP_Mission::Mission_Command& cmd) poscontrol.state = QPOS_POSITION1; poscontrol.speed_scale = 0; + // initialise the position controller pos_control->init_xy_controller(); pos_control->init_z_controller(); @@ -3656,7 +3655,7 @@ void QuadPlane::update_throttle_mix(void) // check for large acceleration - falling or high turbulence bool accel_moving = (throttle_mix_accel_ef_filter.get().length() > LAND_CHECK_ACCEL_MOVING); - // check for requested decent + // check for requested descent 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) {