From 5e547ea76b33449e6e627b64de44e67bb91f527d Mon Sep 17 00:00:00 2001 From: mingxi Date: Sat, 26 Oct 2024 22:13:53 -0400 Subject: [PATCH] updated variable names --- cfg/dynamic_reconfigure_pid.cfg | 4 +- include/mvp_control/dictionary.h | 8 +- src/mvp_control/mimo_pid.cpp | 29 ++-- src/mvp_control/mimo_pid.h | 14 +- src/mvp_control/mvp_control_ros.cpp | 241 +++++++++++++--------------- src/mvp_control/mvp_control_ros.h | 8 - 6 files changed, 136 insertions(+), 168 deletions(-) diff --git a/cfg/dynamic_reconfigure_pid.cfg b/cfg/dynamic_reconfigure_pid.cfg index c50d49c..9aa2db1 100644 --- a/cfg/dynamic_reconfigure_pid.cfg +++ b/cfg/dynamic_reconfigure_pid.cfg @@ -25,7 +25,7 @@ for i in axis: gen.add(i + "_p", double_t, 0, "Controller " + i + " PID (P) Gain", 1, 0, 200) gen.add(i + "_i", double_t, 0, "Controller " + i + " PID (I) Gain", 0, 0, 200) gen.add(i + "_d", double_t, 0, "Controller " + i + " PID (D) Gain", 0, 0, 200) - gen.add(i + "_i_max", double_t, 0, "Controller " + i + " PID max (i) term", 0, -50, 50) - gen.add(i + "_i_min", double_t, 0, "Controller " + i + " PID min (i) term", 0, -50, 50) + gen.add(i + "_pid_max", double_t, 0, "Controller " + i + " PID max term", 0, -50, 50) + gen.add(i + "_pid_min", double_t, 0, "Controller " + i + " PID min term", 0, -50, 50) exit(gen.generate(PACKAGE, "mvp_control_dynamic_configure", "PID")) \ No newline at end of file diff --git a/include/mvp_control/dictionary.h b/include/mvp_control/dictionary.h index 0946b20..57737d8 100644 --- a/include/mvp_control/dictionary.h +++ b/include/mvp_control/dictionary.h @@ -70,8 +70,12 @@ namespace ctrl { static constexpr const char * CONF_PID_P = "p"; static constexpr const char * CONF_PID_I = "i"; static constexpr const char * CONF_PID_D = "d"; - static constexpr const char * CONF_PID_I_MAX = "i_max"; - static constexpr const char * CONF_PID_I_MIN = "i_min"; + // static constexpr const char * CONF_PID_I_MAX = "i_max"; + // static constexpr const char * CONF_PID_I_MIN = "i_min"; + + static constexpr const char * CONF_PID_MAX = "pid_max"; + static constexpr const char * CONF_PID_MIN = "pid_min"; + static constexpr const char * CONF_CONTROL_ALLOCATION_MATRIX = "control_allocation_matrix"; static constexpr const char * CONF_CONTROL_TF = "control_tf"; static constexpr const char * CONF_CONTROLLER_FREQUENCY = "controller_frequency"; diff --git a/src/mvp_control/mimo_pid.cpp b/src/mvp_control/mimo_pid.cpp index c8b7951..76a321e 100644 --- a/src/mvp_control/mimo_pid.cpp +++ b/src/mvp_control/mimo_pid.cpp @@ -63,11 +63,11 @@ bool MimoPID::calculate(Eigen::VectorXd* u, const Eigen::ArrayXd& desired, const Eigen::ArrayXd pid_sum = p + m_i + d; //saturation - pid_sum = (pid_sum > m_i_max).select(m_i_max, pid_sum); - pid_sum = (pid_sum < m_i_min).select(m_i_min, pid_sum); + pid_sum = (pid_sum > m_pid_max).select(m_pid_max, pid_sum); + pid_sum = (pid_sum < m_pid_min).select(m_pid_min, pid_sum); //clamping only update the integral when not saturated - m_i = (pid_sum.array() > m_i_min.array() && pid_sum.array() < m_i_max.array()) + m_i = (pid_sum.array() > m_pid_min.array() && pid_sum.array() < m_pid_max.array()) .select(m_i + delta_i, m_i); // *u = p + m_i + d; *u = pid_sum; @@ -121,22 +121,21 @@ void MimoPID::set_dt_i(const decltype(m_dt_i) &gain) { m_dt_i = gain; } -auto MimoPID::get_i_max() -> decltype(m_i_max) { - return m_i_max; +auto MimoPID::get_pid_max() -> decltype(m_pid_max) { + return m_pid_max; } -void MimoPID::set_i_max(const decltype(m_i_max) &gain) { - m_i_max= gain; +void MimoPID::set_pid_max(const decltype(m_pid_max) &gain) { + m_pid_max= gain; } -auto MimoPID::get_i_min() -> decltype(m_i_max) { - return m_i_min; +auto MimoPID::get_pid_min() -> decltype(m_pid_min) { + return m_pid_min; } -// void MimoPID::reset_m_i(const decltype(m_i)) { -// m_i.setZero(); -// } - +void MimoPID::set_pid_min(const decltype(m_pid_min) &gain) { + m_pid_min= gain; +} void MimoPID::set_m_i(const decltype(m_i) &new_m_i) { @@ -147,10 +146,6 @@ auto MimoPID::get_m_i() -> decltype(m_i) { return m_i; } -void MimoPID::set_i_min(const decltype(m_i_min) &gain) { - m_i_min= gain; -} - auto MimoPID::get_error_function() -> decltype(m_error_function) { return m_error_function; } diff --git a/src/mvp_control/mimo_pid.h b/src/mvp_control/mimo_pid.h index 9f7477c..f08362f 100644 --- a/src/mvp_control/mimo_pid.h +++ b/src/mvp_control/mimo_pid.h @@ -50,14 +50,14 @@ namespace ctrl { * * This variable holds the maximum value of a gain */ - Eigen::ArrayXd m_i_max; + Eigen::ArrayXd m_pid_max; /** * @brief Minimum value * * This variable holds the minimum value of a gain */ - Eigen::ArrayXd m_i_min; + Eigen::ArrayXd m_pid_min; //! @brief Proportional gain Eigen::ArrayXd m_kp; @@ -143,24 +143,22 @@ namespace ctrl { void set_dt_i(const decltype(m_dt_i) &gain); //! @brief Default getter for max - auto get_i_max() -> decltype(m_i_max); + auto get_pid_max() -> decltype(m_pid_max); /*! @brief Default setter for max * * @param gain */ - void set_i_max(const decltype(m_i_max) &gain); + void set_pid_max(const decltype(m_pid_max) &gain); //! @brief Default getter for min - auto get_i_min() -> decltype(m_i_min); + auto get_pid_min() -> decltype(m_pid_min); /*! @brief Default setter for min * * @param gain */ - void set_i_min(const decltype(m_i_min) &gain); - - // void reset_m_i(const decltype(m_i)); + void set_pid_min(const decltype(m_pid_min) &gain); void set_m_i(const decltype(m_i) &new_m_i); diff --git a/src/mvp_control/mvp_control_ros.cpp b/src/mvp_control/mvp_control_ros.cpp index a290150..ab869bb 100644 --- a/src/mvp_control/mvp_control_ros.cpp +++ b/src/mvp_control/mvp_control_ros.cpp @@ -909,8 +909,8 @@ void MvpControlROS::f_cb_dynconf_pid( Eigen::VectorXd p(CONTROLLABLE_DOF_LENGTH); Eigen::VectorXd i(CONTROLLABLE_DOF_LENGTH); Eigen::VectorXd d(CONTROLLABLE_DOF_LENGTH); - Eigen::VectorXd i_max(CONTROLLABLE_DOF_LENGTH); - Eigen::VectorXd i_min(CONTROLLABLE_DOF_LENGTH); + Eigen::VectorXd pid_max(CONTROLLABLE_DOF_LENGTH); + Eigen::VectorXd pid_min(CONTROLLABLE_DOF_LENGTH); p << @@ -955,39 +955,39 @@ void MvpControlROS::f_cb_dynconf_pid( config.pitch_rate_d, config.yaw_rate_d; - i_max << - config.x_i_max, - config.y_i_max, - config.z_i_max, - config.roll_i_max, - config.pitch_i_max, - config.yaw_i_max, - config.surge_i_max, - config.sway_i_max, - config.heave_i_max, - config.roll_rate_i_max, - config.pitch_rate_i_max, - config.yaw_rate_i_max; - - i_min << - config.x_i_min, - config.y_i_min, - config.z_i_min, - config.roll_i_min, - config.pitch_i_min, - config.yaw_i_min, - config.surge_i_min, - config.sway_i_min, - config.heave_i_min, - config.roll_rate_i_min, - config.pitch_rate_i_min, - config.yaw_rate_i_min; + pid_max << + config.x_pid_max, + config.y_pid_max, + config.z_pid_max, + config.roll_pid_max, + config.pitch_pid_max, + config.yaw_pid_max, + config.surge_pid_max, + config.sway_pid_max, + config.heave_pid_max, + config.roll_rate_pid_max, + config.pitch_rate_pid_max, + config.yaw_rate_pid_max; + + pid_min << + config.x_pid_min, + config.y_pid_min, + config.z_pid_min, + config.roll_pid_min, + config.pitch_pid_min, + config.yaw_pid_min, + config.surge_pid_min, + config.sway_pid_min, + config.heave_pid_min, + config.roll_rate_pid_min, + config.pitch_rate_pid_min, + config.yaw_rate_pid_min; m_mvp_control->get_pid()->set_kp(p); m_mvp_control->get_pid()->set_ki(i); m_mvp_control->get_pid()->set_kd(d); - m_mvp_control->get_pid()->set_i_max(i_max); - m_mvp_control->get_pid()->set_i_min(i_min); + m_mvp_control->get_pid()->set_pid_max(pid_max); + m_mvp_control->get_pid()->set_pid_min(pid_min); } @@ -1038,31 +1038,31 @@ void MvpControlROS::f_amend_dynconf() { conf.pitch_rate_d = pid->get_kd()(DOF::PITCH_RATE); conf.yaw_rate_d = pid->get_kd()(DOF::YAW_RATE); - conf.x_i_max = pid->get_i_max()(DOF::X); - conf.y_i_max = pid->get_i_max()(DOF::Y); - conf.z_i_max = pid->get_i_max()(DOF::Z); - conf.roll_i_max = pid->get_i_max()(DOF::ROLL); - conf.pitch_i_max = pid->get_i_max()(DOF::PITCH); - conf.yaw_i_max = pid->get_i_max()(DOF::YAW); - conf.surge_i_max = pid->get_i_max()(DOF::SURGE); - conf.sway_i_max = pid->get_i_max()(DOF::SWAY); - conf.heave_i_max = pid->get_i_max()(DOF::HEAVE); - conf.roll_rate_i_max = pid->get_i_max()(DOF::ROLL_RATE); - conf.pitch_rate_i_max = pid->get_i_max()(DOF::PITCH_RATE); - conf.yaw_rate_i_max = pid->get_i_max()(DOF::YAW_RATE); - - conf.x_i_min = pid->get_i_min()(DOF::X); - conf.y_i_min = pid->get_i_min()(DOF::Y); - conf.z_i_min = pid->get_i_min()(DOF::Z); - conf.roll_i_min = pid->get_i_min()(DOF::ROLL); - conf.pitch_i_min = pid->get_i_min()(DOF::PITCH); - conf.yaw_i_min = pid->get_i_min()(DOF::YAW); - conf.surge_i_min = pid->get_i_min()(DOF::SURGE); - conf.sway_i_min = pid->get_i_min()(DOF::SWAY); - conf.heave_i_min = pid->get_i_min()(DOF::HEAVE); - conf.roll_rate_i_min = pid->get_i_min()(DOF::ROLL_RATE); - conf.pitch_rate_i_min = pid->get_i_min()(DOF::PITCH_RATE); - conf.yaw_rate_i_min = pid->get_i_min()(DOF::YAW_RATE); + conf.x_pid_max = pid->get_pid_max()(DOF::X); + conf.y_pid_max = pid->get_pid_max()(DOF::Y); + conf.z_pid_max = pid->get_pid_max()(DOF::Z); + conf.roll_pid_max = pid->get_pid_max()(DOF::ROLL); + conf.pitch_pid_max = pid->get_pid_max()(DOF::PITCH); + conf.yaw_pid_max = pid->get_pid_max()(DOF::YAW); + conf.surge_pid_max = pid->get_pid_max()(DOF::SURGE); + conf.sway_pid_max = pid->get_pid_max()(DOF::SWAY); + conf.heave_pid_max = pid->get_pid_max()(DOF::HEAVE); + conf.roll_rate_pid_max = pid->get_pid_max()(DOF::ROLL_RATE); + conf.pitch_rate_pid_max = pid->get_pid_max()(DOF::PITCH_RATE); + conf.yaw_rate_pid_max = pid->get_pid_max()(DOF::YAW_RATE); + + conf.x_pid_min = pid->get_pid_min()(DOF::X); + conf.y_pid_min = pid->get_pid_min()(DOF::Y); + conf.z_pid_min = pid->get_pid_min()(DOF::Z); + conf.roll_pid_min = pid->get_pid_min()(DOF::ROLL); + conf.pitch_pid_min = pid->get_pid_min()(DOF::PITCH); + conf.yaw_pid_min = pid->get_pid_min()(DOF::YAW); + conf.surge_pid_min = pid->get_pid_min()(DOF::SURGE); + conf.sway_pid_min = pid->get_pid_min()(DOF::SWAY); + conf.heave_pid_min = pid->get_pid_min()(DOF::HEAVE); + conf.roll_rate_pid_min = pid->get_pid_min()(DOF::ROLL_RATE); + conf.pitch_rate_pid_min = pid->get_pid_min()(DOF::PITCH_RATE); + conf.yaw_rate_pid_min = pid->get_pid_min()(DOF::YAW_RATE); m_dynconf_pid_server->updateConfig(conf); @@ -1147,92 +1147,92 @@ void MvpControlROS::f_read_control_modes() { m_pnh.param(param + CONF_PID_P, m.pid_x.kp, 0); m_pnh.param(param + CONF_PID_I, m.pid_x.ki, 0); m_pnh.param(param + CONF_PID_D, m.pid_x.kd, 0); - m_pnh.param(param + CONF_PID_I_MAX, m.pid_x.k_i_max, 0); - m_pnh.param(param + CONF_PID_I_MIN, m.pid_x.k_i_min, 0); + m_pnh.param(param + CONF_PID_MAX, m.pid_x.pid_max, 0); + m_pnh.param(param + CONF_PID_MIN, m.pid_x.pid_min, 0); } else if (dof == DOF::Y) { m_pnh.param(param + CONF_PID_P, m.pid_y.kp, 0); m_pnh.param(param + CONF_PID_I, m.pid_y.ki, 0); m_pnh.param(param + CONF_PID_D, m.pid_y.kd, 0); - m_pnh.param(param + CONF_PID_I_MAX, m.pid_y.k_i_max, 0); - m_pnh.param(param + CONF_PID_I_MIN, m.pid_y.k_i_min, 0); + m_pnh.param(param + CONF_PID_MAX, m.pid_y.pid_max, 0); + m_pnh.param(param + CONF_PID_MIN, m.pid_y.pid_min, 0); } else if (dof == DOF::Z) { m_pnh.param(param + CONF_PID_P, m.pid_z.kp, 0); m_pnh.param(param + CONF_PID_I, m.pid_z.ki, 0); m_pnh.param(param + CONF_PID_D, m.pid_z.kd, 0); - m_pnh.param(param + CONF_PID_I_MAX, m.pid_z.k_i_max, 0); - m_pnh.param(param + CONF_PID_I_MIN, m.pid_z.k_i_min, 0); + m_pnh.param(param + CONF_PID_MAX, m.pid_z.pid_max, 0); + m_pnh.param(param + CONF_PID_MIN, m.pid_z.pid_min, 0); } else if (dof == DOF::ROLL) { m_pnh.param(param + CONF_PID_P, m.pid_roll.kp, 0); m_pnh.param(param + CONF_PID_I, m.pid_roll.ki, 0); m_pnh.param(param + CONF_PID_D, m.pid_roll.kd, 0); - m_pnh.param(param + CONF_PID_I_MAX, m.pid_roll.k_i_max, + m_pnh.param(param + CONF_PID_MAX, m.pid_roll.pid_max, 0); - m_pnh.param(param + CONF_PID_I_MIN, m.pid_roll.k_i_min, + m_pnh.param(param + CONF_PID_MIN, m.pid_roll.pid_min, 0); } else if (dof == DOF::PITCH) { m_pnh.param(param + CONF_PID_P, m.pid_pitch.kp, 0); m_pnh.param(param + CONF_PID_I, m.pid_pitch.ki, 0); m_pnh.param(param + CONF_PID_D, m.pid_pitch.kd, 0); - m_pnh.param(param + CONF_PID_I_MAX, m.pid_pitch.k_i_max, + m_pnh.param(param + CONF_PID_MAX, m.pid_pitch.pid_max, 0); - m_pnh.param(param + CONF_PID_I_MIN, m.pid_pitch.k_i_min, + m_pnh.param(param + CONF_PID_MIN, m.pid_pitch.pid_min, 0); } else if (dof == DOF::YAW) { m_pnh.param(param + CONF_PID_P, m.pid_yaw.kp, 0); m_pnh.param(param + CONF_PID_I, m.pid_yaw.ki, 0); m_pnh.param(param + CONF_PID_D, m.pid_yaw.kd, 0); - m_pnh.param(param + CONF_PID_I_MAX, m.pid_yaw.k_i_max, + m_pnh.param(param + CONF_PID_MAX, m.pid_yaw.pid_max, 0); - m_pnh.param(param + CONF_PID_I_MIN, m.pid_yaw.k_i_min, + m_pnh.param(param + CONF_PID_MIN, m.pid_yaw.pid_min, 0); } else if (dof == DOF::SURGE) { m_pnh.param(param + CONF_PID_P, m.pid_surge.kp, 0); m_pnh.param(param + CONF_PID_I, m.pid_surge.ki, 0); m_pnh.param(param + CONF_PID_D, m.pid_surge.kd, 0); - m_pnh.param(param + CONF_PID_I_MAX, m.pid_surge.k_i_max, + m_pnh.param(param + CONF_PID_MAX, m.pid_surge.pid_max, 0); - m_pnh.param(param + CONF_PID_I_MIN, m.pid_surge.k_i_min, + m_pnh.param(param + CONF_PID_MIN, m.pid_surge.pid_min, 0); } else if (dof == DOF::SWAY) { m_pnh.param(param + CONF_PID_P, m.pid_sway.kp, 0); m_pnh.param(param + CONF_PID_I, m.pid_sway.ki, 0); m_pnh.param(param + CONF_PID_D, m.pid_sway.kd, 0); - m_pnh.param(param + CONF_PID_I_MAX, m.pid_sway.k_i_max, + m_pnh.param(param + CONF_PID_MAX, m.pid_sway.pid_max, 0); - m_pnh.param(param + CONF_PID_I_MIN, m.pid_sway.k_i_min, + m_pnh.param(param + CONF_PID_MIN, m.pid_sway.pid_min, 0); } else if (dof == DOF::HEAVE) { m_pnh.param(param + CONF_PID_P, m.pid_heave.kp, 0); m_pnh.param(param + CONF_PID_I, m.pid_heave.ki, 0); m_pnh.param(param + CONF_PID_D, m.pid_heave.kd, 0); - m_pnh.param(param + CONF_PID_I_MAX, m.pid_heave.k_i_max, + m_pnh.param(param + CONF_PID_MAX, m.pid_heave.pid_max, 0); - m_pnh.param(param + CONF_PID_I_MIN, m.pid_heave.k_i_min, + m_pnh.param(param + CONF_PID_MIN, m.pid_heave.pid_min, 0); } else if (dof == DOF::ROLL_RATE) { m_pnh.param(param + CONF_PID_P, m.pid_roll_rate.kp, 0); m_pnh.param(param + CONF_PID_I, m.pid_roll_rate.ki, 0); m_pnh.param(param + CONF_PID_D, m.pid_roll_rate.kd, 0); - m_pnh.param(param + CONF_PID_I_MAX, - m.pid_roll_rate.k_i_max, 0); - m_pnh.param(param + CONF_PID_I_MIN, - m.pid_roll_rate.k_i_min, 0); + m_pnh.param(param + CONF_PID_MAX, + m.pid_roll_rate.pid_max, 0); + m_pnh.param(param + CONF_PID_MIN, + m.pid_roll_rate.pid_min, 0); } else if (dof == DOF::PITCH_RATE) { m_pnh.param(param + CONF_PID_P, m.pid_pitch_rate.kp, 0); m_pnh.param(param + CONF_PID_I, m.pid_pitch_rate.ki, 0); m_pnh.param(param + CONF_PID_D, m.pid_pitch_rate.kd, 0); - m_pnh.param(param + CONF_PID_I_MAX, - m.pid_pitch_rate.k_i_max, 0); - m_pnh.param(param + CONF_PID_I_MIN, - m.pid_pitch_rate.k_i_min, 0); + m_pnh.param(param + CONF_PID_MAX, + m.pid_pitch_rate.pid_max, 0); + m_pnh.param(param + CONF_PID_MIN, + m.pid_pitch_rate.pid_min, 0); } else if (dof == DOF::YAW_RATE) { m_pnh.param(param + CONF_PID_P, m.pid_yaw_rate.kp, 0); m_pnh.param(param + CONF_PID_I, m.pid_yaw_rate.ki, 0); m_pnh.param(param + CONF_PID_D, m.pid_yaw_rate.kd, 0); - m_pnh.param(param + CONF_PID_I_MAX, - m.pid_yaw_rate.k_i_max, 0); - m_pnh.param(param + CONF_PID_I_MIN, - m.pid_yaw_rate.k_i_min, 0); + m_pnh.param(param + CONF_PID_MAX, + m.pid_yaw_rate.pid_max, 0); + m_pnh.param(param + CONF_PID_MIN, + m.pid_yaw_rate.pid_min, 0); } } @@ -1263,31 +1263,10 @@ bool MvpControlROS::f_cb_srv_set_control_point( return f_amend_set_point(req.setpoint); } -// bool MvpControlROS::f_cb_srv_reset_integral_error( -// std_srvs::Empty::Request req, std_srvs::Empty::Response res) { - -// Eigen::VectorXd m_i(CONTROLLABLE_DOF_LENGTH); -// m_i.setZero(); -// // m_mvp_control->get_pid()->reset_m_i(m_i); -// m_i.setZero(); -// m_mvp_control->get_pid()->set_m_i(m_i); -// ROS_INFO("MVP_control PID integral errors are set to zero!"); -// return true; -// } - - bool MvpControlROS::f_cb_srv_enable( std_srvs::Empty::Request req, std_srvs::Empty::Response res) { ROS_INFO("Controller enabled!"); - // m_mvp_control->m_pid->m_i.setZero(); - - // Eigen::VectorXd m_i(CONTROLLABLE_DOF_LENGTH); - // m_i.setZero(); - // m_mvp_control->get_pid()->set_m_i(m_i); - // m_i = m_mvp_control->get_pid()->get_m_i(); - - // std::cout << "m_i:\n" << m_i << std::endl; m_enabled = true; std_msgs::Bool controller_state; controller_state.data=m_enabled; @@ -1407,74 +1386,74 @@ bool MvpControlROS::f_amend_control_mode(std::string mode) { pid_conf.x_p = found->pid_x.kp; pid_conf.x_i = found->pid_x.ki; pid_conf.x_d = found->pid_x.kd; - pid_conf.x_i_max = found->pid_x.k_i_max; - pid_conf.x_i_min = found->pid_x.k_i_min; + pid_conf.x_pid_max = found->pid_x.pid_max; + pid_conf.x_pid_min = found->pid_x.pid_min; pid_conf.y_p = found->pid_y.kp; pid_conf.y_i = found->pid_y.ki; pid_conf.y_d = found->pid_y.kd; - pid_conf.y_i_max = found->pid_y.k_i_max; - pid_conf.y_i_min = found->pid_y.k_i_min; + pid_conf.y_pid_max = found->pid_y.pid_max; + pid_conf.y_pid_min = found->pid_y.pid_min; pid_conf.z_p = found->pid_z.kp; pid_conf.z_i = found->pid_z.ki; pid_conf.z_d = found->pid_z.kd; - pid_conf.z_i_max = found->pid_z.k_i_max; - pid_conf.z_i_min = found->pid_z.k_i_min; + pid_conf.z_pid_max = found->pid_z.pid_max; + pid_conf.z_pid_min = found->pid_z.pid_min; pid_conf.roll_p = found->pid_roll.kp; pid_conf.roll_i = found->pid_roll.ki; pid_conf.roll_d = found->pid_roll.kd; - pid_conf.roll_i_max = found->pid_roll.k_i_max; - pid_conf.roll_i_min = found->pid_roll.k_i_min; + pid_conf.roll_pid_max = found->pid_roll.pid_max; + pid_conf.roll_pid_min = found->pid_roll.pid_min; pid_conf.pitch_p = found->pid_pitch.kp; pid_conf.pitch_i = found->pid_pitch.ki; pid_conf.pitch_d = found->pid_pitch.kd; - pid_conf.pitch_i_max = found->pid_pitch.k_i_max; - pid_conf.pitch_i_min = found->pid_pitch.k_i_min; + pid_conf.pitch_pid_max = found->pid_pitch.pid_max; + pid_conf.pitch_pid_min = found->pid_pitch.pid_min; pid_conf.yaw_p = found->pid_yaw.kp; pid_conf.yaw_i = found->pid_yaw.ki; pid_conf.yaw_d = found->pid_yaw.kd; - pid_conf.yaw_i_max = found->pid_yaw.k_i_max; - pid_conf.yaw_i_min = found->pid_yaw.k_i_min; + pid_conf.yaw_pid_max = found->pid_yaw.pid_max; + pid_conf.yaw_pid_min = found->pid_yaw.pid_min; pid_conf.surge_p = found->pid_surge.kp; pid_conf.surge_i = found->pid_surge.ki; pid_conf.surge_d = found->pid_surge.kd; - pid_conf.surge_i_max = found->pid_surge.k_i_max; - pid_conf.surge_i_min = found->pid_surge.k_i_min; + pid_conf.surge_pid_max = found->pid_surge.pid_max; + pid_conf.surge_pid_min = found->pid_surge.pid_min; pid_conf.sway_p = found->pid_sway.kp; pid_conf.sway_i = found->pid_sway.ki; pid_conf.sway_d = found->pid_sway.kd; - pid_conf.sway_i_max = found->pid_sway.k_i_max; - pid_conf.sway_i_min = found->pid_sway.k_i_min; + pid_conf.sway_pid_max = found->pid_sway.pid_max; + pid_conf.sway_pid_min = found->pid_sway.pid_min; pid_conf.heave_p = found->pid_heave.kp; pid_conf.heave_i = found->pid_heave.ki; pid_conf.heave_d = found->pid_heave.kd; - pid_conf.heave_i_max = found->pid_heave.k_i_max; - pid_conf.heave_i_min = found->pid_heave.k_i_min; + pid_conf.heave_pid_max = found->pid_heave.pid_max; + pid_conf.heave_pid_min = found->pid_heave.pid_min; pid_conf.roll_rate_p = found->pid_roll_rate.kp; pid_conf.roll_rate_i = found->pid_roll_rate.ki; pid_conf.roll_rate_d = found->pid_roll_rate.kd; - pid_conf.roll_rate_i_max = found->pid_roll_rate.k_i_max; - pid_conf.roll_rate_i_min = found->pid_roll_rate.k_i_min; + pid_conf.roll_rate_pid_max = found->pid_roll_rate.pid_max; + pid_conf.roll_rate_pid_min = found->pid_roll_rate.pid_min; pid_conf.pitch_rate_p = found->pid_pitch_rate.kp; pid_conf.pitch_rate_i = found->pid_pitch_rate.ki; pid_conf.pitch_rate_d = found->pid_pitch_rate.kd; - pid_conf.pitch_rate_i_max = found->pid_pitch_rate.k_i_max; - pid_conf.pitch_rate_i_min = found->pid_pitch_rate.k_i_min; + pid_conf.pitch_rate_pid_max = found->pid_pitch_rate.pid_max; + pid_conf.pitch_rate_pid_min = found->pid_pitch_rate.pid_min; pid_conf.yaw_rate_p = found->pid_yaw_rate.kp; pid_conf.yaw_rate_i = found->pid_yaw_rate.ki; pid_conf.yaw_rate_d = found->pid_yaw_rate.kd; - pid_conf.yaw_rate_i_max = found->pid_yaw_rate.k_i_max; - pid_conf.yaw_rate_i_min = found->pid_yaw_rate.k_i_min; + pid_conf.yaw_rate_pid_max = found->pid_yaw_rate.pid_max; + pid_conf.yaw_rate_pid_min = found->pid_yaw_rate.pid_min; f_cb_dynconf_pid(pid_conf, 0); diff --git a/src/mvp_control/mvp_control_ros.h b/src/mvp_control/mvp_control_ros.h index 6852f73..8a9a554 100644 --- a/src/mvp_control/mvp_control_ros.h +++ b/src/mvp_control/mvp_control_ros.h @@ -362,14 +362,6 @@ namespace ctrl { bool f_cb_srv_disable( std_srvs::Empty::Request req, std_srvs::Empty::Response resp); - - - /** - * @brief reset the integral error in PID controller - */ - // bool f_cb_srv_reset_integral_error( - // std_srvs::Empty::Request req, - // std_srvs::Empty::Response resp); /** * @brief get controller state (on/off)