From 3355db9bac64942d98d69ba83372f739e15b985c Mon Sep 17 00:00:00 2001 From: Jinjie Li <45286479+Li-Jinjie@users.noreply.github.com> Date: Thu, 23 Nov 2023 16:17:52 +0900 Subject: [PATCH 1/5] [Spinal] add SetControlMode.srv to set control mode: attitude and body_rate. --- aerial_robot_nerve/spinal/CMakeLists.txt | 1 + .../attitude/attitude_control.cpp | 10 ++++++- .../attitude/attitude_control.h | 29 +++++++++++++++++++ .../spinal/srv/SetControlMode.srv | 12 ++++++++ 4 files changed, 51 insertions(+), 1 deletion(-) create mode 100644 aerial_robot_nerve/spinal/srv/SetControlMode.srv diff --git a/aerial_robot_nerve/spinal/CMakeLists.txt b/aerial_robot_nerve/spinal/CMakeLists.txt index 821f5cd3e..3119a1383 100644 --- a/aerial_robot_nerve/spinal/CMakeLists.txt +++ b/aerial_robot_nerve/spinal/CMakeLists.txt @@ -52,6 +52,7 @@ add_service_files( SetAttitudeGains.srv ImuCalib.srv MagDeclination.srv + SetControlMode.srv ) generate_messages( diff --git a/aerial_robot_nerve/spinal/mcu_project/lib/Jsk_Lib/flight_control/attitude/attitude_control.cpp b/aerial_robot_nerve/spinal/mcu_project/lib/Jsk_Lib/flight_control/attitude/attitude_control.cpp index 6c54b4b69..401a1de40 100644 --- a/aerial_robot_nerve/spinal/mcu_project/lib/Jsk_Lib/flight_control/attitude/attitude_control.cpp +++ b/aerial_robot_nerve/spinal/mcu_project/lib/Jsk_Lib/flight_control/attitude/attitude_control.cpp @@ -31,6 +31,8 @@ void AttitudeController::init(ros::NodeHandle* nh, StateEstimate* estimator) att_control_srv_ = nh_->advertiseService("set_attitude_control", &AttitudeController::setAttitudeControlCallback, this); torque_allocation_matrix_inv_sub_ = nh_->subscribe("torque_allocation_matrix_inv", 1, &AttitudeController::torqueAllocationMatrixInvCallback, this); sim_vol_sub_ = nh_->subscribe("set_sim_voltage", 1, &AttitudeController::setSimVolCallback, this); + control_mode_srv_ = nh_->advertiseService("set_control_mode", &AttitudeController::setControlModeCallback, this); + baseInit(); } @@ -46,7 +48,8 @@ AttitudeController::AttitudeController(): p_matrix_pseudo_inverse_inertia_sub_("p_matrix_pseudo_inverse_inertia", &AttitudeController::pMatrixInertiaCallback, this), pwm_test_sub_("pwm_test", &AttitudeController::pwmTestCallback, this ), att_control_srv_("set_attitude_control", &AttitudeController::setAttitudeControlCallback, this), - torque_allocation_matrix_inv_sub_("torque_allocation_matrix_inv", &AttitudeController::torqueAllocationMatrixInvCallback, this) + torque_allocation_matrix_inv_sub_("torque_allocation_matrix_inv", &AttitudeController::torqueAllocationMatrixInvCallback, this), + control_mode_srv_("set_control_mode", &AttitudeController::setControlModeCallback, this) { } @@ -82,6 +85,7 @@ void AttitudeController::init(TIM_HandleTypeDef* htim1, TIM_HandleTypeDef* htim2 nh_->subscribe(torque_allocation_matrix_inv_sub_); nh_->advertiseService(att_control_srv_); + nh_->advertiseService(control_mode_srv_); baseInit(); } @@ -116,6 +120,10 @@ void AttitudeController::baseInit() control_term_pub_last_time_ = 0; control_feedback_state_pub_last_time_ = 0; + // control mode: attitude or body rate + is_attitude_ctrl_ = true; + is_body_rate_ctrl_ = false; + reset(); } diff --git a/aerial_robot_nerve/spinal/mcu_project/lib/Jsk_Lib/flight_control/attitude/attitude_control.h b/aerial_robot_nerve/spinal/mcu_project/lib/Jsk_Lib/flight_control/attitude/attitude_control.h index 6c28aac33..8562c521f 100644 --- a/aerial_robot_nerve/spinal/mcu_project/lib/Jsk_Lib/flight_control/attitude/attitude_control.h +++ b/aerial_robot_nerve/spinal/mcu_project/lib/Jsk_Lib/flight_control/attitude/attitude_control.h @@ -45,6 +45,7 @@ #include #include #include +#include #define IDLE_DUTY 0.5f #define FORCE_LANDING_INTEGRAL 0.0025f // 500Hz * 0.0025 = 1.25 N / sec @@ -129,6 +130,20 @@ class AttitudeController void setSimVolCallback(const std_msgs::Float32 vol_msg) { sim_voltage_ = vol_msg.data; } float sim_voltage_; + ros::ServiceServer control_mode_srv_; + bool setControlModeCallback(spinal::SetControlMode::Request& req, spinal::SetControlMode::Response& res) + { + if (req.is_attitude == false && req.is_body_rate==false) + { + ROS_ERROR("invalid: attitude and body rate control mode can not be set both to false."); + return false; + } + + is_attitude_ctrl_ = req.is_attitude; + is_body_rate_ctrl_ = req.is_body_rate; + return true; + } + #else ros::Subscriber four_axis_cmd_sub_; ros::Subscriber pwm_info_sub_; @@ -140,6 +155,16 @@ class AttitudeController void setAttitudeControlCallback(const std_srvs::SetBool::Request& req, std_srvs::SetBool::Response& res) { att_control_flag_ = req.data; } + ros::ServiceServer control_mode_srv_; + void setControlModeCallback(const spinal::SetControlMode::Request& req, spinal::SetControlMode::Response& res) { + if (req.is_attitude == false && req.is_body_rate == false) + { + ROS_ERROR("invalid: attitude and body rate control mode can not be set both to false."); + } + is_attitude_ctrl_ = req.is_attitude; + is_body_rate_ctrl_ = req.is_body_rate; + } + BatteryStatus* bat_; osMutexId* mutex_; #endif @@ -176,6 +201,10 @@ class AttitudeController float p_matrix_pseudo_inverse_[MAX_MOTOR_NUMBER][4]; ap::Matrix3f inertia_; + // control level + bool is_attitude_ctrl_; + bool is_body_rate_ctrl_; + // Failsafe bool failsafe_; uint32_t flight_command_last_stamp_; diff --git a/aerial_robot_nerve/spinal/srv/SetControlMode.srv b/aerial_robot_nerve/spinal/srv/SetControlMode.srv new file mode 100644 index 000000000..da90b38aa --- /dev/null +++ b/aerial_robot_nerve/spinal/srv/SetControlMode.srv @@ -0,0 +1,12 @@ +# set Control Mode for Spinal +# +# reference: http://docs.ros.org/en/api/mavros_msgs/html/srv/SetMode.html +# and https://github.com/PX4/px4_msgs/blob/main/msg/OffboardControlMode.msg + +# if all false, raise error. if all true, body_rate will be used as a feedforward term. + +bool is_attitude +bool is_body_rate + +--- +bool is_success \ No newline at end of file From fc61820dff4676c22d450c574d4fb3efe8332e71 Mon Sep 17 00:00:00 2001 From: Jinjie Li <45286479+Li-Jinjie@users.noreply.github.com> Date: Thu, 23 Nov 2023 19:36:20 +0900 Subject: [PATCH 2/5] [Spinal] rename true_vel -> true_ang_vel and vel -> ang_vel to reduce ambiguity --- .../attitude/attitude_control.cpp | 21 +++++++++---------- .../attitude/attitude_control.h | 9 ++++++-- 2 files changed, 17 insertions(+), 13 deletions(-) diff --git a/aerial_robot_nerve/spinal/mcu_project/lib/Jsk_Lib/flight_control/attitude/attitude_control.cpp b/aerial_robot_nerve/spinal/mcu_project/lib/Jsk_Lib/flight_control/attitude/attitude_control.cpp index 401a1de40..cfd51db80 100644 --- a/aerial_robot_nerve/spinal/mcu_project/lib/Jsk_Lib/flight_control/attitude/attitude_control.cpp +++ b/aerial_robot_nerve/spinal/mcu_project/lib/Jsk_Lib/flight_control/attitude/attitude_control.cpp @@ -233,20 +233,20 @@ void AttitudeController::update(void) } ap::Vector3f angles; - ap::Vector3f vel; + ap::Vector3f ang_vel; #ifdef SIMULATION if(use_ground_truth_) { angles = true_angles_; - vel = true_vel_; + ang_vel = true_ang_vel_; } else { angles = estimator_->getAttEstimator()->getAttitude(Frame::VIRTUAL); - vel = estimator_->getAttEstimator()->getAngular(Frame::VIRTUAL); + ang_vel = estimator_->getAttEstimator()->getAngular(Frame::VIRTUAL); } - ROS_DEBUG_THROTTLE(0.01, "true vs spinal: r [%f vs %f], p [%f vs %f], y [%f vs %f], ws [%f vs %f], wy [%f vs %f], wz [%f vs %f]", true_angles_.x, angles.x, true_angles_.y, angles.y, true_angles_.z, angles.z, true_vel_.x, vel.x, true_vel_.y, vel.y, true_vel_.z, vel.z); + ROS_DEBUG_THROTTLE(0.01, "true vs spinal: r [%f vs %f], p [%f vs %f], y [%f vs %f], ws [%f vs %f], wy [%f vs %f], wz [%f vs %f]", true_angles_.x, angles.x, true_angles_.y, angles.y, true_angles_.z, angles.z, true_ang_vel_.x, ang_vel.x, true_ang_vel_.y, ang_vel.y, true_ang_vel_.z, ang_vel.z); if(prev_time_ < 0) DELTA_T = 0; else DELTA_T = ros::Time::now().toSec() - prev_time_; @@ -283,7 +283,7 @@ void AttitudeController::update(void) // linear control method { /* gyro moment */ - ap::Vector3f gyro_moment = vel % (inertia_ * vel); + ap::Vector3f gyro_moment = ang_vel % (inertia_ * ang_vel); #ifdef SIMULATION std_msgs::Float32MultiArray anti_gyro_msg; #endif @@ -298,18 +298,17 @@ void AttitudeController::update(void) { control_feedback_state_msg_.roll_p = error_angle[axis] * 1000; control_feedback_state_msg_.roll_i = error_angle_i_[axis] * 1000; - control_feedback_state_msg_.roll_d = vel[axis] * 1000; + control_feedback_state_msg_.roll_d = ang_vel[axis] * 1000; } if(axis == Y) { control_feedback_state_msg_.pitch_p = error_angle[axis] * 1000; control_feedback_state_msg_.pitch_i = error_angle_i_[axis] * 1000; - control_feedback_state_msg_.pitch_d = vel[axis] * 1000; - + control_feedback_state_msg_.pitch_d = ang_vel[axis] * 1000; } if(axis == Z) { - control_feedback_state_msg_.yaw_d = vel[axis] * 1000; + control_feedback_state_msg_.yaw_d = ang_vel[axis] * 1000; } } @@ -322,12 +321,12 @@ void AttitudeController::update(void) { p_term = error_angle[axis] * thrust_p_gain_[i][axis]; i_term = error_angle_i_[axis] * thrust_i_gain_[i][axis]; - d_term = -vel[axis] * thrust_d_gain_[i][axis]; + d_term = -ang_vel[axis] * thrust_d_gain_[i][axis]; if(axis == X) { roll_pitch_term_[i] = p_term + i_term + d_term; // [N] control_term_msg_.motors[i].roll_p = p_term * 1000; - control_term_msg_.motors[i].roll_i= i_term * 1000; + control_term_msg_.motors[i].roll_i = i_term * 1000; control_term_msg_.motors[i].roll_d = d_term * 1000; } if(axis == Y) diff --git a/aerial_robot_nerve/spinal/mcu_project/lib/Jsk_Lib/flight_control/attitude/attitude_control.h b/aerial_robot_nerve/spinal/mcu_project/lib/Jsk_Lib/flight_control/attitude/attitude_control.h index 8562c521f..3afef12b5 100644 --- a/aerial_robot_nerve/spinal/mcu_project/lib/Jsk_Lib/flight_control/attitude/attitude_control.h +++ b/aerial_robot_nerve/spinal/mcu_project/lib/Jsk_Lib/flight_control/attitude/attitude_control.h @@ -254,9 +254,14 @@ class AttitudeController public: void useGroundTruth(bool flag) { use_ground_truth_ = flag; } void setTrueRPY(float r, float p, float y) {true_angles_.x = r; true_angles_.y = p; true_angles_.z = y; } - void setTrueAngular(float wx, float wy, float wz) {true_vel_.x = wx; true_vel_.y = wy; true_vel_.z = wz; } + void setTrueAngular(float wx, float wy, float wz) + { + true_ang_vel_.x = wx; + true_ang_vel_.y = wy; + true_ang_vel_.z = wz; + } ap::Vector3f true_angles_; - ap::Vector3f true_vel_; + ap::Vector3f true_ang_vel_; float DELTA_T; double prev_time_; #endif From cc392144bf7c4b1e2bf10654af5add1b3bfd5fc0 Mon Sep 17 00:00:00 2001 From: Jinjie Li <45286479+Li-Jinjie@users.noreply.github.com> Date: Thu, 23 Nov 2023 20:35:37 +0900 Subject: [PATCH 3/5] [Spinal] add body_rate option in FourAxisCommand.msg. When is_body_rate == true, the body_rate cmd will be set as target_ang_vel and hence becomes the feedforward term. Note that I change the sign of d term in control_feedback_state_msg_ to be compatible with p and i. --- .../attitude/attitude_control.cpp | 135 ++++++++++-------- .../attitude/attitude_control.h | 1 + .../spinal/msg/FourAxisCommand.msg | 5 +- 3 files changed, 80 insertions(+), 61 deletions(-) diff --git a/aerial_robot_nerve/spinal/mcu_project/lib/Jsk_Lib/flight_control/attitude/attitude_control.cpp b/aerial_robot_nerve/spinal/mcu_project/lib/Jsk_Lib/flight_control/attitude/attitude_control.cpp index cfd51db80..99337e1ce 100644 --- a/aerial_robot_nerve/spinal/mcu_project/lib/Jsk_Lib/flight_control/attitude/attitude_control.cpp +++ b/aerial_robot_nerve/spinal/mcu_project/lib/Jsk_Lib/flight_control/attitude/attitude_control.cpp @@ -288,60 +288,68 @@ void AttitudeController::update(void) std_msgs::Float32MultiArray anti_gyro_msg; #endif - float error_angle[3]; - for(int axis = 0; axis < 3; axis++) + float error_angle[3]; + float error_ang_vel[3]; + for (int axis = 0; axis < 3; axis++) + { + error_angle[axis] = target_angle_[axis] - angles[axis]; + + if (is_body_rate_ctrl_) + error_ang_vel[axis] = target_ang_vel_[axis] - ang_vel[axis]; + else + error_ang_vel[axis] = 0 - ang_vel[axis]; + + if (integrate_flag_) + error_angle_i_[axis] += error_angle[axis] * DELTA_T; + + if (axis == X) + { + control_feedback_state_msg_.roll_p = error_angle[axis] * 1000; + control_feedback_state_msg_.roll_i = error_angle_i_[axis] * 1000; + control_feedback_state_msg_.roll_d = error_ang_vel[axis] * 1000; + } + if (axis == Y) + { + control_feedback_state_msg_.pitch_p = error_angle[axis] * 1000; + control_feedback_state_msg_.pitch_i = error_angle_i_[axis] * 1000; + control_feedback_state_msg_.pitch_d = error_ang_vel[axis] * 1000; + } + if (axis == Z) + { + control_feedback_state_msg_.yaw_d = error_ang_vel[axis] * 1000; + } + } + + float p_term = 0; + float i_term = 0; + float d_term = 0; + for (int i = 0; i < motor_number_; i++) + { + for (int axis = 0; axis < 3; axis++) + { + p_term = error_angle[axis] * thrust_p_gain_[i][axis]; + i_term = error_angle_i_[axis] * thrust_i_gain_[i][axis]; + d_term = error_ang_vel[axis] * thrust_d_gain_[i][axis]; + if (axis == X) { - error_angle[axis] = target_angle_[axis] - angles[axis]; - if(integrate_flag_) error_angle_i_[axis] += error_angle[axis] * DELTA_T; - - if(axis == X) - { - control_feedback_state_msg_.roll_p = error_angle[axis] * 1000; - control_feedback_state_msg_.roll_i = error_angle_i_[axis] * 1000; - control_feedback_state_msg_.roll_d = ang_vel[axis] * 1000; - } - if(axis == Y) - { - control_feedback_state_msg_.pitch_p = error_angle[axis] * 1000; - control_feedback_state_msg_.pitch_i = error_angle_i_[axis] * 1000; - control_feedback_state_msg_.pitch_d = ang_vel[axis] * 1000; - } - if(axis == Z) - { - control_feedback_state_msg_.yaw_d = ang_vel[axis] * 1000; - } + roll_pitch_term_[i] = p_term + i_term + d_term; // [N] + control_term_msg_.motors[i].roll_p = p_term * 1000; + control_term_msg_.motors[i].roll_i = i_term * 1000; + control_term_msg_.motors[i].roll_d = d_term * 1000; } - - float p_term = 0; - float i_term = 0; - float d_term = 0; - for(int i = 0; i < motor_number_; i++) + if (axis == Y) + { + roll_pitch_term_[i] += (p_term + i_term + d_term); // [N] + control_term_msg_.motors[i].pitch_p = p_term * 1000; + control_term_msg_.motors[i].pitch_i = i_term * 1000; + control_term_msg_.motors[i].pitch_d = d_term * 1000; + } + if (axis == Z) { - for(int axis = 0; axis < 3; axis++) - { - p_term = error_angle[axis] * thrust_p_gain_[i][axis]; - i_term = error_angle_i_[axis] * thrust_i_gain_[i][axis]; - d_term = -ang_vel[axis] * thrust_d_gain_[i][axis]; - if(axis == X) - { - roll_pitch_term_[i] = p_term + i_term + d_term; // [N] - control_term_msg_.motors[i].roll_p = p_term * 1000; - control_term_msg_.motors[i].roll_i = i_term * 1000; - control_term_msg_.motors[i].roll_d = d_term * 1000; - } - if(axis == Y) - { - roll_pitch_term_[i] += (p_term + i_term + d_term); // [N] - control_term_msg_.motors[i].pitch_p = p_term * 1000; - control_term_msg_.motors[i].pitch_i = i_term * 1000; - control_term_msg_.motors[i].pitch_d = d_term * 1000; - } - if(axis == Z) - { - yaw_term_[i] = extra_yaw_pi_term_[i] + d_term; - control_term_msg_.motors[i].yaw_d = d_term * 1000; //d_term; - } - } + yaw_term_[i] = extra_yaw_pi_term_[i] + d_term; + control_term_msg_.motors[i].yaw_d = d_term * 1000; // d_term; + } + } /* gyro moment compensation */ float gyro_moment_compensate = @@ -402,10 +410,11 @@ void AttitudeController::reset(void) } } - for(int i = 0; i < 3; i++) - { - target_angle_[i] = 0; - error_angle_i_[i] = 0; + for (int i = 0; i < 3; i++) + { + target_angle_[i] = 0; + target_ang_vel_[i] = 0; + error_angle_i_[i] = 0; torque_p_gain_[i] = 0; torque_i_gain_[i] = 0; @@ -474,13 +483,21 @@ void AttitudeController::fourAxisCommandCallback( const spinal::FourAxisCommand if(!failsafe_) failsafe_ = true; flight_command_last_stamp_ = HAL_GetTick(); + // get msg data from spinal::FourAxisCommand target_angle_[X] = cmd_msg.angles[0]; target_angle_[Y] = cmd_msg.angles[1]; - for(int i = 0; i < motor_number_; i++) - { - // base thrust is about the z control - base_thrust_term_[i] = cmd_msg.base_thrust[i]; + if (is_body_rate_ctrl_) + { + target_ang_vel_[X] = cmd_msg.body_rates[0]; + target_ang_vel_[Y] = cmd_msg.body_rates[1]; + target_ang_vel_[Z] = cmd_msg.body_rates[2]; + } + + for (int i = 0; i < motor_number_; i++) + { + // base thrust is about the z control + base_thrust_term_[i] = cmd_msg.base_thrust[i]; // reconstruct the pi term for yaw (temporary measure for pwm saturation avoidance) if(max_yaw_term_index_ != -1) diff --git a/aerial_robot_nerve/spinal/mcu_project/lib/Jsk_Lib/flight_control/attitude/attitude_control.h b/aerial_robot_nerve/spinal/mcu_project/lib/Jsk_Lib/flight_control/attitude/attitude_control.h index 3afef12b5..98f8661c0 100644 --- a/aerial_robot_nerve/spinal/mcu_project/lib/Jsk_Lib/flight_control/attitude/attitude_control.h +++ b/aerial_robot_nerve/spinal/mcu_project/lib/Jsk_Lib/flight_control/attitude/attitude_control.h @@ -181,6 +181,7 @@ class AttitudeController float target_angle_[3]; + float target_ang_vel_[3]; float error_angle_i_[3]; float error_angle_i_limit_[3]; diff --git a/aerial_robot_nerve/spinal/msg/FourAxisCommand.msg b/aerial_robot_nerve/spinal/msg/FourAxisCommand.msg index bf71a8aee..0abcabf52 100644 --- a/aerial_robot_nerve/spinal/msg/FourAxisCommand.msg +++ b/aerial_robot_nerve/spinal/msg/FourAxisCommand.msg @@ -1,2 +1,3 @@ -float32[3] angles -float32[] base_thrust +float32[3] angles # rad target roll angle, pitch angle, and yaw PI term (not target yaw angle) +float32[3] body_rates # rad/s target roll rate, pitch rate, yaw rate +float32[] base_thrust # N From 5ba7c616768da899c9f29882e2cfb62e845729ed Mon Sep 17 00:00:00 2001 From: Jinjie Li <45286479+Li-Jinjie@users.noreply.github.com> Date: Mon, 27 Nov 2023 12:55:37 +0900 Subject: [PATCH 4/5] [Spinal] fix a bug happened in cherry-pick --- .../flight_control/attitude/attitude_control.cpp | 16 +++++++++++++--- 1 file changed, 13 insertions(+), 3 deletions(-) diff --git a/aerial_robot_nerve/spinal/mcu_project/lib/Jsk_Lib/flight_control/attitude/attitude_control.cpp b/aerial_robot_nerve/spinal/mcu_project/lib/Jsk_Lib/flight_control/attitude/attitude_control.cpp index 99337e1ce..1abcca274 100644 --- a/aerial_robot_nerve/spinal/mcu_project/lib/Jsk_Lib/flight_control/attitude/attitude_control.cpp +++ b/aerial_robot_nerve/spinal/mcu_project/lib/Jsk_Lib/flight_control/attitude/attitude_control.cpp @@ -327,9 +327,19 @@ void AttitudeController::update(void) { for (int axis = 0; axis < 3; axis++) { - p_term = error_angle[axis] * thrust_p_gain_[i][axis]; - i_term = error_angle_i_[axis] * thrust_i_gain_[i][axis]; - d_term = error_ang_vel[axis] * thrust_d_gain_[i][axis]; + if (is_attitude_ctrl_) + { + p_term = error_angle[axis] * thrust_p_gain_[i][axis]; + i_term = error_angle_i_[axis] * thrust_i_gain_[i][axis]; + d_term = error_ang_vel[axis] * thrust_d_gain_[i][axis]; + } + else + { + p_term = 0; + i_term = 0; + d_term = error_ang_vel[axis] * thrust_d_gain_[i][axis]; + } + if (axis == X) { roll_pitch_term_[i] = p_term + i_term + d_term; // [N] From 66cbb6fe5093c18991a5e70e37c6cf1cbc635048 Mon Sep 17 00:00:00 2001 From: Jinjie Li <45286479+Li-Jinjie@users.noreply.github.com> Date: Mon, 27 Nov 2023 13:37:19 +0900 Subject: [PATCH 5/5] [Spinal] fix: add is_attitude_ctrl_ support for yaw term --- .../Jsk_Lib/flight_control/attitude/attitude_control.cpp | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/aerial_robot_nerve/spinal/mcu_project/lib/Jsk_Lib/flight_control/attitude/attitude_control.cpp b/aerial_robot_nerve/spinal/mcu_project/lib/Jsk_Lib/flight_control/attitude/attitude_control.cpp index 1abcca274..b73153cd5 100644 --- a/aerial_robot_nerve/spinal/mcu_project/lib/Jsk_Lib/flight_control/attitude/attitude_control.cpp +++ b/aerial_robot_nerve/spinal/mcu_project/lib/Jsk_Lib/flight_control/attitude/attitude_control.cpp @@ -356,7 +356,11 @@ void AttitudeController::update(void) } if (axis == Z) { - yaw_term_[i] = extra_yaw_pi_term_[i] + d_term; + if (is_attitude_ctrl_) + yaw_term_[i] = extra_yaw_pi_term_[i] + d_term; + else + yaw_term_[i] = d_term; + control_term_msg_.motors[i].yaw_d = d_term * 1000; // d_term; } }