Peripherals/Inc/peripherals_hal_st.hpp

@@ -262,30 +262,10 @@ struct SpiRegisters
 
 #ifdef HAL_UART_MODULE_ENABLED
 
-namespace _Details
-{
-
-/**
- * @hack: Temporary workarounds to make the HAL compile for FW1.27. ST made it so that only UART libraries
- * are const-correct. The others remain unconst. So these wrappers will exist until we're no longer partially
- * const-correct.
- */
-inline HAL_StatusTypeDef uartTransmit(UART_HandleTypeDef *huart, std::uint8_t *data, const std::uint16_t size, const std::uint32_t timeout)
-{
-  return HAL_UART_Transmit(huart, data, size, timeout);
-}
-
-inline HAL_StatusTypeDef uartTransmitDma(UART_HandleTypeDef *huart, std::uint8_t *data, const std::uint16_t size)
-{
-  return HAL_UART_Transmit_DMA(huart, data, size);
-}
-
-}
-
 using UartInterface =
-        SerialInterface<UART_HandleTypeDef, _Details::uartTransmit, HAL_UART_Receive>;
+        SerialInterface<UART_HandleTypeDef, HAL_UART_Transmit, HAL_UART_Receive>;
 using UartInterfaceDMA =
-        SerialInterfaceAsync<UART_HandleTypeDef, _Details::uartTransmitDma,
+        SerialInterfaceAsync<UART_HandleTypeDef, HAL_UART_Transmit_DMA,
                              HAL_UART_Receive_DMA>;
 
 #endif// HAL_UART_MODULE_ENABLED

Peripherals/Inc/peripherals_imu_icm.hpp

@@ -80,6 +80,10 @@ public:
     registers.writeRegister(Registers_::ACCEL_CONFIG & Registers_::WRITE_MASK,
                             new_accel_config);
 
+    //    setGyroscopeScale(scale_gyro_);
+    //
+    //    setAccelerometerScale(scale_accel_);
+
     // ACCEL_FCHOICE_B = 0, A_DLPF_CFG = 3 filter=44.8/61.5 rate=1KHz
     registers.writeRegister(Registers_::ACCEL_CONFIG2 & Registers_::WRITE_MASK,
                             0x03);
@@ -125,16 +129,16 @@ public:
     for (std::uint32_t i = 0; i < samples; i++)
     {
       std::array<std::int16_t, 3> raw;
+      auto add_to_avg = [&raw](std::array<std::int32_t, 3>& out) {
+        for (std::uint32_t j = 0; j < 3; j++)
+          out[j] += raw[j];
+      };
 
       readGyroRaw(raw.data());
-      for (std::uint32_t j = 0; j < 3; j++)
+      add_to_avg(avg_gyro);
-        avg_gyro[j] += raw[j];
 
       readAccelerometerRaw(raw.data());
-      for (std::uint32_t j = 0; j < 3; j++)
+      add_to_avg(avg_accel);
-        avg_accel[j] += raw[j];
-
-      hal::delay(2);
     }
 
     for (std::uint32_t i = 0; i < 3; i++)
@@ -244,6 +248,9 @@ private:
   GyroScale scale_gyro_ = GyroScale::DPS_2000;
   AccelerometerScale scale_accel_ = AccelerometerScale::G16;
 
+  std::array<std::int16_t, 3> bias_gyro_;
+  std::array<std::int16_t, 3> bias_accel_;
+
   static constexpr float accel_fs_to_bit_constants_[4] = {
           (2.0f / 32768.0f), (4.0f / 32768.0f), (8.0f / 32768.0f),
           (16.0f / 32768.0f)};
@@ -252,9 +259,6 @@ private:
           (250.0f / 32768.0f), (500.0f / 32768.0f), (1000.0f / 32768.0f),
           (2000.0f / 32768.0f)};
 
-  std::array<std::int16_t, 3> bias_gyro_;
-  std::array<std::int16_t, 3> bias_accel_;
-
   struct Registers_
   {
     static constexpr std::uint32_t ICM20689_ID = 0x98;