accel and gyro calibration refactor and cleanup

- remove all remaining IOCTLs for accel and gyro and handle all calibration entirely in sensors module with parameters
 - sensor_accel and sensor_gyro are now always raw sensor data
 - calibration procedures no longer need to first clear existing values before starting
 - temperature calibration (TC) remove all scale (SCL) parameters
    - gyro and baro scale are completely unused
    - regular accel calibration scale can be used (CAL_ACC*_xSCALE) instead of TC scale

src/lib/drivers/accelerometer/PX4Accelerometer.cpp

@@ -39,7 +39,7 @@
 using namespace time_literals;
 using matrix::Vector3f;
 
-static inline int32_t sum(const int16_t samples[16], uint8_t len)
+static constexpr int32_t sum(const int16_t samples[16], uint8_t len)
 {
 	int32_t sum = 0;
 
@@ -50,7 +50,7 @@ static inline int32_t sum(const int16_t samples[16], uint8_t len)
 	return sum;
 }
 
-static constexpr unsigned clipping(const int16_t samples[16], int16_t clip_limit, uint8_t len)
+static constexpr uint8_t clipping(const int16_t samples[16], int16_t clip_limit, uint8_t len)
 {
 	unsigned clip_count = 0;
 
@@ -64,15 +64,13 @@ static constexpr unsigned clipping(const int16_t samples[16], int16_t clip_limit
 }
 
 PX4Accelerometer::PX4Accelerometer(uint32_t device_id, ORB_PRIO priority, enum Rotation rotation) :
-	CDev(nullptr),
 	ModuleParams(nullptr),
 	_sensor_pub{ORB_ID(sensor_accel), priority},
 	_sensor_fifo_pub{ORB_ID(sensor_accel_fifo), priority},
 	_device_id{device_id},
 	_rotation{rotation}
 {
-	// register class and advertise immediately to keep instance numbering in sync
-	_class_device_instance = register_class_devname(ACCEL_BASE_DEVICE_PATH);
+	// advertise immediately to keep instance numbering in sync
 	_sensor_pub.advertise();
 
 	updateParams();
@@ -80,36 +78,10 @@ PX4Accelerometer::PX4Accelerometer(uint32_t device_id, ORB_PRIO priority, enum R
 
 PX4Accelerometer::~PX4Accelerometer()
 {
-	if (_class_device_instance != -1) {
-		unregister_class_devname(ACCEL_BASE_DEVICE_PATH, _class_device_instance);
-	}
-
 	_sensor_pub.unadvertise();
 	_sensor_fifo_pub.unadvertise();
 }
 
-int PX4Accelerometer::ioctl(cdev::file_t *filp, int cmd, unsigned long arg)
-{
-	switch (cmd) {
-	case ACCELIOCSSCALE: {
-			// Copy offsets and scale factors in
-			accel_calibration_s cal{};
-			memcpy(&cal, (accel_calibration_s *) arg, sizeof(cal));
-
-			_calibration_offset = Vector3f{cal.x_offset, cal.y_offset, cal.z_offset};
-			_calibration_scale = Vector3f{cal.x_scale, cal.y_scale, cal.z_scale};
-		}
-
-		return PX4_OK;
-
-	case DEVIOCGDEVICEID:
-		return _device_id;
-
-	default:
-		return -ENOTTY;
-	}
-}
-
 void PX4Accelerometer::set_device_type(uint8_t devtype)
 {
 	// current DeviceStructure
@@ -123,39 +95,16 @@ void PX4Accelerometer::set_device_type(uint8_t devtype)
 	_device_id = device_id.devid;
 }
 
-void PX4Accelerometer::update(hrt_abstime timestamp_sample, float x, float y, float z)
+void PX4Accelerometer::update(const hrt_abstime &timestamp_sample, float x, float y, float z)
 {
-	// Apply rotation (before scaling)
-	rotate_3f(_rotation, x, y, z);
-
-	const Vector3f raw{x, y, z};
-
 	// clipping
-	float clip_count_x = (fabsf(raw(0)) > _clip_limit);
-	float clip_count_y = (fabsf(raw(1)) > _clip_limit);
-	float clip_count_z = (fabsf(raw(2)) > _clip_limit);
-
-	rotate_3f(_rotation, clip_count_x, clip_count_y, clip_count_z);
-
-	// Apply range scale and the calibrating offset/scale
-	const Vector3f val_calibrated{(((raw * _scale) - _calibration_offset).emult(_calibration_scale))};
+	uint8_t clip_count[3];
+	clip_count[0] = (fabsf(x) >= _clip_limit);
+	clip_count[1] = (fabsf(y) >= _clip_limit);
+	clip_count[2] = (fabsf(z) >= _clip_limit);
 
 	// publish
-	sensor_accel_s report;
-
-	report.timestamp_sample = timestamp_sample;
-	report.device_id = _device_id;
-	report.temperature = _temperature;
-	report.error_count = _error_count;
-	report.x = val_calibrated(0);
-	report.y = val_calibrated(1);
-	report.z = val_calibrated(2);
-	report.clip_counter[0] = fabsf(roundf(clip_count_x));
-	report.clip_counter[1] = fabsf(roundf(clip_count_y));
-	report.clip_counter[2] = fabsf(roundf(clip_count_z));
-	report.timestamp = hrt_absolute_time();
-
-	_sensor_pub.publish(report);
+	Publish(timestamp_sample, x, y, z, clip_count);
 }
 
 void PX4Accelerometer::updateFIFO(const FIFOSample &sample)
@@ -165,7 +114,7 @@ void PX4Accelerometer::updateFIFO(const FIFOSample &sample)
 
 	{
 		// trapezoidal integration (equally spaced, scaled by dt later)
-		Vector3f integral{
+		const Vector3f integral{
 			(0.5f * (_last_sample[0] + sample.x[N - 1]) + sum(sample.x, N - 1)),
 			(0.5f * (_last_sample[1] + sample.y[N - 1]) + sum(sample.y, N - 1)),
 			(0.5f * (_last_sample[2] + sample.z[N - 1]) + sum(sample.z, N - 1)),
@@ -176,43 +125,20 @@ void PX4Accelerometer::updateFIFO(const FIFOSample &sample)
 		_last_sample[2] = sample.z[N - 1];
 
 		// clipping
-		float clip_count_x = clipping(sample.x, _clip_limit, N);
-		float clip_count_y = clipping(sample.y, _clip_limit, N);
-		float clip_count_z = clipping(sample.z, _clip_limit, N);
+		uint8_t clip_count[3] {
+			clipping(sample.x, _clip_limit, N),
+			clipping(sample.y, _clip_limit, N),
+			clipping(sample.z, _clip_limit, N),
+		};
 
-		rotate_3f(_rotation, clip_count_x, clip_count_y, clip_count_z);
-
-
-		// Apply rotation (before scaling)
-		rotate_3f(_rotation, integral(0), integral(1), integral(2));
-
-		// average
 		const float x = integral(0) / (float)N;
 		const float y = integral(1) / (float)N;
 		const float z = integral(2) / (float)N;
 
-		// Apply range scale and the calibration offset/scale
-		const Vector3f val_calibrated{((Vector3f{x, y, z} * _scale) - _calibration_offset).emult(_calibration_scale)};
-
 		// publish
-		sensor_accel_s report;
-
-		report.timestamp_sample = sample.timestamp_sample;
-		report.device_id = _device_id;
-		report.temperature = _temperature;
-		report.error_count = _error_count;
-		report.x = val_calibrated(0);
-		report.y = val_calibrated(1);
-		report.z = val_calibrated(2);
-		report.clip_counter[0] = fabsf(roundf(clip_count_x));
-		report.clip_counter[1] = fabsf(roundf(clip_count_y));
-		report.clip_counter[2] = fabsf(roundf(clip_count_z));
-		report.timestamp = hrt_absolute_time();
-
-		_sensor_pub.publish(report);
+		Publish(sample.timestamp_sample, x, y, z, clip_count);
 	}
 
-
 	// publish fifo
 	sensor_accel_fifo_s fifo{};
 
@@ -221,6 +147,7 @@ void PX4Accelerometer::updateFIFO(const FIFOSample &sample)
 	fifo.dt = dt;
 	fifo.scale = _scale;
 	fifo.samples = N;
+	fifo.rotation = _rotation;
 
 	memcpy(fifo.x, sample.x, sizeof(sample.x[0]) * N);
 	memcpy(fifo.y, sample.y, sizeof(sample.y[0]) * N);
@@ -230,20 +157,35 @@ void PX4Accelerometer::updateFIFO(const FIFOSample &sample)
 	_sensor_fifo_pub.publish(fifo);
 }
 
+void PX4Accelerometer::Publish(const hrt_abstime &timestamp_sample, float x, float y, float z, uint8_t clip_count[3])
+{
+	// Apply rotation (before scaling)
+	rotate_3f(_rotation, x, y, z);
+
+	float clipping_x = clip_count[0];
+	float clipping_y = clip_count[1];
+	float clipping_z = clip_count[2];
+	rotate_3f(_rotation, clipping_x, clipping_y, clipping_z);
+
+	sensor_accel_s report;
+
+	report.timestamp_sample = timestamp_sample;
+	report.device_id = _device_id;
+	report.temperature = _temperature;
+	report.error_count = _error_count;
+	report.x = x * _scale;
+	report.y = y * _scale;
+	report.z = z * _scale;
+	report.clip_counter[0] = fabsf(roundf(clipping_x));
+	report.clip_counter[1] = fabsf(roundf(clipping_y));
+	report.clip_counter[2] = fabsf(roundf(clipping_z));
+	report.timestamp = hrt_absolute_time();
+
+	_sensor_pub.publish(report);
+}
+
 void PX4Accelerometer::UpdateClipLimit()
 {
 	// 99.9% of potential max
 	_clip_limit = fmaxf((_range / _scale) * 0.999f, INT16_MAX);
 }
-
-void PX4Accelerometer::print_status()
-{
-#if !defined(CONSTRAINED_FLASH)
-	PX4_INFO(ACCEL_BASE_DEVICE_PATH " device instance: %d", _class_device_instance);
-
-	PX4_INFO("calibration scale: %.5f %.5f %.5f", (double)_calibration_scale(0), (double)_calibration_scale(1),
-		 (double)_calibration_scale(2));
-	PX4_INFO("calibration offset: %.5f %.5f %.5f", (double)_calibration_offset(0), (double)_calibration_offset(1),
-		 (double)_calibration_offset(2));
-#endif // !CONSTRAINED_FLASH
-}

src/lib/drivers/accelerometer/PX4Accelerometer.hpp

@@ -33,25 +33,20 @@
 
 #pragma once
 
-#include <drivers/drv_accel.h>
 #include <drivers/drv_hrt.h>
-#include <lib/cdev/CDev.hpp>
 #include <lib/conversion/rotation.h>
 #include <lib/ecl/geo/geo.h>
 #include <px4_platform_common/module_params.h>
 #include <uORB/PublicationMulti.hpp>
-#include <uORB/PublicationMulti.hpp>
 #include <uORB/topics/sensor_accel.h>
 #include <uORB/topics/sensor_accel_fifo.h>
 
-class PX4Accelerometer : public cdev::CDev, public ModuleParams
+class PX4Accelerometer : public ModuleParams
 {
 public:
 	PX4Accelerometer(uint32_t device_id, ORB_PRIO priority = ORB_PRIO_DEFAULT, enum Rotation rotation = ROTATION_NONE);
 	~PX4Accelerometer() override;
 
-	int	ioctl(cdev::file_t *filp, int cmd, unsigned long arg) override;
-
 	uint32_t get_device_id() const { return _device_id; }
 
 	float get_max_rate_hz() const { return _param_imu_gyro_rate_max.get(); }
@@ -64,9 +59,7 @@ public:
 	void set_scale(float scale) { _scale = scale; UpdateClipLimit(); }
 	void set_temperature(float temperature) { _temperature = temperature; }
 
-	void update(hrt_abstime timestamp_sample, float x, float y, float z);
-
-	void print_status();
+	void update(const hrt_abstime &timestamp_sample, float x, float y, float z);
 
 	struct FIFOSample {
 		hrt_abstime timestamp_sample;
@@ -84,15 +77,11 @@ public:
 	void updateFIFO(const FIFOSample &sample);
 
 private:
+	void Publish(const hrt_abstime &timestamp_sample, float x, float y, float z, uint8_t clip_count[3]);
 	void UpdateClipLimit();
 
-	uORB::PublicationQueuedMulti<sensor_accel_s>      _sensor_pub;
-	uORB::PublicationMulti<sensor_accel_fifo_s>       _sensor_fifo_pub;
-
-	matrix::Vector3f	_calibration_scale{1.f, 1.f, 1.f};
-	matrix::Vector3f	_calibration_offset{0.f, 0.f, 0.f};
-
-	int			_class_device_instance{-1};
+	uORB::PublicationQueuedMulti<sensor_accel_s> _sensor_pub;
+	uORB::PublicationMulti<sensor_accel_fifo_s>  _sensor_fifo_pub;
 
 	uint32_t		_device_id{0};
 	const enum Rotation	_rotation;

src/lib/drivers/barometer/PX4Barometer.cpp

@@ -79,11 +79,3 @@ PX4Barometer::update(hrt_abstime timestamp, float pressure)
 
 	_sensor_baro_pub.update();
 }
-
-void
-PX4Barometer::print_status()
-{
-	PX4_INFO(BARO_BASE_DEVICE_PATH " device instance: %d", _class_device_instance);
-
-	print_message(_sensor_baro_pub.get());
-}

src/lib/drivers/barometer/PX4Barometer.hpp

@@ -59,8 +59,6 @@ public:
 
 	int get_class_instance() { return _class_device_instance; };
 
-	void print_status();
-
 private:
 
 	uORB::PublicationMultiData<sensor_baro_s>	_sensor_baro_pub;

src/lib/drivers/gyroscope/PX4Gyroscope.cpp

@@ -39,7 +39,7 @@
 using namespace time_literals;
 using matrix::Vector3f;
 
-static inline int32_t sum(const int16_t samples[16], uint8_t len)
+static constexpr int32_t sum(const int16_t samples[16], uint8_t len)
 {
 	int32_t sum = 0;
 
@@ -51,15 +51,13 @@ static inline int32_t sum(const int16_t samples[16], uint8_t len)
 }
 
 PX4Gyroscope::PX4Gyroscope(uint32_t device_id, ORB_PRIO priority, enum Rotation rotation) :
-	CDev(nullptr),
 	ModuleParams(nullptr),
 	_sensor_pub{ORB_ID(sensor_gyro), priority},
 	_sensor_fifo_pub{ORB_ID(sensor_gyro_fifo), priority},
 	_device_id{device_id},
 	_rotation{rotation}
 {
-	// register class and advertise immediately to keep instance numbering in sync
-	_class_device_instance = register_class_devname(GYRO_BASE_DEVICE_PATH);
+	// advertise immediately to keep instance numbering in sync
 	_sensor_pub.advertise();
 
 	updateParams();
@@ -67,35 +65,10 @@ PX4Gyroscope::PX4Gyroscope(uint32_t device_id, ORB_PRIO priority, enum Rotation
 
 PX4Gyroscope::~PX4Gyroscope()
 {
-	if (_class_device_instance != -1) {
-		unregister_class_devname(GYRO_BASE_DEVICE_PATH, _class_device_instance);
-	}
-
 	_sensor_pub.unadvertise();
 	_sensor_fifo_pub.unadvertise();
 }
 
-int PX4Gyroscope::ioctl(cdev::file_t *filp, int cmd, unsigned long arg)
-{
-	switch (cmd) {
-	case GYROIOCSSCALE: {
-			// Copy offsets and scale factors in
-			gyro_calibration_s cal{};
-			memcpy(&cal, (gyro_calibration_s *) arg, sizeof(cal));
-
-			_calibration_offset = Vector3f{cal.x_offset, cal.y_offset, cal.z_offset};
-		}
-
-		return PX4_OK;
-
-	case DEVIOCGDEVICEID:
-		return _device_id;
-
-	default:
-		return -ENOTTY;
-	}
-}
-
 void PX4Gyroscope::set_device_type(uint8_t devtype)
 {
 	// current DeviceStructure
@@ -109,29 +82,10 @@ void PX4Gyroscope::set_device_type(uint8_t devtype)
 	_device_id = device_id.devid;
 }
 
-void PX4Gyroscope::update(hrt_abstime timestamp_sample, float x, float y, float z)
+void PX4Gyroscope::update(const hrt_abstime &timestamp_sample, float x, float y, float z)
 {
-	// Apply rotation (before scaling)
-	rotate_3f(_rotation, x, y, z);
-
-	const Vector3f raw{x, y, z};
-
-	// Apply range scale and the calibrating offset/scale
-	const Vector3f val_calibrated{((raw * _scale) - _calibration_offset)};
-
 	// publish
-	sensor_gyro_s report;
-
-	report.timestamp_sample = timestamp_sample;
-	report.device_id = _device_id;
-	report.temperature = _temperature;
-	report.error_count = _error_count;
-	report.x = val_calibrated(0);
-	report.y = val_calibrated(1);
-	report.z = val_calibrated(2);
-	report.timestamp = hrt_absolute_time();
-
-	_sensor_pub.publish(report);
+	Publish(timestamp_sample, x, y, z);
 }
 
 void PX4Gyroscope::updateFIFO(const FIFOSample &sample)
@@ -141,7 +95,7 @@ void PX4Gyroscope::updateFIFO(const FIFOSample &sample)
 
 	{
 		// trapezoidal integration (equally spaced, scaled by dt later)
-		Vector3f integral{
+		const Vector3f integral{
 			(0.5f * (_last_sample[0] + sample.x[N - 1]) + sum(sample.x, N - 1)),
 			(0.5f * (_last_sample[1] + sample.y[N - 1]) + sum(sample.y, N - 1)),
 			(0.5f * (_last_sample[2] + sample.z[N - 1]) + sum(sample.z, N - 1)),
@@ -151,33 +105,14 @@ void PX4Gyroscope::updateFIFO(const FIFOSample &sample)
 		_last_sample[1] = sample.y[N - 1];
 		_last_sample[2] = sample.z[N - 1];
 
-		// Apply rotation (before scaling)
-		rotate_3f(_rotation, integral(0), integral(1), integral(2));
-
-		// average
 		const float x = integral(0) / (float)N;
 		const float y = integral(1) / (float)N;
 		const float z = integral(2) / (float)N;
 
-		// Apply range scale and the calibration offset
-		const Vector3f val_calibrated{(Vector3f{x, y, z} * _scale) - _calibration_offset};
-
 		// publish
-		sensor_gyro_s report;
-
-		report.timestamp_sample = sample.timestamp_sample;
-		report.device_id = _device_id;
-		report.temperature = _temperature;
-		report.error_count = _error_count;
-		report.x = val_calibrated(0);
-		report.y = val_calibrated(1);
-		report.z = val_calibrated(2);
-		report.timestamp = hrt_absolute_time();
-
-		_sensor_pub.publish(report);
+		Publish(sample.timestamp_sample, x, y, z);
 	}
 
-
 	// publish fifo
 	sensor_gyro_fifo_s fifo{};
 
@@ -186,6 +121,7 @@ void PX4Gyroscope::updateFIFO(const FIFOSample &sample)
 	fifo.dt = dt;
 	fifo.scale = _scale;
 	fifo.samples = N;
+	fifo.rotation = _rotation;
 
 	memcpy(fifo.x, sample.x, sizeof(sample.x[0]) * N);
 	memcpy(fifo.y, sample.y, sizeof(sample.y[0]) * N);
@@ -195,12 +131,21 @@ void PX4Gyroscope::updateFIFO(const FIFOSample &sample)
 	_sensor_fifo_pub.publish(fifo);
 }
 
-void PX4Gyroscope::print_status()
+void PX4Gyroscope::Publish(const hrt_abstime &timestamp_sample, float x, float y, float z)
 {
-#if !defined(CONSTRAINED_FLASH)
-	PX4_INFO(GYRO_BASE_DEVICE_PATH " device instance: %d", _class_device_instance);
+	// Apply rotation (before scaling)
+	rotate_3f(_rotation, x, y, z);
 
-	PX4_INFO("calibration offset: %.5f %.5f %.5f", (double)_calibration_offset(0), (double)_calibration_offset(1),
-		 (double)_calibration_offset(2));
-#endif // !CONSTRAINED_FLASH
+	sensor_gyro_s report;
+
+	report.timestamp_sample = timestamp_sample;
+	report.device_id = _device_id;
+	report.temperature = _temperature;
+	report.error_count = _error_count;
+	report.x = x * _scale;
+	report.y = y * _scale;
+	report.z = z * _scale;
+	report.timestamp = hrt_absolute_time();
+
+	_sensor_pub.publish(report);
 }

src/lib/drivers/gyroscope/PX4Gyroscope.hpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2018 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2018-2020 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,24 +33,19 @@
 
 #pragma once
 
-#include <drivers/drv_gyro.h>
 #include <drivers/drv_hrt.h>
-#include <lib/cdev/CDev.hpp>
 #include <lib/conversion/rotation.h>
 #include <px4_platform_common/module_params.h>
 #include <uORB/PublicationMulti.hpp>
-#include <uORB/PublicationMulti.hpp>
 #include <uORB/topics/sensor_gyro.h>
 #include <uORB/topics/sensor_gyro_fifo.h>
 
-class PX4Gyroscope : public cdev::CDev, public ModuleParams
+class PX4Gyroscope : public ModuleParams
 {
 public:
 	PX4Gyroscope(uint32_t device_id, ORB_PRIO priority = ORB_PRIO_DEFAULT, enum Rotation rotation = ROTATION_NONE);
 	~PX4Gyroscope() override;
 
-	int	ioctl(cdev::file_t *filp, int cmd, unsigned long arg) override;
-
 	uint32_t get_device_id() const { return _device_id; }
 
 	float get_max_rate_hz() const { return _param_imu_gyro_rate_max.get(); }
@@ -63,9 +58,7 @@ public:
 	void set_scale(float scale) { _scale = scale; }
 	void set_temperature(float temperature) { _temperature = temperature; }
 
-	void update(hrt_abstime timestamp_sample, float x, float y, float z);
-
-	void print_status();
+	void update(const hrt_abstime &timestamp_sample, float x, float y, float z);
 
 	struct FIFOSample {
 		hrt_abstime timestamp_sample;
@@ -83,12 +76,10 @@ public:
 	void updateFIFO(const FIFOSample &sample);
 
 private:
-	uORB::PublicationQueuedMulti<sensor_gyro_s>      _sensor_pub;
-	uORB::PublicationMulti<sensor_gyro_fifo_s>       _sensor_fifo_pub;
+	void Publish(const hrt_abstime &timestamp_sample, float x, float y, float z);
 
-	matrix::Vector3f	_calibration_offset{0.f, 0.f, 0.f};
-
-	int			_class_device_instance{-1};
+	uORB::PublicationQueuedMulti<sensor_gyro_s> _sensor_pub;
+	uORB::PublicationMulti<sensor_gyro_fifo_s>  _sensor_fifo_pub;
 
 	uint32_t		_device_id{0};
 	const enum Rotation	_rotation;

src/lib/drivers/rangefinder/PX4Rangefinder.cpp

@@ -86,7 +86,3 @@ void PX4Rangefinder::update(const hrt_abstime &timestamp_sample, const float dis
 
 	_distance_sensor_pub.update();
 }
-
-void PX4Rangefinder::print_status()
-{
-}

src/lib/drivers/rangefinder/PX4Rangefinder.hpp

@@ -48,8 +48,6 @@ public:
 		       const uint8_t device_orientation = distance_sensor_s::ROTATION_DOWNWARD_FACING);
 	~PX4Rangefinder();
 
-	void print_status();
-
 	void set_device_type(uint8_t device_type);
 	//void set_error_count(uint64_t error_count) { _distance_sensor_pub.get().error_count = error_count; }