From 04cdd37665fe14382a3f977479bc394b78fa29c1 Mon Sep 17 00:00:00 2001 From: Junichiro Sugihara Date: Sat, 9 Dec 2023 21:19:43 +0900 Subject: [PATCH] [Spinal][Gimbalcontrol] modified gimbal control publish rate --- .../Jsk_Lib/flight_control/attitude/attitude_control.cpp | 6 +++++- .../lib/Jsk_Lib/flight_control/attitude/attitude_control.h | 3 ++- 2 files changed, 7 insertions(+), 2 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 57b8add1f..44dfb5207 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 @@ -987,7 +987,11 @@ void AttitudeController::pwmConversion() for(int i = 0; i < motor_number_ / (gimbal_dof_ + 1); i++){ gimbal_control_msg.position.push_back(target_gimbal_angles_[i]); } - gimbal_control_pub_.publish(gimbal_control_msg); + if(HAL_GetTick() - gimbal_control_pub_last_time_ > GIMBAL_CONTROL_PUB_INTERVAL) + { + gimbal_control_pub_last_time_ = HAL_GetTick(); + gimbal_control_pub_.publish(gimbal_control_msg); + } } #else if(gimbal_dof_){ 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 907a495e0..31a7af132 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 @@ -61,6 +61,7 @@ #define CONTROL_TERM_PUB_INTERVAL 100 #define CONTROL_FEEDBACK_STATE_PUB_INTERVAL 25 #define PWM_PUB_INTERVAL 100 //100ms +#define GIMBAL_CONTROL_PUB_INTERVAL 25 //25ms #define MOTOR_TEST 0 @@ -201,7 +202,7 @@ class AttitudeController uint8_t motor_ref_index_; float v_factor_; uint32_t voltage_update_last_time_; - uint32_t control_term_pub_last_time_, control_feedback_state_pub_last_time_; + uint32_t control_term_pub_last_time_, control_feedback_state_pub_last_time_, gimbal_control_pub_last_time_;; uint32_t pwm_pub_last_time_; float pwm_test_value_; // PWM Test