add date rate for icm42688&mmc5603.

xrobot-org · Apr 18, 2024 · d7124a3 · d7124a3
1 parent 794e14a
commit d7124a3
Show file tree

Hide file tree

Showing 4 changed files with 60 additions and 19 deletions.
diff --git a/src/device/icm42688/dev_icm42688.cpp b/src/device/icm42688/dev_icm42688.cpp
@@ -37,9 +37,10 @@ void ICM42688::Read(uint8_t reg, uint8_t *data, uint8_t len) {
   bsp_spi_mem_read(BSP_SPI_IMU, reg, data, len, false);
 }
 
-ICM42688::ICM42688(ICM42688::Rotation &rot)
+ICM42688::ICM42688(ICM42688::Rotation &rot, DataRate date_rate)
     : cali_("icm42688_cali"),
       rot_(rot),
+      datarate_(date_rate),
       raw_(0),
       new_(0),
       accl_tp_("imu_accl"),
@@ -66,7 +67,7 @@ ICM42688::ICM42688(ICM42688::Rotation &rot)
                             this);
 
   auto thread_icm42688 = [](ICM42688 *icm42688) {
-    while (!icm42688->Init()) {
+    while (!icm42688->Init(icm42688->datarate_)) {
       System::Thread::Sleep(1);
     }
 
@@ -175,7 +176,7 @@ int ICM42688::CaliCMD(ICM42688 *icm42688, int argc, char **argv) {
   return 0;
 }
 
-bool ICM42688::Init() {
+bool ICM42688::Init(DataRate date_rate) {
   /*指定Bank0*/
   WriteSingle(0x76, 0x00);
   /*软重启*/
@@ -195,9 +196,9 @@ bool ICM42688::Init() {
   /*中断输出设置*/
   WriteSingle(0x14, 0x12);  // INT1 INT2 脉冲模式，低有效
   /*Gyro设置*/
-  WriteSingle(0x4F, 0x06);  // 2000dps 1KHz
+  WriteSingle(0x4F, date_rate);  // 2000dps 1KHz
   /*Accel设置*/
-  WriteSingle(0x50, 0x06);  // 16G 1KHz
+  WriteSingle(0x50, date_rate);  // 16G 1KHz
   /*Tem设置&Gyro_Config1*/
   WriteSingle(0x51, 0x56);  // BW 82Hz Latency = 2ms
   /*GYRO_ACCEL_CONFIG0*/
@@ -207,7 +208,7 @@ bool ICM42688::Init() {
   /*INT_CONFIG0*/
   WriteSingle(0x63, 0x00);  // Null
   /*INT_CONFIG1*/
-  WriteSingle(0x64, 0x00);  //中断引脚正常启用
+  WriteSingle(0x64, 0x00);  // 中断引脚正常启用
   /*INT_SOURCE0*/
   WriteSingle(0x65, 0x08);  // DRDY INT1
   /*INT_SOURCE1*/
@@ -223,7 +224,7 @@ bool ICM42688::Init() {
   /*指定Bank1*/
   WriteSingle(0x76, 0x01);
   /*GYRO抗混叠滤波器配置*/
-  WriteSingle(0x0B, 0xA0);  //开启抗混叠和陷波滤波器
+  WriteSingle(0x0B, 0xA0);  // 开启抗混叠和陷波滤波器
   WriteSingle(0x0C, 0x0C);  // GYRO_AAF_DELT 12 (default 13)
   WriteSingle(0x0D, 0x90);  // GYRO_AAF_DELTSQR 144 (default 170)
   WriteSingle(0x0E, 0x80);  // GYRO_AAF_BITSHIFT 8 (default 8)
@@ -232,7 +233,7 @@ bool ICM42688::Init() {
   /*指定Bank2*/
   WriteSingle(0x76, 0x02);
   /*ACCEL抗混叠滤波器配置*/
-  WriteSingle(0x03, 0x18);  //开启滤波器 ACCEL_AFF_DELT 12 (default 24)
+  WriteSingle(0x03, 0x18);  // 开启滤波器 ACCEL_AFF_DELT 12 (default 24)
   WriteSingle(0x04, 0x90);  // ACCEL_AFF_DELTSQR 144 (default 64)
   WriteSingle(0x05, 0x80);  // ACCEL_AAF_BITSHIFT 8 (default 6)
 

diff --git a/src/device/icm42688/dev_icm42688.hpp b/src/device/icm42688/dev_icm42688.hpp
@@ -18,9 +18,25 @@ class ICM42688 {
     float rot_mat[3][3];
   } Rotation;
 
-  ICM42688(ICM42688::Rotation &rot);
-
-  bool Init();
+  typedef enum {
+    DATA_RATE_UNKNOW,
+    DATA_RATE_32KHZ,
+    DATA_RATE_16KHZ,
+    DATA_RATE_8KHZ,
+    DATA_RATE_4KHZ,
+    DATA_RATE_2KHZ,
+    DATA_RATE_1KHZ,
+    DATA_RATE_200HZ,
+    DATA_RATE_100HZ,
+    DATA_RATE_50HZ,
+    DATA_RATE_25HZ,
+    DATA_RATE_12_5HZ,
+    DATA_RATE_500HZ = 0XF,
+  } DataRate;
+
+  ICM42688(ICM42688::Rotation &rot, DataRate date_rate = DATA_RATE_1KHZ);
+
+  bool Init(DataRate date_rate);
 
   void Prase();
 
@@ -42,6 +58,8 @@ class ICM42688 {
   System::Database::Key<Calibration> cali_;
   Rotation &rot_;
 
+  DataRate datarate_;
+
   System::Semaphore raw_;
   System::Semaphore new_;
 

diff --git a/src/device/mmc5603/dev_mmc5603.cpp b/src/device/mmc5603/dev_mmc5603.cpp
@@ -9,11 +9,19 @@ static uint8_t dma_buff[10];
 
 #define MATRIX_SIZE (6)
 
-MMC5603::MMC5603(MMC5603::Rotation &rot)
+static const MMC5603::Calibration default_cali = {.scale = {
+                                                      .x = 0.5,
+                                                      .y = 0.5,
+                                                      .z = 0.5,
+                                                  }};
+
+MMC5603::MMC5603(MMC5603::Rotation &rot, float max_offset, DataRate data_rate)
     : rot_(rot),
+      data_rate_(data_rate),
+      max_offset_(max_offset),
       magn_tp_("magn"),
       cmd_(this, CaliCMD, "mmc5603"),
-      cali_data_("mmc5603_cali"),
+      cali_data_("mmc5603_cali", default_cali),
       raw_(0) {
   auto recv_cplt_callback = [](void *arg) {
     MMC5603 *mmc5603 = static_cast<MMC5603 *>(arg);
@@ -24,7 +32,7 @@ MMC5603::MMC5603(MMC5603::Rotation &rot)
                             recv_cplt_callback, this);
 
   auto thread_fn = [](MMC5603 *mmc5603) {
-    while (!mmc5603->Init()) {
+    while (!mmc5603->Init(mmc5603->data_rate_)) {
       System::Thread::Sleep(20);
     }
 
@@ -39,15 +47,18 @@ MMC5603::MMC5603(MMC5603::Rotation &rot)
         OMLOG_ERROR("mmc5603 recv timeout");
       }
 
-      mmc5603->thread_.SleepUntil(1, last_wakeup_time);
+      static uint32_t timeout_map[] = {13, 6, 4};
+
+      mmc5603->thread_.SleepUntil(timeout_map[mmc5603->data_rate_],
+                                  last_wakeup_time);
     }
   };
 
   this->thread_.Create(thread_fn, this, "mmc5603_thread", 4096,
                        System::Thread::REALTIME);
 }
 
-bool MMC5603::Init() {
+bool MMC5603::Init(DataRate date_rate) {
   /* Check Product id */
 
   if (uint8_t product_id =
@@ -63,7 +74,7 @@ bool MMC5603::Init() {
   /* Set CTRL_1 bandwith */
   bsp_i2c_mem_write_byte(BSP_I2C_MAGN, 0x30, 0x1C, 0x02);
   /* Set ODR sampling_rate */
-  bsp_i2c_mem_write_byte(BSP_I2C_MAGN, 0x30, 0x1A, 0xff);
+  bsp_i2c_mem_write_byte(BSP_I2C_MAGN, 0x30, 0x1A, date_rate);
   /* Set CTRL_0 Auto_SR_en Cmm_freq_en */
   bsp_i2c_mem_write_byte(BSP_I2C_MAGN, 0x30, 0x1B, 0xa0);
   /* Set CTRL_2 Cmm_en */

diff --git a/src/device/mmc5603/dev_mmc5603.hpp b/src/device/mmc5603/dev_mmc5603.hpp
@@ -13,9 +13,16 @@ class MMC5603 {
     Component::Type::Vector3 scale;  /* 偏置 */
   } Calibration;
 
-  MMC5603(Rotation &rot);
+  typedef enum {
+    DATA_RATE_75,
+    DATA_RATE_150,
+    DATA_RATE_255,
+  } DataRate;
 
-  bool Init();
+  MMC5603(Rotation &rot, float max_offset_ = 0.1f,
+          DataRate data_rate = DATA_RATE_255);
+
+  bool Init(DataRate date_rate);
 
   void StartRecv();
 
@@ -29,6 +36,10 @@ class MMC5603 {
 
   Rotation &rot_;
 
+  DataRate data_rate_;
+
+  float max_offset_;
+
   Component::Type::Vector3 magn_, raw_magn_;
 
   Message::Topic<Component::Type::Vector3> magn_tp_;