From f41f4dc6e568ad0bfc574551604943eed334850c Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Mon, 15 Jan 2018 20:59:09 +0900 Subject: [PATCH] Plane: move pos-control pids to pos-control library --- ArduPlane/Log.cpp | 2 +- ArduPlane/quadplane.cpp | 63 +++------------------------------------- ArduPlane/quadplane.h | 5 ---- ArduPlane/tailsitter.cpp | 2 +- ArduPlane/tuning.cpp | 12 ++++---- 5 files changed, 12 insertions(+), 72 deletions(-) diff --git a/ArduPlane/Log.cpp b/ArduPlane/Log.cpp index ad8adea128..15c9d53ed1 100644 --- a/ArduPlane/Log.cpp +++ b/ArduPlane/Log.cpp @@ -27,7 +27,7 @@ void Plane::Log_Write_Attitude(void) DataFlash.Log_Write_PID(LOG_PIQR_MSG, quadplane.attitude_control->get_rate_roll_pid().get_pid_info()); DataFlash.Log_Write_PID(LOG_PIQP_MSG, quadplane.attitude_control->get_rate_pitch_pid().get_pid_info()); DataFlash.Log_Write_PID(LOG_PIQY_MSG, quadplane.attitude_control->get_rate_yaw_pid().get_pid_info()); - DataFlash.Log_Write_PID(LOG_PIQA_MSG, quadplane.pid_accel_z.get_pid_info() ); + DataFlash.Log_Write_PID(LOG_PIQA_MSG, quadplane.pos_control->get_accel_z_pid().get_pid_info() ); } DataFlash.Log_Write_PID(LOG_PIDR_MSG, rollController.get_pid_info()); diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp index 4e0ddc764b..c80d95a92a 100644 --- a/ArduPlane/quadplane.cpp +++ b/ArduPlane/quadplane.cpp @@ -31,60 +31,6 @@ const AP_Param::GroupInfo QuadPlane::var_info[] = { // @User: Advanced AP_GROUPINFO("TRANSITION_MS", 11, QuadPlane, transition_time_ms, 5000), - // @Param: PZ_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 - AP_SUBGROUPINFO(p_alt_hold, "PZ_", 12, QuadPlane, AC_P), - - // @Param: PXY_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 - AP_SUBGROUPINFO(p_pos_xy, "PXY_", 13, QuadPlane, AC_P), - - // @Param: VZ_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 - AP_SUBGROUPINFO(p_vel_z, "VZ_", 15, QuadPlane, AC_P), - - // @Param: AZ_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 - // @User: Standard - - // @Param: AZ_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: AZ_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: AZ_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: AZ_FILT_HZ - // @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 - AP_SUBGROUPINFO(pid_accel_z, "AZ_", 16, QuadPlane, AC_PID), - // @Group: P_ // @Path: ../libraries/AC_AttitudeControl/AC_PosControl.cpp AP_SUBGROUPPTR(pos_control, "P", 17, QuadPlane, AC_PosControl), @@ -571,8 +517,7 @@ bool QuadPlane::setup(void) goto failed; } AP_Param::load_object_from_eeprom(attitude_control, attitude_control->var_info); - pos_control = new AC_PosControl(*ahrs_view, inertial_nav, *motors, *attitude_control, - p_alt_hold, p_vel_z, pid_accel_z, p_pos_xy); + pos_control = new AC_PosControl(*ahrs_view, inertial_nav, *motors, *attitude_control); if (!pos_control) { hal.console->printf("%s pos_control\n", strUnableToAllocate); goto failed; @@ -589,7 +534,7 @@ bool QuadPlane::setup(void) motors->set_throttle_range(thr_min_pwm, thr_max_pwm); motors->set_update_rate(rc_speed); motors->set_interlock(true); - pid_accel_z.set_dt(loop_delta_t); + pos_control->get_accel_z_pid().set_dt(loop_delta_t); pos_control->set_dt(loop_delta_t); attitude_control->parameter_sanity_check(); @@ -767,7 +712,7 @@ void QuadPlane::run_z_controller(void) // the base throttle we start at is the current throttle we are using float base_throttle = constrain_float(motors->get_throttle() - motors->get_throttle_hover(), -1, 1) * 1000; - pid_accel_z.set_integrator(base_throttle); + pos_control->get_accel_z_pid().set_integrator(base_throttle); last_pidz_init_ms = now; } @@ -2143,7 +2088,7 @@ bool QuadPlane::do_vtol_land(const AP_Mission::Mission_Command& cmd) attitude_control->get_rate_roll_pid().reset_I(); attitude_control->get_rate_pitch_pid().reset_I(); attitude_control->get_rate_yaw_pid().reset_I(); - pid_accel_z.reset_I(); + pos_control->get_accel_z_pid().reset_I(); pos_control->get_vel_xy_pid().reset_I(); plane.set_next_WP(cmd.content.location); diff --git a/ArduPlane/quadplane.h b/ArduPlane/quadplane.h index a54bd31d69..e4d3f1e746 100644 --- a/ArduPlane/quadplane.h +++ b/ArduPlane/quadplane.h @@ -131,11 +131,6 @@ private: AP_InertialNav_NavEKF inertial_nav{ahrs}; - AC_P p_pos_xy{0.7}; - AC_P p_alt_hold{1}; - AC_P p_vel_z{5}; - AC_PID pid_accel_z{0.3, 1, 0, 800, 10, 0.02}; - AP_Int8 frame_class; AP_Int8 frame_type; diff --git a/ArduPlane/tailsitter.cpp b/ArduPlane/tailsitter.cpp index bf01e997ca..1d77e24557 100644 --- a/ArduPlane/tailsitter.cpp +++ b/ArduPlane/tailsitter.cpp @@ -76,7 +76,7 @@ void QuadPlane::tailsitter_output(void) SRV_Channels::set_output_scaled(SRV_Channel::k_throttleLeft, throttle); SRV_Channels::set_output_scaled(SRV_Channel::k_throttleRight, throttle); SRV_Channels::set_output_scaled(SRV_Channel::k_rudder, 0); - pid_accel_z.set_integrator(throttle*10); + pos_control->get_accel_z_pid().set_integrator(throttle*10); } return; } diff --git a/ArduPlane/tuning.cpp b/ArduPlane/tuning.cpp index 03c473628b..1f109b2a78 100644 --- a/ArduPlane/tuning.cpp +++ b/ArduPlane/tuning.cpp @@ -143,10 +143,10 @@ AP_Float *AP_Tuning_Plane::get_param_pointer(uint8_t parm) return &plane.quadplane.attitude_control->get_angle_yaw_p().kP(); case TUNING_PXY_P: - return &plane.quadplane.p_pos_xy.kP(); + return &plane.quadplane.pos_control->get_pos_xy_p().kP(); case TUNING_PZ_P: - return &plane.quadplane.p_alt_hold.kP(); + return &plane.quadplane.pos_control->get_pos_z_p().kP(); case TUNING_VXY_P: return &plane.quadplane.pos_control->get_vel_xy_pid().kP(); @@ -155,16 +155,16 @@ AP_Float *AP_Tuning_Plane::get_param_pointer(uint8_t parm) return &plane.quadplane.pos_control->get_vel_xy_pid().kI(); case TUNING_VZ_P: - return &plane.quadplane.p_vel_z.kP(); + return &plane.quadplane.pos_control->get_vel_z_p().kP(); case TUNING_AZ_P: - return &plane.quadplane.pid_accel_z.kP(); + return &plane.quadplane.pos_control->get_accel_z_pid().kP(); case TUNING_AZ_I: - return &plane.quadplane.pid_accel_z.kI(); + return &plane.quadplane.pos_control->get_accel_z_pid().kI(); case TUNING_AZ_D: - return &plane.quadplane.pid_accel_z.kD(); + return &plane.quadplane.pos_control->get_accel_z_pid().kD(); // fixed wing tuning parameters case TUNING_RLL_P: