Copter: Use PosControl fixes

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;
     }
 

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);
 }

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;
+        des_alt_m = pos_control->get_pos_target_z_cm() / 100.0f;
-        target_climb_rate_cms = pos_control->get_vel_target_z();
+        target_climb_rate_cms = pos_control->get_vel_target_z_cms();
     }
 
     // get surface tracking alts

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();
     }

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();

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()

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;
 

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->init_z_controller();
-        pos_control->set_desired_velocity_z(inertial_nav.get_velocity_z());
     }
 
     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_speed_accel_z(-get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z);
-    pos_control->set_max_accel_z(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->set_pos_target_z_from_climb_rate_cm(target_climb_rate + takeoff_climb_rate, false);
-        pos_control->add_takeoff_climb_rate(takeoff_climb_rate, G_Dt);
         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;
     }
 

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->init_z_controller();
-        pos_control->set_desired_velocity_z(inertial_nav.get_velocity_z());
     }
 
     // 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->init_z_controller();
-        pos_control->set_alt_target(inertial_nav.get_altitude());
-        pos_control->set_desired_velocity_z(inertial_nav.get_velocity_z());
-    }
 
     // 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_accel_z(wp_nav->get_default_speed_down(), wp_nav->get_default_speed_up(), wp_nav->get_accel_z());
-    pos_control->set_max_speed_z(wp_nav->get_default_speed_down(),
-                                 wp_nav->get_default_speed_up());
 }
 
 // do_spline_wp - initiate move to next waypoint

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
+                // Set speed and acceleration limit
-                pos_control->set_max_accel_z(fabsf(_target_climb_rate_adjust));
+                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();

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_speed_accel_z(-copter.get_pilot_speed_dn(), copter.g.pilot_speed_up, copter.g.pilot_accel_z);
-    copter.pos_control->set_max_accel_z(copter.g.pilot_accel_z);
 }
 
 void AutoTune::log_pids()

ArduCopter/mode_brake.cpp

@@ -9,17 +9,18 @@
 // brake_init - initialise brake controller
 bool ModeBrake::init(bool ignore_checks)
 {
-    // set target to current position
+    // initialise pos controller speed and acceleration
-    init_target();
+    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_speed_accel_z(BRAKE_MODE_SPEED_Z, BRAKE_MODE_SPEED_Z, BRAKE_MODE_DECEL_RATE);
-    pos_control->set_max_accel_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->init_z_controller();
-        pos_control->set_desired_velocity_z(inertial_nav.get_velocity_z());
     }
 
     _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
+    pos_control->set_pos_target_z_from_climb_rate_cm(0.0f, false);
-    // 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->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

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_speed_accel_xy(wp_nav->get_default_speed_xy(), wp_nav->get_wp_acceleration());
-    pos_control->set_max_accel_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);
-    pos_control->set_max_speed_z(-get_pilot_speed_dn(), g.pilot_speed_up);
-    pos_control->set_max_accel_z(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_speed_accel_xy(wp_nav->get_default_speed_xy(), wp_nav->get_wp_acceleration());
-    pos_control->set_max_accel_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);
-    pos_control->set_max_speed_z(-get_pilot_speed_dn(), g.pilot_speed_up);
-    pos_control->set_max_accel_z(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();
 }
 

ArduCopter/mode_flowhold.cpp

@@ -88,14 +88,12 @@ bool ModeFlowHold::init(bool ignore_checks)
         return false;
     }
 
-    // initialize vertical speeds and leash lengths
+    // initialize vertical maximum speeds and acceleration
-    copter.pos_control->set_max_speed_z(-get_pilot_speed_dn(), copter.g.pilot_speed_up);
+    pos_control->set_max_speed_accel_z(-get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z);
-    copter.pos_control->set_max_accel_z(copter.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();
+        pos_control->init_z_controller();
-        copter.pos_control->set_desired_velocity_z(copter.inertial_nav.get_velocity_z());
     }
 
     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);
+    pos_control->set_max_speed_accel_z(-get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z);
-    copter.pos_control->set_max_accel_z(copter.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->set_pos_target_z_from_climb_rate_cm(target_climb_rate + takeoff_climb_rate, false);
-        copter.pos_control->add_takeoff_climb_rate(takeoff_climb_rate, copter.G_Dt);
         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;
     }
 

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())) {
+        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() / desired_speed_xy;
+            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()) {

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_speed_accel_xy(wp_nav->get_default_speed_xy(), wp_nav->get_wp_acceleration());
-    pos_control->set_max_accel_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_speed_accel_z(wp_nav->get_default_speed_down(), wp_nav->get_default_speed_up(), wp_nav->get_accel_z());
-    pos_control->set_max_accel_z(g.pilot_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_speed_accel_z(wp_nav->get_default_speed_down(), wp_nav->get_default_speed_up(), wp_nav->get_accel_z());
-    pos_control->set_max_accel_z(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->init_z_controller();
-        pos_control->set_desired_velocity_z(inertial_nav.get_velocity_z());
     }
 
     // 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();
+    float dt = pos_control->get_dt();
-
-    // sanity check dt
-    if (dt >= 0.2f) {
-        dt = 0.0f;
-    }
 
     // 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->input_pos_vel_accel_xy(guided_pos_target_cm, guided_vel_target_cms, Vector3f());
-    pos_control->set_desired_velocity_xy(guided_vel_target_cms.x, guided_vel_target_cms.y);
+    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();
+    Vector3f curr_vel_des = vel_des;
-
-    // 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);
 
 #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:

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
+    // initialize vertical maximum speeds and acceleration
-    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());
-    pos_control->set_max_accel_z(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->init_z_controller();
-        pos_control->set_desired_velocity_z(inertial_nav.get_velocity_z());
     }
 
     land_start_time = millis();

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->init_z_controller();
-        pos_control->set_desired_velocity_z(inertial_nav.get_velocity_z());
     }
 
     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_speed_accel_z(-get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z);
-    pos_control->set_max_accel_z(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->set_pos_target_z_from_climb_rate_cm(target_climb_rate + takeoff_climb_rate, false);
-        pos_control->add_takeoff_climb_rate(takeoff_climb_rate, G_Dt);
         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;
     }

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_speed_accel_z(-get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z);
-    pos_control->set_max_accel_z(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->init_z_controller();
-        pos_control->set_desired_velocity_z(inertial_nav.get_velocity_z());
     }
 
     // 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_speed_accel_z(-get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z);
-    pos_control->set_max_accel_z(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->set_pos_target_z_from_climb_rate_cm(target_climb_rate + takeoff_climb_rate, false);
-        pos_control->add_takeoff_climb_rate(takeoff_climb_rate, G_Dt);
 
         // 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_cmss();
-    const Vector3f& accel_target = pos_control->get_accel_target();
 
     // update wind compensation in earth-frame lean angles
     if (is_zero(wind_comp_ef.x)) {

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->init_z_controller();
-        pos_control->set_desired_velocity_z(inertial_nav.get_velocity_z());
     }
 
     // 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_xy_cm(stopping_point);
-    pos_control->get_stopping_point_z(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);
 

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_xy_cm(stopping_point);
-        pos_control->get_stopping_point_z(stopping_point);
+        pos_control->get_stopping_point_z_cm(stopping_point);
         wp_nav->set_wp_destination(stopping_point);
 
         // initialise yaw to obey user parameter

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_speed_accel_z(-get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z);
-    pos_control->set_max_accel_z(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->init_z_controller();
-        pos_control->set_desired_velocity_z(inertial_nav.get_velocity_z());
     }
 
     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_speed_accel_z(-get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z);
-    pos_control->set_max_accel_z(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->set_pos_target_z_from_climb_rate_cm(target_climb_rate + takeoff_climb_rate, false);
-        pos_control->add_takeoff_climb_rate(takeoff_climb_rate, G_Dt);
         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;
     }
 

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
+        // initialise the z controller
-        // use brake mode values for rapid response
+        pos_control->init_z_controller_no_descent();
-        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 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
+        // initialise position controller
-        loiter_nav->clear_pilot_desired_acceleration();
+        pos_control->init_xy_controller();
-        loiter_nav->init_target();
 
         // 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
+        // use position controller to stop
-        loiter_nav->update();
+        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

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->init_z_controller();
-        pos_control->set_desired_velocity_z(inertial_nav.get_velocity_z());
     }
 
     // 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_speed_accel_z(-get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z);
-    pos_control->set_max_accel_z(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->set_pos_target_z_from_climb_rate_cm(target_climb_rate + takeoff_climb_rate, false);
-        pos_control->add_takeoff_climb_rate(takeoff_climb_rate, G_Dt);
         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;

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;

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");
     }

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

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