From 4bae8f03a42ef6692a071a022f50e1bc9a4269ea Mon Sep 17 00:00:00 2001 From: Leonard Hall Date: Tue, 11 May 2021 14:12:02 +0930 Subject: [PATCH] Copter: Use PosControl fixes --- ArduCopter/Attitude.cpp | 2 +- ArduCopter/GCS_Mavlink.cpp | 6 +-- ArduCopter/Log.cpp | 4 +- ArduCopter/baro_ground_effect.cpp | 4 +- ArduCopter/land_detector.cpp | 2 +- ArduCopter/mode.cpp | 20 +++---- ArduCopter/mode.h | 6 +-- ArduCopter/mode_althold.cpp | 15 +++--- ArduCopter/mode_auto.cpp | 16 ++---- ArduCopter/mode_autorotate.cpp | 11 ++-- ArduCopter/mode_autotune.cpp | 5 +- ArduCopter/mode_brake.cpp | 48 +++++------------ ArduCopter/mode_circle.cpp | 22 ++------ ArduCopter/mode_flowhold.cpp | 20 +++---- ArduCopter/mode_follow.cpp | 14 ++--- ArduCopter/mode_guided.cpp | 86 ++++++++++--------------------- ArduCopter/mode_land.cpp | 8 ++- ArduCopter/mode_loiter.cpp | 21 ++++---- ArduCopter/mode_poshold.cpp | 21 +++----- ArduCopter/mode_rtl.cpp | 13 +++-- ArduCopter/mode_smart_rtl.cpp | 4 +- ArduCopter/mode_sport.cpp | 18 +++---- ArduCopter/mode_throw.cpp | 42 ++++++++------- ArduCopter/mode_zigzag.cpp | 23 ++++----- ArduCopter/surface_tracking.cpp | 2 +- ArduCopter/system.cpp | 4 +- ArduCopter/takeoff.cpp | 4 +- ArduPlane/quadplane.cpp | 4 +- 28 files changed, 173 insertions(+), 272 deletions(-) diff --git a/ArduCopter/Attitude.cpp b/ArduCopter/Attitude.cpp index 1e4659a5c9..57cd97597f 100644 --- a/ArduCopter/Attitude.cpp +++ b/ArduCopter/Attitude.cpp @@ -19,7 +19,7 @@ void Copter::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; } diff --git a/ArduCopter/GCS_Mavlink.cpp b/ArduCopter/GCS_Mavlink.cpp index 7fff1278d3..52eeaa441a 100644 --- a/ArduCopter/GCS_Mavlink.cpp +++ b/ArduCopter/GCS_Mavlink.cpp @@ -149,14 +149,14 @@ void GCS_MAVLINK_Copter::send_position_target_local_ned() type_mask = POSITION_TARGET_TYPEMASK_X_IGNORE | POSITION_TARGET_TYPEMASK_Y_IGNORE | POSITION_TARGET_TYPEMASK_Z_IGNORE | POSITION_TARGET_TYPEMASK_AX_IGNORE | POSITION_TARGET_TYPEMASK_AY_IGNORE | POSITION_TARGET_TYPEMASK_AZ_IGNORE | POSITION_TARGET_TYPEMASK_FORCE_SET | POSITION_TARGET_TYPEMASK_YAW_IGNORE| POSITION_TARGET_TYPEMASK_YAW_RATE_IGNORE; // ignore everything except velocity - target_vel = copter.flightmode->get_desired_velocity() * 0.01f; // convert to m/s + target_vel = copter.flightmode->get_vel_desired_cms() * 0.01f; // convert to m/s break; case ModeGuided::SubMode::TakeOff: case ModeGuided::SubMode::PosVel: type_mask = POSITION_TARGET_TYPEMASK_AX_IGNORE | POSITION_TARGET_TYPEMASK_AY_IGNORE | POSITION_TARGET_TYPEMASK_AZ_IGNORE | POSITION_TARGET_TYPEMASK_FORCE_SET | POSITION_TARGET_TYPEMASK_YAW_IGNORE| POSITION_TARGET_TYPEMASK_YAW_RATE_IGNORE; // ignore everything except position & velocity target_pos = copter.wp_nav->get_wp_destination() * 0.01f; - target_vel = copter.flightmode->get_desired_velocity() * 0.01f; + target_vel = copter.flightmode->get_vel_desired_cms() * 0.01f; break; } @@ -193,7 +193,7 @@ void GCS_MAVLINK_Copter::send_nav_controller_output() const targets.z * 1.0e-2f, flightmode->wp_bearing() * 1.0e-2f, MIN(flightmode->wp_distance() * 1.0e-2f, UINT16_MAX), - copter.pos_control->get_alt_error() * 1.0e-2f, + copter.pos_control->get_pos_error_z_cm() * 1.0e-2f, 0, flightmode->crosstrack_error() * 1.0e-2f); } diff --git a/ArduCopter/Log.cpp b/ArduCopter/Log.cpp index 6192f3e615..e19b3a20fc 100644 --- a/ArduCopter/Log.cpp +++ b/ArduCopter/Log.cpp @@ -35,8 +35,8 @@ void Copter::Log_Write_Control_Tuning() float des_alt_m = 0.0f; int16_t target_climb_rate_cms = 0; if (!flightmode->has_manual_throttle()) { - 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(); } // get surface tracking alts diff --git a/ArduCopter/baro_ground_effect.cpp b/ArduCopter/baro_ground_effect.cpp index 7a251a51ae..47f1d66333 100644 --- a/ArduCopter/baro_ground_effect.cpp +++ b/ArduCopter/baro_ground_effect.cpp @@ -15,10 +15,10 @@ void Copter::update_ground_effect_detector(void) uint32_t tnow_ms = millis(); float xy_des_speed_cms = 0.0f; float xy_speed_cms = 0.0f; - float des_climb_rate_cms = pos_control->get_desired_velocity().z; + float des_climb_rate_cms = pos_control->get_vel_desired_cms().z; if (pos_control->is_active_xy()) { - Vector3f vel_target = pos_control->get_vel_target(); + Vector3f vel_target = pos_control->get_vel_target_cms(); vel_target.z = 0.0f; xy_des_speed_cms = vel_target.length(); } diff --git a/ArduCopter/land_detector.cpp b/ArduCopter/land_detector.cpp index 7515f25e1e..c411eefb91 100644 --- a/ArduCopter/land_detector.cpp +++ b/ArduCopter/land_detector.cpp @@ -187,7 +187,7 @@ void Copter::update_throttle_mix() const bool accel_moving = (land_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; // check if landing const bool landing = flightmode->is_landing(); diff --git a/ArduCopter/mode.cpp b/ArduCopter/mode.cpp index 4e54c35f67..b32e14420a 100644 --- a/ArduCopter/mode.cpp +++ b/ArduCopter/mode.cpp @@ -468,7 +468,7 @@ void Mode::make_safe_spool_down() break; } - pos_control->relax_alt_hold_controllers(0.0f); // forces throttle output to go to zero + pos_control->relax_z_controller(0.0f); // forces throttle output to go to zero pos_control->update_z_controller(); // we may need to move this out attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(0.0f, 0.0f, 0.0f); @@ -502,14 +502,14 @@ void Mode::land_run_vertical_control(bool pause_descent) if (g.land_speed_high > 0) { max_land_descent_velocity = -g.land_speed_high; } else { - max_land_descent_velocity = pos_control->get_max_speed_down(); + max_land_descent_velocity = pos_control->get_max_speed_down_cms(); } // Don't speed up for landing. max_land_descent_velocity = MIN(max_land_descent_velocity, -abs(g.land_speed)); // Compute a vertical velocity demand such that the vehicle approaches g2.land_alt_low. Without the below constraint, this would cause the vehicle to hover at g2.land_alt_low. - cmb_rate = sqrt_controller(MAX(g2.land_alt_low,100)-get_alt_above_ground_cm(), pos_control->get_pos_z_p().kP(), pos_control->get_max_accel_z(), G_Dt); + cmb_rate = sqrt_controller(MAX(g2.land_alt_low,100)-get_alt_above_ground_cm(), pos_control->get_pos_z_p().kP(), pos_control->get_max_accel_z_cmss(), G_Dt); // Constrain the demanded vertical velocity so that it is between the configured maximum descent speed and the configured minimum descent speed. cmb_rate = constrain_float(cmb_rate, max_land_descent_velocity, -abs(g.land_speed)); @@ -524,14 +524,14 @@ void Mode::land_run_vertical_control(bool pause_descent) const float precland_min_descent_speed = 10.0f; float max_descent_speed = abs(g.land_speed)*0.5f; - float land_slowdown = MAX(0.0f, pos_control->get_pos_error_xy()*(max_descent_speed/precland_acceptable_error)); + float land_slowdown = MAX(0.0f, pos_control->get_pos_error_xy_cm()*(max_descent_speed/precland_acceptable_error)); cmb_rate = MIN(-precland_min_descent_speed, -max_descent_speed+land_slowdown); } #endif } // update altitude target and call position controller - pos_control->set_alt_target_from_climb_rate_ff(cmb_rate, G_Dt, true); + pos_control->set_pos_target_z_from_climb_rate_cm(cmb_rate, true); pos_control->update_z_controller(); } @@ -592,13 +592,13 @@ void Mode::land_run_horizontal_control() target_vel_rel.x = -inertial_nav.get_velocity().x; target_vel_rel.y = -inertial_nav.get_velocity().y; } - pos_control->set_xy_target(target_pos.x, target_pos.y); + pos_control->set_pos_target_xy_cm(target_pos.x, target_pos.y); pos_control->override_vehicle_velocity_xy(-target_vel_rel); } #endif // process roll, pitch inputs - loiter_nav->set_pilot_desired_acceleration(target_roll, target_pitch, G_Dt); + loiter_nav->set_pilot_desired_acceleration(target_roll, target_pitch); // run loiter controller loiter_nav->update(); @@ -622,7 +622,7 @@ void Mode::land_run_horizontal_control() thrust_vector.y *= ratio; // tell position controller we are applying an external limit - pos_control->set_limit_accel_xy(); + pos_control->set_limit_xy(); } } @@ -676,7 +676,7 @@ float Mode::get_pilot_desired_throttle() const float Mode::get_avoidance_adjusted_climbrate(float target_rate) { #if AC_AVOID_ENABLED == ENABLED - AP::ac_avoid()->adjust_velocity_z(pos_control->get_pos_z_p().kP(), pos_control->get_max_accel_z(), target_rate, G_Dt); + AP::ac_avoid()->adjust_velocity_z(pos_control->get_pos_z_p().kP(), pos_control->get_max_accel_z_cmss(), target_rate, G_Dt); return target_rate; #else return target_rate; @@ -802,7 +802,7 @@ GCS_Copter &Mode::gcs() void Mode::set_throttle_takeoff() { // tell position controller to reset alt target and reset I terms - pos_control->init_takeoff(); + pos_control->init_z_controller(); } uint16_t Mode::get_pilot_speed_dn() diff --git a/ArduCopter/mode.h b/ArduCopter/mode.h index 718f36ced6..4b2e6f750a 100644 --- a/ArduCopter/mode.h +++ b/ArduCopter/mode.h @@ -95,10 +95,10 @@ public: // altitude fence float get_avoidance_adjusted_climbrate(float target_rate); - const Vector3f& get_desired_velocity() { + const Vector3f& get_vel_desired_cms() { // note that position control isn't used in every mode, so // this may return bogus data: - return pos_control->get_desired_velocity(); + return pos_control->get_vel_desired_cms(); } protected: @@ -630,8 +630,6 @@ protected: private: - void init_target(); - uint32_t _timeout_start; uint32_t _timeout_ms; diff --git a/ArduCopter/mode_althold.cpp b/ArduCopter/mode_althold.cpp index b4f2f7dbc5..bb35bfed94 100644 --- a/ArduCopter/mode_althold.cpp +++ b/ArduCopter/mode_althold.cpp @@ -10,8 +10,7 @@ bool ModeAltHold::init(bool ignore_checks) { // initialise position and desired velocity if (!pos_control->is_active_z()) { - pos_control->set_alt_target_to_current_alt(); - pos_control->set_desired_velocity_z(inertial_nav.get_velocity_z()); + pos_control->init_z_controller(); } return true; @@ -24,8 +23,7 @@ void ModeAltHold::run() float takeoff_climb_rate = 0.0f; // initialize vertical speeds and acceleration - pos_control->set_max_speed_z(-get_pilot_speed_dn(), g.pilot_speed_up); - pos_control->set_max_accel_z(g.pilot_accel_z); + pos_control->set_max_speed_accel_z(-get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z); // apply SIMPLE mode transform to pilot inputs update_simple_mode(); @@ -50,7 +48,7 @@ void ModeAltHold::run() case AltHold_MotorStopped: attitude_control->reset_rate_controller_I_terms(); attitude_control->set_yaw_target_to_current_heading(); - pos_control->relax_alt_hold_controllers(0.0f); // forces throttle output to go to zero + pos_control->relax_z_controller(0.0f); // forces throttle output to go to zero break; case AltHold_Landed_Ground_Idle: @@ -59,7 +57,7 @@ void ModeAltHold::run() case AltHold_Landed_Pre_Takeoff: attitude_control->reset_rate_controller_I_terms_smoothly(); - pos_control->relax_alt_hold_controllers(0.0f); // forces throttle output to go to zero + pos_control->relax_z_controller(0.0f); // forces throttle output to go to zero break; case AltHold_Takeoff: @@ -75,8 +73,7 @@ void ModeAltHold::run() target_climb_rate = get_avoidance_adjusted_climbrate(target_climb_rate); // set position controller targets - pos_control->set_alt_target_from_climb_rate_ff(target_climb_rate, G_Dt, false); - pos_control->add_takeoff_climb_rate(takeoff_climb_rate, G_Dt); + pos_control->set_pos_target_z_from_climb_rate_cm(target_climb_rate + takeoff_climb_rate, false); break; case AltHold_Flying: @@ -93,7 +90,7 @@ void ModeAltHold::run() // get avoidance adjusted climb rate target_climb_rate = get_avoidance_adjusted_climbrate(target_climb_rate); - pos_control->set_alt_target_from_climb_rate_ff(target_climb_rate, G_Dt, false); + pos_control->set_pos_target_z_from_climb_rate_cm(target_climb_rate, false); break; } diff --git a/ArduCopter/mode_auto.cpp b/ArduCopter/mode_auto.cpp index 57ffb66e1c..b9f3722b7b 100644 --- a/ArduCopter/mode_auto.cpp +++ b/ArduCopter/mode_auto.cpp @@ -272,8 +272,7 @@ void ModeAuto::land_start(const Vector3f& destination) // initialise position and desired velocity if (!pos_control->is_active_z()) { - pos_control->set_alt_target(inertial_nav.get_altitude()); - pos_control->set_desired_velocity_z(inertial_nav.get_velocity_z()); + pos_control->init_z_controller(); } // initialise yaw @@ -976,13 +975,13 @@ void ModeAuto::loiter_to_alt_run() float target_climb_rate = sqrt_controller( -alt_error_cm, pos_control->get_pos_z_p().kP(), - pos_control->get_max_accel_z(), + pos_control->get_max_accel_z_cmss(), G_Dt); // get avoidance adjusted climb rate target_climb_rate = get_avoidance_adjusted_climbrate(target_climb_rate); - pos_control->set_alt_target_from_climb_rate_ff(target_climb_rate, G_Dt, false); + pos_control->set_pos_target_z_from_climb_rate_cm(target_climb_rate, false); pos_control->update_z_controller(); } @@ -996,10 +995,7 @@ void ModeAuto::payload_place_start(const Vector3f& destination) loiter_nav->init_target(destination); // initialise position and desired velocity - if (!pos_control->is_active_z()) { - pos_control->set_alt_target(inertial_nav.get_altitude()); - pos_control->set_desired_velocity_z(inertial_nav.get_velocity_z()); - } + pos_control->init_z_controller(); // initialise yaw auto_yaw.set_mode(AUTO_YAW_HOLD); @@ -1330,9 +1326,7 @@ void ModeAuto::do_loiter_to_alt(const AP_Mission::Mission_Command& cmd) loiter_to_alt.reached_alt = false; loiter_to_alt.alt_error_cm = 0; - pos_control->set_max_accel_z(wp_nav->get_accel_z()); - pos_control->set_max_speed_z(wp_nav->get_default_speed_down(), - wp_nav->get_default_speed_up()); + pos_control->set_max_speed_accel_z(wp_nav->get_default_speed_down(), wp_nav->get_default_speed_up(), wp_nav->get_accel_z()); } // do_spline_wp - initiate move to next waypoint diff --git a/ArduCopter/mode_autorotate.cpp b/ArduCopter/mode_autorotate.cpp index e0e310509c..84162c67bc 100644 --- a/ArduCopter/mode_autorotate.cpp +++ b/ArduCopter/mode_autorotate.cpp @@ -221,16 +221,13 @@ void ModeAutorotate::run() // Initialise position and desired velocity if (!pos_control->is_active_z()) { - pos_control->relax_alt_hold_controllers(g2.arot.get_last_collective()); + pos_control->relax_z_controller(g2.arot.get_last_collective()); } // Get pilot parameter limits const float pilot_spd_dn = -get_pilot_speed_dn(); const float pilot_spd_up = g.pilot_speed_up; - // Set speed limit - pos_control->set_max_speed_z(curr_vel_z, pilot_spd_up); - float pilot_des_v_z = get_pilot_desired_climb_rate(channel_throttle->get_control_in()); pilot_des_v_z = constrain_float(pilot_des_v_z, pilot_spd_dn, pilot_spd_up); @@ -240,8 +237,8 @@ void ModeAutorotate::run() // Calculate pitch target adjustment rate to return to level _target_pitch_adjust = _pitch_target/_bail_time; - // Set acceleration limit - pos_control->set_max_accel_z(fabsf(_target_climb_rate_adjust)); + // Set speed and acceleration limit + pos_control->set_max_speed_accel_z(curr_vel_z, pilot_spd_up, fabsf(_target_climb_rate_adjust)); motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED); @@ -254,7 +251,7 @@ void ModeAutorotate::run() _pitch_target -= _target_pitch_adjust*G_Dt; } // Set position controller - pos_control->set_alt_target_from_climb_rate(_desired_v_z, G_Dt, false); + pos_control->set_pos_target_z_from_climb_rate_cm(_desired_v_z, false); // Update controllers pos_control->update_z_controller(); diff --git a/ArduCopter/mode_autotune.cpp b/ArduCopter/mode_autotune.cpp index 4be6620afd..f87b51fb6d 100644 --- a/ArduCopter/mode_autotune.cpp +++ b/ArduCopter/mode_autotune.cpp @@ -56,7 +56,7 @@ void AutoTune::run() get_pilot_desired_rp_yrate_cd(target_roll, target_pitch, target_yaw_rate); copter.attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate); - copter.pos_control->relax_alt_hold_controllers(0.0f); + copter.pos_control->relax_z_controller(0.0f); copter.pos_control->update_z_controller(); } else { // run autotune mode @@ -93,8 +93,7 @@ void AutoTune::get_pilot_desired_rp_yrate_cd(float &des_roll_cd, float &des_pitc */ void AutoTune::init_z_limits() { - copter.pos_control->set_max_speed_z(-copter.get_pilot_speed_dn(), copter.g.pilot_speed_up); - copter.pos_control->set_max_accel_z(copter.g.pilot_accel_z); + copter.pos_control->set_max_speed_accel_z(-copter.get_pilot_speed_dn(), copter.g.pilot_speed_up, copter.g.pilot_accel_z); } void AutoTune::log_pids() diff --git a/ArduCopter/mode_brake.cpp b/ArduCopter/mode_brake.cpp index 39364d66de..c34c3271b4 100644 --- a/ArduCopter/mode_brake.cpp +++ b/ArduCopter/mode_brake.cpp @@ -9,17 +9,18 @@ // brake_init - initialise brake controller bool ModeBrake::init(bool ignore_checks) { - // set target to current position - init_target(); + // initialise pos controller speed and acceleration + pos_control->set_max_speed_accel_xy(inertial_nav.get_velocity().length(), BRAKE_MODE_DECEL_RATE); + + // initialise position controller + pos_control->init_xy_controller(); // initialize vertical speed and acceleration - pos_control->set_max_speed_z(BRAKE_MODE_SPEED_Z, BRAKE_MODE_SPEED_Z); - pos_control->set_max_accel_z(BRAKE_MODE_DECEL_RATE); + pos_control->set_max_speed_accel_z(BRAKE_MODE_SPEED_Z, BRAKE_MODE_SPEED_Z, BRAKE_MODE_DECEL_RATE); // initialise position and desired velocity if (!pos_control->is_active_z()) { - pos_control->set_alt_target_to_current_alt(); - pos_control->set_desired_velocity_z(inertial_nav.get_velocity_z()); + pos_control->init_z_controller(); } _timeout_ms = 0; @@ -34,7 +35,8 @@ void ModeBrake::run() // if not armed set throttle to zero and exit immediately if (is_disarmed_or_landed()) { make_safe_spool_down(); - init_target(); + pos_control->relax_velocity_controller_xy(); + pos_control->relax_z_controller(0.0f); return; } @@ -47,19 +49,15 @@ void ModeBrake::run() } // use position controller to stop - pos_control->set_desired_velocity_xy(0.0f, 0.0f); + Vector3f vel; + Vector3f accel; + pos_control->input_vel_accel_xy(vel, accel); pos_control->update_xy_controller(); // call attitude controller - attitude_control->input_thrust_vector_rate_heading(wp_nav->get_thrust_vector(), 0.0f); + attitude_control->input_thrust_vector_rate_heading(pos_control->get_thrust_vector(), 0.0f); - // update altitude target and call position controller - // protects heli's from inflight motor interlock disable - if (motors->get_desired_spool_state() == AP_Motors::DesiredSpoolState::GROUND_IDLE && !copter.ap.land_complete) { - pos_control->set_alt_target_from_climb_rate(-abs(g.land_speed), G_Dt, false); - } else { - pos_control->set_alt_target_from_climb_rate_ff(0.0f, G_Dt, false); - } + pos_control->set_pos_target_z_from_climb_rate_cm(0.0f, false); pos_control->update_z_controller(); if (_timeout_ms != 0 && millis()-_timeout_start >= _timeout_ms) { @@ -75,22 +73,4 @@ void ModeBrake::timeout_to_loiter_ms(uint32_t timeout_ms) _timeout_ms = timeout_ms; } -void ModeBrake::init_target() -{ - // initialise position controller - pos_control->set_desired_velocity_xy(0.0f,0.0f); - pos_control->set_desired_accel_xy(0.0f,0.0f); - pos_control->init_xy_controller(); - - // initialise pos controller speed and acceleration - pos_control->set_max_speed_xy(inertial_nav.get_velocity().length()); - pos_control->set_max_accel_xy(BRAKE_MODE_DECEL_RATE); - pos_control->calc_leash_length_xy(); - - // set target position - Vector3f stopping_point; - pos_control->get_stopping_point_xy(stopping_point); - pos_control->set_xy_target(stopping_point.x, stopping_point.y); -} - #endif diff --git a/ArduCopter/mode_circle.cpp b/ArduCopter/mode_circle.cpp index 9c77e63fcd..19102318d6 100644 --- a/ArduCopter/mode_circle.cpp +++ b/ArduCopter/mode_circle.cpp @@ -13,10 +13,8 @@ bool ModeCircle::init(bool ignore_checks) speed_changing = false; // initialize speeds and accelerations - 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_z(-get_pilot_speed_dn(), g.pilot_speed_up); - pos_control->set_max_accel_z(g.pilot_accel_z); + pos_control->set_max_speed_accel_xy(wp_nav->get_default_speed_xy(), wp_nav->get_wp_acceleration()); + pos_control->set_max_speed_accel_z(-get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z); // initialise circle controller including setting the circle center based on vehicle speed copter.circle_nav->init(); @@ -29,10 +27,8 @@ bool ModeCircle::init(bool ignore_checks) void ModeCircle::run() { // initialize speeds and accelerations - 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_z(-get_pilot_speed_dn(), g.pilot_speed_up); - pos_control->set_max_accel_z(g.pilot_accel_z); + pos_control->set_max_speed_accel_xy(wp_nav->get_default_speed_xy(), wp_nav->get_wp_acceleration()); + pos_control->set_max_speed_accel_z(-get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z); // get pilot's desired yaw rate (or zero if in radio failsafe) float target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in()); @@ -107,7 +103,7 @@ void ModeCircle::run() motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED); // run circle controller - copter.failsafe_terrain_set_status(copter.circle_nav->update()); + copter.failsafe_terrain_set_status(copter.circle_nav->update(target_climb_rate)); // call attitude controller if (pilot_yaw_override) { @@ -115,14 +111,6 @@ void ModeCircle::run() } else { attitude_control->input_thrust_vector_heading(copter.circle_nav->get_thrust_vector(), copter.circle_nav->get_yaw()); } - - // update altitude target and call position controller - // protects heli's from inflight motor interlock disable - if (motors->get_desired_spool_state() == AP_Motors::DesiredSpoolState::GROUND_IDLE && !copter.ap.land_complete) { - pos_control->set_alt_target_from_climb_rate(-abs(g.land_speed), G_Dt, false); - } else { - pos_control->set_alt_target_from_climb_rate(target_climb_rate, G_Dt, false); - } pos_control->update_z_controller(); } diff --git a/ArduCopter/mode_flowhold.cpp b/ArduCopter/mode_flowhold.cpp index 979cf66a6a..feefc4433c 100644 --- a/ArduCopter/mode_flowhold.cpp +++ b/ArduCopter/mode_flowhold.cpp @@ -88,14 +88,12 @@ bool ModeFlowHold::init(bool ignore_checks) return false; } - // initialize vertical speeds and leash lengths - copter.pos_control->set_max_speed_z(-get_pilot_speed_dn(), copter.g.pilot_speed_up); - copter.pos_control->set_max_accel_z(copter.g.pilot_accel_z); + // initialize vertical maximum speeds and acceleration + pos_control->set_max_speed_accel_z(-get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z); // initialise position and desired velocity if (!copter.pos_control->is_active_z()) { - copter.pos_control->set_alt_target_to_current_alt(); - copter.pos_control->set_desired_velocity_z(copter.inertial_nav.get_velocity_z()); + pos_control->init_z_controller(); } flow_filter.set_cutoff_frequency(copter.scheduler.get_loop_rate_hz(), flow_filter_hz.get()); @@ -234,8 +232,7 @@ void ModeFlowHold::run() update_height_estimate(); // initialize vertical speeds and acceleration - copter.pos_control->set_max_speed_z(-get_pilot_speed_dn(), copter.g.pilot_speed_up); - copter.pos_control->set_max_accel_z(copter.g.pilot_accel_z); + pos_control->set_max_speed_accel_z(-get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z); // apply SIMPLE mode transform to pilot inputs update_simple_mode(); @@ -269,7 +266,7 @@ void ModeFlowHold::run() copter.motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::SHUT_DOWN); copter.attitude_control->reset_rate_controller_I_terms(); copter.attitude_control->set_yaw_target_to_current_heading(); - copter.pos_control->relax_alt_hold_controllers(0.0f); // forces throttle output to go to zero + copter.pos_control->relax_z_controller(0.0f); // forces throttle output to go to zero flow_pi_xy.reset_I(); break; @@ -289,8 +286,7 @@ void ModeFlowHold::run() target_climb_rate = get_avoidance_adjusted_climbrate(target_climb_rate); // call position controller - copter.pos_control->set_alt_target_from_climb_rate_ff(target_climb_rate, copter.G_Dt, false); - copter.pos_control->add_takeoff_climb_rate(takeoff_climb_rate, copter.G_Dt); + copter.pos_control->set_pos_target_z_from_climb_rate_cm(target_climb_rate + takeoff_climb_rate, false); break; case AltHold_Landed_Ground_Idle: @@ -299,7 +295,7 @@ void ModeFlowHold::run() case AltHold_Landed_Pre_Takeoff: attitude_control->reset_rate_controller_I_terms_smoothly(); - pos_control->relax_alt_hold_controllers(0.0f); // forces throttle output to go to zero + pos_control->relax_z_controller(0.0f); // forces throttle output to go to zero break; case AltHold_Flying: @@ -311,7 +307,7 @@ void ModeFlowHold::run() // get avoidance adjusted climb rate target_climb_rate = get_avoidance_adjusted_climbrate(target_climb_rate); - copter.pos_control->set_alt_target_from_climb_rate_ff(target_climb_rate, G_Dt, false); + copter.pos_control->set_pos_target_z_from_climb_rate_cm(target_climb_rate, false); break; } diff --git a/ArduCopter/mode_follow.cpp b/ArduCopter/mode_follow.cpp index e8d225993b..0d858c9009 100644 --- a/ArduCopter/mode_follow.cpp +++ b/ArduCopter/mode_follow.cpp @@ -65,15 +65,15 @@ void ModeFollow::run() // scale desired velocity to stay within horizontal speed limit float desired_speed_xy = safe_sqrt(sq(desired_velocity_neu_cms.x) + sq(desired_velocity_neu_cms.y)); - if (!is_zero(desired_speed_xy) && (desired_speed_xy > pos_control->get_max_speed_xy())) { - const float scalar_xy = pos_control->get_max_speed_xy() / desired_speed_xy; + if (!is_zero(desired_speed_xy) && (desired_speed_xy > pos_control->get_max_speed_xy_cms())) { + const float scalar_xy = pos_control->get_max_speed_xy_cms() / desired_speed_xy; desired_velocity_neu_cms.x *= scalar_xy; desired_velocity_neu_cms.y *= scalar_xy; - desired_speed_xy = pos_control->get_max_speed_xy(); + desired_speed_xy = pos_control->get_max_speed_xy_cms(); } // limit desired velocity to be between maximum climb and descent rates - desired_velocity_neu_cms.z = constrain_float(desired_velocity_neu_cms.z, -fabsf(pos_control->get_max_speed_down()), pos_control->get_max_speed_up()); + desired_velocity_neu_cms.z = constrain_float(desired_velocity_neu_cms.z, -fabsf(pos_control->get_max_speed_down_cms()), pos_control->get_max_speed_up_cms()); // unit vector towards target position (i.e. vector to lead vehicle + offset) Vector3f dir_to_target_neu = dist_vec_offs_neu; @@ -93,17 +93,17 @@ void ModeFollow::run() // slow down horizontally as we approach target (use 1/2 of maximum deceleration for gentle slow down) const float dist_to_target_xy = Vector2f(dist_vec_offs_neu.x, dist_vec_offs_neu.y).length(); - copter.avoid.limit_velocity_2D(pos_control->get_pos_xy_p().kP().get(), pos_control->get_max_accel_xy() * 0.5f, desired_velocity_xy_cms, dir_to_target_xy, dist_to_target_xy, copter.G_Dt); + copter.avoid.limit_velocity_2D(pos_control->get_pos_xy_p().kP().get(), pos_control->get_max_accel_xy_cmss() * 0.5f, desired_velocity_xy_cms, dir_to_target_xy, dist_to_target_xy, copter.G_Dt); // copy horizontal velocity limits back to 3d vector desired_velocity_neu_cms.x = desired_velocity_xy_cms.x; desired_velocity_neu_cms.y = desired_velocity_xy_cms.y; // limit vertical desired_velocity_neu_cms to slow as we approach target (we use 1/2 of maximum deceleration for gentle slow down) - const float des_vel_z_max = copter.avoid.get_max_speed(pos_control->get_pos_z_p().kP().get(), pos_control->get_max_accel_z() * 0.5f, fabsf(dist_vec_offs_neu.z), copter.G_Dt); + const float des_vel_z_max = copter.avoid.get_max_speed(pos_control->get_pos_z_p().kP().get(), pos_control->get_max_accel_z_cmss() * 0.5f, fabsf(dist_vec_offs_neu.z), copter.G_Dt); desired_velocity_neu_cms.z = constrain_float(desired_velocity_neu_cms.z, -des_vel_z_max, des_vel_z_max); // limit the velocity for obstacle/fence avoidance - copter.avoid.adjust_velocity(desired_velocity_neu_cms, pos_control->get_pos_xy_p().kP().get(), pos_control->get_max_accel_xy(), pos_control->get_pos_z_p().kP().get(), pos_control->get_max_accel_z(), G_Dt); + copter.avoid.adjust_velocity(desired_velocity_neu_cms, pos_control->get_pos_xy_p().kP().get(), pos_control->get_max_accel_xy_cmss(), pos_control->get_pos_z_p().kP().get(), pos_control->get_max_accel_z_cmss(), G_Dt); // calculate vehicle heading switch (g2.follow.get_yaw_behave()) { diff --git a/ArduCopter/mode_guided.cpp b/ArduCopter/mode_guided.cpp index a7cd3b997a..a2d9ea5388 100644 --- a/ArduCopter/mode_guided.cpp +++ b/ArduCopter/mode_guided.cpp @@ -162,15 +162,14 @@ void ModeGuided::vel_control_start() guided_mode = SubMode::Velocity; // initialise horizontal speed, 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()); // initialize vertical speeds and acceleration - pos_control->set_max_speed_z(-get_pilot_speed_dn(), g.pilot_speed_up); - pos_control->set_max_accel_z(g.pilot_accel_z); + pos_control->set_max_speed_accel_z(wp_nav->get_default_speed_down(), wp_nav->get_default_speed_up(), wp_nav->get_accel_z()); // initialise velocity controller - pos_control->init_vel_controller_xyz(); + pos_control->init_z_controller(); + pos_control->init_xy_controller(); } // initialise guided mode's posvel controller @@ -179,23 +178,16 @@ void ModeGuided::posvel_control_start() // set guided_mode to velocity controller guided_mode = SubMode::PosVel; + // initialise horizontal speed, acceleration + pos_control->set_max_speed_accel_xy(wp_nav->get_default_speed_xy(), wp_nav->get_wp_acceleration()); + + // initialize vertical speeds and acceleration + pos_control->set_max_speed_accel_z(wp_nav->get_default_speed_down(), wp_nav->get_default_speed_up(), wp_nav->get_accel_z()); + + // initialise velocity controller + pos_control->init_z_controller(); pos_control->init_xy_controller(); - // set speed and acceleration from wpnav's 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()); - - const Vector3f& curr_pos = inertial_nav.get_position(); - const Vector3f& curr_vel = inertial_nav.get_velocity(); - - // set target position and velocity to current position and velocity - pos_control->set_xy_target(curr_pos.x, curr_pos.y); - pos_control->set_desired_velocity_xy(curr_vel.x, curr_vel.y); - - // set vertical speed and acceleration - pos_control->set_max_speed_z(wp_nav->get_default_speed_down(), wp_nav->get_default_speed_up()); - pos_control->set_max_accel_z(wp_nav->get_accel_z()); - // pilot always controls yaw auto_yaw.set_mode(AUTO_YAW_HOLD); } @@ -212,13 +204,11 @@ void ModeGuided::angle_control_start() guided_mode = SubMode::Angle; // set vertical speed and acceleration - pos_control->set_max_speed_z(wp_nav->get_default_speed_down(), wp_nav->get_default_speed_up()); - pos_control->set_max_accel_z(wp_nav->get_accel_z()); + pos_control->set_max_speed_accel_z(wp_nav->get_default_speed_down(), wp_nav->get_default_speed_up(), wp_nav->get_accel_z()); // initialise position and desired velocity if (!pos_control->is_active_z()) { - pos_control->set_alt_target_to_current_alt(); - pos_control->set_desired_velocity_z(inertial_nav.get_velocity_z()); + pos_control->init_z_controller(); } // initialise targets @@ -360,7 +350,7 @@ bool ModeGuided::set_destination_posvel(const Vector3f& destination, const Vecto guided_pos_target_cm = destination; guided_vel_target_cms = velocity; - copter.pos_control->set_pos_target(guided_pos_target_cm); + copter.pos_control->set_pos_vel_accel(guided_pos_target_cm, guided_vel_target_cms, Vector3f()); // log target copter.Log_Write_GuidedTarget(guided_mode, destination, velocity); @@ -502,7 +492,7 @@ void ModeGuided::vel_control_run() // set velocity to zero and stop rotating if no updates received for 3 seconds uint32_t tnow = millis(); if (tnow - vel_update_time_ms > GUIDED_POSVEL_TIMEOUT_MS) { - if (!pos_control->get_desired_velocity().is_zero()) { + if (!pos_control->get_vel_desired_cms().is_zero()) { set_desired_velocity_with_accel_and_fence_limits(Vector3f(0.0f, 0.0f, 0.0f)); } if (auto_yaw.mode() == AUTO_YAW_RATE) { @@ -513,7 +503,8 @@ void ModeGuided::vel_control_run() } // call velocity controller which includes z axis controller - pos_control->update_vel_controller_xyz(); + pos_control->update_xy_controller(); + pos_control->update_z_controller(); // call attitude controller if (auto_yaw.mode() == AUTO_YAW_HOLD) { @@ -562,19 +553,14 @@ void ModeGuided::posvel_control_run() } // calculate dt - float dt = pos_control->time_since_last_xy_update(); - - // sanity check dt - if (dt >= 0.2f) { - dt = 0.0f; - } + float dt = pos_control->get_dt(); // advance position target using velocity target guided_pos_target_cm += guided_vel_target_cms * dt; // send position and velocity targets to position controller - pos_control->set_pos_target(guided_pos_target_cm); - pos_control->set_desired_velocity_xy(guided_vel_target_cms.x, guided_vel_target_cms.y); + pos_control->input_pos_vel_accel_xy(guided_pos_target_cm, guided_vel_target_cms, Vector3f()); + pos_control->input_pos_vel_accel_z(guided_pos_target_cm, guided_vel_target_cms, Vector3f()); // run position controllers pos_control->update_xy_controller(); @@ -669,7 +655,7 @@ void ModeGuided::angle_control_run() if (guided_angle_state.use_thrust) { attitude_control->set_throttle_out(guided_angle_state.thrust, true, copter.g.throttle_filt); } else { - pos_control->set_alt_target_from_climb_rate_ff(climb_rate_cms, G_Dt, false); + pos_control->set_pos_target_z_from_climb_rate_cm(climb_rate_cms, false); pos_control->update_z_controller(); } } @@ -678,32 +664,16 @@ void ModeGuided::angle_control_run() void ModeGuided::set_desired_velocity_with_accel_and_fence_limits(const Vector3f& vel_des) { // get current desired velocity - Vector3f curr_vel_des = pos_control->get_desired_velocity(); - - // get change in desired velocity - Vector3f vel_delta = vel_des - curr_vel_des; - - // limit xy change - float vel_delta_xy = safe_sqrt(sq(vel_delta.x)+sq(vel_delta.y)); - float vel_delta_xy_max = G_Dt * pos_control->get_max_accel_xy(); - float ratio_xy = 1.0f; - if (!is_zero(vel_delta_xy) && (vel_delta_xy > vel_delta_xy_max)) { - ratio_xy = vel_delta_xy_max / vel_delta_xy; - } - curr_vel_des.x += (vel_delta.x * ratio_xy); - curr_vel_des.y += (vel_delta.y * ratio_xy); - - // limit z change - float vel_delta_z_max = G_Dt * pos_control->get_max_accel_z(); - curr_vel_des.z += constrain_float(vel_delta.z, -vel_delta_z_max, vel_delta_z_max); + Vector3f curr_vel_des = vel_des; #if AC_AVOID_ENABLED // limit the velocity for obstacle/fence avoidance - copter.avoid.adjust_velocity(curr_vel_des, pos_control->get_pos_xy_p().kP(), pos_control->get_max_accel_xy(), pos_control->get_pos_z_p().kP(), pos_control->get_max_accel_z(), G_Dt); + copter.avoid.adjust_velocity(curr_vel_des, pos_control->get_pos_xy_p().kP(), pos_control->get_max_accel_xy_cmss(), pos_control->get_pos_z_p().kP(), pos_control->get_max_accel_z_cmss(), G_Dt); #endif // update position controller with new target - pos_control->set_desired_velocity(curr_vel_des); + pos_control->input_vel_accel_xy(curr_vel_des, Vector3f()); + pos_control->input_vel_accel_z(curr_vel_des, Vector3f(), false); } // helper function to set yaw state and targets @@ -795,7 +765,7 @@ uint32_t ModeGuided::wp_distance() const return wp_nav->get_wp_distance_to_destination(); break; case SubMode::PosVel: - return pos_control->get_pos_error_xy(); + return pos_control->get_pos_error_xy_cm(); break; default: return 0; @@ -809,7 +779,7 @@ int32_t ModeGuided::wp_bearing() const return wp_nav->get_wp_bearing_to_destination(); break; case SubMode::PosVel: - return pos_control->get_bearing_to_target(); + return pos_control->get_bearing_to_target_cd(); break; case SubMode::TakeOff: case SubMode::Velocity: diff --git a/ArduCopter/mode_land.cpp b/ArduCopter/mode_land.cpp index 1e0f2105f8..d8630edb51 100644 --- a/ArduCopter/mode_land.cpp +++ b/ArduCopter/mode_land.cpp @@ -12,14 +12,12 @@ bool ModeLand::init(bool ignore_checks) loiter_nav->init_target(stopping_point); } - // initialize vertical speeds and leash lengths - pos_control->set_max_speed_z(wp_nav->get_default_speed_down(), wp_nav->get_default_speed_up()); - pos_control->set_max_accel_z(wp_nav->get_accel_z()); + // initialize vertical maximum speeds and acceleration + pos_control->set_max_speed_accel_z(wp_nav->get_default_speed_down(), wp_nav->get_default_speed_up(), wp_nav->get_accel_z()); // initialise position and desired velocity if (!pos_control->is_active_z()) { - pos_control->set_alt_target_to_current_alt(); - pos_control->set_desired_velocity_z(inertial_nav.get_velocity_z()); + pos_control->init_z_controller(); } land_start_time = millis(); diff --git a/ArduCopter/mode_loiter.cpp b/ArduCopter/mode_loiter.cpp index 1972698445..8a53b0434c 100644 --- a/ArduCopter/mode_loiter.cpp +++ b/ArduCopter/mode_loiter.cpp @@ -18,7 +18,7 @@ bool ModeLoiter::init(bool ignore_checks) get_pilot_desired_lean_angles(target_roll, target_pitch, loiter_nav->get_angle_max_cd(), attitude_control->get_althold_lean_angle_max()); // process pilot's roll and pitch input - loiter_nav->set_pilot_desired_acceleration(target_roll, target_pitch, G_Dt); + loiter_nav->set_pilot_desired_acceleration(target_roll, target_pitch); } else { // clear out pilot desired acceleration in case radio failsafe event occurs and we do not switch to RTL for some reason loiter_nav->clear_pilot_desired_acceleration(); @@ -27,8 +27,7 @@ bool ModeLoiter::init(bool ignore_checks) // initialise position and desired velocity if (!pos_control->is_active_z()) { - pos_control->set_alt_target_to_current_alt(); - pos_control->set_desired_velocity_z(inertial_nav.get_velocity_z()); + pos_control->init_z_controller(); } return true; @@ -65,7 +64,7 @@ void ModeLoiter::precision_loiter_xy() target_vel_rel.x = -inertial_nav.get_velocity().x; target_vel_rel.y = -inertial_nav.get_velocity().y; } - pos_control->set_xy_target(target_pos.x, target_pos.y); + pos_control->set_pos_target_xy_cm(target_pos.x, target_pos.y); pos_control->override_vehicle_velocity_xy(-target_vel_rel); } #endif @@ -80,8 +79,7 @@ void ModeLoiter::run() float takeoff_climb_rate = 0.0f; // initialize vertical speed and acceleration - pos_control->set_max_speed_z(-get_pilot_speed_dn(), g.pilot_speed_up); - pos_control->set_max_accel_z(g.pilot_accel_z); + pos_control->set_max_speed_accel_z(-get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z); // process pilot inputs unless we are in radio failsafe if (!copter.failsafe.radio) { @@ -92,7 +90,7 @@ void ModeLoiter::run() get_pilot_desired_lean_angles(target_roll, target_pitch, loiter_nav->get_angle_max_cd(), attitude_control->get_althold_lean_angle_max()); // process pilot's roll and pitch input - loiter_nav->set_pilot_desired_acceleration(target_roll, target_pitch, G_Dt); + loiter_nav->set_pilot_desired_acceleration(target_roll, target_pitch); // get pilot's desired yaw rate target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in()); @@ -119,7 +117,7 @@ void ModeLoiter::run() case AltHold_MotorStopped: attitude_control->reset_rate_controller_I_terms(); attitude_control->set_yaw_target_to_current_heading(); - pos_control->relax_alt_hold_controllers(0.0f); // forces throttle output to go to zero + pos_control->relax_z_controller(0.0f); // forces throttle output to go to zero loiter_nav->init_target(); attitude_control->input_thrust_vector_rate_heading(loiter_nav->get_thrust_vector(), target_yaw_rate); pos_control->update_z_controller(); @@ -144,8 +142,7 @@ void ModeLoiter::run() attitude_control->input_thrust_vector_rate_heading(loiter_nav->get_thrust_vector(), target_yaw_rate); // update altitude target and call position controller - pos_control->set_alt_target_from_climb_rate_ff(target_climb_rate, G_Dt, false); - pos_control->add_takeoff_climb_rate(takeoff_climb_rate, G_Dt); + pos_control->set_pos_target_z_from_climb_rate_cm(target_climb_rate + takeoff_climb_rate, false); pos_control->update_z_controller(); break; @@ -157,7 +154,7 @@ void ModeLoiter::run() attitude_control->reset_rate_controller_I_terms_smoothly(); loiter_nav->init_target(); attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(0.0f, 0.0f, 0.0f); - pos_control->relax_alt_hold_controllers(0.0f); // forces throttle output to go to zero + pos_control->relax_z_controller(0.0f); // forces throttle output to go to zero pos_control->update_z_controller(); break; @@ -183,7 +180,7 @@ void ModeLoiter::run() // get avoidance adjusted climb rate target_climb_rate = get_avoidance_adjusted_climbrate(target_climb_rate); - pos_control->set_alt_target_from_climb_rate_ff(target_climb_rate, G_Dt, false); + pos_control->set_pos_target_z_from_climb_rate_cm(target_climb_rate, false); pos_control->update_z_controller(); break; } diff --git a/ArduCopter/mode_poshold.cpp b/ArduCopter/mode_poshold.cpp index db6355bd06..5cf58ec7d0 100644 --- a/ArduCopter/mode_poshold.cpp +++ b/ArduCopter/mode_poshold.cpp @@ -28,13 +28,11 @@ bool ModePosHold::init(bool ignore_checks) { // initialize vertical speeds and acceleration - pos_control->set_max_speed_z(-get_pilot_speed_dn(), g.pilot_speed_up); - pos_control->set_max_accel_z(g.pilot_accel_z); + pos_control->set_max_speed_accel_z(-get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z); // initialise position and desired velocity if (!pos_control->is_active_z()) { - pos_control->set_alt_target_to_current_alt(); - pos_control->set_desired_velocity_z(inertial_nav.get_velocity_z()); + pos_control->init_z_controller(); } // initialise lean angles to current attitude @@ -74,8 +72,7 @@ void ModePosHold::run() const Vector3f& vel = inertial_nav.get_velocity(); // initialize vertical speeds and acceleration - pos_control->set_max_speed_z(-get_pilot_speed_dn(), g.pilot_speed_up); - pos_control->set_max_accel_z(g.pilot_accel_z); + pos_control->set_max_speed_accel_z(-get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z); loiter_nav->clear_pilot_desired_acceleration(); // apply SIMPLE mode transform to pilot inputs @@ -106,7 +103,7 @@ void ModePosHold::run() case AltHold_MotorStopped: attitude_control->reset_rate_controller_I_terms(); attitude_control->set_yaw_target_to_current_heading(); - pos_control->relax_alt_hold_controllers(0.0f); // forces throttle output to go to zero + pos_control->relax_z_controller(0.0f); // forces throttle output to go to zero loiter_nav->clear_pilot_desired_acceleration(); loiter_nav->init_target(); loiter_nav->update(false); @@ -137,8 +134,7 @@ void ModePosHold::run() loiter_nav->update(false); // set position controller targets - pos_control->set_alt_target_from_climb_rate_ff(target_climb_rate, G_Dt, false); - pos_control->add_takeoff_climb_rate(takeoff_climb_rate, G_Dt); + pos_control->set_pos_target_z_from_climb_rate_cm(target_climb_rate + takeoff_climb_rate, false); // set poshold state to pilot override roll_mode = RPMode::PILOT_OVERRIDE; @@ -155,7 +151,7 @@ void ModePosHold::run() case AltHold_Landed_Pre_Takeoff: attitude_control->reset_rate_controller_I_terms_smoothly(); - pos_control->relax_alt_hold_controllers(0.0f); // forces throttle output to go to zero + pos_control->relax_z_controller(0.0f); // forces throttle output to go to zero // set poshold state to pilot override roll_mode = RPMode::PILOT_OVERRIDE; @@ -174,7 +170,7 @@ void ModePosHold::run() // get avoidance adjusted climb rate target_climb_rate = get_avoidance_adjusted_climbrate(target_climb_rate); - pos_control->set_alt_target_from_climb_rate_ff(target_climb_rate, G_Dt, false); + pos_control->set_pos_target_z_from_climb_rate_cm(target_climb_rate, false); break; } @@ -583,8 +579,7 @@ void ModePosHold::update_wind_comp_estimate() } // get position controller accel target - // To-Do: clean this up by using accessor in loiter controller (or move entire PosHold controller to a library shared with loiter) - const Vector3f& accel_target = pos_control->get_accel_target(); + const Vector3f& accel_target = pos_control->get_accel_target_cmss(); // update wind compensation in earth-frame lean angles if (is_zero(wind_comp_ef.x)) { diff --git a/ArduCopter/mode_rtl.cpp b/ArduCopter/mode_rtl.cpp index 3e5c8d47c9..598ca9843f 100644 --- a/ArduCopter/mode_rtl.cpp +++ b/ArduCopter/mode_rtl.cpp @@ -264,7 +264,7 @@ void ModeRTL::descent_start() loiter_nav->init_target(wp_nav->get_wp_destination()); // initialise altitude target to stopping point - pos_control->set_target_to_stopping_point_z(); + pos_control->init_z_controller_stopping_point(); // initialise yaw auto_yaw.set_mode(AUTO_YAW_HOLD); @@ -330,13 +330,13 @@ void ModeRTL::descent_run() motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED); // process roll, pitch inputs - loiter_nav->set_pilot_desired_acceleration(target_roll, target_pitch, G_Dt); + loiter_nav->set_pilot_desired_acceleration(target_roll, target_pitch); // run loiter controller loiter_nav->update(); // call z-axis position controller - pos_control->set_alt_target_with_slew(rtl_path.descent_target.alt, G_Dt); + pos_control->set_alt_target_with_slew(rtl_path.descent_target.alt); pos_control->update_z_controller(); // roll & pitch from waypoint controller, yaw rate from pilot @@ -357,8 +357,7 @@ void ModeRTL::land_start() // initialise position and desired velocity if (!pos_control->is_active_z()) { - pos_control->set_alt_target_to_current_alt(); - pos_control->set_desired_velocity_z(inertial_nav.get_velocity_z()); + pos_control->init_z_controller(); } // initialise yaw @@ -411,8 +410,8 @@ void ModeRTL::build_path() { // origin point is our stopping point Vector3f stopping_point; - pos_control->get_stopping_point_xy(stopping_point); - pos_control->get_stopping_point_z(stopping_point); + pos_control->get_stopping_point_xy_cm(stopping_point); + pos_control->get_stopping_point_z_cm(stopping_point); rtl_path.origin_point = Location(stopping_point, Location::AltFrame::ABOVE_ORIGIN); rtl_path.origin_point.change_alt_frame(Location::AltFrame::ABOVE_HOME); diff --git a/ArduCopter/mode_smart_rtl.cpp b/ArduCopter/mode_smart_rtl.cpp index 9e7469474e..61044fe6c1 100644 --- a/ArduCopter/mode_smart_rtl.cpp +++ b/ArduCopter/mode_smart_rtl.cpp @@ -17,8 +17,8 @@ bool ModeSmartRTL::init(bool ignore_checks) // set current target to a reasonable stopping point Vector3f stopping_point; - pos_control->get_stopping_point_xy(stopping_point); - pos_control->get_stopping_point_z(stopping_point); + pos_control->get_stopping_point_xy_cm(stopping_point); + pos_control->get_stopping_point_z_cm(stopping_point); wp_nav->set_wp_destination(stopping_point); // initialise yaw to obey user parameter diff --git a/ArduCopter/mode_sport.cpp b/ArduCopter/mode_sport.cpp index c998ccc599..538eed4c9d 100644 --- a/ArduCopter/mode_sport.cpp +++ b/ArduCopter/mode_sport.cpp @@ -10,13 +10,11 @@ bool ModeSport::init(bool ignore_checks) { // initialize vertical speed and acceleration - pos_control->set_max_speed_z(-get_pilot_speed_dn(), g.pilot_speed_up); - pos_control->set_max_accel_z(g.pilot_accel_z); + pos_control->set_max_speed_accel_z(-get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z); // initialise position and desired velocity if (!pos_control->is_active_z()) { - pos_control->set_alt_target_to_current_alt(); - pos_control->set_desired_velocity_z(inertial_nav.get_velocity_z()); + pos_control->init_z_controller(); } return true; @@ -29,8 +27,7 @@ void ModeSport::run() float takeoff_climb_rate = 0.0f; // initialize vertical speed and acceleration - pos_control->set_max_speed_z(-get_pilot_speed_dn(), g.pilot_speed_up); - pos_control->set_max_accel_z(g.pilot_accel_z); + pos_control->set_max_speed_accel_z(-get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z); // apply SIMPLE mode transform update_simple_mode(); @@ -81,7 +78,7 @@ void ModeSport::run() case AltHold_MotorStopped: attitude_control->reset_rate_controller_I_terms(); attitude_control->set_yaw_target_to_current_heading(); - pos_control->relax_alt_hold_controllers(0.0f); // forces throttle output to go to zero + pos_control->relax_z_controller(0.0f); // forces throttle output to go to zero break; case AltHold_Takeoff: @@ -97,8 +94,7 @@ void ModeSport::run() target_climb_rate = get_avoidance_adjusted_climbrate(target_climb_rate); // call position controller - pos_control->set_alt_target_from_climb_rate_ff(target_climb_rate, G_Dt, false); - pos_control->add_takeoff_climb_rate(takeoff_climb_rate, G_Dt); + pos_control->set_pos_target_z_from_climb_rate_cm(target_climb_rate + takeoff_climb_rate, false); break; case AltHold_Landed_Ground_Idle: @@ -107,7 +103,7 @@ void ModeSport::run() case AltHold_Landed_Pre_Takeoff: attitude_control->reset_rate_controller_I_terms_smoothly(); - pos_control->relax_alt_hold_controllers(0.0f); // forces throttle output to go to zero + pos_control->relax_z_controller(0.0f); // forces throttle output to go to zero break; case AltHold_Flying: @@ -119,7 +115,7 @@ void ModeSport::run() // get avoidance adjusted climb rate target_climb_rate = get_avoidance_adjusted_climbrate(target_climb_rate); - pos_control->set_alt_target_from_climb_rate_ff(target_climb_rate, G_Dt, false); + pos_control->set_pos_target_z_from_climb_rate_cm(target_climb_rate, false); break; } diff --git a/ArduCopter/mode_throw.cpp b/ArduCopter/mode_throw.cpp index 5e96856ab2..ea94d3f0fe 100644 --- a/ArduCopter/mode_throw.cpp +++ b/ArduCopter/mode_throw.cpp @@ -19,6 +19,12 @@ bool ModeThrow::init(bool ignore_checks) stage = Throw_Disarmed; nextmode_attempted = false; + // initialise pos controller speed and acceleration + pos_control->set_max_speed_accel_xy(wp_nav->get_default_speed_xy(), BRAKE_MODE_DECEL_RATE); + + // initialize vertical speed and acceleration + pos_control->set_max_speed_accel_z(BRAKE_MODE_SPEED_Z, BRAKE_MODE_SPEED_Z, BRAKE_MODE_DECEL_RATE); + return true; } @@ -53,23 +59,17 @@ void ModeThrow::run() gcs().send_text(MAV_SEVERITY_INFO,"uprighted - controlling height"); stage = Throw_HgtStabilise; - // initialize vertical speed and acceleration limits - // use brake mode values for rapid response - pos_control->set_max_speed_z(BRAKE_MODE_SPEED_Z, BRAKE_MODE_SPEED_Z); - pos_control->set_max_accel_z(BRAKE_MODE_DECEL_RATE); + // initialise the z controller + pos_control->init_z_controller_no_descent(); // initialise the demanded height to 3m above the throw height // we want to rapidly clear surrounding obstacles if (g2.throw_type == ThrowType::Drop) { - pos_control->set_alt_target(inertial_nav.get_altitude() - 100); + pos_control->set_pos_target_z_cm(inertial_nav.get_altitude() - 100); } else { - pos_control->set_alt_target(inertial_nav.get_altitude() + 300); + pos_control->set_pos_target_z_cm(inertial_nav.get_altitude() + 300); } - // set the initial velocity of the height controller demand to the measured velocity if it is going up - // if it is going down, set it to zero to enforce a very hard stop - pos_control->set_desired_velocity_z(fmaxf(inertial_nav.get_velocity_z(),0.0f)); - // Set the auto_arm status to true to avoid a possible automatic disarm caused by selection of an auto mode with throttle at minimum copter.set_auto_armed(true); @@ -77,9 +77,8 @@ void ModeThrow::run() gcs().send_text(MAV_SEVERITY_INFO,"height achieved - controlling position"); stage = Throw_PosHold; - // initialise the loiter target to the current position and velocity - loiter_nav->clear_pilot_desired_acceleration(); - loiter_nav->init_target(); + // initialise position controller + pos_control->init_xy_controller(); // Set the auto_arm status to true to avoid a possible automatic disarm caused by selection of an auto mode with throttle at minimum copter.set_auto_armed(true); @@ -161,7 +160,7 @@ void ModeThrow::run() attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(0.0f, 0.0f, 0.0f); // call height controller - pos_control->set_alt_target_from_climb_rate_ff(0.0f, G_Dt, false); + pos_control->set_pos_target_z_from_climb_rate_cm(0.0f, false); pos_control->update_z_controller(); break; @@ -171,14 +170,17 @@ void ModeThrow::run() // set motors to full range motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED); - // run loiter controller - loiter_nav->update(); + // use position controller to stop + Vector3f vel; + Vector3f accel; + pos_control->input_vel_accel_xy(vel, accel); + pos_control->update_xy_controller(); // call attitude controller - attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(loiter_nav->get_roll(), loiter_nav->get_pitch(), 0.0f); + attitude_control->input_thrust_vector_rate_heading(pos_control->get_thrust_vector(), 0.0f); // call height controller - pos_control->set_alt_target_from_climb_rate_ff(0.0f, G_Dt, false); + pos_control->set_pos_target_z_from_climb_rate_cm(0.0f, false); pos_control->update_z_controller(); break; @@ -282,12 +284,12 @@ bool ModeThrow::throw_attitude_good() bool ModeThrow::throw_height_good() { // Check that we are within 0.5m of the demanded height - return (pos_control->get_alt_error() < 50.0f); + return (pos_control->get_pos_error_z_cm() < 50.0f); } bool ModeThrow::throw_position_good() { // check that our horizontal position error is within 50cm - return (pos_control->get_pos_error_xy() < 50.0f); + return (pos_control->get_pos_error_xy_cm() < 50.0f); } #endif diff --git a/ArduCopter/mode_zigzag.cpp b/ArduCopter/mode_zigzag.cpp index c5a1fb3d82..a1f2b72366 100644 --- a/ArduCopter/mode_zigzag.cpp +++ b/ArduCopter/mode_zigzag.cpp @@ -76,7 +76,7 @@ bool ModeZigZag::init(bool ignore_checks) get_pilot_desired_lean_angles(target_roll, target_pitch, loiter_nav->get_angle_max_cd(), attitude_control->get_althold_lean_angle_max()); // process pilot's roll and pitch input - loiter_nav->set_pilot_desired_acceleration(target_roll, target_pitch, G_Dt); + loiter_nav->set_pilot_desired_acceleration(target_roll, target_pitch); } else { // clear out pilot desired acceleration in case radio failsafe event occurs and we do not switch to RTL for some reason loiter_nav->clear_pilot_desired_acceleration(); @@ -85,8 +85,7 @@ bool ModeZigZag::init(bool ignore_checks) // initialise position_z and desired velocity_z if (!pos_control->is_active_z()) { - pos_control->set_alt_target_to_current_alt(); - pos_control->set_desired_velocity_z(inertial_nav.get_velocity_z()); + pos_control->init_z_controller(); } // initialise waypoint state @@ -112,8 +111,7 @@ void ModeZigZag::exit() void ModeZigZag::run() { // initialize vertical speed and acceleration's range - pos_control->set_max_speed_z(-get_pilot_speed_dn(), g.pilot_speed_up); - pos_control->set_max_accel_z(g.pilot_accel_z); + pos_control->set_max_speed_accel_z(-get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z); // set the direction and the total number of lines zigzag_direction = (Direction)constrain_int16(_direction, 0, 3); @@ -301,7 +299,7 @@ void ModeZigZag::manual_control() get_pilot_desired_lean_angles(target_roll, target_pitch, loiter_nav->get_angle_max_cd(), attitude_control->get_althold_lean_angle_max()); // process pilot's roll and pitch input - loiter_nav->set_pilot_desired_acceleration(target_roll, target_pitch, G_Dt); + loiter_nav->set_pilot_desired_acceleration(target_roll, target_pitch); // get pilot's desired yaw rate target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in()); @@ -329,7 +327,7 @@ void ModeZigZag::manual_control() case AltHold_MotorStopped: attitude_control->reset_rate_controller_I_terms(); attitude_control->set_yaw_target_to_current_heading(); - pos_control->relax_alt_hold_controllers(0.0f); // forces throttle output to go to zero + pos_control->relax_z_controller(0.0f); // forces throttle output to go to zero loiter_nav->init_target(); attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(loiter_nav->get_roll(), loiter_nav->get_pitch(), target_yaw_rate); pos_control->update_z_controller(); @@ -354,8 +352,7 @@ void ModeZigZag::manual_control() attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(loiter_nav->get_roll(), loiter_nav->get_pitch(), target_yaw_rate); // update altitude target and call position controller - pos_control->set_alt_target_from_climb_rate_ff(target_climb_rate, G_Dt, false); - pos_control->add_takeoff_climb_rate(takeoff_climb_rate, G_Dt); + pos_control->set_pos_target_z_from_climb_rate_cm(target_climb_rate + takeoff_climb_rate, false); pos_control->update_z_controller(); break; @@ -367,7 +364,7 @@ void ModeZigZag::manual_control() attitude_control->reset_rate_controller_I_terms_smoothly(); loiter_nav->init_target(); attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(0.0f, 0.0f, 0.0f); - pos_control->relax_alt_hold_controllers(0.0f); // forces throttle output to go to zero + pos_control->relax_z_controller(0.0f); // forces throttle output to go to zero pos_control->update_z_controller(); break; @@ -387,7 +384,7 @@ void ModeZigZag::manual_control() // get avoidance adjusted climb rate target_climb_rate = get_avoidance_adjusted_climbrate(target_climb_rate); - pos_control->set_alt_target_from_climb_rate_ff(target_climb_rate, G_Dt, false); + pos_control->set_pos_target_z_from_climb_rate_cm(target_climb_rate, false); pos_control->update_z_controller(); break; } @@ -463,7 +460,7 @@ bool ModeZigZag::calculate_next_dest(Destination ab_dest, bool use_wpnav_alt, Ve next_dest.z = copter.rangefinder_state.alt_cm_filt.get(); } } else { - next_dest.z = pos_control->is_active_z() ? pos_control->get_alt_target() : curr_pos.z; + next_dest.z = pos_control->is_active_z() ? pos_control->get_pos_target_z_cm() : curr_pos.z; } } @@ -516,7 +513,7 @@ bool ModeZigZag::calculate_side_dest(Vector3f& next_dest, bool& terrain_alt) con next_dest.z = copter.rangefinder_state.alt_cm_filt.get(); } } else { - next_dest.z = pos_control->is_active_z() ? pos_control->get_alt_target() : curr_pos.z; + next_dest.z = pos_control->is_active_z() ? pos_control->get_pos_target_z_cm() : curr_pos.z; } return true; diff --git a/ArduCopter/surface_tracking.cpp b/ArduCopter/surface_tracking.cpp index 65cfd08115..b085f83348 100644 --- a/ArduCopter/surface_tracking.cpp +++ b/ArduCopter/surface_tracking.cpp @@ -14,7 +14,7 @@ float Copter::SurfaceTracking::adjust_climb_rate(float target_rate) } // calculate current ekf based altitude error - const float current_alt_error = copter.pos_control->get_alt_target() - copter.inertial_nav.get_altitude(); + const float current_alt_error = copter.pos_control->get_pos_target_z_cm() - copter.inertial_nav.get_altitude(); // init based on tracking direction/state RangeFinderState &rf_state = (surface == Surface::GROUND) ? copter.rangefinder_state : copter.rangefinder_up_state; diff --git a/ArduCopter/system.cpp b/ArduCopter/system.cpp index dfbd04a273..69d5cd550e 100644 --- a/ArduCopter/system.cpp +++ b/ArduCopter/system.cpp @@ -122,8 +122,6 @@ void Copter::init_ardupilot() #endif attitude_control->parameter_sanity_check(); - pos_control->set_dt(scheduler.get_loop_period_s()); - // init the optical flow sensor init_optflow(); @@ -544,7 +542,7 @@ void Copter::allocate_motors(void) } AP_Param::load_object_from_eeprom(attitude_control, ac_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, scheduler.get_loop_period_s()); if (pos_control == nullptr) { AP_BoardConfig::config_error("Unable to allocate PosControl"); } diff --git a/ArduCopter/takeoff.cpp b/ArduCopter/takeoff.cpp index 57c16ae446..bf1d0c1658 100644 --- a/ArduCopter/takeoff.cpp +++ b/ArduCopter/takeoff.cpp @@ -165,7 +165,7 @@ void Mode::auto_takeoff_run() } else { // motors have not completed spool up yet so relax navigation and position controllers wp_nav->shift_wp_origin_and_destination_to_current_pos_xy(); - pos_control->relax_alt_hold_controllers(0.0f); // forces throttle output to go to zero + pos_control->relax_z_controller(0.0f); // forces throttle output to go to zero pos_control->update_z_controller(); attitude_control->set_yaw_target_to_current_heading(); attitude_control->reset_rate_controller_I_terms(); @@ -184,7 +184,7 @@ void Mode::auto_takeoff_run() wp_nav->shift_wp_origin_and_destination_to_current_pos_xy(); } // tell the position controller that we have limited roll/pitch demand to prevent integrator buildup - pos_control->set_limit_accel_xy(); + pos_control->set_limit_xy(); } // run waypoint controller diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp index 2469893be3..7021351629 100644 --- a/ArduPlane/quadplane.cpp +++ b/ArduPlane/quadplane.cpp @@ -1463,7 +1463,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_accel_xy(); + pos_control->set_limit_xy(); } } @@ -2539,7 +2539,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_accel_xy(); + pos_control->set_limit_xy(); } // call attitude controller