yemai/bizhang_-obav
1
0
Fork 0
mirror of https://gitee.com/xiaohuolufeihua/bizhang_-obav.git synced 2026-05-22 01:12:31 +00:00
Code Issues Packages Projects Releases Wiki Activity

imu/invensense/mpu9250: sync with other recent invensense improvements

Browse Source 
- clenaup ak8963 with simplifed setup and health check
This commit is contained in:
Daniel Agar
2020-06-18 11:15:28 -04:00
parent 22daa26955
commit 8c34f47b3d
7 changed files with 264 additions and 377 deletions
Download Patch File Download Diff File Expand all files Collapse all files

8
src/drivers/imu/invensense/mpu9250/AKM_AK8963_registers.hpp
View File

@@ -34,7 +34,7 @@
/**
* @file AKM_AK8963_registers.hpp
*
* AKM AK8963 registers.
* Asahi Kasei Microdevices (AKM) AK8963 registers.
*
*/
@@ -58,7 +58,7 @@ static constexpr uint8_t Bit7 = (1 << 7);
static constexpr uint32_t I2C_SPEED = 400 * 1000; // 400 kHz I2C serial interface
static constexpr uint8_t I2C_ADDRESS_DEFAULT = 0x0C;
static constexpr uint8_t WHOAMI = 0x48;
static constexpr uint8_t Device_ID = 0x48; // Device ID of AKM
enum class Register : uint8_t {
WIA = 0x00, // Device ID
@@ -99,6 +99,7 @@ enum CNTL1_BIT : uint8_t {
SINGLE_MEASUREMENT_MODE = Bit0,
CONTINUOUS_MODE_1 = Bit1, // 8 Hz
CONTINUOUS_MODE_2 = Bit2 | Bit1, // 100 Hz
FUSE_ROM_ACCESS_MODE = Bit3 | Bit2 | Bit1 | Bit0, // MODE[3:0]=“1111”
};
// CNTL2
@@ -106,5 +107,4 @@ enum CNTL2_BIT : uint8_t {
SRST = Bit0, // Reset
};
} // namespace InvenSense_MPU9250
} // namespace AKM_AK8963

3
src/drivers/imu/invensense/mpu9250/CMakeLists.txt
View File

@@ -35,6 +35,9 @@ px4_add_module(
MODULE drivers__imu__invensense__mpu9250
MAIN mpu9250
COMPILE_FLAGS
-O0
-DDEBUG_BUILD
-Wno-error
SRCS
AKM_AK8963_registers.hpp
InvenSense_MPU9250_registers.hpp

1
src/drivers/imu/invensense/mpu9250/InvenSense_MPU9250_registers.hpp
View File

@@ -78,7 +78,6 @@ enum class Register : uint8_t {
I2C_SLV4_CTRL = 0x34,
I2C_MST_STATUS = 0x36,
INT_PIN_CFG = 0x37,
INT_ENABLE = 0x38,

289
src/drivers/imu/invensense/mpu9250/MPU9250.cpp
View File

@@ -50,6 +50,10 @@ MPU9250::MPU9250(I2CSPIBusOption bus_option, int bus, uint32_t device, enum Rota
_px4_accel(get_device_id(), ORB_PRIO_HIGH, rotation),
_px4_gyro(get_device_id(), ORB_PRIO_HIGH, rotation)
{
if (drdy_gpio != 0) {
_drdy_interval_perf = perf_alloc(PC_INTERVAL, MODULE_NAME": DRDY interval");
}
ConfigureSampleRate(_px4_gyro.get_max_rate_hz());
if (enable_magnetometer) {
@@ -63,9 +67,6 @@ MPU9250::MPU9250(I2CSPIBusOption bus_option, int bus, uint32_t device, enum Rota
} else if (r.reg == Register::I2C_MST_CTRL) {
r.set_bits = I2C_MST_CTRL_BIT::I2C_MST_P_NSR | I2C_MST_CTRL_BIT::I2C_MST_CLK_400_kHz;
} else if (r.reg == Register::I2C_MST_DELAY_CTRL) {
r.set_bits = I2C_MST_DELAY_CTRL_BIT::I2C_SLVX_DLY_EN;
} else if (r.reg == Register::I2C_MST_DELAY_CTRL) {
r.set_bits = I2C_MST_DELAY_CTRL_BIT::I2C_SLVX_DLY_EN;
}
@@ -83,6 +84,8 @@ MPU9250::~MPU9250()
perf_free(_fifo_overflow_perf);
perf_free(_fifo_reset_perf);
perf_free(_drdy_interval_perf);
delete _slave_ak8963_magnetometer;
}
int MPU9250::init()
@@ -100,6 +103,7 @@ int MPU9250::init()
bool MPU9250::Reset()
{
_state = STATE::RESET;
DataReadyInterruptDisable();
ScheduleClear();
ScheduleNow();
return true;
@@ -114,8 +118,8 @@ void MPU9250::exit_and_cleanup()
void MPU9250::print_status()
{
I2CSPIDriverBase::print_status();
PX4_INFO("FIFO empty interval: %d us (%.3f Hz)", _fifo_empty_interval_us,
static_cast<double>(1000000 / _fifo_empty_interval_us));
PX4_INFO("FIFO empty interval: %d us (%.3f Hz)", _fifo_empty_interval_us, 1e6 / _fifo_empty_interval_us);
perf_print_counter(_transfer_perf);
perf_print_counter(_bad_register_perf);
@@ -145,11 +149,14 @@ int MPU9250::probe()
void MPU9250::RunImpl()
{
const hrt_abstime now = hrt_absolute_time();
switch (_state) {
case STATE::RESET:
// PWR_MGMT_1: Device Reset
RegisterWrite(Register::PWR_MGMT_1, PWR_MGMT_1_BIT::H_RESET);
_reset_timestamp = hrt_absolute_time();
_reset_timestamp = now;
_consecutive_failures = 0;
_state = STATE::WAIT_FOR_RESET;
ScheduleDelayed(100_ms);
break;
@@ -161,9 +168,12 @@ void MPU9250::RunImpl()
if ((RegisterRead(Register::WHO_AM_I) == WHOAMI)
&& (RegisterRead(Register::PWR_MGMT_1) == 0x01)) {
// SIGNAL_PATH_RESET: ensure the reset is performed properly
// Wakeup and reset digital signal path
RegisterWrite(Register::PWR_MGMT_1, 0);
RegisterWrite(Register::SIGNAL_PATH_RESET,
SIGNAL_PATH_RESET_BIT::GYRO_RESET | SIGNAL_PATH_RESET_BIT::ACCEL_RESET | SIGNAL_PATH_RESET_BIT::TEMP_RESET);
RegisterWrite(Register::USER_CTRL, USER_CTRL_BIT::I2C_MST_EN | USER_CTRL_BIT::I2C_IF_DIS | USER_CTRL_BIT::I2C_MST_RST |
USER_CTRL_BIT::SIG_COND_RST);
// if reset succeeded then configure
_state = STATE::CONFIGURE;
@@ -171,7 +181,7 @@ void MPU9250::RunImpl()
} else {
// RESET not complete
if (hrt_elapsed_time(&_reset_timestamp) > 100_ms) {
if (hrt_elapsed_time(&_reset_timestamp) > 1000_ms) {
PX4_DEBUG("Reset failed, retrying");
_state = STATE::RESET;
ScheduleDelayed(100_ms);
@@ -186,7 +196,6 @@ void MPU9250::RunImpl()
case STATE::CONFIGURE:
if (Configure()) {
// start AK8963 magnetometer (I2C aux)
if (_slave_ak8963_magnetometer) {
_slave_ak8963_magnetometer->Reset();
@@ -199,7 +208,7 @@ void MPU9250::RunImpl()
_data_ready_interrupt_enabled = true;
// backup schedule as a watchdog timeout
ScheduleDelayed(10_ms);
ScheduleDelayed(100_ms);
} else {
_data_ready_interrupt_enabled = false;
@@ -209,73 +218,87 @@ void MPU9250::RunImpl()
FIFOReset();
} else {
PX4_DEBUG("Configure failed, retrying");
// try again in 10 ms
ScheduleDelayed(10_ms);
// CONFIGURE not complete
if (hrt_elapsed_time(&_reset_timestamp) > 1000_ms) {
PX4_DEBUG("Configure failed, resetting");
_state = STATE::RESET;
} else {
PX4_DEBUG("Configure failed, retrying");
}
ScheduleDelayed(100_ms);
}
break;
case STATE::FIFO_READ: {
hrt_abstime timestamp_sample = 0;
if (_data_ready_interrupt_enabled) {
// re-schedule as watchdog timeout
ScheduleDelayed(10_ms);
}
if (_data_ready_interrupt_enabled && (hrt_elapsed_time(&timestamp_sample) < (_fifo_empty_interval_us / 2))) {
// use timestamp from data ready interrupt if enabled and seems valid
timestamp_sample = _fifo_watermark_interrupt_timestamp;
} else {
// use the time now roughly corresponding with the last sample we'll pull from the FIFO
timestamp_sample = hrt_absolute_time();
}
const uint16_t fifo_count = FIFOReadCount();
const uint8_t samples = (fifo_count / sizeof(FIFO::DATA) / SAMPLES_PER_TRANSFER) *
SAMPLES_PER_TRANSFER; // round down to nearest
bool failure = false;
if (samples > FIFO_MAX_SAMPLES) {
// not technically an overflow, but more samples than we expected or can publish
perf_count(_fifo_overflow_perf);
failure = true;
FIFOReset();
} else if (samples >= SAMPLES_PER_TRANSFER) {
// require at least SAMPLES_PER_TRANSFER (we want at least 1 new accel sample per transfer)
if (!FIFORead(timestamp_sample, samples)) {
failure = true;
_px4_accel.increase_error_count();
_px4_gyro.increase_error_count();
// scheduled from interrupt if _drdy_fifo_read_samples was set
if (_drdy_fifo_read_samples.fetch_and(0) == _fifo_gyro_samples) {
perf_count_interval(_drdy_interval_perf, now);
}
} else if (samples == 0) {
failure = true;
perf_count(_fifo_empty_perf);
// push backup schedule back
ScheduleDelayed(_fifo_empty_interval_us * 2);
}
if (failure || hrt_elapsed_time(&_last_config_check_timestamp) > 10_ms) {
// check registers incrementally
if (RegisterCheck(_register_cfg[_checked_register], true)) {
_last_config_check_timestamp = timestamp_sample;
// always check current FIFO count
bool success = false;
const uint16_t fifo_count = FIFOReadCount();
if (fifo_count >= FIFO::SIZE) {
FIFOReset();
perf_count(_fifo_overflow_perf);
} else if (fifo_count == 0) {
perf_count(_fifo_empty_perf);
} else {
// FIFO count (size in bytes) should be a multiple of the FIFO::DATA structure
const uint8_t samples = (fifo_count / sizeof(FIFO::DATA) / SAMPLES_PER_TRANSFER) *
SAMPLES_PER_TRANSFER; // round down to nearest
if (samples > FIFO_MAX_SAMPLES) {
// not technically an overflow, but more samples than we expected or can publish
FIFOReset();
perf_count(_fifo_overflow_perf);
} else if (samples >= 1) {
if (FIFORead(now, samples)) {
success = true;
_consecutive_failures = 0;
}
}
}
if (!success) {
_consecutive_failures++;
// full reset if things are failing consistently
if (_consecutive_failures > 10) {
Reset();
return;
}
}
if (!success || hrt_elapsed_time(&_last_config_check_timestamp) > 10_ms) {
// check configuration registers periodically or immediately following any failure
if (RegisterCheck(_register_cfg[_checked_register])) {
_last_config_check_timestamp = now;
_checked_register = (_checked_register + 1) % size_register_cfg;
} else {
// register check failed, force reconfigure
PX4_DEBUG("Health check failed, reconfiguring");
_state = STATE::CONFIGURE;
ScheduleNow();
// register check failed, force reset
perf_count(_bad_register_perf);
Reset();
}
} else {
// periodically update temperature (1 Hz)
if (hrt_elapsed_time(&_temperature_update_timestamp) > 1_s) {
// periodically update temperature (~1 Hz)
if (hrt_elapsed_time(&_temperature_update_timestamp) >= 1_s) {
UpdateTemperature();
_temperature_update_timestamp = timestamp_sample;
_temperature_update_timestamp = now;
}
}
}
@@ -290,23 +313,23 @@ void MPU9250::ConfigureAccel()
switch (ACCEL_FS_SEL) {
case ACCEL_FS_SEL_2G:
_px4_accel.set_scale(CONSTANTS_ONE_G / 16384);
_px4_accel.set_range(2 * CONSTANTS_ONE_G);
_px4_accel.set_scale(CONSTANTS_ONE_G / 16384.f);
_px4_accel.set_range(2.f * CONSTANTS_ONE_G);
break;
case ACCEL_FS_SEL_4G:
_px4_accel.set_scale(CONSTANTS_ONE_G / 8192);
_px4_accel.set_range(4 * CONSTANTS_ONE_G);
_px4_accel.set_scale(CONSTANTS_ONE_G / 8192.f);
_px4_accel.set_range(4.f * CONSTANTS_ONE_G);
break;
case ACCEL_FS_SEL_8G:
_px4_accel.set_scale(CONSTANTS_ONE_G / 4096);
_px4_accel.set_range(8 * CONSTANTS_ONE_G);
_px4_accel.set_scale(CONSTANTS_ONE_G / 4096.f);
_px4_accel.set_range(8.f * CONSTANTS_ONE_G);
break;
case ACCEL_FS_SEL_16G:
_px4_accel.set_scale(CONSTANTS_ONE_G / 2048);
_px4_accel.set_range(16 * CONSTANTS_ONE_G);
_px4_accel.set_scale(CONSTANTS_ONE_G / 2048.f);
_px4_accel.set_range(16.f * CONSTANTS_ONE_G);
break;
}
}
@@ -346,23 +369,27 @@ void MPU9250::ConfigureSampleRate(int sample_rate)
}
// round down to nearest FIFO sample dt * SAMPLES_PER_TRANSFER
const float min_interval = SAMPLES_PER_TRANSFER * FIFO_SAMPLE_DT;
const float min_interval = FIFO_SAMPLE_DT * SAMPLES_PER_TRANSFER;
_fifo_empty_interval_us = math::max(roundf((1e6f / (float)sample_rate) / min_interval) * min_interval, min_interval);
_fifo_gyro_samples = math::min((float)_fifo_empty_interval_us / (1e6f / GYRO_RATE), (float)FIFO_MAX_SAMPLES);
_fifo_gyro_samples = roundf(math::min((float)_fifo_empty_interval_us / (1e6f / GYRO_RATE), (float)FIFO_MAX_SAMPLES));
// recompute FIFO empty interval (us) with actual gyro sample limit
_fifo_empty_interval_us = _fifo_gyro_samples * (1e6f / GYRO_RATE);
_fifo_accel_samples = math::min(_fifo_empty_interval_us / (1e6f / ACCEL_RATE), (float)FIFO_MAX_SAMPLES);
}
bool MPU9250::Configure()
{
// first set and clear all configured register bits
for (const auto &reg_cfg : _register_cfg) {
RegisterSetAndClearBits(reg_cfg.reg, reg_cfg.set_bits, reg_cfg.clear_bits);
}
// now check that all are configured
bool success = true;
for (const auto &reg : _register_cfg) {
if (!RegisterCheck(reg)) {
for (const auto &reg_cfg : _register_cfg) {
if (!RegisterCheck(reg_cfg)) {
success = false;
}
}
@@ -381,12 +408,13 @@ int MPU9250::DataReadyInterruptCallback(int irq, void *context, void *arg)
void MPU9250::DataReady()
{
perf_count(_drdy_interval_perf);
const uint8_t count = _drdy_count.fetch_add(1) + 1;
if (_data_ready_count.fetch_add(1) >= (_fifo_gyro_samples - 1)) {
_data_ready_count.store(0);
_fifo_watermark_interrupt_timestamp = hrt_absolute_time();
_fifo_read_samples.store(_fifo_gyro_samples);
uint8_t expected = 0;
// at least the required number of samples in the FIFO
if ((count >= _fifo_gyro_samples) && _drdy_fifo_read_samples.compare_exchange(&expected, _fifo_gyro_samples)) {
_drdy_count.store(0);
ScheduleNow();
}
}
@@ -398,7 +426,7 @@ bool MPU9250::DataReadyInterruptConfigure()
}
// Setup data ready on falling edge
return px4_arch_gpiosetevent(_drdy_gpio, false, true, true, &MPU9250::DataReadyInterruptCallback, this) == 0;
return px4_arch_gpiosetevent(_drdy_gpio, false, true, true, &DataReadyInterruptCallback, this) == 0;
}
bool MPU9250::DataReadyInterruptDisable()
@@ -410,7 +438,7 @@ bool MPU9250::DataReadyInterruptDisable()
return px4_arch_gpiosetevent(_drdy_gpio, false, false, false, nullptr, nullptr) == 0;
}
bool MPU9250::RegisterCheck(const register_config_t &reg_cfg, bool notify)
bool MPU9250::RegisterCheck(const register_config_t &reg_cfg)
{
bool success = true;
@@ -426,16 +454,6 @@ bool MPU9250::RegisterCheck(const register_config_t &reg_cfg, bool notify)
success = false;
}
if (!success) {
RegisterSetAndClearBits(reg_cfg.reg, reg_cfg.set_bits, reg_cfg.clear_bits);
if (notify) {
perf_count(_bad_register_perf);
_px4_accel.increase_error_count();
_px4_gyro.increase_error_count();
}
}
return success;
}
@@ -458,17 +476,12 @@ void MPU9250::RegisterWrite(Register reg, uint8_t value)
void MPU9250::RegisterSetAndClearBits(Register reg, uint8_t setbits, uint8_t clearbits)
{
const uint8_t orig_val = RegisterRead(reg);
uint8_t val = orig_val;
if (setbits) {
val |= setbits;
uint8_t val = (orig_val & ~clearbits) | setbits;
if (orig_val != val) {
RegisterWrite(reg, val);
}
if (clearbits) {
val &= ~clearbits;
}
RegisterWrite(reg, val);
}
uint16_t MPU9250::FIFOReadCount()
@@ -486,10 +499,9 @@ uint16_t MPU9250::FIFOReadCount()
return combine(fifo_count_buf[1], fifo_count_buf[2]);
}
bool MPU9250::FIFORead(const hrt_abstime &timestamp_sample, uint16_t samples)
bool MPU9250::FIFORead(const hrt_abstime &timestamp_sample, uint8_t samples)
{
perf_begin(_transfer_perf);
FIFOTransferBuffer buffer{};
const size_t transfer_size = math::min(samples * sizeof(FIFO::DATA) + 1, FIFO::SIZE);
set_frequency(SPI_SPEED_SENSOR);
@@ -502,8 +514,8 @@ bool MPU9250::FIFORead(const hrt_abstime &timestamp_sample, uint16_t samples)
perf_end(_transfer_perf);
ProcessGyro(timestamp_sample, buffer, samples);
return ProcessAccel(timestamp_sample, buffer, samples);
ProcessGyro(timestamp_sample, buffer.f, samples);
return ProcessAccel(timestamp_sample, buffer.f, samples);
}
void MPU9250::FIFOReset()
@@ -517,9 +529,8 @@ void MPU9250::FIFOReset()
RegisterSetAndClearBits(Register::USER_CTRL, USER_CTRL_BIT::FIFO_RST, USER_CTRL_BIT::FIFO_EN);
// reset while FIFO is disabled
_data_ready_count.store(0);
_fifo_watermark_interrupt_timestamp = 0;
_fifo_read_samples.store(0);
_drdy_count.store(0);
_drdy_fifo_read_samples.store(0);
// FIFO_EN: enable both gyro and accel
// USER_CTRL: re-enable FIFO
@@ -535,11 +546,12 @@ static bool fifo_accel_equal(const FIFO::DATA &f0, const FIFO::DATA &f1)
return (memcmp(&f0.ACCEL_XOUT_H, &f1.ACCEL_XOUT_H, 6) == 0);
}
bool MPU9250::ProcessAccel(const hrt_abstime &timestamp_sample, const FIFOTransferBuffer &buffer, const uint8_t samples)
bool MPU9250::ProcessAccel(const hrt_abstime &timestamp_sample, const FIFO::DATA fifo[], const uint8_t samples)
{
sensor_accel_fifo_s accel{};
accel.timestamp_sample = timestamp_sample;
accel.dt = _fifo_empty_interval_us / _fifo_accel_samples;
accel.samples = 0;
accel.dt = FIFO_SAMPLE_DT * SAMPLES_PER_TRANSFER;
bool bad_data = false;
@@ -547,58 +559,57 @@ bool MPU9250::ProcessAccel(const hrt_abstime &timestamp_sample, const FIFOTransf
int accel_first_sample = 1;
if (samples >= 4) {
if (fifo_accel_equal(buffer.f[0], buffer.f[1]) && fifo_accel_equal(buffer.f[2], buffer.f[3])) {
if (fifo_accel_equal(fifo[0], fifo[1]) && fifo_accel_equal(fifo[2], fifo[3])) {
// [A0, A1, A2, A3]
// A0==A1, A2==A3
accel_first_sample = 1;
} else if (fifo_accel_equal(buffer.f[1], buffer.f[2])) {
} else if (fifo_accel_equal(fifo[1], fifo[2])) {
// [A0, A1, A2, A3]
// A0, A1==A2, A3
accel_first_sample = 0;
} else {
perf_count(_bad_transfer_perf);
// no matching accel samples is an error
bad_data = true;
perf_count(_bad_transfer_perf);
}
}
int accel_samples = 0;
for (int i = accel_first_sample; i < samples; i = i + 2) {
const FIFO::DATA &fifo_sample = buffer.f[i];
int16_t accel_x = combine(fifo_sample.ACCEL_XOUT_H, fifo_sample.ACCEL_XOUT_L);
int16_t accel_y = combine(fifo_sample.ACCEL_YOUT_H, fifo_sample.ACCEL_YOUT_L);
int16_t accel_z = combine(fifo_sample.ACCEL_ZOUT_H, fifo_sample.ACCEL_ZOUT_L);
for (int i = accel_first_sample; i < samples; i = i + SAMPLES_PER_TRANSFER) {
int16_t accel_x = combine(fifo[i].ACCEL_XOUT_H, fifo[i].ACCEL_XOUT_L);
int16_t accel_y = combine(fifo[i].ACCEL_YOUT_H, fifo[i].ACCEL_YOUT_L);
int16_t accel_z = combine(fifo[i].ACCEL_ZOUT_H, fifo[i].ACCEL_ZOUT_L);
// sensor's frame is +x forward, +y left, +z up
// flip y & z to publish right handed with z down (x forward, y right, z down)
accel.x[accel_samples] = accel_x;
accel.y[accel_samples] = (accel_y == INT16_MIN) ? INT16_MAX : -accel_y;
accel.z[accel_samples] = (accel_z == INT16_MIN) ? INT16_MAX : -accel_z;
accel_samples++;
accel.x[accel.samples] = accel_x;
accel.y[accel.samples] = (accel_y == INT16_MIN) ? INT16_MAX : -accel_y;
accel.z[accel.samples] = (accel_z == INT16_MIN) ? INT16_MAX : -accel_z;
accel.samples++;
}
accel.samples = accel_samples;
_px4_accel.set_error_count(perf_event_count(_bad_register_perf) + perf_event_count(_bad_transfer_perf) +
perf_event_count(_fifo_empty_perf) + perf_event_count(_fifo_overflow_perf));
_px4_accel.updateFIFO(accel);
if (accel.samples > 0) {
_px4_accel.updateFIFO(accel);
}
return !bad_data;
}
void MPU9250::ProcessGyro(const hrt_abstime &timestamp_sample, const FIFOTransferBuffer &buffer, const uint8_t samples)
void MPU9250::ProcessGyro(const hrt_abstime &timestamp_sample, const FIFO::DATA fifo[], const uint8_t samples)
{
sensor_gyro_fifo_s gyro{};
gyro.timestamp_sample = timestamp_sample;
gyro.samples = samples;
gyro.dt = _fifo_empty_interval_us / _fifo_gyro_samples;
gyro.dt = FIFO_SAMPLE_DT;
for (int i = 0; i < samples; i++) {
const FIFO::DATA &fifo_sample = buffer.f[i];
const int16_t gyro_x = combine(fifo_sample.GYRO_XOUT_H, fifo_sample.GYRO_XOUT_L);
const int16_t gyro_y = combine(fifo_sample.GYRO_YOUT_H, fifo_sample.GYRO_YOUT_L);
const int16_t gyro_z = combine(fifo_sample.GYRO_ZOUT_H, fifo_sample.GYRO_ZOUT_L);
const int16_t gyro_x = combine(fifo[i].GYRO_XOUT_H, fifo[i].GYRO_XOUT_L);
const int16_t gyro_y = combine(fifo[i].GYRO_YOUT_H, fifo[i].GYRO_YOUT_L);
const int16_t gyro_z = combine(fifo[i].GYRO_ZOUT_H, fifo[i].GYRO_ZOUT_L);
// sensor's frame is +x forward, +y left, +z up
// flip y & z to publish right handed with z down (x forward, y right, z down)
@@ -607,6 +618,9 @@ void MPU9250::ProcessGyro(const hrt_abstime &timestamp_sample, const FIFOTransfe
gyro.z[i] = (gyro_z == INT16_MIN) ? INT16_MAX : -gyro_z;
}
_px4_gyro.set_error_count(perf_event_count(_bad_register_perf) + perf_event_count(_bad_transfer_perf) +
perf_event_count(_fifo_empty_perf) + perf_event_count(_fifo_overflow_perf));
_px4_gyro.updateFIFO(gyro);
}
@@ -628,29 +642,18 @@ void MPU9250::UpdateTemperature()
if (PX4_ISFINITE(TEMP_degC)) {
_px4_accel.set_temperature(TEMP_degC);
_px4_gyro.set_temperature(TEMP_degC);
if (_slave_ak8963_magnetometer) {
_slave_ak8963_magnetometer->set_temperature(TEMP_degC);
}
}
}
void MPU9250::I2CSlaveRegisterStartRead(uint8_t slave_i2c_addr, uint8_t reg)
{
I2CSlaveExternalSensorDataEnable(slave_i2c_addr, reg, 1);
}
void MPU9250::I2CSlaveRegisterWrite(uint8_t slave_i2c_addr, uint8_t reg, uint8_t val)
{
RegisterWrite(Register::I2C_SLV0_ADDR, slave_i2c_addr);
RegisterWrite(Register::I2C_SLV0_REG, reg);
RegisterWrite(Register::I2C_SLV0_DO, val);
RegisterSetBits(Register::I2C_SLV0_CTRL, 1 | I2C_SLV0_CTRL_BIT::I2C_SLV0_EN);
}
void MPU9250::I2CSlaveExternalSensorDataEnable(uint8_t slave_i2c_addr, uint8_t reg, uint8_t size)
{
//RegisterWrite(Register::I2C_SLV0_ADDR, 0); // disable slave
RegisterWrite(Register::I2C_SLV0_ADDR, slave_i2c_addr | I2C_SLV0_ADDR_BIT::I2C_SLV0_RNW);
RegisterWrite(Register::I2C_SLV0_REG, reg);
RegisterWrite(Register::I2C_SLV0_CTRL, size | I2C_SLV0_CTRL_BIT::I2C_SLV0_EN);

41
src/drivers/imu/invensense/mpu9250/MPU9250.hpp
View File

@@ -75,10 +75,10 @@ private:
void exit_and_cleanup() override;
// Sensor Configuration
static constexpr float FIFO_SAMPLE_DT{125.f};
static constexpr uint32_t SAMPLES_PER_TRANSFER{2}; // ensure at least 1 new accel sample per transfer
static constexpr float GYRO_RATE{1e6f / FIFO_SAMPLE_DT}; // 8 kHz gyro
static constexpr float ACCEL_RATE{GYRO_RATE / 2.f}; // 4 kHz accel
static constexpr float FIFO_SAMPLE_DT{1e6f / 8000.f};
static constexpr uint32_t SAMPLES_PER_TRANSFER{2}; // ensure at least 1 new accel sample per transfer
static constexpr float GYRO_RATE{1e6f / FIFO_SAMPLE_DT}; // 8000 Hz gyro
static constexpr float ACCEL_RATE{GYRO_RATE / SAMPLES_PER_TRANSFER}; // 4000 Hz accel
// maximum FIFO samples per transfer is limited to the size of sensor_accel_fifo/sensor_gyro_fifo
static constexpr uint32_t FIFO_MAX_SAMPLES{math::min(math::min(FIFO::SIZE / sizeof(FIFO::DATA), sizeof(sensor_gyro_fifo_s::x) / sizeof(sensor_gyro_fifo_s::x[0])), sizeof(sensor_accel_fifo_s::x) / sizeof(sensor_accel_fifo_s::x[0]) * (int)(GYRO_RATE / ACCEL_RATE))};
@@ -111,7 +111,7 @@ private:
bool DataReadyInterruptConfigure();
bool DataReadyInterruptDisable();
bool RegisterCheck(const register_config_t &reg_cfg, bool notify = false);
bool RegisterCheck(const register_config_t &reg_cfg);
uint8_t RegisterRead(Register reg);
void RegisterWrite(Register reg, uint8_t value);
@@ -120,25 +120,23 @@ private:
void RegisterClearBits(Register reg, uint8_t clearbits) { RegisterSetAndClearBits(reg, 0, clearbits); }
uint16_t FIFOReadCount();
bool FIFORead(const hrt_abstime &timestamp_sample, uint16_t samples);
bool FIFORead(const hrt_abstime &timestamp_sample, uint8_t samples);
void FIFOReset();
bool ProcessAccel(const hrt_abstime &timestamp_sample, const FIFOTransferBuffer &buffer, const uint8_t samples);
void ProcessGyro(const hrt_abstime &timestamp_sample, const FIFOTransferBuffer &buffer, const uint8_t samples);
bool ProcessAccel(const hrt_abstime &timestamp_sample, const FIFO::DATA fifo[], const uint8_t samples);
void ProcessGyro(const hrt_abstime &timestamp_sample, const FIFO::DATA fifo[], const uint8_t samples);
void UpdateTemperature();
const spi_drdy_gpio_t _drdy_gpio;
// I2C AUX interface (slave 1 - 4)
AKM_AK8963::MPU9250_AK8963 *_slave_ak8963_magnetometer{nullptr};
friend class AKM_AK8963::MPU9250_AK8963;
void I2CSlaveRegisterStartRead(uint8_t slave_i2c_addr, uint8_t reg);
void I2CSlaveRegisterWrite(uint8_t slave_i2c_addr, uint8_t reg, uint8_t val);
void I2CSlaveExternalSensorDataEnable(uint8_t slave_i2c_addr, uint8_t reg, uint8_t size);
bool I2CSlaveExternalSensorDataRead(uint8_t *buffer, uint8_t length);
AKM_AK8963::MPU9250_AK8963 *_slave_ak8963_magnetometer{nullptr};
PX4Accelerometer _px4_accel;
PX4Gyroscope _px4_gyro;
@@ -148,15 +146,15 @@ private:
perf_counter_t _fifo_empty_perf{perf_alloc(PC_COUNT, MODULE_NAME": FIFO empty")};
perf_counter_t _fifo_overflow_perf{perf_alloc(PC_COUNT, MODULE_NAME": FIFO overflow")};
perf_counter_t _fifo_reset_perf{perf_alloc(PC_COUNT, MODULE_NAME": FIFO reset")};
perf_counter_t _drdy_interval_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": DRDY interval")};
perf_counter_t _drdy_interval_perf{nullptr};
hrt_abstime _reset_timestamp{0};
hrt_abstime _last_config_check_timestamp{0};
hrt_abstime _fifo_watermark_interrupt_timestamp{0};
hrt_abstime _temperature_update_timestamp{0};
unsigned _consecutive_failures{0};
px4::atomic<uint8_t> _data_ready_count{0};
px4::atomic<uint8_t> _fifo_read_samples{0};
px4::atomic<uint8_t> _drdy_fifo_read_samples{0};
px4::atomic<uint8_t> _drdy_count{0};
bool _data_ready_interrupt_enabled{false};
enum class STATE : uint8_t {
@@ -170,23 +168,22 @@ private:
uint16_t _fifo_empty_interval_us{1250}; // default 1250 us / 800 Hz transfer interval
uint8_t _fifo_gyro_samples{static_cast<uint8_t>(_fifo_empty_interval_us / (1000000 / GYRO_RATE))};
uint8_t _fifo_accel_samples{static_cast<uint8_t>(_fifo_empty_interval_us / (1000000 / ACCEL_RATE))};
uint8_t _checked_register{0};
static constexpr uint8_t size_register_cfg{12};
register_config_t _register_cfg[size_register_cfg] {
// Register | Set bits, Clear bits
{ Register::PWR_MGMT_1, PWR_MGMT_1_BIT::CLKSEL_0, PWR_MGMT_1_BIT::H_RESET | PWR_MGMT_1_BIT::SLEEP },
{ Register::CONFIG, CONFIG_BIT::FIFO_MODE | CONFIG_BIT::DLPF_CFG_BYPASS_DLPF_8KHZ, 0 },
{ Register::GYRO_CONFIG, GYRO_CONFIG_BIT::GYRO_FS_SEL_2000_DPS, GYRO_CONFIG_BIT::FCHOICE_B_8KHZ_BYPASS_DLPF },
{ Register::ACCEL_CONFIG, ACCEL_CONFIG_BIT::ACCEL_FS_SEL_16G, 0 },
{ Register::ACCEL_CONFIG2, ACCEL_CONFIG2_BIT::ACCEL_FCHOICE_B_BYPASS_DLPF, 0 },
{ Register::GYRO_CONFIG, GYRO_CONFIG_BIT::GYRO_FS_SEL_2000_DPS, GYRO_CONFIG_BIT::FCHOICE_B_8KHZ_BYPASS_DLPF },
{ Register::CONFIG, CONFIG_BIT::FIFO_MODE | CONFIG_BIT::DLPF_CFG_BYPASS_DLPF_8KHZ, 0 },
{ Register::USER_CTRL, USER_CTRL_BIT::FIFO_EN | USER_CTRL_BIT::I2C_MST_EN | USER_CTRL_BIT::I2C_IF_DIS, 0 },
{ Register::FIFO_EN, FIFO_EN_BIT::GYRO_XOUT | FIFO_EN_BIT::GYRO_YOUT | FIFO_EN_BIT::GYRO_ZOUT | FIFO_EN_BIT::ACCEL, 0 },
{ Register::I2C_SLV4_CTRL, I2C_SLV4_CTRL_BIT::I2C_MST_DLY, 0 },
{ Register::I2C_MST_CTRL, I2C_MST_CTRL_BIT::I2C_MST_P_NSR | I2C_MST_CTRL_BIT::I2C_MST_CLK_400_kHz, 0 },
{ Register::I2C_MST_DELAY_CTRL, I2C_MST_DELAY_CTRL_BIT::I2C_SLVX_DLY_EN, 0 },
{ Register::INT_PIN_CFG, INT_PIN_CFG_BIT::ACTL, 0 },
{ Register::INT_ENABLE, INT_ENABLE_BIT::RAW_RDY_EN, 0 }
{ Register::INT_ENABLE, INT_ENABLE_BIT::RAW_RDY_EN, 0 },
{ Register::I2C_MST_DELAY_CTRL, I2C_MST_DELAY_CTRL_BIT::I2C_SLVX_DLY_EN, 0 },
{ Register::USER_CTRL, USER_CTRL_BIT::FIFO_EN | USER_CTRL_BIT::I2C_MST_EN | USER_CTRL_BIT::I2C_IF_DIS, 0 },
{ Register::PWR_MGMT_1, PWR_MGMT_1_BIT::CLKSEL_0, 0 },
};
};

260
src/drivers/imu/invensense/mpu9250/MPU9250_AK8963.cpp
View File

@@ -52,36 +52,21 @@ MPU9250_AK8963::MPU9250_AK8963(MPU9250 &mpu9250, enum Rotation rotation) :
{
_px4_mag.set_device_type(DRV_MAG_DEVTYPE_AK8963);
_px4_mag.set_external(mpu9250.external());
// in 16-bit sampling mode the mag resolution is 1.5 milli Gauss per bit */
_px4_mag.set_scale(1.5e-3f);
}
MPU9250_AK8963::~MPU9250_AK8963()
{
Stop();
perf_free(_transfer_perf);
perf_free(_bad_register_perf);
perf_free(_bad_transfer_perf);
perf_free(_duplicate_data_perf);
}
bool MPU9250_AK8963::Init()
{
return Reset();
}
void MPU9250_AK8963::Stop()
{
// wait until stopped
while (_state.load() != STATE::STOPPED) {
_state.store(STATE::REQUEST_STOP);
ScheduleNow();
px4_usleep(10);
}
perf_free(_magnetic_sensor_overflow_perf);
}
bool MPU9250_AK8963::Reset()
{
_state.store(STATE::RESET);
_state = STATE::RESET;
ScheduleClear();
ScheduleNow();
return true;
@@ -90,28 +75,28 @@ bool MPU9250_AK8963::Reset()
void MPU9250_AK8963::PrintInfo()
{
perf_print_counter(_transfer_perf);
perf_print_counter(_bad_register_perf);
perf_print_counter(_bad_transfer_perf);
perf_print_counter(_duplicate_data_perf);
perf_print_counter(_magnetic_sensor_overflow_perf);
_px4_mag.print_status();
}
void MPU9250_AK8963::Run()
{
switch (_state.load()) {
switch (_state) {
case STATE::RESET:
// CNTL2 SRST: Soft reset
RegisterWrite(Register::CNTL2, CNTL2_BIT::SRST);
_mpu9250.I2CSlaveRegisterWrite(I2C_ADDRESS_DEFAULT, (uint8_t)Register::CNTL2, CNTL2_BIT::SRST);
_reset_timestamp = hrt_absolute_time();
_state.store(STATE::READ_WHO_AM_I);
_consecutive_failures = 0;
_state = STATE::READ_WHO_AM_I;
ScheduleDelayed(100_ms);
break;
case STATE::READ_WHO_AM_I:
_mpu9250.I2CSlaveRegisterStartRead(I2C_ADDRESS_DEFAULT, (uint8_t)Register::WIA);
_state.store(STATE::WAIT_FOR_RESET);
ScheduleDelayed(10_ms);
_mpu9250.I2CSlaveExternalSensorDataEnable(I2C_ADDRESS_DEFAULT, (uint8_t)Register::WIA, 1);
_state = STATE::WAIT_FOR_RESET;
ScheduleDelayed(100_ms);
break;
case STATE::WAIT_FOR_RESET: {
@@ -119,20 +104,36 @@ void MPU9250_AK8963::Run()
uint8_t WIA = 0;
_mpu9250.I2CSlaveExternalSensorDataRead(&WIA, 1);
if (WIA == WHOAMI) {
if (WIA == Device_ID) {
// if reset succeeded then configure
_state.store(STATE::CONFIGURE);
ScheduleDelayed(10_ms);
if (!_sensitivity_adjustments_loaded) {
// Set Fuse ROM Access mode before reading Fuse ROM data.
_mpu9250.I2CSlaveRegisterWrite(I2C_ADDRESS_DEFAULT, (uint8_t)Register::CNTL1,
CNTL1_BIT::BIT_16 | CNTL1_BIT::FUSE_ROM_ACCESS_MODE);
} else {
// RESET not complete
if (hrt_elapsed_time(&_reset_timestamp) > 100_ms) {
PX4_DEBUG("Reset failed, retrying");
_state.store(STATE::RESET);
// Read ASAX, ASAY, ASAZ
_mpu9250.I2CSlaveExternalSensorDataEnable(I2C_ADDRESS_DEFAULT, (uint8_t)Register::ASAX, 3);
_state = STATE::READ_SENSITIVITY_ADJUSTMENTS;
ScheduleDelayed(100_ms);
} else {
PX4_DEBUG("Reset not complete, check again in 100 ms");
// set continuous mode 2 (100 Hz)
_mpu9250.I2CSlaveRegisterWrite(I2C_ADDRESS_DEFAULT, (uint8_t)Register::CNTL1,
CNTL1_BIT::BIT_16 | CNTL1_BIT::CONTINUOUS_MODE_2);
_state = STATE::READ;
ScheduleOnInterval(10_ms, 100_ms); // 100 Hz
}
} else {
// RESET not complete
if (hrt_elapsed_time(&_reset_timestamp) > 1000_ms) {
PX4_DEBUG("AK8963 reset failed, retrying");
_state = STATE::RESET;
ScheduleDelayed(1000_ms);
} else {
PX4_DEBUG("AK8963 reset not complete, check again in 100 ms");
ScheduleDelayed(100_ms);
}
}
@@ -140,171 +141,80 @@ void MPU9250_AK8963::Run()
break;
// TODO: read FUSE ROM (to get ASA corrections)
case STATE::READ_SENSITIVITY_ADJUSTMENTS: {
// read FUSE ROM (to get ASA corrections)
uint8_t response[3] {};
_mpu9250.I2CSlaveExternalSensorDataRead((uint8_t *)&response, sizeof(response));
case STATE::CONFIGURE:
if (Configure()) {
// if configure succeeded then start reading
_mpu9250.I2CSlaveExternalSensorDataEnable(I2C_ADDRESS_DEFAULT, (uint8_t)Register::HXL, sizeof(TransferBuffer));
_state.store(STATE::READ);
ScheduleOnInterval(10_ms, 10_ms); // 100 Hz
bool valid = true;
} else {
PX4_DEBUG("Configure failed, retrying");
// try again in 100 ms
for (int i = 0; i < 3; i++) {
if (response[i] != 0 && response[i] != 0xFF) {
_sensitivity[i] = ((float)(response[i] - 128) / 256.f) + 1.f;
} else {
valid = false;
}
}
_sensitivity_adjustments_loaded = valid;
// After reading fuse ROM data, set power-down mode (MODE[3:0]=“0000”) before the transition to another mode.
_mpu9250.I2CSlaveRegisterWrite(I2C_ADDRESS_DEFAULT, (uint8_t)Register::CNTL1, 0);
_state = STATE::RESET;
ScheduleDelayed(100_ms);
}
break;
case STATE::READ: {
perf_begin(_transfer_perf);
TransferBuffer buffer{};
const hrt_abstime timestamp_sample = hrt_absolute_time();
bool success = _mpu9250.I2CSlaveExternalSensorDataRead((uint8_t *)&buffer, sizeof(TransferBuffer));
bool ret = _mpu9250.I2CSlaveExternalSensorDataRead((uint8_t *)&buffer, sizeof(TransferBuffer));
perf_end(_transfer_perf);
if (success && !(buffer.ST2 & ST2_BIT::HOFL)) {
// sensor's frame is +y forward (x), -x right, +z down
int16_t x = combine(buffer.HYH, buffer.HYL); // +Y
int16_t y = combine(buffer.HXH, buffer.HXL); // +X
y = (y == INT16_MIN) ? INT16_MAX : -y; // flip y
int16_t z = combine(buffer.HZH, buffer.HZL);
bool success = false;
const bool all_zero = (x == 0 && y == 0 && z == 0);
const bool new_data = (_last_measurement[0] != x || _last_measurement[1] != y || _last_measurement[2] != z);
if (ret) {
if (buffer.ST2 & ST2_BIT::HOFL) {
perf_count(_magnetic_sensor_overflow_perf);
if (!new_data) {
perf_count(_duplicate_data_perf);
}
} else if ((buffer.ST1 & ST1_BIT::DRDY) && (buffer.ST2 & ST2_BIT::BITM)) {
if (!all_zero && new_data) {
_px4_mag.update(timestamp_sample, x, y, z);
const int16_t x = combine(buffer.HXH, buffer.HXL);
const int16_t y = combine(buffer.HYH, buffer.HYL);
const int16_t z = combine(buffer.HZH, buffer.HZL);
_last_measurement[0] = x;
_last_measurement[1] = y;
_last_measurement[2] = z;
// sensor's frame is +Y forward (X), -X right (Y), +Z down (Z)
// adjust with sensitivity scale factors
float x_f = y * _sensitivity[0]; // X := +Y
float y_f = -x * _sensitivity[1]; // Y := -X
float z_f = z * _sensitivity[2]; // Z := +Z
} else {
success = false;
_px4_mag.update(timestamp_sample, x_f, y_f, z_f);
success = true;
_consecutive_failures = 0;
}
}
if (!success) {
perf_count(_bad_transfer_perf);
_consecutive_failures++;
if (_consecutive_failures > 10) {
Reset();
return;
}
}
// ensure mpu9250 slave sensor reading is configured
_mpu9250.I2CSlaveExternalSensorDataEnable(I2C_ADDRESS_DEFAULT, (uint8_t)Register::ST1, sizeof(TransferBuffer));
}
break;
case STATE::REQUEST_STOP:
ScheduleClear();
_state.store(STATE::STOPPED);
break;
case STATE::STOPPED:
// DO NOTHING
break;
}
}
bool MPU9250_AK8963::Configure()
{
bool success = true;
for (const auto &reg : _register_cfg) {
if (!RegisterCheck(reg)) {
success = false;
}
}
// TODO: read ASA and set sensitivity
//const uint8_t ASAX = RegisterRead(Register::ASAX);
//const uint8_t ASAY = RegisterRead(Register::ASAY);
//const uint8_t ASAZ = RegisterRead(Register::ASAZ);
// float ak8963_ASA[3] {};
// for (int i = 0; i < 3; i++) {
// if (0 != response[i] && 0xff != response[i]) {
// ak8963_ASA[i] = ((float)(response[i] - 128) / 256.0f) + 1.0f;
// } else {
// return false;
// }
// }
// _px4_mag.set_sensitivity(ak8963_ASA[0], ak8963_ASA[1], ak8963_ASA[2]);
// in 16-bit sampling mode the mag resolution is 1.5 milli Gauss per bit */
_px4_mag.set_scale(1.5e-3f);
return success;
}
bool MPU9250_AK8963::RegisterCheck(const register_config_t &reg_cfg, bool notify)
{
bool success = true;
const uint8_t reg_value = RegisterRead(reg_cfg.reg);
if (reg_cfg.set_bits && ((reg_value & reg_cfg.set_bits) != reg_cfg.set_bits)) {
PX4_DEBUG("0x%02hhX: 0x%02hhX (0x%02hhX not set)", (uint8_t)reg_cfg.reg, reg_value, reg_cfg.set_bits);
success = false;
}
if (reg_cfg.clear_bits && ((reg_value & reg_cfg.clear_bits) != 0)) {
PX4_DEBUG("0x%02hhX: 0x%02hhX (0x%02hhX not cleared)", (uint8_t)reg_cfg.reg, reg_value, reg_cfg.clear_bits);
success = false;
}
if (!success) {
RegisterSetAndClearBits(reg_cfg.reg, reg_cfg.set_bits, reg_cfg.clear_bits);
if (notify) {
perf_count(_bad_register_perf);
_px4_mag.increase_error_count();
}
}
return success;
}
uint8_t MPU9250_AK8963::RegisterRead(Register reg)
{
// TODO: use slave 4 and check register
_mpu9250.I2CSlaveRegisterStartRead(I2C_ADDRESS_DEFAULT, static_cast<uint8_t>(reg));
usleep(1000);
uint8_t buffer{};
_mpu9250.I2CSlaveExternalSensorDataRead(&buffer, 1);
return buffer;
}
void MPU9250_AK8963::RegisterWrite(Register reg, uint8_t value)
{
return _mpu9250.I2CSlaveRegisterWrite(I2C_ADDRESS_DEFAULT, static_cast<uint8_t>(reg), value);
}
void MPU9250_AK8963::RegisterSetAndClearBits(Register reg, uint8_t setbits, uint8_t clearbits)
{
const uint8_t orig_val = RegisterRead(reg);
uint8_t val = orig_val;
if (setbits) {
val |= setbits;
}
if (clearbits) {
val &= ~clearbits;
}
RegisterWrite(reg, val);
}
} // namespace AKM_AK8963

39
src/drivers/imu/invensense/mpu9250/MPU9250_AK8963.hpp
View File

@@ -46,7 +46,6 @@
#include <lib/drivers/device/i2c.h>
#include <lib/drivers/magnetometer/PX4Magnetometer.hpp>
#include <lib/perf/perf_counter.h>
#include <px4_platform_common/atomic.h>
#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
class MPU9250;
@@ -60,17 +59,13 @@ public:
MPU9250_AK8963(MPU9250 &mpu9250, enum Rotation rotation = ROTATION_NONE);
~MPU9250_AK8963() override;
bool Init();
void Stop();
bool Reset();
void PrintInfo();
void set_temperature(float temperature) { _px4_mag.set_temperature(temperature); }
private:
struct TransferBuffer {
//uint8_t ST1;
uint8_t ST1;
uint8_t HXL;
uint8_t HXH;
uint8_t HYL;
@@ -86,18 +81,8 @@ private:
uint8_t clear_bits{0};
};
int probe();
void Run() override;
bool Configure();
bool RegisterCheck(const register_config_t &reg_cfg, bool notify = false);
uint8_t RegisterRead(AKM_AK8963::Register reg);
void RegisterWrite(AKM_AK8963::Register reg, uint8_t value);
void RegisterSetAndClearBits(AKM_AK8963::Register reg, uint8_t setbits, uint8_t clearbits);
MPU9250 &_mpu9250;
PX4Magnetometer _px4_mag;
@@ -105,32 +90,22 @@ private:
perf_counter_t _transfer_perf{perf_alloc(PC_ELAPSED, MODULE_NAME"_ak8963: transfer")};
perf_counter_t _bad_register_perf{perf_alloc(PC_COUNT, MODULE_NAME"_ak8963: bad register")};
perf_counter_t _bad_transfer_perf{perf_alloc(PC_COUNT, MODULE_NAME"_ak8963: bad transfer")};
perf_counter_t _duplicate_data_perf{perf_alloc(PC_COUNT, MODULE_NAME"_ak8963: duplicate data")};
perf_counter_t _magnetic_sensor_overflow_perf{perf_alloc(PC_COUNT, MODULE_NAME"_ak09916: magnetic sensor overflow")};
hrt_abstime _reset_timestamp{0};
hrt_abstime _last_config_check_timestamp{0};
unsigned _consecutive_failures{0};
int16_t _last_measurement[3] {};
uint8_t _checked_register{0};
bool _sensitivity_adjustments_loaded{false};
float _sensitivity[3] {1.f, 1.f, 1.f};
enum class STATE : uint8_t {
RESET,
READ_WHO_AM_I,
WAIT_FOR_RESET,
CONFIGURE,
READ_SENSITIVITY_ADJUSTMENTS,
READ,
REQUEST_STOP,
STOPPED,
};
px4::atomic<STATE> _state{STATE::RESET};
static constexpr uint8_t size_register_cfg{1};
register_config_t _register_cfg[size_register_cfg] {
// Register | Set bits, Clear bits
{ AKM_AK8963::Register::CNTL1, AKM_AK8963::CNTL1_BIT::CONTINUOUS_MODE_2 | AKM_AK8963::CNTL1_BIT::BIT_16 },
};
} _state{STATE::RESET};
};
} // namespace AKM_AK8963