From 6a701b241286034f72c435bd235c84567af72ac8 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Mon, 15 Jan 2018 20:58:57 +0900 Subject: [PATCH] Sub: move pos-control pids to pos-control library --- ArduSub/ArduSub.cpp | 4 +-- ArduSub/GCS_Mavlink.cpp | 2 +- ArduSub/Parameters.cpp | 55 ----------------------------------------- ArduSub/Parameters.h | 29 ++++------------------ ArduSub/Sub.cpp | 3 +-- 5 files changed, 9 insertions(+), 84 deletions(-) diff --git a/ArduSub/ArduSub.cpp b/ArduSub/ArduSub.cpp index b7135e85db..a868569d3e 100644 --- a/ArduSub/ArduSub.cpp +++ b/ArduSub/ArduSub.cpp @@ -264,7 +264,7 @@ void Sub::ten_hz_logging_loop() 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()); } } if (should_log(MASK_LOG_MOTBATT)) { @@ -298,7 +298,7 @@ void Sub::twentyfive_hz_logging() 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()); } } diff --git a/ArduSub/GCS_Mavlink.cpp b/ArduSub/GCS_Mavlink.cpp index c0bc3177bd..ce4ce7d134 100644 --- a/ArduSub/GCS_Mavlink.cpp +++ b/ArduSub/GCS_Mavlink.cpp @@ -451,7 +451,7 @@ void Sub::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 = 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), diff --git a/ArduSub/Parameters.cpp b/ArduSub/Parameters.cpp index 80b76a9df1..c768823aeb 100644 --- a/ArduSub/Parameters.cpp +++ b/ArduSub/Parameters.cpp @@ -451,61 +451,6 @@ const AP_Param::Info Sub::var_info[] = { // @User: Advanced GSCALAR(acro_expo, "ACRO_EXPO", ACRO_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 diff --git a/ArduSub/Parameters.h b/ArduSub/Parameters.h index a51a680b96..2b8e350863 100644 --- a/ArduSub/Parameters.h +++ b/ArduSub/Parameters.h @@ -149,11 +149,11 @@ public: // PID Controllers - k_param_p_pos_xy = 126, - k_param_p_alt_hold, + k_param_p_pos_xy = 126, // deprecated + k_param_p_alt_hold, // deprecated k_param_pi_vel_xy, // deprecated - k_param_p_vel_z, - k_param_pid_accel_z, + k_param_p_vel_z, // deprecated + k_param_pid_accel_z, // deprecated // Failsafes @@ -304,31 +304,12 @@ public: AP_Int8 acro_trainer; AP_Float acro_expo; - // PI/D controllers - AC_P p_vel_z; - AC_PID pid_accel_z; - - AC_P p_pos_xy; - AC_P p_alt_hold; - AP_Float surface_depth; AP_Int8 frame_configuration; // Note: keep initializers here in the same order as they are declared // above. - 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, MAIN_LOOP_SECONDS), - - // P controller initial P - //---------------------------------------------------------------------- - p_pos_xy(POS_XY_P), - - p_alt_hold(ALT_HOLD_P) - + Parameters() { } }; diff --git a/ArduSub/Sub.cpp b/ArduSub/Sub.cpp index 0c55187995..a0767e1d45 100644 --- a/ArduSub/Sub.cpp +++ b/ArduSub/Sub.cpp @@ -48,8 +48,7 @@ Sub::Sub(void) inertial_nav(ahrs), ahrs_view(ahrs, ROTATION_NONE), attitude_control(ahrs_view, aparm, motors, MAIN_LOOP_SECONDS), - pos_control(ahrs_view, inertial_nav, motors, attitude_control, - g.p_alt_hold, g.p_vel_z, g.pid_accel_z, g.p_pos_xy), + pos_control(ahrs_view, inertial_nav, motors, attitude_control), wp_nav(inertial_nav, ahrs_view, pos_control, attitude_control), circle_nav(inertial_nav, ahrs_view, pos_control), pmTest1(0),