[Spinal][DShot] modified volatage sensing update procudure

sugihara-16 · Oct 4, 2024 · cc7932b · cc7932b
1 parent 43387bb
commit cc7932b
Show file tree

Hide file tree

Showing 4 changed files with 36 additions and 36 deletions.
diff --git a/aerial_robot_nerve/spinal/mcu_project/boards/stm32H7_v2/Src/main.c b/aerial_robot_nerve/spinal/mcu_project/boards/stm32H7_v2/Src/main.c
@@ -250,7 +250,7 @@ int main(void)
   IMU_ROS_CMD::addImu(&imu_);
   baro_.init(&hi2c1, &nh_, BAROCS_GPIO_Port, BAROCS_Pin);
   gps_.init(&huart3, &nh_, LED2_GPIO_Port, LED2_Pin);
-  battery_status_.init(&hadc1, &nh_, false);
+  battery_status_.init(&hadc1, &nh_, &dshot_);
   estimator_.init(&imu_, &baro_, &gps_, &nh_);  // imu + baro + gps => att + alt + pos(xy)
   dshot_.init(DSHOT600, &htim1,TIM_CHANNEL_1, &htim1,TIM_CHANNEL_2, &htim1,TIM_CHANNEL_3, &htim1, TIM_CHANNEL_4);
   dshot_.initTelemetry(&huart6);

diff --git a/aerial_robot_nerve/spinal/mcu_project/lib/Jsk_Lib/battery_status/battery_status.h b/aerial_robot_nerve/spinal/mcu_project/lib/Jsk_Lib/battery_status/battery_status.h
@@ -14,6 +14,7 @@
 
 #include <config.h>
 #include "flashmemory/flashmemory.h"
+#include "dshot_esc/dshot.h"
 
 /* ros */
 #include <ros.h>
@@ -28,29 +29,36 @@ class BatteryStatus
 {
 public:
   BatteryStatus():  voltage_status_pub_("battery_voltage_status", &voltage_status_msg_),
-                    adc_scale_sub_("set_adc_scale", &BatteryStatus::adcScaleCallback, this)
+                    adc_scale_sub_("set_adc_scale", &BatteryStatus::adcScaleCallback, this),
+                    is_adc_measure_(true)
   {
   }
 
   ~BatteryStatus(){}
 
 
-  void init(ADC_HandleTypeDef *hadc, ros::NodeHandle* nh, bool is_adc_measure=true)
+  void init(ADC_HandleTypeDef *hadc, ros::NodeHandle* nh)
   {
     nh_ = nh;
     nh_->advertise(voltage_status_pub_);
     nh_->subscribe(adc_scale_sub_);
     hadc_ = hadc;
 
     voltage_ = -1;
-    is_adc_measure_ = is_adc_measure;
     ros_pub_last_time_ = HAL_GetTick();
 
     FlashMemory::addValue(&adc_scale_, sizeof(float));
 
     HAL_ADC_Start(hadc_);
   }
 
+  void init(ADC_HandleTypeDef *hadc, ros::NodeHandle* nh, DShot* dshot)
+  {
+    dshot_ = dshot;
+    is_adc_measure_ = false;
+    init(hadc, nh);
+  }
+
   void adcScaleCallback(const std_msgs::Float32& cmd_msg)
   {
     adc_scale_ = cmd_msg.data;
@@ -60,20 +68,28 @@ class BatteryStatus
     nh_->loginfo("overwrite adc scale");
   }
 
-  void update(float voltage = -1)
+  void update()
   {
-    if (voltage == -1)  // no new data
-    {
-      if (!is_adc_measure_)
-        return;
-
-      if (HAL_ADC_PollForConversion(hadc_, 10) == HAL_OK)
-        adc_value_ = HAL_ADC_GetValue(hadc_);
-
-      HAL_ADC_Start(hadc_);
-
-      voltage = adc_scale_ * adc_value_;
-    }
+    float voltage;
+    if(is_adc_measure_)
+      {
+        if (HAL_ADC_PollForConversion(hadc_, 10) == HAL_OK) adc_value_ = HAL_ADC_GetValue(hadc_);
+        HAL_ADC_Start(hadc_);
+        voltage = adc_scale_ * adc_value_;
+      }
+    else
+      {
+        if (dshot_->is_telemetry_)
+          {
+            if (dshot_->esc_reader_.is_update_all_msg_)
+              {
+                float voltage_ave = (float)(dshot_->esc_reader_.esc_msg_1_.voltage + dshot_->esc_reader_.esc_msg_2_.voltage +
+                                            dshot_->esc_reader_.esc_msg_3_.voltage + dshot_->esc_reader_.esc_msg_4_.voltage) / 400.0;
+                voltage_ = TELE_VOLTAGE_SCALE * voltage_ave;
+                dshot_->esc_reader_.is_update_all_msg_ = false;
+              }
+          }
+      }
 
     if(voltage_ < 0) voltage_ = voltage;
 
@@ -99,6 +115,7 @@ class BatteryStatus
 private:
   ros::NodeHandle* nh_;
   ADC_HandleTypeDef *hadc_;
+  DShot* dshot_;
 
   float adc_value_;
   float adc_scale_;

diff --git a/aerial_robot_nerve/spinal/mcu_project/lib/Jsk_Lib/dshot_esc/esc_telem.h b/aerial_robot_nerve/spinal/mcu_project/lib/Jsk_Lib/dshot_esc/esc_telem.h
@@ -19,6 +19,8 @@
 
 #define ESC_BUFFER_SIZE 512
 
+#define TELE_VOLTAGE_SCALE 1.022f
+
 namespace
 {
 #ifdef STM32H7

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
@@ -209,25 +209,6 @@ void AttitudeController::pwmsControl(void)
         }
 
       dshot_->write(motor_value, dshot_->is_telemetry_);
-
-      if (dshot_->is_telemetry_)
-        {
-          if (dshot_->esc_reader_.is_update_all_msg_)
-            {
-              esc_telem_msg_.stamp = nh_->now();
-              esc_telem_msg_.esc_telemetry_1 = dshot_->esc_reader_.esc_msg_1_;
-              esc_telem_msg_.esc_telemetry_2 = dshot_->esc_reader_.esc_msg_2_;
-              esc_telem_msg_.esc_telemetry_3 = dshot_->esc_reader_.esc_msg_3_;
-              esc_telem_msg_.esc_telemetry_4 = dshot_->esc_reader_.esc_msg_4_;
-              esc_telem_pub_.publish(&esc_telem_msg_);
-
-              float voltage_ave = (float)(dshot_->esc_reader_.esc_msg_1_.voltage + dshot_->esc_reader_.esc_msg_2_.voltage +
-                                          dshot_->esc_reader_.esc_msg_3_.voltage + dshot_->esc_reader_.esc_msg_4_.voltage) / 400.0;
-              bat_->update(voltage_ave);
-
-              dshot_->esc_reader_.is_update_all_msg_ = false;
-            }
-        }
     }
   else
     {