diff --git a/src/drivers/magnetometer/rm3100/rm3100.cpp b/src/drivers/magnetometer/rm3100/rm3100.cpp index 0fae2784e7..b360c394c1 100644 --- a/src/drivers/magnetometer/rm3100/rm3100.cpp +++ b/src/drivers/magnetometer/rm3100/rm3100.cpp @@ -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"); - } - - /* 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; + PX4_ERR("Failed to disable built-in self test"); } - 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"); - } - - return pass; -} - -int RM3100::check_measurement() -{ - uint8_t status = 0; - int ret = _interface->read(ADDR_STATUS, &status, 1); - - if (ret != 0) { - return ret; + PX4_ERR("built-in self test incomplete"); } - return !(status & STATUS_DRDY); + return PX4_ERROR; } -int RM3100::collect() +void RM3100::RunImpl() { - /* 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) { + + _failure_count++; - ret = OK; + perf_count(_range_error_perf); + return; + } + + // 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; - return ret; + 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 (ret != OK) { - perf_count(_comms_errors); + if (set_default_register_values() == PX4_OK) { + ScheduleOnInterval(RM3100_INTERVAL); + return PX4_OK; } - 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(); -} diff --git a/src/drivers/magnetometer/rm3100/rm3100.h b/src/drivers/magnetometer/rm3100/rm3100.h index d3d058a436..7e4737d48b 100644 --- a/src/drivers/magnetometer/rm3100/rm3100.h +++ b/src/drivers/magnetometer/rm3100/rm3100.h @@ -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 @@ -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")}; + + int32_t _raw_data_prev[3] {}; -}; // class RM3100 + int _failure_count{0}; + +}; diff --git a/src/drivers/magnetometer/rm3100/rm3100_main.cpp b/src/drivers/magnetometer/rm3100/rm3100_main.cpp index c63da901bb..997c6f2243 100644 --- a/src/drivers/magnetometer/rm3100/rm3100_main.cpp +++ b/src/drivers/magnetometer/rm3100/rm3100_main.cpp @@ -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; }