diff --git a/ArduPlane/GCS_Mavlink.cpp b/ArduPlane/GCS_Mavlink.cpp index f0ad6c6646..4ffb021e3d 100644 --- a/ArduPlane/GCS_Mavlink.cpp +++ b/ArduPlane/GCS_Mavlink.cpp @@ -515,7 +515,7 @@ void Plane::send_pid_tuning(mavlink_channel_t chan) const DataFlash_Class::PID_Info *pid_info; if (g.gcs_pid_mask & 1) { if (quadplane.in_vtol_mode()) { - pid_info = &quadplane.pid_rate_roll.get_pid_info(); + pid_info = &quadplane.attitude_control->get_rate_roll_pid().get_pid_info(); } else { pid_info = &rollController.get_pid_info(); } @@ -532,7 +532,7 @@ void Plane::send_pid_tuning(mavlink_channel_t chan) } if (g.gcs_pid_mask & 2) { if (quadplane.in_vtol_mode()) { - pid_info = &quadplane.pid_rate_pitch.get_pid_info(); + pid_info = &quadplane.attitude_control->get_rate_pitch_pid().get_pid_info(); } else { pid_info = &pitchController.get_pid_info(); } @@ -549,7 +549,7 @@ void Plane::send_pid_tuning(mavlink_channel_t chan) } if (g.gcs_pid_mask & 4) { if (quadplane.in_vtol_mode()) { - pid_info = &quadplane.pid_rate_yaw.get_pid_info(); + pid_info = &quadplane.attitude_control->get_rate_yaw_pid().get_pid_info(); } else { pid_info = &yawController.get_pid_info(); } diff --git a/ArduPlane/Log.cpp b/ArduPlane/Log.cpp index fbb6d8b8a8..9b66f30a21 100644 --- a/ArduPlane/Log.cpp +++ b/ArduPlane/Log.cpp @@ -165,9 +165,9 @@ void Plane::Log_Write_Attitude(void) DataFlash.Log_Write_Attitude(ahrs, targets); if (quadplane.in_vtol_mode()) { - DataFlash.Log_Write_PID(LOG_PIDR_MSG, quadplane.pid_rate_roll.get_pid_info() ); - DataFlash.Log_Write_PID(LOG_PIDP_MSG, quadplane.pid_rate_pitch.get_pid_info() ); - DataFlash.Log_Write_PID(LOG_PIDY_MSG, quadplane.pid_rate_yaw.get_pid_info() ); + DataFlash.Log_Write_PID(LOG_PIDR_MSG, quadplane.attitude_control->get_rate_roll_pid().get_pid_info()); + DataFlash.Log_Write_PID(LOG_PIDP_MSG, quadplane.attitude_control->get_rate_pitch_pid().get_pid_info()); + DataFlash.Log_Write_PID(LOG_PIDY_MSG, quadplane.attitude_control->get_rate_yaw_pid().get_pid_info()); DataFlash.Log_Write_PID(LOG_PIDA_MSG, quadplane.pid_accel_z.get_pid_info() ); } else { DataFlash.Log_Write_PID(LOG_PIDR_MSG, rollController.get_pid_info()); diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp index b0d18c65ed..f06c8c27af 100644 --- a/ArduPlane/quadplane.cpp +++ b/ArduPlane/quadplane.cpp @@ -15,118 +15,7 @@ const AP_Param::GroupInfo QuadPlane::var_info[] = { // @Path: ../libraries/AP_Motors/AP_MotorsMulticopter.cpp AP_SUBGROUPPTR(motors, "M_", 2, QuadPlane, AP_MotorsMulticopter), - // @Param: RT_RLL_P - // @DisplayName: Roll axis rate controller P gain - // @Description: Roll axis rate controller P gain. Converts the difference between desired roll rate and actual roll rate into a motor speed output - // @Range: 0.08 0.30 - // @Increment: 0.005 - // @User: Standard - - // @Param: RT_RLL_I - // @DisplayName: Roll axis rate controller I gain - // @Description: Roll axis rate controller I gain. Corrects long-term difference in desired roll rate vs actual roll rate - // @Range: 0.01 0.5 - // @Increment: 0.01 - // @User: Standard - - // @Param: RT_RLL_IMAX - // @DisplayName: Roll axis rate controller I gain maximum - // @Description: Roll axis rate controller I gain maximum. Constrains the maximum motor output that the I gain will output - // @Range: 0 4500 - // @Increment: 10 - // @Units: Percent*10 - // @User: Standard - - // @Param: RT_RLL_D - // @DisplayName: Roll axis rate controller D gain - // @Description: Roll axis rate controller D gain. Compensates for short-term change in desired roll rate vs actual roll rate - // @Range: 0.001 0.02 - // @Increment: 0.001 - // @User: Standard - AP_SUBGROUPINFO(pid_rate_roll, "RT_RLL_", 3, QuadPlane, AC_PID), - - // @Param: RT_PIT_P - // @DisplayName: Pitch axis rate controller P gain - // @Description: Pitch axis rate controller P gain. Converts the difference between desired pitch rate and actual pitch rate into a motor speed output - // @Range: 0.08 0.30 - // @Increment: 0.005 - // @User: Standard - - // @Param: RT_PIT_I - // @DisplayName: Pitch axis rate controller I gain - // @Description: Pitch axis rate controller I gain. Corrects long-term difference in desired pitch rate vs actual pitch rate - // @Range: 0.01 0.5 - // @Increment: 0.01 - // @User: Standard - - // @Param: RT_PIT_IMAX - // @DisplayName: Pitch axis rate controller I gain maximum - // @Description: Pitch axis rate controller I gain maximum. Constrains the maximum motor output that the I gain will output - // @Range: 0 4500 - // @Increment: 10 - // @Units: Percent*10 - // @User: Standard - - // @Param: RT_PIT_D - // @DisplayName: Pitch axis rate controller D gain - // @Description: Pitch axis rate controller D gain. Compensates for short-term change in desired pitch rate vs actual pitch rate - // @Range: 0.001 0.02 - // @Increment: 0.001 - // @User: Standard - AP_SUBGROUPINFO(pid_rate_pitch, "RT_PIT_", 4, QuadPlane, AC_PID), - - // @Param: RT_YAW_P - // @DisplayName: Yaw axis rate controller P gain - // @Description: Yaw axis rate controller P gain. Converts the difference between desired yaw rate and actual yaw rate into a motor speed output - // @Range: 0.150 0.50 - // @Increment: 0.005 - // @User: Standard - - // @Param: RT_YAW_I - // @DisplayName: Yaw axis rate controller I gain - // @Description: Yaw axis rate controller I gain. Corrects long-term difference in desired yaw rate vs actual yaw rate - // @Range: 0.010 0.05 - // @Increment: 0.01 - // @User: Standard - - // @Param: RT_YAW_IMAX - // @DisplayName: Yaw axis rate controller I gain maximum - // @Description: Yaw axis rate controller I gain maximum. Constrains the maximum motor output that the I gain will output - // @Range: 0 4500 - // @Increment: 10 - // @Units: Percent*10 - // @User: Standard - - // @Param: RT_YAW_D - // @DisplayName: Yaw axis rate controller D gain - // @Description: Yaw axis rate controller D gain. Compensates for short-term change in desired yaw rate vs actual yaw rate - // @Range: 0.000 0.02 - // @Increment: 0.001 - // @User: Standard - AP_SUBGROUPINFO(pid_rate_yaw, "RT_YAW_", 5, QuadPlane, AC_PID), - - // P controllers - //-------------- - // @Param: STB_RLL_P - // @DisplayName: Roll axis stabilize controller P gain - // @Description: Roll axis stabilize (i.e. angle) controller P gain. Converts the error between the desired roll angle and actual angle to a desired roll rate - // @Range: 3.000 12.000 - // @User: Standard - AP_SUBGROUPINFO(p_stabilize_roll, "STB_R_", 6, QuadPlane, AC_P), - - // @Param: STB_PIT_P - // @DisplayName: Pitch axis stabilize controller P gain - // @Description: Pitch axis stabilize (i.e. angle) controller P gain. Converts the error between the desired pitch angle and actual angle to a desired pitch rate - // @Range: 3.000 12.000 - // @User: Standard - AP_SUBGROUPINFO(p_stabilize_pitch, "STB_P_", 7, QuadPlane, AC_P), - - // @Param: STB_YAW_P - // @DisplayName: Yaw axis stabilize controller P gain - // @Description: Yaw axis stabilize (i.e. angle) controller P gain. Converts the error between the desired yaw angle and actual angle to a desired yaw rate - // @Range: 3.000 6.000 - // @User: Standard - AP_SUBGROUPINFO(p_stabilize_yaw, "STB_Y_", 8, QuadPlane, AC_P), + // 3 ~ 8 were used by quadplane attitude control PIDs // @Group: ATC_ // @Path: ../libraries/AC_AttitudeControl/AC_AttitudeControl.cpp @@ -413,9 +302,7 @@ bool QuadPlane::setup(void) } AP_Param::load_object_from_eeprom(motors, motors->var_info); - attitude_control = new AC_AttitudeControl_Multi(ahrs, aparm, *motors, - p_stabilize_roll, p_stabilize_pitch, p_stabilize_yaw, - pid_rate_roll, pid_rate_pitch, pid_rate_yaw); + attitude_control = new AC_AttitudeControl_Multi(ahrs, aparm, *motors, plane.ins.get_loop_delta_t()); if (!attitude_control) { hal.console->printf("Unable to allocate attitude_control\n"); goto failed; @@ -442,10 +329,6 @@ bool QuadPlane::setup(void) motors->set_hover_throttle(throttle_mid); motors->set_update_rate(rc_speed); motors->set_interlock(true); - attitude_control->set_dt(plane.ins.get_loop_delta_t()); - pid_rate_roll.set_dt(plane.ins.get_loop_delta_t()); - pid_rate_pitch.set_dt(plane.ins.get_loop_delta_t()); - pid_rate_yaw.set_dt(plane.ins.get_loop_delta_t()); pid_accel_z.set_dt(plane.ins.get_loop_delta_t()); pos_control->set_dt(plane.ins.get_loop_delta_t()); @@ -1227,9 +1110,9 @@ bool QuadPlane::do_vtol_land(const AP_Mission::Mission_Command& cmd) if (!setup()) { return false; } - pid_rate_roll.reset_I(); - pid_rate_pitch.reset_I(); - pid_rate_yaw.reset_I(); + attitude_control->get_rate_roll_pid().reset_I(); + attitude_control->get_rate_pitch_pid().reset_I(); + attitude_control->get_rate_pitch_pid().reset_I(); pid_accel_z.reset_I(); pi_vel_xy.reset_I(); diff --git a/ArduPlane/quadplane.h b/ArduPlane/quadplane.h index f50f56a8a3..b4f81f768b 100644 --- a/ArduPlane/quadplane.h +++ b/ArduPlane/quadplane.h @@ -72,12 +72,6 @@ public: private: AP_AHRS_NavEKF &ahrs; AP_Vehicle::MultiCopter aparm; - AC_PID pid_rate_roll {0.25, 0.25, 0.004, 2000, 10, 0.02}; - AC_PID pid_rate_pitch{0.25, 0.25, 0.004, 2000, 10, 0.02}; - AC_PID pid_rate_yaw {0.15, 0.1, 0.004, 2000, 5, 0.02}; - AC_P p_stabilize_roll{4.5}; - AC_P p_stabilize_pitch{4.5}; - AC_P p_stabilize_yaw{4.5}; AP_InertialNav_NavEKF inertial_nav{ahrs};