Fix/moved mag reset outside of constructor (#33)

* removed mag and icm reset calls from constructor; included the calls in the demo file

* moved only the rest_mag() outside of the constructor for a consistent and reliable startup of icm

* added steady clock into the icm as well as other supporting functions

* specified the error catch to hal::no_such_device and removed having steady clock as a member var

Author: yyaatt0

include/libhal-sensor/imu/icm20948.hpp

@@ -224,7 +224,18 @@ class icm20948
    * @brief private constructor for icm20948 objects
    * @param p_i2c The I2C peripheral used for communication with the device.
    */
-  icm20948(hal::i2c& p_i2c);
+  [[deprecated(
+    "Use the constructor with hal::steady_clock instead.")]] icm20948(hal::i2c&
+                                                                        p_i2c);
+
+  /**
+   * @brief private constructor for icm20948 objects
+   * @param p_i2c The I2C peripheral used for communication with the device.
+   * @param p_steady_clock After an ICM reset, the I2C master may not be ready;
+   thus, the Clock peripheral is used to provide delay until I2C master is
+   ready.
+   */
+  icm20948(hal::i2c& p_i2c, hal::steady_clock& p_steady_clock);
 
   void auto_offsets();
 
@@ -252,8 +263,8 @@ class icm20948
   void set_acc_dlpf(digital_lowpass_filter p_dlpf);
   void set_acc_sample_rate_div(uint16_t p_acc_spl_rate_div);
   void enable_gyro(bool p_enable_gyro);
-  [[deprecated("Use the API `enable_gyro()` with a full name.")]]
-  void enable_gyr(bool p_enable_gyro)
+  [[deprecated("Use the API `enable_gyro()` with a full name.")]] void
+  enable_gyr(bool p_enable_gyro)
   {
     enable_gyro(p_enable_gyro);
   }
@@ -278,10 +289,13 @@ class icm20948
   hal::byte check_mag_mode();
   hal::byte whoami_ak09916_wia1_direct();
   hal::byte whoami_ak09916_wia2_direct();
+  [[nodiscard]] bool mag_whoami_ok();
   void set_mag_op_mode(ak09916_op_mode p_op_mode);
   void write_ak09916_register8(hal::byte p_reg, hal::byte p_val);
 
 private:
+  static constexpr std::uint8_t max_magnetometer_starts = 5;
+
   void set_clock_auto_select();
   void switch_bank(hal::byte p_new_bank);
 
@@ -312,6 +326,7 @@ class icm20948
   void enable_mag_data_read(hal::byte p_reg, hal::byte p_bytes);
 
   void reset_icm20948();
+  void reset_i2c_master();
 
   /* The I2C peripheral used for communication with the device. */
   hal::i2c* m_i2c;

src/imu/icm20948.cpp

@@ -96,7 +96,7 @@ constexpr hal::byte temp_config = 0x53;
 /* Registers ICM20948 USER BANK 3*/
 [[maybe_unused]] constexpr hal::byte i2c_mst_odr_cfg = 0x00;
 [[maybe_unused]] constexpr hal::byte i2c_mst_ctrl = 0x01;
-[[maybe_unused]] constexpr hal::byte i2c_mst_delay_ctrl = 0x02;
+constexpr hal::byte i2c_mst_delay_ctrl = 0x02;
 constexpr hal::byte i2c_slv0_addr = 0x03;
 constexpr hal::byte i2c_slv0_reg = 0x04;
 constexpr hal::byte i2c_slv0_ctrl = 0x05;
@@ -177,6 +177,53 @@ icm20948::icm20948(hal::i2c& p_i2c)
   write_register8({ .bank = 2, .reg = odr_align_en, .val = 1 });  // aligns ODR
 }
 
+icm20948::icm20948(hal::i2c& p_i2c, hal::steady_clock& p_steady_clock)
+  : m_i2c(&p_i2c)
+{
+  m_current_bank = 0;
+
+  reset_icm20948();
+  hal::delay(*p_steady_clock, 100ms);
+  set_clock_auto_select();
+  sleep(false);
+  hal::delay(*p_steady_clock, 10ms);
+  reset_mag();
+
+  std::uint8_t tries = 0;
+  while (tries < max_magnetometer_starts) {
+    tries++;
+
+    if (mag_whoami_ok()) {
+      break;
+    }
+
+    i2c_master_reset();
+    hal::delay(*p_steady_clock, 10ms);
+  }
+
+  if (auto id = whoami(); id != who_am_i_content) {
+    hal::safe_throw(hal::no_such_device(id, this));
+  }
+
+  m_acc_offset_val.x = 0.0;
+  m_acc_offset_val.y = 0.0;
+  m_acc_offset_val.z = 0.0;
+  m_acc_corr_factor.x = 1.0;
+  m_acc_corr_factor.y = 1.0;
+  m_acc_corr_factor.z = 1.0;
+  m_acc_range_factor = 1.0;
+  m_gyro_offset_val.x = 0.0;
+  m_gyro_offset_val.y = 0.0;
+  m_gyro_offset_val.z = 0.0;
+  m_gyro_range_factor = 1.0;
+
+  sleep(false);
+  enable_acc(true);
+  enable_gyro(true);
+
+  write_register8({ .bank = 2, .reg = odr_align_en, .val = 1 });  // aligns ODR
+}
+
 void icm20948::auto_offsets()
 {
   set_gyro_dlpf(dlpf_6);           // lowest noise
@@ -662,4 +709,26 @@ void icm20948::enable_mag_data_read(hal::byte p_reg,   // NOLINT
   hal::byte const enable_and_bytes = 0x80 | p_bytes;
   write_register8({ .bank = 3, .reg = i2c_slv0_ctrl, .val = enable_and_bytes });
 }
+
+bool icm20948::mag_whoami_ok()
+{
+  try {
+    enable_bypass_mode();
+
+    auto const wia1 = whoami_ak09916_wia1_direct();
+    auto const wia2 = whoami_ak09916_wia2_direct();
+
+    return (wia1 == static_cast<hal::byte>(ak09916_who_am_i_1)) &&
+           (wia2 == static_cast<hal::byte>(ak09916_who_am_i_2));
+  } catch (hal::no_such_device const&) {
+    return false;
+  }
+}
+
+void icm20948::reset_i2c_master()
+{
+  auto ctrl = read_register8({ .bank = 0, .reg = user_ctrl });
+  ctrl |= i2c_mst_delay_ctrl;
+  write_register8({ .bank = 0, .reg = user_ctrl, .val = ctrl });
+}
 }  // namespace hal::sensor