mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-05-14 18:07:34 +08:00
drivers/magnetometer/st/iis2mdc: refactor to monitor registers
- if there's any configuration error the driver is now able to reset/reconfigure itself
This commit is contained in:
Daniel Agar
@@ -1,6 +1,6 @@
|
||||
############################################################################
|
||||
#
|
||||
# Copyright (c) 2024 PX4 Development Team. All rights reserved.
|
||||
# Copyright (c) 2024-2025 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
|
||||
@@ -30,15 +30,16 @@
|
||||
# POSSIBILITY OF SUCH DAMAGE.
|
||||
#
|
||||
############################################################################
|
||||
|
||||
px4_add_module(
|
||||
MODULE drivers__magnetometer__st__iis2mdc
|
||||
MAIN iis2mdc
|
||||
COMPILE_FLAGS
|
||||
# -DDEBUG_BUILD
|
||||
SRCS
|
||||
iis2mdc_i2c.cpp
|
||||
IIS2MDC.cpp
|
||||
IIS2MDC.hpp
|
||||
iis2mdc_main.cpp
|
||||
iis2mdc.cpp
|
||||
ST_IIS2MDC_registers.hpp
|
||||
DEPENDS
|
||||
drivers_magnetometer
|
||||
px4_work_queue
|
||||
|
||||
@@ -0,0 +1,321 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2024-2025 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
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
#include "IIS2MDC.hpp"
|
||||
|
||||
using namespace time_literals;
|
||||
|
||||
static constexpr int16_t combine(uint8_t msb, uint8_t lsb)
|
||||
{
|
||||
return (msb << 8u) | lsb;
|
||||
}
|
||||
|
||||
IIS2MDC::IIS2MDC(const I2CSPIDriverConfig &config) :
|
||||
I2C(config),
|
||||
I2CSPIDriver(config),
|
||||
_px4_mag(get_device_id(), config.rotation)
|
||||
{
|
||||
}
|
||||
|
||||
IIS2MDC::~IIS2MDC()
|
||||
{
|
||||
perf_free(_reboot_perf);
|
||||
perf_free(_reset_perf);
|
||||
perf_free(_bad_register_perf);
|
||||
perf_free(_bad_transfer_perf);
|
||||
}
|
||||
|
||||
int IIS2MDC::init()
|
||||
{
|
||||
int ret = I2C::init();
|
||||
|
||||
if (ret != PX4_OK) {
|
||||
PX4_DEBUG("I2C::init failed (%i)", ret);
|
||||
return ret;
|
||||
}
|
||||
|
||||
return Reboot() ? 0 : -1;
|
||||
}
|
||||
|
||||
bool IIS2MDC::Reboot()
|
||||
{
|
||||
_state = STATE::REBOOT;
|
||||
ScheduleClear();
|
||||
ScheduleNow();
|
||||
return true;
|
||||
}
|
||||
|
||||
bool IIS2MDC::Reset()
|
||||
{
|
||||
_state = STATE::RESET;
|
||||
ScheduleClear();
|
||||
ScheduleNow();
|
||||
return true;
|
||||
}
|
||||
|
||||
void IIS2MDC::print_status()
|
||||
{
|
||||
I2CSPIDriverBase::print_status();
|
||||
|
||||
perf_print_counter(_reboot_perf);
|
||||
perf_print_counter(_reset_perf);
|
||||
perf_print_counter(_bad_register_perf);
|
||||
perf_print_counter(_bad_transfer_perf);
|
||||
}
|
||||
|
||||
int IIS2MDC::probe()
|
||||
{
|
||||
for (int retry = 0; retry < 3; retry++) {
|
||||
|
||||
uint8_t id = RegisterRead(Register::WHO_AM_I);
|
||||
|
||||
if (id == Device_ID) {
|
||||
_retries = 2;
|
||||
return PX4_OK;
|
||||
|
||||
} else {
|
||||
PX4_DEBUG("unexpected WHO_AM_I 0x%02x", id);
|
||||
}
|
||||
}
|
||||
|
||||
return PX4_ERROR;
|
||||
}
|
||||
|
||||
void IIS2MDC::RunImpl()
|
||||
{
|
||||
const hrt_abstime now = hrt_absolute_time();
|
||||
|
||||
switch (_state) {
|
||||
case STATE::REBOOT:
|
||||
// CFG_REG_A: Reboot
|
||||
RegisterWrite(Register::CFG_REG_A, CFG_REG_A_BIT::REBOOT);
|
||||
perf_count(_reboot_perf);
|
||||
_state = STATE::RESET;
|
||||
ScheduleDelayed(40_ms); // Wait > 20 ms
|
||||
break;
|
||||
|
||||
case STATE::RESET:
|
||||
// CFG_REG_A: Software Reset
|
||||
RegisterWrite(Register::CFG_REG_A, CFG_REG_A_BIT::SOFT_RST);
|
||||
_reset_timestamp = now;
|
||||
_failure_count = 0;
|
||||
_state = STATE::WAIT_FOR_RESET;
|
||||
perf_count(_reset_perf);
|
||||
ScheduleDelayed(10_ms);
|
||||
break;
|
||||
|
||||
case STATE::WAIT_FOR_RESET:
|
||||
|
||||
// SOFT_RST: This bit is automatically reset to zero after reset
|
||||
if ((RegisterRead(Register::WHO_AM_I) == Device_ID)
|
||||
&& ((RegisterRead(Register::CFG_REG_A) & CFG_REG_A_BIT::SOFT_RST) == 0)) {
|
||||
|
||||
// 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 10 ms");
|
||||
ScheduleDelayed(10_ms);
|
||||
}
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
case STATE::CONFIGURE:
|
||||
if (Configure()) {
|
||||
// if configure succeeded then start read cycle
|
||||
_state = STATE::READ;
|
||||
ScheduleOnInterval(20_ms); // 50 Hz
|
||||
|
||||
} 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 STATUS_REG;
|
||||
uint8_t OUTX_L_REG;
|
||||
uint8_t OUTX_H_REG;
|
||||
uint8_t OUTY_L_REG;
|
||||
uint8_t OUTY_H_REG;
|
||||
uint8_t OUTZ_L_REG;
|
||||
uint8_t OUTZ_H_REG;
|
||||
uint8_t TEMP_OUT_L_REG;
|
||||
uint8_t TEMP_OUT_H_REG;
|
||||
} buffer{};
|
||||
|
||||
bool success = false;
|
||||
uint8_t cmd = static_cast<uint8_t>(Register::STATUS_REG);
|
||||
|
||||
if (transfer(&cmd, 1, (uint8_t *)&buffer, sizeof(buffer)) == PX4_OK) {
|
||||
|
||||
// new set of data is available (Zyxda = 1)
|
||||
if (buffer.STATUS_REG & STATUS_REG_BIT::Zyxda) {
|
||||
int16_t x = combine(buffer.OUTX_H_REG, buffer.OUTX_L_REG);
|
||||
int16_t y = combine(buffer.OUTY_H_REG, buffer.OUTY_L_REG);
|
||||
int16_t z = combine(buffer.OUTZ_H_REG, buffer.OUTZ_L_REG);
|
||||
|
||||
// sensor's frame is +x forward, +y right, +z up
|
||||
z = math::negate(z);
|
||||
|
||||
int16_t t = combine(buffer.TEMP_OUT_H_REG, buffer.TEMP_OUT_L_REG);
|
||||
|
||||
// temperature sensitivity of 8 LSB/°C. Typically, the output zero level corresponds to 25 °C
|
||||
const float temperature = (t / 8.f) + 25.f;
|
||||
_px4_mag.set_temperature(temperature);
|
||||
|
||||
_px4_mag.set_error_count(perf_event_count(_bad_register_perf) + perf_event_count(_bad_transfer_perf));
|
||||
_px4_mag.update(now, x, y, z);
|
||||
|
||||
success = true;
|
||||
|
||||
if (_failure_count > 0) {
|
||||
_failure_count--;
|
||||
}
|
||||
}
|
||||
|
||||
} else {
|
||||
perf_count(_bad_transfer_perf);
|
||||
}
|
||||
|
||||
if (!success) {
|
||||
_failure_count++;
|
||||
|
||||
// full reboot/reset if things are failing consistently
|
||||
if (_failure_count > 10) {
|
||||
Reboot();
|
||||
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 reboot/reset
|
||||
perf_count(_bad_register_perf);
|
||||
Reboot();
|
||||
return;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
bool IIS2MDC::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;
|
||||
}
|
||||
}
|
||||
|
||||
_px4_mag.set_scale(0.0015f); // M_So = 1.5 mG / LSB
|
||||
|
||||
return success;
|
||||
}
|
||||
|
||||
bool IIS2MDC::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 IIS2MDC::RegisterRead(Register reg)
|
||||
{
|
||||
const uint8_t cmd = static_cast<uint8_t>(reg);
|
||||
uint8_t buffer{};
|
||||
transfer(&cmd, 1, &buffer, 1);
|
||||
return buffer;
|
||||
}
|
||||
|
||||
void IIS2MDC::RegisterWrite(Register reg, uint8_t value)
|
||||
{
|
||||
uint8_t buffer[2] { (uint8_t)reg, value };
|
||||
transfer(buffer, sizeof(buffer), nullptr, 0);
|
||||
}
|
||||
|
||||
void IIS2MDC::RegisterSetAndClearBits(Register reg, uint8_t setbits, uint8_t clearbits)
|
||||
{
|
||||
const uint8_t orig_val = RegisterRead(reg);
|
||||
uint8_t val = (orig_val & ~clearbits) | setbits;
|
||||
|
||||
if (orig_val != val) {
|
||||
RegisterWrite(reg, val);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,114 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2024-2025 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
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file IIS2MDC.hpp
|
||||
*
|
||||
* Driver for the ST IIS2MDC connected via I2C.
|
||||
*
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "ST_IIS2MDC_registers.hpp"
|
||||
|
||||
#include <drivers/drv_hrt.h>
|
||||
#include <lib/drivers/device/i2c.h>
|
||||
#include <lib/drivers/magnetometer/PX4Magnetometer.hpp>
|
||||
#include <lib/perf/perf_counter.h>
|
||||
#include <px4_platform_common/i2c_spi_buses.h>
|
||||
|
||||
using namespace ST_IIS2MDC;
|
||||
|
||||
class IIS2MDC : public device::I2C, public I2CSPIDriver<IIS2MDC>
|
||||
{
|
||||
public:
|
||||
IIS2MDC(const I2CSPIDriverConfig &config);
|
||||
~IIS2MDC() override;
|
||||
|
||||
static void print_usage();
|
||||
|
||||
void RunImpl();
|
||||
|
||||
int init() override;
|
||||
void print_status() override;
|
||||
|
||||
private:
|
||||
// Sensor Configuration
|
||||
struct register_config_t {
|
||||
Register reg;
|
||||
uint8_t set_bits{0};
|
||||
uint8_t clear_bits{0};
|
||||
};
|
||||
|
||||
int probe() override;
|
||||
|
||||
bool Reboot();
|
||||
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 _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")};
|
||||
perf_counter_t _reboot_perf{perf_alloc(PC_COUNT, MODULE_NAME": reboot")};
|
||||
perf_counter_t _reset_perf{perf_alloc(PC_COUNT, MODULE_NAME": reset")};
|
||||
|
||||
hrt_abstime _reset_timestamp{0};
|
||||
hrt_abstime _last_config_check_timestamp{0};
|
||||
int _failure_count{0};
|
||||
|
||||
enum class STATE : uint8_t {
|
||||
REBOOT,
|
||||
RESET,
|
||||
WAIT_FOR_RESET,
|
||||
CONFIGURE,
|
||||
READ,
|
||||
} _state{STATE::RESET};
|
||||
|
||||
uint8_t _checked_register{0};
|
||||
static constexpr uint8_t size_register_cfg{3};
|
||||
register_config_t _register_cfg[size_register_cfg] {
|
||||
// Register | Set bits, Clear bits
|
||||
{ Register::CFG_REG_A, CFG_REG_A_BIT::COMP_TEMP_EN | CFG_REG_A_BIT::ODR_50_HZ_SET, CFG_REG_A_BIT::LP | CFG_REG_A_BIT::ODR_50_HZ_CLEAR | CFG_REG_A_BIT::MD_CONTINUOUS_CLEAR },
|
||||
{ Register::CFG_REG_B, CFG_REG_B_BIT::OFF_CANC, CFG_REG_B_BIT::LPF },
|
||||
{ Register::CFG_REG_C, CFG_REG_C_BIT::BDU, CFG_REG_C_BIT::BLE | CFG_REG_C_BIT::I2C_DIS },
|
||||
};
|
||||
};
|
||||
@@ -2,4 +2,4 @@ menuconfig DRIVERS_MAGNETOMETER_ST_IIS2MDC
|
||||
bool "iis2mdc"
|
||||
default n
|
||||
---help---
|
||||
Enable support for iis2mdc
|
||||
Enable support for ST IIS2MDC
|
||||
|
||||
+66
-53
@@ -1,6 +1,6 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2024 PX4 Development Team. All rights reserved.
|
||||
* Copyright (c) 2024-2025 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
|
||||
@@ -31,65 +31,78 @@
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file ST_IIS2MDC_registers.hpp
|
||||
*
|
||||
* ST IIS2MDC registers.
|
||||
*
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <px4_platform_common/i2c_spi_buses.h>
|
||||
#include <lib/drivers/magnetometer/PX4Magnetometer.hpp>
|
||||
#include <cstdint>
|
||||
|
||||
// IIS2MDC Registers
|
||||
#define IIS2MDC_ADDR_CFG_REG_A 0x60
|
||||
#define IIS2MDC_ADDR_CFG_REG_B 0x61
|
||||
#define IIS2MDC_ADDR_CFG_REG_C 0x62
|
||||
#define IIS2MDC_ADDR_STATUS_REG 0x67
|
||||
#define IIS2MDC_ADDR_OUTX_L_REG 0x68
|
||||
#define IIS2MDC_ADDR_WHO_AM_I 0x4F
|
||||
// TODO: move to a central header
|
||||
static constexpr uint8_t Bit0 = (1 << 0);
|
||||
static constexpr uint8_t Bit1 = (1 << 1);
|
||||
static constexpr uint8_t Bit2 = (1 << 2);
|
||||
static constexpr uint8_t Bit3 = (1 << 3);
|
||||
static constexpr uint8_t Bit4 = (1 << 4);
|
||||
static constexpr uint8_t Bit5 = (1 << 5);
|
||||
static constexpr uint8_t Bit6 = (1 << 6);
|
||||
static constexpr uint8_t Bit7 = (1 << 7);
|
||||
|
||||
// IIS2MDC Definitions
|
||||
#define IIS2MDC_WHO_AM_I 0b01000000
|
||||
#define IIS2MDC_STATUS_REG_READY 0b00001111
|
||||
// CFG_REG_A
|
||||
#define COMP_TEMP_EN (1 << 7)
|
||||
#define MD_CONTINUOUS (0 << 0)
|
||||
#define ODR_100 ((1 << 3) | (1 << 2))
|
||||
// CFG_REG_B
|
||||
#define OFF_CANC (1 << 1)
|
||||
// CFG_REG_C
|
||||
#define BDU (1 << 4)
|
||||
|
||||
extern device::Device *IIS2MDC_I2C_interface(const I2CSPIDriverConfig &config);
|
||||
|
||||
class IIS2MDC : public I2CSPIDriver<IIS2MDC>
|
||||
namespace ST_IIS2MDC
|
||||
{
|
||||
public:
|
||||
IIS2MDC(device::Device *interface, const I2CSPIDriverConfig &config);
|
||||
virtual ~IIS2MDC();
|
||||
static constexpr uint32_t I2C_SPEED = 400 * 1000; // 400 kHz I2C serial interface
|
||||
static constexpr uint8_t I2C_ADDRESS_DEFAULT = 0b001'1110;
|
||||
|
||||
struct SensorData {
|
||||
uint8_t xout0;
|
||||
uint8_t xout1;
|
||||
uint8_t yout0;
|
||||
uint8_t yout1;
|
||||
uint8_t zout0;
|
||||
uint8_t zout1;
|
||||
uint8_t tout0;
|
||||
uint8_t tout1;
|
||||
};
|
||||
static constexpr uint8_t Device_ID = 0b0100'0000;
|
||||
|
||||
static I2CSPIDriverBase *instantiate(const I2CSPIDriverConfig &config, int runtime_instance);
|
||||
static void print_usage();
|
||||
enum class Register : uint8_t {
|
||||
WHO_AM_I = 0x4F,
|
||||
|
||||
int init();
|
||||
void print_status() override;
|
||||
CFG_REG_A = 0x60,
|
||||
CFG_REG_B = 0x61,
|
||||
CFG_REG_C = 0x62,
|
||||
|
||||
void RunImpl();
|
||||
|
||||
private:
|
||||
uint8_t read_register_block(SensorData *data);
|
||||
uint8_t read_register(uint8_t reg);
|
||||
void write_register(uint8_t reg, uint8_t value);
|
||||
|
||||
device::Device *_interface;
|
||||
PX4Magnetometer _px4_mag;
|
||||
perf_counter_t _sample_count;
|
||||
perf_counter_t _comms_errors;
|
||||
STATUS_REG = 0x67,
|
||||
};
|
||||
|
||||
// CFG_REG_A
|
||||
enum CFG_REG_A_BIT : uint8_t {
|
||||
COMP_TEMP_EN = Bit7, // internal temperature sensor
|
||||
REBOOT = Bit6,
|
||||
SOFT_RST = Bit5,
|
||||
LP = Bit4, // Low-Power mode
|
||||
|
||||
// 3:2 ODR: 10 50 Hz
|
||||
ODR_50_HZ_SET = Bit3, // ODR1: 1
|
||||
ODR_50_HZ_CLEAR = Bit2, // ODR0: 0
|
||||
|
||||
// 1:0 MD: 00 Continuous mode
|
||||
MD_CONTINUOUS_CLEAR = Bit1 | Bit0, // MD1: 0, MD0: 0 Continuous mode
|
||||
};
|
||||
|
||||
// CFG_REG_B
|
||||
enum CFG_REG_B_BIT : uint8_t {
|
||||
OFF_CANC = Bit1, // offset cancellation
|
||||
LPF = Bit0, // digital low-pass filter
|
||||
};
|
||||
|
||||
// CFG_REG_C
|
||||
enum CFG_REG_C_BIT : uint8_t {
|
||||
I2C_DIS = Bit5,
|
||||
BDU = Bit4,
|
||||
BLE = Bit3, // 1: Big Endian
|
||||
};
|
||||
|
||||
// STATUS_REG
|
||||
enum STATUS_REG_BIT : uint8_t {
|
||||
Zyxor = Bit7, // X, Y, Z axis data overrun
|
||||
|
||||
Zyxda = Bit3, // X, Y, Z new data available
|
||||
};
|
||||
|
||||
|
||||
} // namespace ST_IIS2MDC
|
||||
@@ -1,137 +0,0 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2024 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
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
#include "iis2mdc.h"
|
||||
|
||||
using namespace time_literals;
|
||||
|
||||
IIS2MDC::IIS2MDC(device::Device *interface, const I2CSPIDriverConfig &config) :
|
||||
I2CSPIDriver(config),
|
||||
_interface(interface),
|
||||
_px4_mag(interface->get_device_id(), config.rotation),
|
||||
_sample_count(perf_alloc(PC_COUNT, "iis2mdc_read")),
|
||||
_comms_errors(perf_alloc(PC_COUNT, "iis2mdc_comms_errors"))
|
||||
{}
|
||||
|
||||
IIS2MDC::~IIS2MDC()
|
||||
{
|
||||
perf_free(_sample_count);
|
||||
perf_free(_comms_errors);
|
||||
delete _interface;
|
||||
}
|
||||
|
||||
int IIS2MDC::init()
|
||||
{
|
||||
if (hrt_absolute_time() < 20_ms) {
|
||||
px4_usleep(20_ms); // ~10ms power-on time
|
||||
}
|
||||
|
||||
write_register(IIS2MDC_ADDR_CFG_REG_A, MD_CONTINUOUS | ODR_100 | COMP_TEMP_EN);
|
||||
write_register(IIS2MDC_ADDR_CFG_REG_B, OFF_CANC);
|
||||
write_register(IIS2MDC_ADDR_CFG_REG_C, BDU);
|
||||
|
||||
_px4_mag.set_scale(100.f / 65535.f); // +/- 50 Gauss, 16bit
|
||||
|
||||
ScheduleDelayed(20_ms);
|
||||
|
||||
return PX4_OK;
|
||||
}
|
||||
|
||||
void IIS2MDC::RunImpl()
|
||||
{
|
||||
uint8_t status = read_register(IIS2MDC_ADDR_STATUS_REG);
|
||||
|
||||
if (status & IIS2MDC_STATUS_REG_READY) {
|
||||
SensorData data = {};
|
||||
|
||||
if (read_register_block(&data) == PX4_OK) {
|
||||
int16_t x = int16_t((data.xout1 << 8) | data.xout0);
|
||||
int16_t y = int16_t((data.yout1 << 8) | data.yout0);
|
||||
int16_t z = -int16_t((data.zout1 << 8) | data.zout0);
|
||||
int16_t t = int16_t((data.tout1 << 8) | data.tout0);
|
||||
// 16 bits twos complement with a sensitivity of 8 LSB/°C. Typically, the output zero level corresponds to 25 °C.
|
||||
_px4_mag.set_temperature(float(t) / 8.f + 25.f);
|
||||
_px4_mag.update(hrt_absolute_time(), x, y, z);
|
||||
_px4_mag.set_error_count(perf_event_count(_comms_errors));
|
||||
perf_count(_sample_count);
|
||||
|
||||
} else {
|
||||
PX4_DEBUG("read failed");
|
||||
perf_count(_comms_errors);
|
||||
}
|
||||
|
||||
} else {
|
||||
PX4_DEBUG("not ready: %u", status);
|
||||
perf_count(_comms_errors);
|
||||
}
|
||||
|
||||
ScheduleDelayed(10_ms);
|
||||
}
|
||||
|
||||
uint8_t IIS2MDC::read_register_block(SensorData *data)
|
||||
{
|
||||
uint8_t reg = IIS2MDC_ADDR_OUTX_L_REG;
|
||||
|
||||
if (_interface->read(reg, data, sizeof(SensorData)) != PX4_OK) {
|
||||
perf_count(_comms_errors);
|
||||
|
||||
return PX4_ERROR;
|
||||
}
|
||||
|
||||
return PX4_OK;
|
||||
}
|
||||
|
||||
uint8_t IIS2MDC::read_register(uint8_t reg)
|
||||
{
|
||||
uint8_t value = 0;
|
||||
|
||||
if (_interface->read(reg, &value, sizeof(value)) != PX4_OK) {
|
||||
perf_count(_comms_errors);
|
||||
}
|
||||
|
||||
return value;
|
||||
}
|
||||
|
||||
void IIS2MDC::write_register(uint8_t reg, uint8_t value)
|
||||
{
|
||||
if (_interface->write(reg, &value, sizeof(value)) != PX4_OK) {
|
||||
perf_count(_comms_errors);
|
||||
}
|
||||
}
|
||||
|
||||
void IIS2MDC::print_status()
|
||||
{
|
||||
I2CSPIDriverBase::print_status();
|
||||
perf_print_counter(_sample_count);
|
||||
perf_print_counter(_comms_errors);
|
||||
}
|
||||
@@ -1,97 +0,0 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2024 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
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
#include "iis2mdc.h"
|
||||
#include <drivers/device/i2c.h>
|
||||
|
||||
class IIS2MDC_I2C : public device::I2C
|
||||
{
|
||||
public:
|
||||
IIS2MDC_I2C(const I2CSPIDriverConfig &config);
|
||||
virtual ~IIS2MDC_I2C() = default;
|
||||
|
||||
virtual int read(unsigned address, void *data, unsigned count) override;
|
||||
virtual int write(unsigned address, void *data, unsigned count) override;
|
||||
|
||||
protected:
|
||||
virtual int probe();
|
||||
};
|
||||
|
||||
IIS2MDC_I2C::IIS2MDC_I2C(const I2CSPIDriverConfig &config) :
|
||||
I2C(config)
|
||||
{
|
||||
}
|
||||
|
||||
int IIS2MDC_I2C::probe()
|
||||
{
|
||||
uint8_t data = 0;
|
||||
|
||||
if (read(IIS2MDC_ADDR_WHO_AM_I, &data, 1)) {
|
||||
DEVICE_DEBUG("read_reg fail");
|
||||
return -EIO;
|
||||
}
|
||||
|
||||
if (data != IIS2MDC_WHO_AM_I) {
|
||||
DEVICE_DEBUG("IIS2MDC bad ID: %02x", data);
|
||||
return -EIO;
|
||||
}
|
||||
|
||||
_retries = 1;
|
||||
|
||||
return OK;
|
||||
}
|
||||
|
||||
int IIS2MDC_I2C::read(unsigned address, void *data, unsigned count)
|
||||
{
|
||||
uint8_t cmd = address;
|
||||
return transfer(&cmd, 1, (uint8_t *)data, count);
|
||||
}
|
||||
|
||||
int IIS2MDC_I2C::write(unsigned address, void *data, unsigned count)
|
||||
{
|
||||
uint8_t buf[32];
|
||||
|
||||
if (sizeof(buf) < (count + 1)) {
|
||||
return -EIO;
|
||||
}
|
||||
|
||||
buf[0] = address;
|
||||
memcpy(&buf[1], data, count);
|
||||
|
||||
return transfer(&buf[0], count + 1, nullptr, 0);
|
||||
}
|
||||
|
||||
device::Device *IIS2MDC_I2C_interface(const I2CSPIDriverConfig &config)
|
||||
{
|
||||
return new IIS2MDC_I2C(config);
|
||||
}
|
||||
@@ -1,6 +1,6 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2024 PX4 Development Team. All rights reserved.
|
||||
* Copyright (c) 2024-2025 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
|
||||
@@ -31,56 +31,29 @@
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
#include "iis2mdc.h"
|
||||
#include "IIS2MDC.hpp"
|
||||
|
||||
#include <px4_platform_common/getopt.h>
|
||||
#include <px4_platform_common/module.h>
|
||||
|
||||
I2CSPIDriverBase *IIS2MDC::instantiate(const I2CSPIDriverConfig &config, int runtime_instance)
|
||||
{
|
||||
device::Device *interface = IIS2MDC_I2C_interface(config);
|
||||
|
||||
if (interface == nullptr) {
|
||||
PX4_ERR("alloc failed");
|
||||
return nullptr;
|
||||
}
|
||||
|
||||
if (interface->init() != OK) {
|
||||
delete interface;
|
||||
PX4_DEBUG("no device on bus %i (devid 0x%lx)", config.bus, config.spi_devid);
|
||||
return nullptr;
|
||||
}
|
||||
|
||||
IIS2MDC *dev = new IIS2MDC(interface, config);
|
||||
|
||||
if (dev == nullptr) {
|
||||
delete interface;
|
||||
return nullptr;
|
||||
}
|
||||
|
||||
if (OK != dev->init()) {
|
||||
delete dev;
|
||||
return nullptr;
|
||||
}
|
||||
|
||||
return dev;
|
||||
}
|
||||
|
||||
void IIS2MDC::print_usage()
|
||||
{
|
||||
PRINT_MODULE_USAGE_NAME("iis2mdc", "driver");
|
||||
PRINT_MODULE_USAGE_SUBCATEGORY("magnetometer");
|
||||
PRINT_MODULE_USAGE_COMMAND("start");
|
||||
PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(true, false);
|
||||
PRINT_MODULE_USAGE_PARAMS_I2C_ADDRESS(0x30);
|
||||
PRINT_MODULE_USAGE_PARAMS_I2C_ADDRESS(0x1E);
|
||||
PRINT_MODULE_USAGE_PARAM_INT('R', 0, 0, 35, "Rotation", true);
|
||||
PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
|
||||
}
|
||||
|
||||
extern "C" int iis2mdc_main(int argc, char *argv[])
|
||||
{
|
||||
using ThisDriver = IIS2MDC;
|
||||
int ch;
|
||||
using ThisDriver = IIS2MDC;
|
||||
BusCLIArguments cli{true, false};
|
||||
cli.i2c_address = 0x1E;
|
||||
cli.default_i2c_frequency = 400000;
|
||||
cli.i2c_address = I2C_ADDRESS_DEFAULT;
|
||||
cli.default_i2c_frequency = I2C_SPEED;
|
||||
|
||||
while ((ch = cli.getOpt(argc, argv, "R:")) != EOF) {
|
||||
switch (ch) {
|
||||
|
||||
Reference in New Issue
Block a user