[Spinal][PwmTest] workaround to send pwm value to each servo independ…

…ently.
sugihara-16 · May 22, 2024 · 1088c14 · 1088c14
1 parent 7ac7184
commit 1088c14
Show file tree

Hide file tree

Showing 4 changed files with 41 additions and 7 deletions.
diff --git a/aerial_robot_nerve/spinal/CMakeLists.txt b/aerial_robot_nerve/spinal/CMakeLists.txt
@@ -38,6 +38,7 @@ add_message_files(
   MotorInfo.msg
   PwmInfo.msg
   Pwms.msg
+  PwmTest.msg
   UavInfo.msg
   DesireCoord.msg
   FlightConfigCmd.msg

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
@@ -653,10 +653,40 @@ void AttitudeController::maxYawGainIndex()
     }
 }
 
-void AttitudeController::pwmTestCallback(const std_msgs::Float32& pwm_msg)
+void AttitudeController::pwmTestCallback(const spinal::PwmTest& pwm_msg)
 {
-  pwm_test_flag_ = true;
-  pwm_test_value_ = pwm_msg.data; //2000ms
+  if(pwm_msg.pwms_length && !pwm_test_flag_)
+    {
+      pwm_test_flag_ = true;
+      nh_->logwarn("Enter pwm test mode"); 
+    }
+  else if(!pwm_msg.pwms_length && pwm_test_flag_)
+    {
+      pwm_test_flag_ = false;
+      nh_->logwarn("Escape from pwm test mode");
+      return;
+    }
+
+  if(pwm_msg.motor_index_length)
+    {
+      /*Individual test mode*/
+      if(pwm_msg.motor_index_length != pwm_msg.pwms_length)
+        {
+          nh_->logerror("The number of index does not match the number of pwms.");
+          return;
+        }
+      for(int i = 0; i < pwm_msg.motor_index_length; i++){
+        int motor_index = pwm_msg.motor_index[i];
+        pwm_test_value_[motor_index] = pwm_msg.pwms[i];
+      }
+    }
+  else
+    {
+      /*Simultaneous test mode*/
+      for(int i = 0; i < MAX_MOTOR_NUMBER; i++){
+        pwm_test_value_[i] = pwm_msg.pwms[0];
+      }
+    }
 }
 
 void AttitudeController::setStartControlFlag(bool start_control_flag)
@@ -801,7 +831,7 @@ void AttitudeController::pwmConversion()
     {
       for(int i = 0; i < MAX_MOTOR_NUMBER; i++)
         {
-          target_pwm_[i] = pwm_test_value_;
+          target_pwm_[i] = pwm_test_value_[i];
         }
       return;
     }

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
@@ -39,6 +39,7 @@
 #include <std_msgs/Float32MultiArray.h>
 #include <std_srvs/SetBool.h>
 #include <spinal/Pwms.h>
+#include <spinal/PwmTest.h>
 #include <spinal/FourAxisCommand.h>
 #include <spinal/RollPitchYawTerms.h>
 #include <spinal/PwmInfo.h>
@@ -133,7 +134,7 @@ class AttitudeController
   ros::Subscriber<spinal::FourAxisCommand, AttitudeController> four_axis_cmd_sub_;
   ros::Subscriber<spinal::PwmInfo, AttitudeController> pwm_info_sub_;
   ros::Subscriber<spinal::RollPitchYawTerms, AttitudeController> rpy_gain_sub_;
-  ros::Subscriber<std_msgs::Float32, AttitudeController> pwm_test_sub_;
+  ros::Subscriber<spinal::PwmTest, AttitudeController> pwm_test_sub_;
   ros::Subscriber<spinal::PMatrixPseudoInverseWithInertia, AttitudeController> p_matrix_pseudo_inverse_inertia_sub_;
   ros::Subscriber<spinal::TorqueAllocationMatrixInv, AttitudeController> torque_allocation_matrix_inv_sub_;
   ros::ServiceServer<std_srvs::SetBool::Request, std_srvs::SetBool::Response, AttitudeController> att_control_srv_;
@@ -195,7 +196,7 @@ class AttitudeController
   uint32_t voltage_update_last_time_;
   uint32_t control_term_pub_last_time_, control_feedback_state_pub_last_time_;
   uint32_t pwm_pub_last_time_;
-  float pwm_test_value_; // PWM Test
+  float pwm_test_value_[MAX_MOTOR_NUMBER]; // PWM Test
 
   void fourAxisCommandCallback( const spinal::FourAxisCommand &cmd_msg);
   void pwmInfoCallback( const spinal::PwmInfo &info_msg);
@@ -204,7 +205,7 @@ class AttitudeController
   void torqueAllocationMatrixInvCallback(const spinal::TorqueAllocationMatrixInv& msg);
   void thrustGainMapping();
   void maxYawGainIndex();
-  void pwmTestCallback(const std_msgs::Float32& pwm_msg);
+  void pwmTestCallback(const spinal::PwmTest& pwm_msg);
   void pwmConversion(void);
   void pwmsControl(void);
 

diff --git a/aerial_robot_nerve/spinal/msg/PwmTest.msg b/aerial_robot_nerve/spinal/msg/PwmTest.msg
@@ -0,0 +1,2 @@
+uint8[] motor_index
+float32[] pwms