simulated sensors minor cleanup

- mavlink receiver use PX4Accelerometer/PX4Gyroscope for hil_sensor and hil_state_quaternion
 - simulator module remove unnecessary fake scaling
 - sih module remove unnecessary fake scaling

src/modules/mavlink/CMakeLists.txt

@@ -63,6 +63,10 @@ px4_add_module(
 		module.yaml
 	DEPENDS
 		airspeed
+		drivers_accelerometer
+		drivers_barometer
+		drivers_gyroscope
+		drivers_magnetometer
 		git_mavlink_v2
 		conversion
 		git_ecl

src/modules/mavlink/mavlink_receiver.cpp

@@ -80,6 +80,14 @@
 
 using matrix::wrap_2pi;
 
+MavlinkReceiver::~MavlinkReceiver()
+{
+	delete _px4_accel;
+	delete _px4_baro;
+	delete _px4_gyro;
+	delete _px4_mag;
+}
+
 MavlinkReceiver::MavlinkReceiver(Mavlink *parent) :
 	ModuleParams(nullptr),
 	_mavlink(parent),
@@ -2085,57 +2093,70 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg)
 
 	/* gyro */
 	{
-		sensor_gyro_s gyro{};
+		if (_px4_gyro == nullptr) {
+			// 2294028: DRV_GYR_DEVTYPE_GYROSIM, BUS: 1, ADDR: 2, TYPE: SIMULATION
+			_px4_gyro = new PX4Gyroscope(2294028);
 
-		gyro.timestamp = timestamp;
+			if (_px4_gyro == nullptr) {
-		gyro.x = imu.xgyro;
+				PX4_ERR("PX4Gyroscope alloc failed");
-		gyro.y = imu.ygyro;
+			}
-		gyro.z = imu.zgyro;
+		}
-		gyro.temperature = imu.temperature;
 
-		_gyro_pub.publish(gyro);
+		if (_px4_gyro != nullptr) {
+			_px4_gyro->set_temperature(imu.temperature);
+			_px4_gyro->update(timestamp, imu.xgyro, imu.ygyro, imu.zgyro);
+		}
 	}
 
 	/* accelerometer */
 	{
-		sensor_accel_s accel{};
+		if (_px4_accel == nullptr) {
+			// 1311244: DRV_ACC_DEVTYPE_ACCELSIM, BUS: 1, ADDR: 1, TYPE: SIMULATION
+			_px4_accel = new PX4Accelerometer(1311244);
 
-		accel.timestamp = timestamp;
+			if (_px4_accel == nullptr) {
-		accel.x = imu.xacc;
+				PX4_ERR("PX4Accelerometer alloc failed");
-		accel.y = imu.yacc;
+			}
-		accel.z = imu.zacc;
+		}
-		accel.temperature = imu.temperature;
 
-		_accel_pub.publish(accel);
+		if (_px4_accel != nullptr) {
+			_px4_accel->set_temperature(imu.temperature);
+			_px4_accel->update(timestamp, imu.xacc, imu.yacc, imu.zacc);
+		}
 	}
 
 	/* magnetometer */
 	{
-		sensor_mag_s mag{};
+		if (_px4_mag == nullptr) {
+			// 197388: DRV_MAG_DEVTYPE_MAGSIM, BUS: 3, ADDR: 1, TYPE: SIMULATION
+			_px4_mag = new PX4Magnetometer(197388);
 
-		mag.timestamp = timestamp;
+			if (_px4_mag == nullptr) {
-		mag.x_raw = imu.xmag * 1000.0f;
+				PX4_ERR("PX4Magnetometer alloc failed");
-		mag.y_raw = imu.ymag * 1000.0f;
+			}
-		mag.z_raw = imu.zmag * 1000.0f;
+		}
-		mag.x = imu.xmag;
-		mag.y = imu.ymag;
-		mag.z = imu.zmag;
 
-		_mag_pub.publish(mag);
+		if (_px4_mag != nullptr) {
+			_px4_mag->set_temperature(imu.temperature);
+			_px4_mag->update(timestamp, imu.xmag, imu.ymag, imu.zmag);
+		}
 	}
 
 	/* baro */
 	{
-		sensor_baro_s baro{};
+		if (_px4_baro == nullptr) {
+			// 6620172: DRV_BARO_DEVTYPE_BAROSIM, BUS: 1, ADDR: 4, TYPE: SIMULATION
+			_px4_baro = new PX4Barometer(6620172);
 
-		baro.timestamp = timestamp;
+			if (_px4_baro == nullptr) {
-		baro.pressure = imu.abs_pressure;
+				PX4_ERR("PX4Barometer alloc failed");
-		baro.temperature = imu.temperature;
+			}
+		}
 
-		/* fake device ID */
+		if (_px4_baro != nullptr) {
-		baro.device_id = 1234567;
+			_px4_baro->set_temperature(imu.temperature);
-
+			_px4_baro->update(timestamp, imu.abs_pressure);
-		_baro_pub.publish(baro);
+		}
 	}
 
 	/* battery status */
@@ -2498,28 +2519,36 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg)
 
 	/* accelerometer */
 	{
-		sensor_accel_s accel{};
+		if (_px4_accel == nullptr) {
+			// 1311244: DRV_ACC_DEVTYPE_ACCELSIM, BUS: 1, ADDR: 1, TYPE: SIMULATION
+			_px4_accel = new PX4Accelerometer(1311244);
 
-		accel.timestamp = timestamp;
+			if (_px4_accel == nullptr) {
-		accel.x = hil_state.xacc;
+				PX4_ERR("PX4Accelerometer alloc failed");
-		accel.y = hil_state.yacc;
+			}
-		accel.z = hil_state.zacc;
+		}
-		accel.temperature = 25.0f;
 
-		_accel_pub.publish(accel);
+		if (_px4_accel != nullptr) {
+			// accel in mG
+			_px4_accel->set_scale(CONSTANTS_ONE_G / 1000.0f);
+			_px4_accel->update(timestamp, hil_state.xacc, hil_state.yacc, hil_state.zacc);
+		}
 	}
 
 	/* gyroscope */
 	{
-		sensor_gyro_s gyro{};
+		if (_px4_gyro == nullptr) {
+			// 2294028: DRV_GYR_DEVTYPE_GYROSIM, BUS: 1, ADDR: 2, TYPE: SIMULATION
+			_px4_gyro = new PX4Gyroscope(2294028);
 
-		gyro.timestamp = timestamp;
+			if (_px4_gyro == nullptr) {
-		gyro.x = hil_state.rollspeed;
+				PX4_ERR("PX4Gyroscope alloc failed");
-		gyro.y = hil_state.pitchspeed;
+			}
-		gyro.z = hil_state.yawspeed;
+		}
-		gyro.temperature = 25.0f;
 
-		_gyro_pub.publish(gyro);
+		if (_px4_gyro != nullptr) {
+			_px4_gyro->update(timestamp, hil_state.rollspeed, hil_state.pitchspeed, hil_state.yawspeed);
+		}
 	}
 
 	/* battery status */

src/modules/mavlink/mavlink_receiver.h

@@ -47,6 +47,10 @@
 #include "mavlink_parameters.h"
 #include "mavlink_timesync.h"
 
+#include <lib/drivers/accelerometer/PX4Accelerometer.hpp>
+#include <lib/drivers/barometer/PX4Barometer.hpp>
+#include <lib/drivers/gyroscope/PX4Gyroscope.hpp>
+#include <lib/drivers/magnetometer/PX4Magnetometer.hpp>
 #include <px4_platform_common/module_params.h>
 #include <uORB/Publication.hpp>
 #include <uORB/PublicationQueued.hpp>
@@ -79,11 +83,6 @@
 #include <uORB/topics/position_setpoint_triplet.h>
 #include <uORB/topics/radio_status.h>
 #include <uORB/topics/rc_channels.h>
-#include <uORB/topics/sensor_accel.h>
-#include <uORB/topics/sensor_baro.h>
-#include <uORB/topics/sensor_combined.h>
-#include <uORB/topics/sensor_gyro.h>
-#include <uORB/topics/sensor_mag.h>
 #include <uORB/topics/telemetry_status.h>
 #include <uORB/topics/transponder_report.h>
 #include <uORB/topics/vehicle_attitude.h>
@@ -107,7 +106,7 @@ class MavlinkReceiver : public ModuleParams
 {
 public:
 	MavlinkReceiver(Mavlink *parent);
-	~MavlinkReceiver() = default;
+	~MavlinkReceiver() override;
 
 	/**
 	 * Start the receiver thread
@@ -258,10 +257,6 @@ private:
 	uORB::PublicationMulti<manual_control_setpoint_s>	_manual_pub{ORB_ID(manual_control_setpoint), ORB_PRIO_LOW};
 	uORB::PublicationMulti<ping_s>				_ping_pub{ORB_ID(ping), ORB_PRIO_LOW};
 	uORB::PublicationMulti<radio_status_s>			_radio_status_pub{ORB_ID(radio_status), ORB_PRIO_LOW};
-	uORB::PublicationMulti<sensor_accel_s>			_accel_pub{ORB_ID(sensor_accel), ORB_PRIO_LOW};
-	uORB::PublicationMulti<sensor_baro_s>			_baro_pub{ORB_ID(sensor_baro), ORB_PRIO_LOW};
-	uORB::PublicationMulti<sensor_gyro_s>			_gyro_pub{ORB_ID(sensor_gyro), ORB_PRIO_LOW};
-	uORB::PublicationMulti<sensor_mag_s>			_mag_pub{ORB_ID(sensor_mag), ORB_PRIO_LOW};
 
 	// ORB publications (queue length > 1)
 	uORB::PublicationQueued<gps_inject_data_s>	_gps_inject_data_pub{ORB_ID(gps_inject_data)};
@@ -277,6 +272,12 @@ private:
 	uORB::Subscription	_vehicle_local_position_sub{ORB_ID(vehicle_local_position)};
 	uORB::Subscription	_vehicle_status_sub{ORB_ID(vehicle_status)};
 
+	// hil_sensor and hil_state_quaternion
+	PX4Accelerometer *_px4_accel{nullptr};
+	PX4Barometer *_px4_baro{nullptr};
+	PX4Gyroscope *_px4_gyro{nullptr};
+	PX4Magnetometer *_px4_mag{nullptr};
+
 	static constexpr unsigned int	MOM_SWITCH_COUNT{8};
 	uint8_t				_mom_switch_pos[MOM_SWITCH_COUNT] {};
 	uint16_t			_mom_switch_state{0};

src/modules/sih/sih.cpp

@@ -92,8 +92,8 @@ Sih *Sih::instantiate(int argc, char *argv[])
 
 Sih::Sih() :
 	ModuleParams(nullptr),
-	_loop_perf(perf_alloc(PC_ELAPSED, "sih_execution")),
+	_loop_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": execution")),
-	_sampling_perf(perf_alloc(PC_ELAPSED, "sih_sampling"))
+	_sampling_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": sampling"))
 {
 }
 
@@ -333,34 +333,20 @@ void Sih::reconstruct_sensors_signals()
 void Sih::send_IMU()
 {
 	// gyro
-	{
+	_px4_gyro.set_temperature(T1_C);
-		static constexpr float scaling = 1000.0f;
+	_px4_gyro.update(_now, _gyro(0), _gyro(1), _gyro(2));
-		_px4_gyro.set_scale(1 / scaling);
-		_px4_gyro.set_temperature(T1_C);
-		_px4_gyro.update(_now, _gyro(0) * scaling, _gyro(1) * scaling, _gyro(2) * scaling);
-	}
 
 	// accel
-	{
+	_px4_accel.set_temperature(T1_C);
-		static constexpr float scaling = 1000.0f;
+	_px4_accel.update(_now, _acc(0), _acc(1), _acc(2));
-		_px4_accel.set_scale(1 / scaling);
-		_px4_accel.set_temperature(T1_C);
-		_px4_accel.update(_now, _acc(0) * scaling, _acc(1) * scaling, _acc(2) * scaling);
-	}
 
 	// magnetometer
-	{
+	_px4_mag.set_temperature(T1_C);
-		static constexpr float scaling = 1000.0f;
+	_px4_mag.update(_now, _mag(0), _mag(1), _mag(2));
-		_px4_mag.set_scale(1 / scaling);
-		_px4_mag.set_temperature(T1_C);
-		_px4_mag.update(_now, _mag(0) * scaling, _mag(1) * scaling, _mag(2) * scaling);
-	}
 
 	// baro
-	{
+	_px4_baro.set_temperature(_baro_temp_c);
-		_px4_baro.set_temperature(_baro_temp_c);
+	_px4_baro.update(_now, _baro_p_mBar);
-		_px4_baro.update(_now, _baro_p_mBar);
-	}
 }
 
 void Sih::send_gps()

src/modules/simulator/simulator_mavlink.cpp

@@ -195,26 +195,20 @@ void Simulator::update_sensors(const hrt_abstime &time, const mavlink_hil_sensor
 
 	// gyro
 	if (!_param_sim_gyro_block.get()) {
-		static constexpr float scaling = 1000.0f;
-		_px4_gyro.set_scale(1 / scaling);
 		_px4_gyro.set_temperature(imu.temperature);
-		_px4_gyro.update(time, imu.xgyro * scaling, imu.ygyro * scaling, imu.zgyro * scaling);
+		_px4_gyro.update(time, imu.xgyro, imu.ygyro, imu.zgyro);
 	}
 
 	// accel
 	if (!_param_sim_accel_block.get()) {
-		static constexpr float scaling = 1000.0f;
-		_px4_accel.set_scale(1 / scaling);
 		_px4_accel.set_temperature(imu.temperature);
-		_px4_accel.update(time, imu.xacc * scaling, imu.yacc * scaling, imu.zacc * scaling);
+		_px4_accel.update(time, imu.xacc, imu.yacc, imu.zacc);
 	}
 
 	// magnetometer
 	if (!_param_sim_mag_block.get()) {
-		static constexpr float scaling = 1000.0f;
-		_px4_mag.set_scale(1 / scaling);
 		_px4_mag.set_temperature(imu.temperature);
-		_px4_mag.update(time, imu.xmag * scaling, imu.ymag * scaling, imu.zmag * scaling);
+		_px4_mag.update(time, imu.xmag, imu.ymag, imu.zmag);
 	}
 
 	// baro