[Spinal][Gimbalcontrol] modified gimbal control publish rate

sugihara-16 · Dec 9, 2023 · 04cdd37 · 04cdd37
1 parent 59aced3
commit 04cdd37
Show file tree

Hide file tree

Showing 2 changed files with 7 additions and 2 deletions.
diff --git a/...l_robot_nerve/spinal/mcu_project/lib/Jsk_Lib/flight_control/attitude/attitude_control.cpp b/...l_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
@@ -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