From a70473c6c233faa1088c379916125749a30ad98a Mon Sep 17 00:00:00 2001 From: Junichiro Sugihara Date: Sat, 9 Dec 2023 21:14:38 +0900 Subject: [PATCH] [Gimbalrotor] set another rpy gain if i term is calculated in pc --- .../src/control/gimbalrotor_controller.cpp | 31 +++++++++++++------ 1 file changed, 21 insertions(+), 10 deletions(-) diff --git a/robots/gimbalrotor/src/control/gimbalrotor_controller.cpp b/robots/gimbalrotor/src/control/gimbalrotor_controller.cpp index a324aee4d..b937495b2 100644 --- a/robots/gimbalrotor/src/control/gimbalrotor_controller.cpp +++ b/robots/gimbalrotor/src/control/gimbalrotor_controller.cpp @@ -269,16 +269,27 @@ namespace aerial_robot_control { spinal::RollPitchYawTerms rpy_gain_msg; //for rosserial /* to flight controller via rosserial scaling by 1000 */ - rpy_gain_msg.motors.resize(1); - rpy_gain_msg.motors.at(0).roll_p = pid_controllers_.at(ROLL).getPGain() * 1000; - rpy_gain_msg.motors.at(0).roll_i = pid_controllers_.at(ROLL).getIGain() * 1000; - rpy_gain_msg.motors.at(0).roll_d = pid_controllers_.at(ROLL).getDGain() * 1000; - rpy_gain_msg.motors.at(0).pitch_p = pid_controllers_.at(PITCH).getPGain() * 1000; - rpy_gain_msg.motors.at(0).pitch_i = pid_controllers_.at(PITCH).getIGain() * 1000; - rpy_gain_msg.motors.at(0).pitch_d = pid_controllers_.at(PITCH).getDGain() * 1000; - rpy_gain_msg.motors.at(0).yaw_d = pid_controllers_.at(YAW).getDGain() * 1000; - rpy_gain_pub_.publish(rpy_gain_msg); - } + if(i_term_rp_calc_in_pc_){ + rpy_gain_msg.motors.resize(1); + rpy_gain_msg.motors.at(0).roll_p = pid_controllers_.at(ROLL).getPGain() * 1000; + rpy_gain_msg.motors.at(0).roll_i = 0; + rpy_gain_msg.motors.at(0).roll_d = pid_controllers_.at(ROLL).getDGain() * 1000; + rpy_gain_msg.motors.at(0).pitch_p = pid_controllers_.at(PITCH).getPGain() * 1000; + rpy_gain_msg.motors.at(0).pitch_i = 0; + rpy_gain_msg.motors.at(0).pitch_d = pid_controllers_.at(PITCH).getDGain() * 1000; + rpy_gain_msg.motors.at(0).yaw_d = pid_controllers_.at(YAW).getDGain() * 1000; + rpy_gain_pub_.publish(rpy_gain_msg); + }else{ + rpy_gain_msg.motors.resize(1); + rpy_gain_msg.motors.at(0).roll_p = pid_controllers_.at(ROLL).getPGain() * 1000; + rpy_gain_msg.motors.at(0).roll_i = pid_controllers_.at(ROLL).getIGain() * 1000; + rpy_gain_msg.motors.at(0).roll_d = pid_controllers_.at(ROLL).getDGain() * 1000; + rpy_gain_msg.motors.at(0).pitch_p = pid_controllers_.at(PITCH).getPGain() * 1000; + rpy_gain_msg.motors.at(0).pitch_i = pid_controllers_.at(PITCH).getIGain() * 1000; + rpy_gain_msg.motors.at(0).pitch_d = pid_controllers_.at(PITCH).getDGain() * 1000; + rpy_gain_msg.motors.at(0).yaw_d = pid_controllers_.at(YAW).getDGain() * 1000; + rpy_gain_pub_.publish(rpy_gain_msg); + } } //namespace aerial_robot_controller