forked from Archive/PX4-Autopilot
magnetometer/lsm9ds1_mag: fix register typo and refactor to new style with state machine and configuration monitoring
This commit is contained in:
Daniel Agar
GitHub
parent
a124426541
commit
a9b47558b1
|
|
@ -1,6 +1,6 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2020 PX4 Development Team. All rights reserved.
|
||||
* Copyright (c) 2020-2021 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
|
|
@ -33,12 +33,12 @@
|
|||
|
||||
#include "LSM9DS1_MAG.hpp"
|
||||
|
||||
using namespace ST_LSM9DS1_MAG;
|
||||
using ST_LSM9DS1_MAG::Register;
|
||||
|
||||
using namespace time_literals;
|
||||
|
||||
static constexpr int16_t combine(uint8_t lsb, uint8_t msb) { return (msb << 8u) | lsb; }
|
||||
static constexpr int16_t combine(uint8_t msb, uint8_t lsb)
|
||||
{
|
||||
return (msb << 8u) | lsb;
|
||||
}
|
||||
|
||||
LSM9DS1_MAG::LSM9DS1_MAG(I2CSPIBusOption bus_option, int bus, uint32_t device, enum Rotation rotation,
|
||||
int bus_frequency, spi_mode_e spi_mode) :
|
||||
|
|
@ -51,18 +51,8 @@ LSM9DS1_MAG::LSM9DS1_MAG(I2CSPIBusOption bus_option, int bus, uint32_t device, e
|
|||
|
||||
LSM9DS1_MAG::~LSM9DS1_MAG()
|
||||
{
|
||||
perf_free(_interval_perf);
|
||||
perf_free(_transfer_perf);
|
||||
perf_free(_data_overrun_perf);
|
||||
}
|
||||
|
||||
int LSM9DS1_MAG::probe()
|
||||
{
|
||||
if (RegisterRead(Register::WHO_AM_I) == LSM9DS1_MAG_WHO_AM_I) {
|
||||
return PX4_OK;
|
||||
}
|
||||
|
||||
return PX4_ERROR;
|
||||
perf_free(_bad_register_perf);
|
||||
perf_free(_bad_transfer_perf);
|
||||
}
|
||||
|
||||
int LSM9DS1_MAG::init()
|
||||
|
|
@ -70,46 +60,205 @@ int LSM9DS1_MAG::init()
|
|||
int ret = SPI::init();
|
||||
|
||||
if (ret != PX4_OK) {
|
||||
DEVICE_DEBUG("SPI::init failed (%i)", ret);
|
||||
return ret;
|
||||
}
|
||||
|
||||
if (!Reset()) {
|
||||
PX4_ERR("reset failed");
|
||||
return PX4_ERROR;
|
||||
}
|
||||
|
||||
Start();
|
||||
|
||||
return PX4_OK;
|
||||
return Reset() ? 0 : -1;
|
||||
}
|
||||
|
||||
bool LSM9DS1_MAG::Reset()
|
||||
{
|
||||
// Reset
|
||||
// CTRL_REG2_M: SOFT_RST
|
||||
RegisterWrite(Register::CTRL_REG2_M, CTRL_REG2_M_BIT::SOFT_RST);
|
||||
usleep(50);
|
||||
|
||||
// CTRL_REG1_M: Temp comp, ultra high perofmrance mode, ODR 80 Hz, fast ODR
|
||||
RegisterWrite(Register::CTRL_REG1_M,
|
||||
CTRL_REG1_M_BIT::TEMP_COMP | CTRL_REG1_M_BIT::OM_ULTRA_HIGH_PERFORMANCE | CTRL_REG1_M_BIT::DO_80HZ);
|
||||
|
||||
// CTRL_REG2_M:
|
||||
RegisterSetBits(Register::CTRL_REG2_M, CTRL_REG2_M_BIT::FS_16_GAUSS);
|
||||
_px4_mag.set_scale(0.58f / 1000.0f); // Magnetic FS = ±16 gauss 0.58 mgauss/LSB
|
||||
|
||||
// CTRL_REG3_M: I2C_DISABLE, Continuous-conversion mode
|
||||
RegisterClearBits(Register::CTRL_REG3_M, CTRL_REG3_M_BIT::MD_CONTINUOUS_MODE);
|
||||
|
||||
// CTRL_REG4_M: Z-axis Ultra-high performance mode
|
||||
RegisterSetBits(Register::CTRL_REG4_M, CTRL_REG4_M_BIT::OMZ_ULTRA_HIGH_PERFORMANCE);
|
||||
|
||||
// CTRL_REG5_M: Block data update for magnetic data.
|
||||
RegisterSetBits(Register::CTRL_REG5_M, CTRL_REG5_M_BIT::BDU);
|
||||
|
||||
_state = STATE::RESET;
|
||||
ScheduleClear();
|
||||
ScheduleNow();
|
||||
return true;
|
||||
}
|
||||
|
||||
void LSM9DS1_MAG::print_status()
|
||||
{
|
||||
I2CSPIDriverBase::print_status();
|
||||
|
||||
perf_print_counter(_bad_register_perf);
|
||||
perf_print_counter(_bad_transfer_perf);
|
||||
}
|
||||
|
||||
int LSM9DS1_MAG::probe()
|
||||
{
|
||||
const uint8_t WHO_AM_I_M = RegisterRead(Register::WHO_AM_I_M);
|
||||
|
||||
if (WHO_AM_I_M != Device_identification) {
|
||||
DEVICE_DEBUG("unexpected WHO_AM_I_M 0x%02x", WHO_AM_I_M);
|
||||
return PX4_ERROR;
|
||||
}
|
||||
|
||||
return PX4_OK;
|
||||
}
|
||||
|
||||
void LSM9DS1_MAG::RunImpl()
|
||||
{
|
||||
const hrt_abstime now = hrt_absolute_time();
|
||||
|
||||
switch (_state) {
|
||||
case STATE::RESET:
|
||||
// CTRL_REG2_M: SOFT_RST
|
||||
RegisterWrite(Register::CTRL_REG2_M, CTRL_REG2_M_BIT::SOFT_RST);
|
||||
_reset_timestamp = now;
|
||||
_failure_count = 0;
|
||||
_state = STATE::WAIT_FOR_RESET;
|
||||
ScheduleDelayed(100_ms);
|
||||
break;
|
||||
|
||||
case STATE::WAIT_FOR_RESET:
|
||||
if (RegisterRead(Register::WHO_AM_I_M) == Device_identification) {
|
||||
// if reset succeeded then configure
|
||||
_state = STATE::CONFIGURE;
|
||||
ScheduleDelayed(10_ms);
|
||||
|
||||
} else {
|
||||
// RESET not complete
|
||||
if (hrt_elapsed_time(&_reset_timestamp) > 1000_ms) {
|
||||
PX4_DEBUG("Reset failed, retrying");
|
||||
_state = STATE::RESET;
|
||||
ScheduleDelayed(100_ms);
|
||||
|
||||
} else {
|
||||
PX4_DEBUG("Reset not complete, check again in 100 ms");
|
||||
ScheduleDelayed(100_ms);
|
||||
}
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
case STATE::CONFIGURE:
|
||||
if (Configure()) {
|
||||
// if configure succeeded then start reading
|
||||
_state = STATE::READ;
|
||||
ScheduleOnInterval(1000000 / ST_LSM9DS1_MAG::M_ODR);
|
||||
|
||||
} else {
|
||||
// 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::READ: {
|
||||
struct TransferBuffer {
|
||||
uint8_t cmd;
|
||||
uint8_t STATUS_REG_M;
|
||||
uint8_t OUT_X_L_M;
|
||||
uint8_t OUT_X_H_M;
|
||||
uint8_t OUT_Y_L_M;
|
||||
uint8_t OUT_Y_H_M;
|
||||
uint8_t OUT_Z_L_M;
|
||||
uint8_t OUT_Z_H_M;
|
||||
} buffer{};
|
||||
|
||||
buffer.cmd = static_cast<uint8_t>(Register::STATUS_REG_M) | RW_BIT_READ | MS_BIT_AUTO_INCREMENT;
|
||||
|
||||
bool success = false;
|
||||
|
||||
if (transfer((uint8_t *)&buffer, (uint8_t *)&buffer, sizeof(buffer)) == PX4_OK) {
|
||||
if (buffer.STATUS_REG_M & STATUS_REG_M_BIT::ZYXDA) {
|
||||
// X, Y and Z-axis new data available.
|
||||
int16_t x = combine(buffer.OUT_X_H_M, buffer.OUT_X_L_M);
|
||||
int16_t y = combine(buffer.OUT_Y_H_M, buffer.OUT_Y_L_M);
|
||||
int16_t z = combine(buffer.OUT_Z_H_M, buffer.OUT_Z_L_M);
|
||||
|
||||
// sensor Z is up (RHC), flip z for publication
|
||||
// sensor X is aligned with -X of lsm9ds1 accel/gyro
|
||||
x = (x == INT16_MIN) ? INT16_MAX : -x;
|
||||
y = y;
|
||||
z = (z == INT16_MIN) ? INT16_MAX : -z;
|
||||
|
||||
_px4_mag.update(now, x, y, z);
|
||||
|
||||
success = true;
|
||||
|
||||
if (_failure_count > 0) {
|
||||
_failure_count--;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if (!success) {
|
||||
_failure_count++;
|
||||
|
||||
// full reset if things are failing consistently
|
||||
if (_failure_count > 10) {
|
||||
Reset();
|
||||
return;
|
||||
}
|
||||
}
|
||||
|
||||
if (!success || hrt_elapsed_time(&_last_config_check_timestamp) > 100_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 reset
|
||||
perf_count(_bad_register_perf);
|
||||
Reset();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
bool LSM9DS1_MAG::Configure()
|
||||
{
|
||||
// first set and clear all configured register bits
|
||||
for (const auto ®_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 ®_cfg : _register_cfg) {
|
||||
if (!RegisterCheck(reg_cfg)) {
|
||||
success = false;
|
||||
}
|
||||
}
|
||||
|
||||
// Magnetic FS = ±16 gauss 0.58 mgauss/LSB
|
||||
_px4_mag.set_scale(0.58f / 1000.0f);
|
||||
|
||||
return success;
|
||||
}
|
||||
|
||||
bool LSM9DS1_MAG::RegisterCheck(const register_config_t ®_cfg)
|
||||
{
|
||||
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;
|
||||
}
|
||||
|
||||
return success;
|
||||
}
|
||||
|
||||
uint8_t LSM9DS1_MAG::RegisterRead(Register reg)
|
||||
{
|
||||
uint8_t cmd[2] {};
|
||||
|
|
@ -124,80 +273,13 @@ void LSM9DS1_MAG::RegisterWrite(Register reg, uint8_t value)
|
|||
transfer(cmd, cmd, sizeof(cmd));
|
||||
}
|
||||
|
||||
void LSM9DS1_MAG::RegisterSetBits(Register reg, uint8_t setbits)
|
||||
void LSM9DS1_MAG::RegisterSetAndClearBits(Register reg, uint8_t setbits, uint8_t clearbits)
|
||||
{
|
||||
uint8_t val = RegisterRead(reg);
|
||||
const uint8_t orig_val = RegisterRead(reg);
|
||||
|
||||
if (!(val & setbits)) {
|
||||
val |= setbits;
|
||||
uint8_t val = (orig_val & ~clearbits) | setbits;
|
||||
|
||||
if (orig_val != val) {
|
||||
RegisterWrite(reg, val);
|
||||
}
|
||||
}
|
||||
|
||||
void LSM9DS1_MAG::RegisterClearBits(Register reg, uint8_t clearbits)
|
||||
{
|
||||
uint8_t val = RegisterRead(reg);
|
||||
|
||||
if (val & clearbits) {
|
||||
val &= !clearbits;
|
||||
RegisterWrite(reg, val);
|
||||
}
|
||||
}
|
||||
|
||||
void LSM9DS1_MAG::Start()
|
||||
{
|
||||
ScheduleOnInterval(1000000 / ST_LSM9DS1_MAG::M_ODR / 2);
|
||||
}
|
||||
|
||||
void LSM9DS1_MAG::RunImpl()
|
||||
{
|
||||
perf_count(_interval_perf);
|
||||
|
||||
struct MagReport {
|
||||
uint8_t cmd;
|
||||
uint8_t STATUS_REG_M;
|
||||
uint8_t OUT_X_L_M;
|
||||
uint8_t OUT_X_H_M;
|
||||
uint8_t OUT_Y_L_M;
|
||||
uint8_t OUT_Y_H_M;
|
||||
uint8_t OUT_Z_L_M;
|
||||
uint8_t OUT_Z_H_M;
|
||||
} mreport{};
|
||||
mreport.cmd = static_cast<uint8_t>(Register::STATUS_REG_M) | RW_BIT_READ | MS_BIT_AUTO_INCREMENT;
|
||||
|
||||
perf_begin(_transfer_perf);
|
||||
const hrt_abstime timestamp_sample = hrt_absolute_time();
|
||||
|
||||
if (transfer((uint8_t *)&mreport, (uint8_t *)&mreport, sizeof(MagReport)) != PX4_OK) {
|
||||
perf_end(_transfer_perf);
|
||||
return;
|
||||
}
|
||||
|
||||
perf_end(_transfer_perf);
|
||||
|
||||
if (mreport.STATUS_REG_M & STATUS_REG_M_BIT::ZYXOR) {
|
||||
// X, Y and Z-axis data overrun.
|
||||
perf_count(_data_overrun_perf);
|
||||
return;
|
||||
}
|
||||
|
||||
if (mreport.STATUS_REG_M & STATUS_REG_M_BIT::ZYXDA) {
|
||||
// X, Y and Z-axis new data available.
|
||||
|
||||
// sensor Z is up (RHC), flip z for publication
|
||||
// sensor X is aligned with -X of lsm9ds1 accel/gyro
|
||||
int16_t x = -combine(mreport.OUT_X_L_M, mreport.OUT_X_H_M);
|
||||
int16_t y = combine(mreport.OUT_Y_L_M, mreport.OUT_Y_H_M);
|
||||
int16_t z = -combine(mreport.OUT_Z_L_M, mreport.OUT_Z_H_M);
|
||||
|
||||
_px4_mag.update(timestamp_sample, x, y, z);
|
||||
}
|
||||
}
|
||||
|
||||
void LSM9DS1_MAG::print_status()
|
||||
{
|
||||
I2CSPIDriverBase::print_status();
|
||||
perf_print_counter(_interval_perf);
|
||||
perf_print_counter(_transfer_perf);
|
||||
perf_print_counter(_data_overrun_perf);
|
||||
}
|
||||
|
|
|
|||
|
|
@ -1,6 +1,6 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2020 PX4 Development Team. All rights reserved.
|
||||
* Copyright (c) 2020-2021 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
|
|
@ -46,10 +46,10 @@
|
|||
#include <lib/drivers/device/spi.h>
|
||||
#include <lib/drivers/magnetometer/PX4Magnetometer.hpp>
|
||||
#include <lib/perf/perf_counter.h>
|
||||
#include <px4_platform_common/px4_config.h>
|
||||
#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
|
||||
#include <px4_platform_common/i2c_spi_buses.h>
|
||||
|
||||
using namespace ST_LSM9DS1_MAG;
|
||||
|
||||
class LSM9DS1_MAG : public device::SPI, public I2CSPIDriver<LSM9DS1_MAG>
|
||||
{
|
||||
public:
|
||||
|
|
@ -61,26 +61,55 @@ public:
|
|||
int runtime_instance);
|
||||
static void print_usage();
|
||||
|
||||
void print_status();
|
||||
void RunImpl();
|
||||
|
||||
int init() override;
|
||||
|
||||
void Start();
|
||||
bool Reset();
|
||||
|
||||
void RunImpl();
|
||||
void print_status() override;
|
||||
|
||||
private:
|
||||
int probe() override;
|
||||
|
||||
uint8_t RegisterRead(ST_LSM9DS1_MAG::Register reg);
|
||||
void RegisterWrite(ST_LSM9DS1_MAG::Register reg, uint8_t value);
|
||||
void RegisterSetBits(ST_LSM9DS1_MAG::Register reg, uint8_t setbits);
|
||||
void RegisterClearBits(ST_LSM9DS1_MAG::Register reg, uint8_t clearbits);
|
||||
struct register_config_t {
|
||||
Register reg;
|
||||
uint8_t set_bits{0};
|
||||
uint8_t clear_bits{0};
|
||||
};
|
||||
|
||||
int probe() override;
|
||||
|
||||
bool Reset();
|
||||
|
||||
bool Configure();
|
||||
|
||||
bool RegisterCheck(const register_config_t ®_cfg);
|
||||
|
||||
uint8_t RegisterRead(Register reg);
|
||||
void RegisterWrite(Register reg, uint8_t value);
|
||||
void RegisterSetAndClearBits(Register reg, uint8_t setbits, uint8_t clearbits);
|
||||
|
||||
PX4Magnetometer _px4_mag;
|
||||
|
||||
perf_counter_t _interval_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": run interval")};
|
||||
perf_counter_t _transfer_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": transfer")};
|
||||
perf_counter_t _data_overrun_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": overrun")};
|
||||
perf_counter_t _bad_register_perf{perf_alloc(PC_COUNT, MODULE_NAME": bad register")};
|
||||
perf_counter_t _bad_transfer_perf{perf_alloc(PC_COUNT, MODULE_NAME": bad transfer")};
|
||||
|
||||
hrt_abstime _reset_timestamp{0};
|
||||
hrt_abstime _last_config_check_timestamp{0};
|
||||
int _failure_count{0};
|
||||
|
||||
enum class STATE : uint8_t {
|
||||
RESET,
|
||||
WAIT_FOR_RESET,
|
||||
CONFIGURE,
|
||||
READ,
|
||||
} _state{STATE::RESET};
|
||||
|
||||
uint8_t _checked_register{0};
|
||||
static constexpr uint8_t size_register_cfg{5};
|
||||
register_config_t _register_cfg[size_register_cfg] {
|
||||
// Register | Set bits, Clear bits
|
||||
{ Register::CTRL_REG1_M, CTRL_REG1_M_BIT::TEMP_COMP | CTRL_REG1_M_BIT::OM_ULTRA_HIGH_PERFORMANCE | CTRL_REG1_M_BIT::DO_80HZ, CTRL_REG1_M_BIT::FAST_ODR | CTRL_REG1_M_BIT::ST },
|
||||
{ Register::CTRL_REG2_M, CTRL_REG2_M_BIT::FS_16_GAUSS, 0 },
|
||||
{ Register::CTRL_REG3_M, CTRL_REG3_M_BIT::I2C_DISABLE, CTRL_REG3_M_BIT::SIM | CTRL_REG3_M_BIT::MD_CONTINUOUS_MODE },
|
||||
{ Register::CTRL_REG4_M, CTRL_REG4_M_BIT::OMZ_ULTRA_HIGH_PERFORMANCE, 0 },
|
||||
{ Register::CTRL_REG5_M, CTRL_REG5_M_BIT::BDU, 0 },
|
||||
};
|
||||
};
|
||||
|
|
|
|||
|
|
@ -1,6 +1,6 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2020 PX4 Development Team. All rights reserved.
|
||||
* Copyright (c) 2020-2021 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
|
|
@ -55,24 +55,23 @@ static constexpr uint8_t Bit5 = (1 << 5);
|
|||
static constexpr uint8_t Bit6 = (1 << 6);
|
||||
static constexpr uint8_t Bit7 = (1 << 7);
|
||||
|
||||
static constexpr uint32_t SPI_SPEED = 10 * 1000 * 1000; // 10 MHz SPI clock frequency
|
||||
|
||||
static constexpr uint8_t RW_BIT_READ = Bit7;
|
||||
static constexpr uint8_t MS_BIT_AUTO_INCREMENT = Bit6;
|
||||
|
||||
static constexpr uint8_t LSM9DS1_MAG_WHO_AM_I = 0b00111101; // Who I am ID
|
||||
|
||||
static constexpr uint32_t SPI_SPEED = 10 * 1000 * 1000; // 10 MHz SPI clock frequency
|
||||
static constexpr uint8_t Device_identification = 0b00111101; // Who I am ID
|
||||
|
||||
static constexpr uint32_t M_ODR = 80; // Magnetometer output data rate
|
||||
|
||||
enum class
|
||||
Register : uint8_t {
|
||||
WHO_AM_I = 0x0F,
|
||||
enum class Register : uint8_t {
|
||||
WHO_AM_I_M = 0x0F,
|
||||
|
||||
CTRL_REG1_M = 0x20,
|
||||
CTRL_REG2_M = 0x21,
|
||||
CTRL_REG3_M = 0x22,
|
||||
CTRL_REG4_M = 0x23,
|
||||
CTRL_REG5_M = 0x25,
|
||||
CTRL_REG5_M = 0x24,
|
||||
|
||||
STATUS_REG_M = 0x27,
|
||||
OUT_X_L_M = 0x28,
|
||||
|
|
@ -84,8 +83,7 @@ Register : uint8_t {
|
|||
};
|
||||
|
||||
// CTRL_REG1_M
|
||||
enum
|
||||
CTRL_REG1_M_BIT : uint8_t {
|
||||
enum CTRL_REG1_M_BIT : uint8_t {
|
||||
TEMP_COMP = Bit7, // Temperature compensation enable.
|
||||
OM_ULTRA_HIGH_PERFORMANCE = Bit6 | Bit5, // X and Y axes operative mode selection.
|
||||
DO_80HZ = Bit4 | Bit3 | Bit2, // 80 Hz Output data rate selection.
|
||||
|
|
@ -94,37 +92,30 @@ CTRL_REG1_M_BIT : uint8_t {
|
|||
};
|
||||
|
||||
// CTRL_REG2_M
|
||||
enum
|
||||
CTRL_REG2_M_BIT : uint8_t {
|
||||
enum CTRL_REG2_M_BIT : uint8_t {
|
||||
FS_16_GAUSS = Bit6 | Bit5, // Full-scale selection ± 16 gauss
|
||||
|
||||
SOFT_RST = Bit2,
|
||||
SOFT_RST = Bit2,
|
||||
};
|
||||
|
||||
// CTRL_REG3_M
|
||||
enum
|
||||
CTRL_REG3_M_BIT : uint8_t {
|
||||
I2C_DISABLE = Bit7,
|
||||
|
||||
MD_CONTINUOUS_MODE = Bit1 | Bit0,
|
||||
enum CTRL_REG3_M_BIT : uint8_t {
|
||||
I2C_DISABLE = Bit7,
|
||||
SIM = Bit2, // SPI Serial Interface mode selection.
|
||||
MD_CONTINUOUS_MODE = Bit1 | Bit0, // Continuous-conversion mode
|
||||
};
|
||||
|
||||
// CTRL_REG4_M
|
||||
enum
|
||||
CTRL_REG4_M_BIT : uint8_t {
|
||||
enum CTRL_REG4_M_BIT : uint8_t {
|
||||
OMZ_ULTRA_HIGH_PERFORMANCE = Bit4 | Bit3, // Ultra-high performance mode
|
||||
};
|
||||
|
||||
// CTRL_REG5_M
|
||||
enum
|
||||
CTRL_REG5_M_BIT : uint8_t {
|
||||
enum CTRL_REG5_M_BIT : uint8_t {
|
||||
BDU = Bit6, // Block data update for magnetic data.
|
||||
};
|
||||
|
||||
|
||||
// STATUS_REG_M
|
||||
enum
|
||||
STATUS_REG_M_BIT : uint8_t {
|
||||
enum STATUS_REG_M_BIT : uint8_t {
|
||||
ZYXOR = Bit7, // X, Y and Z-axis data overrun.
|
||||
ZYXDA = Bit3, // X, Y and Z-axis new data available.
|
||||
};
|
||||
|
|
|
|||
|
|
@ -1,6 +1,6 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2020 PX4 Development Team. All rights reserved.
|
||||
* Copyright (c) 2020-2021 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
|
|
@ -36,8 +36,27 @@
|
|||
#include <px4_platform_common/getopt.h>
|
||||
#include <px4_platform_common/module.h>
|
||||
|
||||
void
|
||||
LSM9DS1_MAG::print_usage()
|
||||
I2CSPIDriverBase *LSM9DS1_MAG::instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator,
|
||||
int runtime_instance)
|
||||
{
|
||||
LSM9DS1_MAG *instance = new LSM9DS1_MAG(iterator.configuredBusOption(), iterator.bus(), iterator.devid(), cli.rotation,
|
||||
cli.bus_frequency, cli.spi_mode);
|
||||
|
||||
if (instance == nullptr) {
|
||||
PX4_ERR("alloc failed");
|
||||
return nullptr;
|
||||
}
|
||||
|
||||
if (OK != instance->init()) {
|
||||
delete instance;
|
||||
PX4_DEBUG("no device on bus %i (devid 0x%x)", iterator.bus(), iterator.devid());
|
||||
return nullptr;
|
||||
}
|
||||
|
||||
return instance;
|
||||
}
|
||||
|
||||
void LSM9DS1_MAG::print_usage()
|
||||
{
|
||||
PRINT_MODULE_USAGE_NAME("lsm9ds1_mag", "driver");
|
||||
PRINT_MODULE_USAGE_SUBCATEGORY("magnetometer");
|
||||
|
|
@ -47,32 +66,12 @@ LSM9DS1_MAG::print_usage()
|
|||
PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
|
||||
}
|
||||
|
||||
I2CSPIDriverBase *LSM9DS1_MAG::instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator,
|
||||
int runtime_instance)
|
||||
{
|
||||
LSM9DS1_MAG *instance = new LSM9DS1_MAG(iterator.configuredBusOption(), iterator.bus(), iterator.devid(),
|
||||
cli.rotation, cli.bus_frequency, cli.spi_mode);
|
||||
|
||||
if (instance == nullptr) {
|
||||
PX4_ERR("alloc failed");
|
||||
return nullptr;
|
||||
}
|
||||
|
||||
if (OK != instance->init()) {
|
||||
delete instance;
|
||||
return nullptr;
|
||||
}
|
||||
|
||||
return instance;
|
||||
}
|
||||
|
||||
|
||||
extern "C" __EXPORT int lsm9ds1_mag_main(int argc, char *argv[])
|
||||
{
|
||||
using ThisDriver = LSM9DS1_MAG;
|
||||
int ch;
|
||||
using ThisDriver = LSM9DS1_MAG;
|
||||
BusCLIArguments cli{false, true};
|
||||
cli.default_spi_frequency = ST_LSM9DS1_MAG::SPI_SPEED;
|
||||
cli.default_spi_frequency = SPI_SPEED;
|
||||
|
||||
while ((ch = cli.getopt(argc, argv, "R:")) != EOF) {
|
||||
switch (ch) {
|
||||
|
|
|
|||
Loading…
Reference in New Issue