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

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),
+	_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)
 {
 	_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(_reset_perf);
-	perf_free(_comms_errors);
+	perf_free(_range_error_perf);
-	perf_free(_range_errors);
+	perf_free(_bad_transfer_perf);
-	perf_free(_conf_errors);
 
 	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 */
+		uint8_t status = 0;
-		if (!check_measurement()) {
+		ret = _interface->read(ADDR_STATUS, &status, 1);
-			/* Check BIST register to evaluate the test result*/
+
+		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 */
+				// If the test passed, disable self-test mode by clearing the STE bit
-				ret = !(cmd & BIST_XYZ_OK);
+				if (cmd & BIST_XYZ_OK) {
-
-				if (!ret) {
 					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;
+					return PX4_OK;
-					break;
 
 				} 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;
-}
-
-int RM3100::check_measurement()
-{
-	uint8_t status = 0;
-	int ret = _interface->read(ADDR_STATUS, &status, 1);
-
-	if (ret != 0) {
-		return ret;
 	}
 
-	return !(status & STATUS_DRDY);
+	return PX4_ERROR;
 }
 
-int RM3100::collect()
+void RM3100::RunImpl()
 {
-	/* Check whether a measurement is available or not, otherwise return immediately */
+	// full reset if things are failing consistently
-	if (check_measurement() != 0) {
+	if (_failure_count > 10) {
-		PX4_DEBUG("No measurement available");
+		_failure_count = 0;
-		return 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(_bad_transfer_perf);
-		perf_count(_comms_errors);
+		_failure_count++;
-		return ret;
+		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;
+	if (set_default_register_values() == PX4_OK) {
-	start();
+		ScheduleOnInterval(RM3100_INTERVAL);
-
+		return PX4_OK;
-	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);
 	}
 
-	return ret;
+	return PX4_ERROR;
 }
 
 void RM3100::print_status()
 {
 	I2CSPIDriverBase::print_status();
-	perf_print_counter(_sample_perf);
+	perf_print_counter(_reset_perf);
-	perf_print_counter(_comms_errors);
+	perf_print_counter(_range_error_perf);
-	PX4_INFO("poll interval:  %u", _measure_interval);
+	perf_print_counter(_bad_transfer_perf);
-}
-
-int RM3100::reset()
-{
-	int ret = set_default_register_values();
-
-	if (ret != OK) {
-		return PX4_ERROR;
-	}
-
-	return PX4_OK;
 }
 
 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();
-}

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_INTERVAL			13000	// 13000 Microseconds, corresponds to ~75 Hz (TMRC 0x95)
-#define RM3100_CONVERSION_INTERVAL	10000	// Microseconds, corresponds to 100 Hz (cycle count 200 on 3 axis)
 #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 TMRC_DEFAULT		0x95 // ~13 ms, ~75 Hz
-#define BIST_SELFTEST		0x8F
+#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);
 
-	/**
+	PX4Magnetometer _px4_mag;
-	 * Issue a measurement command.
-	 *
-	 * @return              OK if the measurement command was successful.
-	 */
-	int measure();
 
-	/**
+	device::Device *_interface{nullptr};
-	 * @brief Resets the device
-	 */
-	int reset();
 
-	/**
+	perf_counter_t _reset_perf{perf_alloc(PC_COUNT, MODULE_NAME": reset")};
-	 * @brief Initialises the automatic measurement state machine and start it.
+	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")};
-	 * @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.
+	int32_t _raw_data_prev[3] {};
-	 */
-	void start();
 
-}; // class RM3100
+	int _failure_count{0};
+
+};

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;
 }