mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-04-14 10:07:39 +08:00
drivers/magnetometer/rm3100: cleanup and simplify (#19238)
* switch to continuous mode at 75 Hz * add simple failure count and reset mechanism * add range checks and perf counts
This commit is contained in:
Daniel Agar
GitHub
parent
976c994156
commit
710185d2ad
@ -1,6 +1,6 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2018 PX4 Development Team. All rights reserved.
|
||||
* Copyright (c) 2018-2022 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
|
||||
@ -44,15 +44,7 @@
|
||||
RM3100::RM3100(device::Device *interface, const I2CSPIDriverConfig &config) :
|
||||
I2CSPIDriver(config),
|
||||
_px4_mag(interface->get_device_id(), config.rotation),
|
||||
_interface(interface),
|
||||
_comms_errors(perf_alloc(PC_COUNT, MODULE_NAME": comms_errors")),
|
||||
_conf_errors(perf_alloc(PC_COUNT, MODULE_NAME": conf_errors")),
|
||||
_range_errors(perf_alloc(PC_COUNT, MODULE_NAME": range_errors")),
|
||||
_sample_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": read")),
|
||||
_continuous_mode_set(false),
|
||||
_mode(SINGLE),
|
||||
_measure_interval(0),
|
||||
_check_state_cnt(0)
|
||||
_interface(interface)
|
||||
{
|
||||
_px4_mag.set_scale(1.f / (RM3100_SENSITIVITY * UTESLA_TO_GAUSS));
|
||||
}
|
||||
@ -60,22 +52,25 @@ RM3100::RM3100(device::Device *interface, const I2CSPIDriverConfig &config) :
|
||||
RM3100::~RM3100()
|
||||
{
|
||||
// free perf counters
|
||||
perf_free(_sample_perf);
|
||||
perf_free(_comms_errors);
|
||||
perf_free(_range_errors);
|
||||
perf_free(_conf_errors);
|
||||
perf_free(_reset_perf);
|
||||
perf_free(_range_error_perf);
|
||||
perf_free(_bad_transfer_perf);
|
||||
|
||||
delete _interface;
|
||||
}
|
||||
|
||||
int RM3100::self_test()
|
||||
{
|
||||
int ret = 0;
|
||||
uint8_t cmd = 0;
|
||||
bool complete = false;
|
||||
int pass = PX4_ERROR;
|
||||
|
||||
/* Configure sensor to execute BIST upon receipt of a POLL command */
|
||||
uint8_t cmd = (CMM_DEFAULT | POLLING_MODE);
|
||||
int ret = _interface->write(ADDR_CMM, &cmd, 1);
|
||||
|
||||
if (ret != PX4_OK) {
|
||||
return ret;
|
||||
}
|
||||
|
||||
// Configure sensor to execute BIST upon receipt of a POLL command
|
||||
cmd = BIST_SELFTEST;
|
||||
ret = _interface->write(ADDR_BIST, &cmd, 1);
|
||||
|
||||
@ -88,7 +83,7 @@ int RM3100::self_test()
|
||||
|
||||
while ((hrt_absolute_time() - t_start) < BIST_DUR_USEC) {
|
||||
|
||||
/* Re-disable DRDY clear */
|
||||
// Re-disable DRDY clear
|
||||
cmd = HSHAKE_NO_DRDY_CLEAR;
|
||||
ret = _interface->write(ADDR_HSHAKE, &cmd, 1);
|
||||
|
||||
@ -96,7 +91,7 @@ int RM3100::self_test()
|
||||
return ret;
|
||||
}
|
||||
|
||||
/* Poll for a measurement */
|
||||
// Poll for a measurement
|
||||
cmd = POLL_XYZ;
|
||||
ret = _interface->write(ADDR_POLL, &cmd, 1);
|
||||
|
||||
@ -104,73 +99,59 @@ int RM3100::self_test()
|
||||
return ret;
|
||||
}
|
||||
|
||||
/* If the DRDY bit in the status register is set, BIST should be complete */
|
||||
if (!check_measurement()) {
|
||||
/* Check BIST register to evaluate the test result*/
|
||||
uint8_t status = 0;
|
||||
ret = _interface->read(ADDR_STATUS, &status, 1);
|
||||
|
||||
if (ret != PX4_OK) {
|
||||
return ret;
|
||||
}
|
||||
|
||||
// If the DRDY bit in the status register is set, BIST should be complete
|
||||
if (status & STATUS_DRDY) {
|
||||
// Check BIST register to evaluate the test result
|
||||
ret = _interface->read(ADDR_BIST, &cmd, 1);
|
||||
|
||||
if (ret != PX4_OK) {
|
||||
return ret;
|
||||
}
|
||||
|
||||
/* The test results are not valid if STE is not set. In this case, we try again */
|
||||
// The test results are not valid if STE is not set. In this case, we try again
|
||||
if (cmd & BIST_STE) {
|
||||
complete = true;
|
||||
|
||||
/* If the test passed, disable self-test mode by clearing the STE bit */
|
||||
ret = !(cmd & BIST_XYZ_OK);
|
||||
|
||||
if (!ret) {
|
||||
// If the test passed, disable self-test mode by clearing the STE bit
|
||||
if (cmd & BIST_XYZ_OK) {
|
||||
cmd = 0;
|
||||
ret = _interface->write(ADDR_BIST, &cmd, 1);
|
||||
|
||||
if (ret != PX4_OK) {
|
||||
PX4_ERR("Failed to disable BIST");
|
||||
PX4_ERR("Failed to disable built-in self test");
|
||||
}
|
||||
|
||||
/* Re-enable DRDY clear upon register writes and measurements */
|
||||
cmd = HSHAKE_DEFAULT;
|
||||
ret = _interface->write(ADDR_HSHAKE, &cmd, 1);
|
||||
|
||||
if (ret != PX4_OK) {
|
||||
return ret;
|
||||
}
|
||||
|
||||
pass = PX4_OK;
|
||||
break;
|
||||
return PX4_OK;
|
||||
|
||||
} else {
|
||||
PX4_ERR("BIST failed");
|
||||
PX4_ERR("built-in self test failed");
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if (!complete) {
|
||||
PX4_ERR("BIST incomplete");
|
||||
PX4_ERR("built-in self test incomplete");
|
||||
}
|
||||
|
||||
return pass;
|
||||
return PX4_ERROR;
|
||||
}
|
||||
|
||||
int RM3100::check_measurement()
|
||||
void RM3100::RunImpl()
|
||||
{
|
||||
uint8_t status = 0;
|
||||
int ret = _interface->read(ADDR_STATUS, &status, 1);
|
||||
|
||||
if (ret != 0) {
|
||||
return ret;
|
||||
}
|
||||
|
||||
return !(status & STATUS_DRDY);
|
||||
}
|
||||
|
||||
int RM3100::collect()
|
||||
{
|
||||
/* Check whether a measurement is available or not, otherwise return immediately */
|
||||
if (check_measurement() != 0) {
|
||||
PX4_DEBUG("No measurement available");
|
||||
return 0;
|
||||
// full reset if things are failing consistently
|
||||
if (_failure_count > 10) {
|
||||
_failure_count = 0;
|
||||
set_default_register_values();
|
||||
ScheduleOnInterval(RM3100_INTERVAL);
|
||||
return;
|
||||
}
|
||||
|
||||
struct {
|
||||
@ -179,21 +160,15 @@ int RM3100::collect()
|
||||
uint8_t z[3];
|
||||
} rm_report{};
|
||||
|
||||
_px4_mag.set_error_count(perf_event_count(_comms_errors));
|
||||
|
||||
perf_begin(_sample_perf);
|
||||
|
||||
const hrt_abstime timestamp_sample = hrt_absolute_time();
|
||||
int ret = _interface->read(ADDR_MX, (uint8_t *)&rm_report, sizeof(rm_report));
|
||||
|
||||
if (ret != OK) {
|
||||
perf_end(_sample_perf);
|
||||
perf_count(_comms_errors);
|
||||
return ret;
|
||||
perf_count(_bad_transfer_perf);
|
||||
_failure_count++;
|
||||
return;
|
||||
}
|
||||
|
||||
perf_end(_sample_perf);
|
||||
|
||||
/* Rearrange mag data */
|
||||
int32_t xraw = ((rm_report.x[0] << 16) | (rm_report.x[1] << 8) | rm_report.x[2]);
|
||||
int32_t yraw = ((rm_report.y[0] << 16) | (rm_report.y[1] << 8) | rm_report.y[2]);
|
||||
@ -204,12 +179,36 @@ int RM3100::collect()
|
||||
convert_signed(&yraw);
|
||||
convert_signed(&zraw);
|
||||
|
||||
_px4_mag.update(timestamp_sample, xraw, yraw, zraw);
|
||||
// valid range: -8388608 to 8388607
|
||||
if (xraw < -8388608 || xraw > 8388607 ||
|
||||
yraw < -8388608 || yraw > 8388607 ||
|
||||
zraw < -8388608 || zraw > 8388607) {
|
||||
|
||||
ret = OK;
|
||||
_failure_count++;
|
||||
|
||||
perf_count(_range_error_perf);
|
||||
return;
|
||||
}
|
||||
|
||||
return ret;
|
||||
// only publish changes
|
||||
if (_raw_data_prev[0] != xraw || _raw_data_prev[1] != yraw || _raw_data_prev[2] != zraw) {
|
||||
|
||||
_px4_mag.set_error_count(perf_event_count(_bad_transfer_perf)
|
||||
+ perf_event_count(_range_error_perf));
|
||||
|
||||
_px4_mag.update(timestamp_sample, xraw, yraw, zraw);
|
||||
|
||||
_raw_data_prev[0] = xraw;
|
||||
_raw_data_prev[1] = yraw;
|
||||
_raw_data_prev[2] = zraw;
|
||||
|
||||
if (_failure_count > 0) {
|
||||
_failure_count--;
|
||||
}
|
||||
|
||||
} else {
|
||||
_failure_count++;
|
||||
}
|
||||
}
|
||||
|
||||
void RM3100::convert_signed(int32_t *n)
|
||||
@ -220,106 +219,34 @@ void RM3100::convert_signed(int32_t *n)
|
||||
}
|
||||
}
|
||||
|
||||
void RM3100::RunImpl()
|
||||
{
|
||||
/* _measure_interval == 0 is used as _task_should_exit */
|
||||
if (_measure_interval == 0) {
|
||||
return;
|
||||
}
|
||||
|
||||
/* Collect last measurement at the start of every cycle */
|
||||
if (collect() != OK) {
|
||||
PX4_DEBUG("collection error");
|
||||
/* restart the measurement state machine */
|
||||
start();
|
||||
return;
|
||||
}
|
||||
|
||||
if (measure() != OK) {
|
||||
PX4_DEBUG("measure error");
|
||||
}
|
||||
|
||||
if (_measure_interval > 0) {
|
||||
/* schedule a fresh cycle call when the measurement is done */
|
||||
ScheduleDelayed(_measure_interval);
|
||||
}
|
||||
}
|
||||
|
||||
int RM3100::init()
|
||||
{
|
||||
/* reset the device configuration */
|
||||
reset();
|
||||
|
||||
int ret = self_test();
|
||||
|
||||
if (ret != PX4_OK) {
|
||||
PX4_ERR("self test failed");
|
||||
}
|
||||
|
||||
_measure_interval = RM3100_CONVERSION_INTERVAL;
|
||||
start();
|
||||
|
||||
return ret;
|
||||
}
|
||||
|
||||
int RM3100::measure()
|
||||
{
|
||||
int ret = 0;
|
||||
uint8_t cmd = 0;
|
||||
|
||||
/* Send the command to begin a measurement. */
|
||||
if ((_mode == CONTINUOUS) && !_continuous_mode_set) {
|
||||
cmd = (CMM_DEFAULT | CONTINUOUS_MODE);
|
||||
ret = _interface->write(ADDR_CMM, &cmd, 1);
|
||||
_continuous_mode_set = true;
|
||||
|
||||
} else if (_mode == SINGLE) {
|
||||
if (_continuous_mode_set) {
|
||||
/* This is needed for polling mode */
|
||||
cmd = (CMM_DEFAULT | POLLING_MODE);
|
||||
ret = _interface->write(ADDR_CMM, &cmd, 1);
|
||||
|
||||
if (ret != OK) {
|
||||
perf_count(_comms_errors);
|
||||
return ret;
|
||||
}
|
||||
|
||||
_continuous_mode_set = false;
|
||||
}
|
||||
|
||||
cmd = POLL_XYZ;
|
||||
ret = _interface->write(ADDR_POLL, &cmd, 1);
|
||||
if (set_default_register_values() == PX4_OK) {
|
||||
ScheduleOnInterval(RM3100_INTERVAL);
|
||||
return PX4_OK;
|
||||
}
|
||||
|
||||
|
||||
if (ret != OK) {
|
||||
perf_count(_comms_errors);
|
||||
}
|
||||
|
||||
return ret;
|
||||
return PX4_ERROR;
|
||||
}
|
||||
|
||||
void RM3100::print_status()
|
||||
{
|
||||
I2CSPIDriverBase::print_status();
|
||||
perf_print_counter(_sample_perf);
|
||||
perf_print_counter(_comms_errors);
|
||||
PX4_INFO("poll interval: %u", _measure_interval);
|
||||
}
|
||||
|
||||
int RM3100::reset()
|
||||
{
|
||||
int ret = set_default_register_values();
|
||||
|
||||
if (ret != OK) {
|
||||
return PX4_ERROR;
|
||||
}
|
||||
|
||||
return PX4_OK;
|
||||
perf_print_counter(_reset_perf);
|
||||
perf_print_counter(_range_error_perf);
|
||||
perf_print_counter(_bad_transfer_perf);
|
||||
}
|
||||
|
||||
int RM3100::set_default_register_values()
|
||||
{
|
||||
perf_count(_reset_perf);
|
||||
|
||||
uint8_t cmd[2] = {0, 0};
|
||||
|
||||
cmd[0] = CCX_DEFAULT_MSB;
|
||||
@ -345,11 +272,3 @@ int RM3100::set_default_register_values()
|
||||
|
||||
return PX4_OK;
|
||||
}
|
||||
|
||||
void RM3100::start()
|
||||
{
|
||||
set_default_register_values();
|
||||
|
||||
/* schedule a cycle to start things */
|
||||
ScheduleNow();
|
||||
}
|
||||
|
||||
@ -1,6 +1,6 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2018 PX4 Development Team. All rights reserved.
|
||||
* Copyright (c) 2018-2022 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
|
||||
@ -50,8 +50,7 @@
|
||||
* RM3100 internal constants and data structures.
|
||||
*/
|
||||
|
||||
/* At 146 Hz we encounter errors, 100 Hz is safer */
|
||||
#define RM3100_CONVERSION_INTERVAL 10000 // Microseconds, corresponds to 100 Hz (cycle count 200 on 3 axis)
|
||||
#define RM3100_INTERVAL 13000 // 13000 Microseconds, corresponds to ~75 Hz (TMRC 0x95)
|
||||
#define UTESLA_TO_GAUSS 100.0f
|
||||
#define RM3100_SENSITIVITY 75.0f
|
||||
|
||||
@ -75,32 +74,26 @@
|
||||
#define CCY_DEFAULT_LSB CCX_DEFAULT_LSB
|
||||
#define CCZ_DEFAULT_MSB CCX_DEFAULT_MSB
|
||||
#define CCZ_DEFAULT_LSB CCX_DEFAULT_LSB
|
||||
#define CMM_DEFAULT 0x70 // No continuous mode
|
||||
#define CMM_DEFAULT 0b0111'0001 // continuous mode
|
||||
#define CONTINUOUS_MODE (1 << 0)
|
||||
#define POLLING_MODE (0 << 0)
|
||||
#define TMRC_DEFAULT 0x94
|
||||
#define BIST_SELFTEST 0x8F
|
||||
#define TMRC_DEFAULT 0x95 // ~13 ms, ~75 Hz
|
||||
#define BIST_SELFTEST 0b1000'1111
|
||||
#define BIST_DEFAULT 0x00
|
||||
#define BIST_XYZ_OK ((1 << 4) | (1 << 5) | (1 << 6))
|
||||
#define BIST_STE (1 << 7)
|
||||
#define BIST_DUR_USEC (2*RM3100_CONVERSION_INTERVAL)
|
||||
#define BIST_DUR_USEC (2*RM3100_INTERVAL)
|
||||
#define HSHAKE_DEFAULT (0x0B)
|
||||
#define HSHAKE_NO_DRDY_CLEAR (0x08)
|
||||
#define STATUS_DRDY (1 << 7)
|
||||
#define POLL_XYZ 0x70
|
||||
#define RM3100_REVID 0x22
|
||||
|
||||
#define NUM_BUS_OPTIONS (sizeof(bus_options)/sizeof(bus_options[0]))
|
||||
#define RM3100_REVID 0x22
|
||||
|
||||
/* interface factories */
|
||||
extern device::Device *RM3100_SPI_interface(int bus, uint32_t devid, int bus_frequency, spi_mode_e spi_mode);
|
||||
extern device::Device *RM3100_I2C_interface(int bus, int bus_frequency);
|
||||
|
||||
enum OPERATING_MODE {
|
||||
CONTINUOUS = 0,
|
||||
SINGLE
|
||||
};
|
||||
|
||||
#define RM3100_ADDRESS 0x20
|
||||
|
||||
class RM3100 : public I2CSPIDriver<RM3100>
|
||||
@ -112,8 +105,6 @@ public:
|
||||
static I2CSPIDriverBase *instantiate(const I2CSPIDriverConfig &config, int runtime_instance);
|
||||
static void print_usage();
|
||||
|
||||
void custom_method(const BusCLIArguments &cli) override;
|
||||
|
||||
int init();
|
||||
|
||||
void print_status() override;
|
||||
@ -126,29 +117,6 @@ public:
|
||||
void RunImpl();
|
||||
|
||||
private:
|
||||
PX4Magnetometer _px4_mag;
|
||||
|
||||
device::Device *_interface;
|
||||
|
||||
perf_counter_t _comms_errors;
|
||||
perf_counter_t _conf_errors;
|
||||
perf_counter_t _range_errors;
|
||||
perf_counter_t _sample_perf;
|
||||
|
||||
/* status reporting */
|
||||
bool _continuous_mode_set;
|
||||
|
||||
enum OPERATING_MODE _mode;
|
||||
|
||||
unsigned int _measure_interval;
|
||||
|
||||
uint8_t _check_state_cnt;
|
||||
|
||||
/**
|
||||
* Collect the result of the most recent measurement.
|
||||
*/
|
||||
int collect();
|
||||
|
||||
/**
|
||||
* Run sensor self-test
|
||||
*
|
||||
@ -156,36 +124,21 @@ private:
|
||||
*/
|
||||
int self_test();
|
||||
|
||||
/**
|
||||
* Check whether new data is available or not
|
||||
*
|
||||
* @return 0 if new data is available, 1 else
|
||||
*/
|
||||
int check_measurement();
|
||||
|
||||
/**
|
||||
* Converts int24_t stored in 32-bit container to int32_t
|
||||
*/
|
||||
void convert_signed(int32_t *n);
|
||||
|
||||
/**
|
||||
* Issue a measurement command.
|
||||
*
|
||||
* @return OK if the measurement command was successful.
|
||||
*/
|
||||
int measure();
|
||||
PX4Magnetometer _px4_mag;
|
||||
|
||||
/**
|
||||
* @brief Resets the device
|
||||
*/
|
||||
int reset();
|
||||
device::Device *_interface{nullptr};
|
||||
|
||||
/**
|
||||
* @brief Initialises the automatic measurement state machine and start it.
|
||||
*
|
||||
* @note This function is called at open and error time. It might make sense
|
||||
* to make it more aggressive about resetting the bus in case of errors.
|
||||
*/
|
||||
void start();
|
||||
perf_counter_t _reset_perf{perf_alloc(PC_COUNT, MODULE_NAME": reset")};
|
||||
perf_counter_t _range_error_perf{perf_alloc(PC_COUNT, MODULE_NAME": range error")};
|
||||
perf_counter_t _bad_transfer_perf{perf_alloc(PC_COUNT, MODULE_NAME": bad transfer")};
|
||||
|
||||
}; // class RM3100
|
||||
int32_t _raw_data_prev[3] {};
|
||||
|
||||
int _failure_count{0};
|
||||
|
||||
};
|
||||
|
||||
@ -78,12 +78,6 @@ I2CSPIDriverBase *RM3100::instantiate(const I2CSPIDriverConfig &config, int runt
|
||||
return dev;
|
||||
}
|
||||
|
||||
void
|
||||
RM3100::custom_method(const BusCLIArguments &cli)
|
||||
{
|
||||
reset();
|
||||
}
|
||||
|
||||
void RM3100::print_usage()
|
||||
{
|
||||
PRINT_MODULE_USAGE_NAME("rm3100", "driver");
|
||||
@ -91,7 +85,6 @@ void RM3100::print_usage()
|
||||
PRINT_MODULE_USAGE_COMMAND("start");
|
||||
PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(true, true);
|
||||
PRINT_MODULE_USAGE_PARAM_INT('R', 0, 0, 35, "Rotation", true);
|
||||
PRINT_MODULE_USAGE_COMMAND("reset");
|
||||
PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
|
||||
}
|
||||
|
||||
@ -124,20 +117,14 @@ extern "C" int rm3100_main(int argc, char *argv[])
|
||||
|
||||
if (!strcmp(verb, "start")) {
|
||||
return ThisDriver::module_start(cli, iterator);
|
||||
}
|
||||
|
||||
if (!strcmp(verb, "stop")) {
|
||||
} else if (!strcmp(verb, "stop")) {
|
||||
return ThisDriver::module_stop(iterator);
|
||||
}
|
||||
|
||||
if (!strcmp(verb, "status")) {
|
||||
} else if (!strcmp(verb, "status")) {
|
||||
return ThisDriver::module_status(iterator);
|
||||
}
|
||||
|
||||
if (!strcmp(verb, "reset")) {
|
||||
return ThisDriver::module_custom_method(cli, iterator);
|
||||
}
|
||||
|
||||
ThisDriver::print_usage();
|
||||
return -1;
|
||||
}
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user