erki/skullc-peripherals
1
0
Fork 0
Code Issues Pull Requests Actions Projects Releases Wiki Activity

Temporary working commit

Browse Source
This commit is contained in:
erki 2023-09-30 19:01:57 +03:00
parent 56c16b8984
commit 481f1d2c8d
1 changed files with 19 additions and 34 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

53
Peripherals/Inc/peripherals_imu_icm.hpp
View File

@ -13,6 +13,8 @@
#include "peripherals_imu.hpp" #include "peripherals_imu.hpp"
#include "utility_logging.hpp"
namespace Peripherals namespace Peripherals
{ {
@ -43,10 +45,7 @@ public:
ImuIcm() = delete; ImuIcm() = delete;
ImuIcm(const registers_handle& registers) : registers(registers) ImuIcm(const registers_handle& registers) : registers(registers)
{ { }
bias_accel_.fill(0);
bias_gyro_.fill(0);
}
void setup() override void setup() override
{ {
@ -71,13 +70,9 @@ public:
// rate = 1KHz, temp filter = 42 // rate = 1KHz, temp filter = 42
hal::delay(10); hal::delay(10);
const std::uint8_t new_gyro_conf = (std::uint32_t(scale_gyro_) << 3); setAccelerometerScale(scale_accel_);
registers.writeRegister(Registers_::GYRO_CONFIG & Registers_::WRITE_MASK,
new_gyro_conf);
const std::uint8_t new_accel_config = (std::uint32_t(scale_accel_) << 3); setGyroscopeScale(scale_gyro_);
registers.writeRegister(Registers_::ACCEL_CONFIG & Registers_::WRITE_MASK,
new_accel_config);
// ACCEL_FCHOICE_B = 0, A_DLPF_CFG = 3 filter=44.8/61.5 rate=1KHz // ACCEL_FCHOICE_B = 0, A_DLPF_CFG = 3 filter=44.8/61.5 rate=1KHz
registers.writeRegister(Registers_::ACCEL_CONFIG2 & Registers_::WRITE_MASK, registers.writeRegister(Registers_::ACCEL_CONFIG2 & Registers_::WRITE_MASK,
@ -99,20 +94,10 @@ public:
0); 0);
hal::delay(10); hal::delay(10);
registers.writeRegister(Registers_::INT_ENABLE & Registers_::WRITE_MASK, 1); registers.writeRegister(Registers_::INT_ENABLE & Registers_::WRITE_MASK, 0);
hal::delay(10); hal::delay(10);
} }
std::array<std::int16_t, 3>& gyroBias()
{
return bias_gyro_;
}
std::array<std::int16_t, 3>& accelBias()
{
return bias_accel_;
}
void calibrate(const std::uint32_t samples) override void calibrate(const std::uint32_t samples) override
{ {
const auto prev_accel_scale = scale_accel_; const auto prev_accel_scale = scale_accel_;
@ -126,7 +111,7 @@ public:
std::array<std::int32_t, 3> bias_gyro = {0, 0, 0}; std::array<std::int32_t, 3> bias_gyro = {0, 0, 0};
std::array<std::int32_t, 3> bias_accel = {0, 0, 0}; std::array<std::int32_t, 3> bias_accel = {0, 0, 0};
const std::uint16_t gyro_sensitivity = 131; // = 131 LSB/degrees/sec [[maybe_unused]] const std::uint16_t gyro_sensitivity = 131; // = 131 LSB/degrees/sec
const std::uint16_t accel_sensitivity = 16384; // = 16384 LSB/g const std::uint16_t accel_sensitivity = 16384; // = 16384 LSB/g
for (std::uint32_t i = 0; i < samples; i++) for (std::uint32_t i = 0; i < samples; i++)
@ -157,8 +142,6 @@ public:
std::array<std::uint8_t, 6> push_data; std::array<std::uint8_t, 6> push_data;
// -3888, -2972, 0033
// Construct the gyro biases for push to the hardware gyro bias registers, // Construct the gyro biases for push to the hardware gyro bias registers,
// which are reset to zero upon device startup. // which are reset to zero upon device startup.
// Divide by 4 to get 32.9 LSB per deg/s to conform to expected bias input // Divide by 4 to get 32.9 LSB per deg/s to conform to expected bias input
@ -174,11 +157,11 @@ public:
push_data.data(), push_data.size()); push_data.data(), push_data.size());
std::array<std::int32_t, 3> factory_values = {0, 0, 0}; std::array<std::int32_t, 3> factory_values = {0, 0, 0};
registers.readRegisterMultibyte(Registers_::XA_OFFSET_H | Registers_::READ_MASK, // registers.readRegisterMultibyte(Registers_::XA_OFFSET_H | Registers_::READ_MASK,
push_data.data(), push_data.size()); // push_data.data(), push_data.size());
factory_values[0] = std::int32_t((std::int16_t(push_data[0]) << 8) | push_data[1]); // factory_values[0] = std::int32_t((std::int16_t(push_data[0]) << 8) | push_data[1]);
factory_values[1] = std::int32_t((std::int16_t(push_data[2]) << 8) | push_data[3]); // factory_values[1] = std::int32_t((std::int16_t(push_data[2]) << 8) | push_data[3]);
factory_values[2] = std::int32_t((std::int16_t(push_data[4]) << 8) | push_data[5]); // factory_values[2] = std::int32_t((std::int16_t(push_data[4]) << 8) | push_data[5]);
// Construct total accelerometer bias, including calculated average // Construct total accelerometer bias, including calculated average
// accelerometer bias from above // accelerometer bias from above
@ -198,9 +181,14 @@ public:
registers.writeRegisterMultibyte(Registers_::XA_OFFSET_H & Registers_::WRITE_MASK, registers.writeRegisterMultibyte(Registers_::XA_OFFSET_H & Registers_::WRITE_MASK,
push_data.data(), push_data.size()); push_data.data(), push_data.size());
// __asm__("BKPT");
setAccelerometerScale(prev_accel_scale); setAccelerometerScale(prev_accel_scale);
setGyroscopeScale(prev_gyro_scale); setGyroscopeScale(prev_gyro_scale);
SKULLC_LOG_DEBUG("ICM: Accelerometer bias: %d, %d, %d; accel total: %d, %d, %d.",
bias_accel[0], bias_accel[1], bias_accel[2],
avg_accel[0], avg_accel[1], avg_accel[2]);
// TODO: remove manual bias calculations. These are now done in hardware. // TODO: remove manual bias calculations. These are now done in hardware.
} }
@ -285,12 +273,12 @@ public:
float accelerometerRawToReading(const std::int16_t bit, const std::size_t axis) const float accelerometerRawToReading(const std::int16_t bit, const std::size_t axis) const
{ {
return (float(bit - bias_accel_[axis]) * accel_fs_to_bit_constants_[std::uint32_t(scale_accel_)]) * 9.8f; return (float(bit) * accel_fs_to_bit_constants_[std::uint32_t(scale_accel_)]) * 9.8f;
} }
float gyroRawToReading(const std::int16_t bit, const std::size_t axis) const float gyroRawToReading(const std::int16_t bit, const std::size_t axis) const
{ {
return float(bit - bias_gyro_[axis]) * gyro_fs_to_bit_constants_[std::uint32_t(scale_gyro_)]; return float(bit) * gyro_fs_to_bit_constants_[std::uint32_t(scale_gyro_)];
} }
std::uint8_t readProductId() std::uint8_t readProductId()
@ -310,9 +298,6 @@ private:
(250.0f / 32768.0f), (500.0f / 32768.0f), (1000.0f / 32768.0f), (250.0f / 32768.0f), (500.0f / 32768.0f), (1000.0f / 32768.0f),
(2000.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_ struct Registers_
{ {
static constexpr std::uint32_t ICM20689_ID = 0x98; static constexpr std::uint32_t ICM20689_ID = 0x98;