Copter: move pos-control pids to pos-control library

ArduCopter/AP_Arming.cpp

@@ -256,20 +256,20 @@ bool AP_Arming_Copter::parameter_checks(bool display_failure)
 
         // Check for 0 value PID's - some items can / should be 0 and as such are not checked.
         // If the ATC_RAT_*_FF is non zero then the corresponding ATC_RAT_* PIDS can be 0.
-        if (is_zero(copter.g.p_pos_xy.kP())) {
+        if (is_zero(copter.pos_control->get_pos_xy_p().kP())) {
-            parameter_checks_pid_warning_message(display_failure, "POS_XY_P");
+            parameter_checks_pid_warning_message(display_failure, "PSC_POSXY_P");
             return false;
-        } else if (is_zero(copter.g.p_alt_hold.kP())) {
+        } else if (is_zero(copter.pos_control->get_pos_z_p().kP())) {
-            parameter_checks_pid_warning_message(display_failure, "POS_Z_P");
+            parameter_checks_pid_warning_message(display_failure, "PSC_POSZ_P");
             return false;
-        } else if (is_zero(copter.g.p_vel_z.kP())) {
+        } else if (is_zero(copter.pos_control->get_vel_z_p().kP())) {
-            parameter_checks_pid_warning_message(display_failure, "VEL_Z_P");
+            parameter_checks_pid_warning_message(display_failure, "PSC_VELZ_P");
             return false;
-        } else if (is_zero(copter.g.pid_accel_z.kP())) {
+        } else if (is_zero(copter.pos_control->get_accel_z_pid().kP())) {
-            parameter_checks_pid_warning_message(display_failure, "ACCEL_Z_P");
+            parameter_checks_pid_warning_message(display_failure, "PSC_ACCELZ_P");
             return false;
-        } else if (is_zero(copter.g.pid_accel_z.kI())) {
+        } else if (is_zero(copter.pos_control->get_accel_z_pid().kI())) {
-            parameter_checks_pid_warning_message(display_failure, "ACCEL_Z_I");
+            parameter_checks_pid_warning_message(display_failure, "PSC_ACCELZ_I");
             return false;
         } else if (is_zero(copter.attitude_control->get_rate_roll_pid().kP()) && is_zero(copter.attitude_control->get_rate_roll_pid().ff())) {
             parameter_checks_pid_warning_message(display_failure, "ATC_RAT_RLL_P");

ArduCopter/Attitude.cpp

@@ -283,7 +283,7 @@ float Copter::get_surface_tracking_climb_rate(int16_t target_rate, float current
 float Copter::get_avoidance_adjusted_climbrate(float target_rate)
 {
 #if AC_AVOID_ENABLED == ENABLED
-    avoid.adjust_velocity_z(pos_control->get_pos_z_kP(), pos_control->get_accel_z(), target_rate, G_Dt);
+    avoid.adjust_velocity_z(pos_control->get_pos_z_p().kP(), pos_control->get_accel_z(), target_rate, G_Dt);
     return target_rate;
 #else
     return target_rate;
@@ -296,7 +296,7 @@ void Copter::set_accel_throttle_I_from_pilot_throttle()
     // get last throttle input sent to attitude controller
     float pilot_throttle = constrain_float(attitude_control->get_throttle_in(), 0.0f, 1.0f);
     // shift difference between pilot's throttle and hover throttle into accelerometer I
-    g.pid_accel_z.set_integrator((pilot_throttle-motors->get_throttle_hover()) * 1000.0f);
+    pos_control->get_accel_z_pid().set_integrator((pilot_throttle-motors->get_throttle_hover()) * 1000.0f);
 }
 
 // rotate vector from vehicle's perspective to North-East frame

ArduCopter/GCS_Mavlink.cpp

@@ -253,7 +253,7 @@ void Copter::send_pid_tuning(mavlink_channel_t chan)
         }
     }
     if (g.gcs_pid_mask & 8) {
-        const DataFlash_Class::PID_Info &pid_info = g.pid_accel_z.get_pid_info();
+        const DataFlash_Class::PID_Info &pid_info = copter.pos_control->get_accel_z_pid().get_pid_info();
         mavlink_msg_pid_tuning_send(chan, PID_TUNING_ACCZ, 
                                     pid_info.desired*0.01f,
                                     -(ahrs.get_accel_ef_blended().z + GRAVITY_MSS),

ArduCopter/Log.cpp

@@ -231,7 +231,7 @@ void Copter::Log_Write_Attitude()
         DataFlash.Log_Write_PID(LOG_PIDR_MSG, attitude_control->get_rate_roll_pid().get_pid_info());
         DataFlash.Log_Write_PID(LOG_PIDP_MSG, attitude_control->get_rate_pitch_pid().get_pid_info());
         DataFlash.Log_Write_PID(LOG_PIDY_MSG, attitude_control->get_rate_yaw_pid().get_pid_info());
-        DataFlash.Log_Write_PID(LOG_PIDA_MSG, g.pid_accel_z.get_pid_info() );
+        DataFlash.Log_Write_PID(LOG_PIDA_MSG, pos_control->get_accel_z_pid().get_pid_info() );
     }
 }
 

ArduCopter/Parameters.cpp

@@ -555,61 +555,6 @@ const AP_Param::Info Copter::var_info[] = {
     // @User: Advanced
     GSCALAR(acro_rp_expo,  "ACRO_RP_EXPO",    ACRO_RP_EXPO_DEFAULT),
 
-    // @Param: VEL_Z_P
-    // @DisplayName: Velocity (vertical) P gain
-    // @Description: Velocity (vertical) P gain.  Converts the difference between desired vertical speed and actual speed into a desired acceleration that is passed to the throttle acceleration controller
-    // @Range: 1.000 8.000
-    // @User: Standard
-    GGROUP(p_vel_z,     "VEL_Z_", AC_P),
-
-    // @Param: ACCEL_Z_P
-    // @DisplayName: Throttle acceleration controller P gain
-    // @Description: Throttle acceleration controller P gain.  Converts the difference between desired vertical acceleration and actual acceleration into a motor output
-    // @Range: 0.500 1.500
-    // @Increment: 0.05
-    // @User: Standard
-
-    // @Param: ACCEL_Z_I
-    // @DisplayName: Throttle acceleration controller I gain
-    // @Description: Throttle acceleration controller I gain.  Corrects long-term difference in desired vertical acceleration and actual acceleration
-    // @Range: 0.000 3.000
-    // @User: Standard
-
-    // @Param: ACCEL_Z_IMAX
-    // @DisplayName: Throttle acceleration controller I gain maximum
-    // @Description: Throttle acceleration controller I gain maximum.  Constrains the maximum pwm that the I term will generate
-    // @Range: 0 1000
-    // @Units: d%
-    // @User: Standard
-
-    // @Param: ACCEL_Z_D
-    // @DisplayName: Throttle acceleration controller D gain
-    // @Description: Throttle acceleration controller D gain.  Compensates for short-term change in desired vertical acceleration vs actual acceleration
-    // @Range: 0.000 0.400
-    // @User: Standard
-
-    // @Param: ACCEL_Z_FILT
-    // @DisplayName: Throttle acceleration filter
-    // @Description: Filter applied to acceleration to reduce noise.  Lower values reduce noise but add delay.
-    // @Range: 1.000 100.000
-    // @Units: Hz
-    // @User: Standard
-    GGROUP(pid_accel_z, "ACCEL_Z_", AC_PID),
-
-    // @Param: POS_Z_P
-    // @DisplayName: Position (vertical) controller P gain
-    // @Description: Position (vertical) controller P gain.  Converts the difference between the desired altitude and actual altitude into a climb or descent rate which is passed to the throttle rate controller
-    // @Range: 1.000 3.000
-    // @User: Standard
-    GGROUP(p_alt_hold,              "POS_Z_", AC_P),
-
-    // @Param: POS_XY_P
-    // @DisplayName: Position (horizonal) controller P gain
-    // @Description: Loiter position controller P gain.  Converts the distance (in the latitude direction) to the target location into a desired speed which is then passed to the loiter latitude rate controller
-    // @Range: 0.500 2.000
-    // @User: Standard
-    GGROUP(p_pos_xy,                "POS_XY_", AC_P),
-
     // variables not in the g class which contain EEPROM saved variables
 
 #if CAMERA == ENABLED
@@ -1124,6 +1069,14 @@ void Copter::convert_pid_parameters(void)
         { Parameters::k_param_pi_vel_xy, 1, AP_PARAM_FLOAT, "PSC_VELXY_I" },
         { Parameters::k_param_pi_vel_xy, 2, AP_PARAM_FLOAT, "PSC_VELXY_IMAX" },
         { Parameters::k_param_pi_vel_xy, 3, AP_PARAM_FLOAT, "PSC_VELXY_FILT" },
+        { Parameters::k_param_p_vel_z, 0, AP_PARAM_FLOAT, "PSC_VELZ_P" },
+        { Parameters::k_param_pid_accel_z, 0, AP_PARAM_FLOAT, "PSC_ACCELZ_P" },
+        { Parameters::k_param_pid_accel_z, 1, AP_PARAM_FLOAT, "PSC_ACCELZ_I" },
+        { Parameters::k_param_pid_accel_z, 2, AP_PARAM_FLOAT, "PSC_ACCELZ_D" },
+        { Parameters::k_param_pid_accel_z, 5, AP_PARAM_FLOAT, "PSC_ACCELZ_IMAX" },
+        { Parameters::k_param_pid_accel_z, 6, AP_PARAM_FLOAT, "PSC_ACCELZ_FILT" },
+        { Parameters::k_param_p_alt_hold, 0, AP_PARAM_FLOAT, "PSC_POSZ_P" },
+        { Parameters::k_param_p_pos_xy, 0, AP_PARAM_FLOAT, "PSC_POSXY_P" },
     };
     const AP_Param::ConversionInfo throttle_conversion_info[] = {
         { Parameters::k_param_throttle_min, 0, AP_PARAM_FLOAT, "MOT_SPIN_MIN" },

ArduCopter/Parameters.h

@@ -340,19 +340,19 @@ public:
         k_param_p_stabilize_roll,   // remove
         k_param_p_stabilize_pitch,  // remove
         k_param_p_stabilize_yaw,    // remove
-        k_param_p_pos_xy,
+        k_param_p_pos_xy,           // remove
         k_param_p_loiter_lon,       // remove
         k_param_pid_loiter_rate_lat,    // remove
         k_param_pid_loiter_rate_lon,    // remove
         k_param_pid_nav_lat,        // remove
         k_param_pid_nav_lon,        // remove
-        k_param_p_alt_hold,
+        k_param_p_alt_hold,             // remove
-        k_param_p_vel_z,
+        k_param_p_vel_z,                // remove
         k_param_pid_optflow_roll,       // remove
         k_param_pid_optflow_pitch,      // remove
         k_param_acro_balance_roll_old,  // remove
         k_param_acro_balance_pitch_old, // remove
-        k_param_pid_accel_z,
+        k_param_pid_accel_z,            // remove
         k_param_acro_balance_roll,
         k_param_acro_balance_pitch,
         k_param_acro_yaw_p,
@@ -468,13 +468,6 @@ public:
     AP_Int8                 acro_trainer;
     AP_Float                acro_rp_expo;
 
-    // PI/D controllers
-    AC_P                    p_vel_z;
-    AC_PID                  pid_accel_z;
-
-    AC_P                    p_pos_xy;
-    AC_P                    p_alt_hold;
-
     // Autotune
     AP_Int8                 autotune_axis_bitmask;
     AP_Float                autotune_aggressiveness;
@@ -482,17 +475,7 @@ public:
 
     // Note: keep initializers here in the same order as they are declared
     // above.
-    Parameters() :
+    Parameters()
-        // PID controller	    initial P	      initial I         initial D       initial imax        initial filt hz     pid rate
-        //---------------------------------------------------------------------------------------------------------------------------------
-        p_vel_z                 (VEL_Z_P),
-        pid_accel_z             (ACCEL_Z_P,       ACCEL_Z_I,        ACCEL_Z_D,      ACCEL_Z_IMAX,       ACCEL_Z_FILT_HZ,    0),
-
-        // P controller	        initial P
-        //----------------------------------------------------------------------
-        p_pos_xy                (POS_XY_P),
-
-        p_alt_hold              (ALT_HOLD_P)
     {
     }
 };

ArduCopter/config.h

@@ -460,13 +460,6 @@
  # define ANGLE_RATE_MAX            18000           // default maximum rotation rate in roll/pitch axis requested by angle controller used in stabilize, loiter, rtl, auto flight modes
 #endif
 
-//////////////////////////////////////////////////////////////////////////////
-// Loiter position control gains
-//
-#ifndef POS_XY_P
- # define POS_XY_P             	1.0f
-#endif
-
 //////////////////////////////////////////////////////////////////////////////
 // Stop mode defaults
 //
@@ -488,39 +481,13 @@
 #endif
 
 //////////////////////////////////////////////////////////////////////////////
-// Throttle control gains
+// Throttle control defaults
 //
 
 #ifndef THR_DZ_DEFAULT
 # define THR_DZ_DEFAULT         100             // the deadzone above and below mid throttle while in althold or loiter
 #endif
 
-#ifndef ALT_HOLD_P
- # define ALT_HOLD_P            1.0f
-#endif
-
-// Velocity (vertical) control gains
-#ifndef VEL_Z_P
- # define VEL_Z_P       5.0f
-#endif
-
-// Accel (vertical) control gains
-#ifndef ACCEL_Z_P
- # define ACCEL_Z_P     0.50f
-#endif
-#ifndef ACCEL_Z_I
- # define ACCEL_Z_I     1.00f
-#endif
-#ifndef ACCEL_Z_D
- # define ACCEL_Z_D     0.0f
-#endif
-#ifndef ACCEL_Z_IMAX
- # define ACCEL_Z_IMAX  800
-#endif
-#ifndef ACCEL_Z_FILT_HZ
- # define ACCEL_Z_FILT_HZ   20.0f
-#endif
-
 // default maximum vertical velocity and acceleration the pilot may request
 #ifndef PILOT_VELZ_MAX
  # define PILOT_VELZ_MAX    250     // maximum vertical velocity in cm/s

ArduCopter/mode_guided.cpp

@@ -653,7 +653,7 @@ void Copter::ModeGuided::set_desired_velocity_with_accel_and_fence_limits(const
 
 #if AC_AVOID_ENABLED
     // limit the velocity to prevent fence violations
-    _copter.avoid.adjust_velocity(pos_control->get_pos_xy_kP(), pos_control->get_accel_xy(), curr_vel_des, G_Dt);
+    _copter.avoid.adjust_velocity(pos_control->get_pos_xy_p().kP(), pos_control->get_accel_xy(), curr_vel_des, G_Dt);
     // get avoidance adjusted climb rate
     curr_vel_des.z = get_avoidance_adjusted_climbrate(curr_vel_des.z);    
 #endif

ArduCopter/mode_land.cpp

@@ -184,7 +184,7 @@ void Copter::land_run_vertical_control(bool pause_descent)
         max_land_descent_velocity = MIN(max_land_descent_velocity, -abs(g.land_speed));
 
         // Compute a vertical velocity demand such that the vehicle approaches LAND_START_ALT. Without the below constraint, this would cause the vehicle to hover at LAND_START_ALT.
-        cmb_rate = AC_AttitudeControl::sqrt_controller(LAND_START_ALT-alt_above_ground, g.p_alt_hold.kP(), pos_control->get_accel_z(), G_Dt);
+        cmb_rate = AC_AttitudeControl::sqrt_controller(LAND_START_ALT-alt_above_ground, pos_control->get_pos_z_p().kP(), pos_control->get_accel_z(), 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));

ArduCopter/system.cpp

@@ -597,9 +597,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);
-                                    g.p_alt_hold, g.p_vel_z, g.pid_accel_z,
-                                    g.p_pos_xy);
     if (pos_control == nullptr) {
         AP_HAL::panic("Unable to allocate PosControl");
     }

ArduCopter/tuning.cpp

@@ -63,28 +63,28 @@ void Copter::tuning() {
 
     // Altitude and throttle tuning
     case TUNING_ALTITUDE_HOLD_KP:
-        g.p_alt_hold.kP(tuning_value);
+        pos_control->get_pos_z_p().kP(tuning_value);
         break;
 
     case TUNING_THROTTLE_RATE_KP:
-        g.p_vel_z.kP(tuning_value);
+        pos_control->get_vel_z_p().kP(tuning_value);
         break;
 
     case TUNING_ACCEL_Z_KP:
-        g.pid_accel_z.kP(tuning_value);
+        pos_control->get_accel_z_pid().kP(tuning_value);
         break;
 
     case TUNING_ACCEL_Z_KI:
-        g.pid_accel_z.kI(tuning_value);
+        pos_control->get_accel_z_pid().kI(tuning_value);
         break;
 
     case TUNING_ACCEL_Z_KD:
-        g.pid_accel_z.kD(tuning_value);
+        pos_control->get_accel_z_pid().kD(tuning_value);
         break;
 
     // Loiter and navigation tuning
     case TUNING_LOITER_POSITION_KP:
-        g.p_pos_xy.kP(tuning_value);
+        pos_control->get_pos_xy_p().kP(tuning_value);
         break;
 
     case TUNING_VEL_XY_KP: