simulator move to PX4Accelerometer, PX4Gyroscope, PX4Magnetometer, PX4Barometer helpers (#12081)

ROMFS/px4fmu_common/init.d-posix/rcS

@@ -94,15 +94,16 @@ then
 
 	param set BAT_N_CELLS 3
 
-	param set CAL_ACC0_ID 1376264
+	param set CAL_ACC0_ID 1311244
-	param set CAL_ACC1_ID 1310728
+	param set CAL_ACC_PRIME 1311244
-	param set CAL_ACC_PRIME 1376264
 
-	param set CAL_GYRO0_ID 2293768
+	param set CAL_GYRO0_ID 2294028
-	param set CAL_GYRO_PRIME 2293768
+	param set CAL_GYRO_PRIME 2294028
 
-	param set CAL_MAG0_ID 196616
+	param set CAL_MAG0_ID 197388
-	param set CAL_MAG_PRIME 196616
+	param set CAL_MAG_PRIME 197388
+
+	param set CAL_BARO_PRIME 6620172
 
 	param set CBRK_AIRSPD_CHK 0
 
@@ -201,11 +202,8 @@ sh "$autostart_file"
 
 dataman start
 replay tryapplyparams
-simulator start -s -c $simulator_tcp_port
+simulator start -c $simulator_tcp_port
 tone_alarm start
-gyrosim start
-accelsim start
-barosim start
 gpssim start
 sensors start
 commander start
@@ -227,15 +225,6 @@ then
 	camera_feedback start
 fi
 
-
-if [ ${VEHICLE_TYPE} = fw -o ${VEHICLE_TYPE} = vtol ]
-then
-	if param compare CBRK_AIRSPD_CHK 0
-	then
-		measairspeedsim start
-	fi
-fi
-
 # Configure vehicle type specific parameters.
 # Note: rc.vehicle_setup is the entry point for rc.interface,
 #       rc.fw_apps, rc.mc_apps, rc.ugv_apps, and rc.vtol_apps.

posix-configs/SITL/init/test/cmd_template.in

@@ -10,13 +10,9 @@ param set SYS_RESTART_TYPE 0
 
 dataman start
 
-simulator start -t
+simulator start
 tone_alarm start
-gyrosim start
-accelsim start
-barosim start
 gpssim start
-measairspeedsim start
 pwm_out_sim start
 
 ver all

posix-configs/SITL/init/test/test_mavlink

@@ -10,13 +10,9 @@ param set SYS_RESTART_TYPE 0
 
 dataman start
 
-simulator start -t
+simulator start
 tone_alarm start
-gyrosim start
-accelsim start
-barosim start
 gpssim start
-measairspeedsim start
 pwm_out_sim start
 
 ver all

posix-configs/SITL/init/test/test_shutdown

@@ -14,13 +14,9 @@ param set SYS_RESTART_TYPE 0
 
 dataman start
 
-simulator start -t
+simulator start
 tone_alarm start
-gyrosim start
-accelsim start
-barosim start
 gpssim start
-measairspeedsim start
 pwm_out_sim start
 
 sensors start
@@ -83,11 +79,7 @@ sleep 2
 
 simulator stop
 tone_alarm stop
-gyrosim stop
-#accelsim stop
-#barosim stop
 gpssim stop
-measairspeedsim stop
 
 dataman stop
 #uorb stop

posix-configs/SITL/init/test/test_template.in

@@ -10,13 +10,9 @@ param set SYS_RESTART_TYPE 0
 
 dataman start
 
-simulator start -t
+simulator start
 tone_alarm start
-gyrosim start
-accelsim start
-barosim start
 gpssim start
-measairspeedsim start
 pwm_out_sim start
 
 ver all

posix-configs/eagle/hil/mainapphil.config

@@ -8,4 +8,4 @@ sleep 1
 mavlink stream -u 14556 -s HIGHRES_IMU -r 50
 mavlink stream -u 14556 -s ATTITUDE -r 50
 mavlink boot_complete
-simulator start -p
+simulator start

src/drivers/drv_sensor.h

@@ -54,7 +54,7 @@
 
 #define DRV_MAG_DEVTYPE_HMC5883  0x01
 #define DRV_MAG_DEVTYPE_LSM303D  0x02
-#define DRV_MAG_DEVTYPE_ACCELSIM 0x03
+#define DRV_MAG_DEVTYPE_MAGSIM   0x03
 #define DRV_MAG_DEVTYPE_MPU9250  0x04
 #define DRV_MAG_DEVTYPE_LIS3MDL  0x05
 #define DRV_MAG_DEVTYPE_IST8310  0x06
@@ -65,7 +65,6 @@
 #define DRV_ACC_DEVTYPE_BMA180   0x12
 #define DRV_ACC_DEVTYPE_MPU6000  0x13
 #define DRV_ACC_DEVTYPE_ACCELSIM 0x14
-#define DRV_ACC_DEVTYPE_GYROSIM  0x15
 #define DRV_ACC_DEVTYPE_MPU9250  0x16
 #define DRV_ACC_DEVTYPE_BMI160   0x17
 #define DRV_GYR_DEVTYPE_MPU6000  0x21
@@ -113,6 +112,7 @@
 #define DRV_MAG_DEVTYPE_LSM303AGR	0x62
 #define DRV_ACC_DEVTYPE_ADIS16497	0x63
 #define DRV_GYR_DEVTYPE_ADIS16497	0x64
+#define DRV_BARO_DEVTYPE_BAROSIM	0x65
 
 /*
  * ioctl() definitions

src/lib/drivers/CMakeLists.txt

@@ -33,6 +33,7 @@
 
 add_subdirectory(accelerometer)
 add_subdirectory(airspeed)
+add_subdirectory(barometer)
 add_subdirectory(device)
 add_subdirectory(gyroscope)
 add_subdirectory(led)

src/lib/drivers/accelerometer/CMakeLists.txt

@@ -32,3 +32,4 @@
 ############################################################################
 
 px4_add_library(drivers_accelerometer PX4Accelerometer.cpp)
+target_link_libraries(drivers_accelerometer PRIVATE drivers__device)

src/lib/drivers/accelerometer/PX4Accelerometer.cpp

@@ -98,6 +98,12 @@ void PX4Accelerometer::set_device_type(uint8_t devtype)
 	_sensor_accel_pub.get().device_id = device_id.devid;
 }
 
+void PX4Accelerometer::set_sample_rate(unsigned rate)
+{
+	_sample_rate = rate;
+	_filter.set_cutoff_frequency(_sample_rate, _filter.get_cutoff_freq());
+}
+
 void PX4Accelerometer::update(hrt_abstime timestamp, int16_t x, int16_t y, int16_t z)
 {
 	sensor_accel_s &report = _sensor_accel_pub.get();

src/lib/drivers/accelerometer/PX4Accelerometer.hpp

@@ -47,7 +47,7 @@ class PX4Accelerometer : public cdev::CDev, public ModuleParams
 {
 
 public:
-	PX4Accelerometer(uint32_t device_id, uint8_t priority, enum Rotation rotation);
+	PX4Accelerometer(uint32_t device_id, uint8_t priority = ORB_PRIO_DEFAULT, enum Rotation rotation = ROTATION_NONE);
 	~PX4Accelerometer() override;
 
 	int	ioctl(cdev::file_t *filp, int cmd, unsigned long arg) override;
@@ -57,9 +57,7 @@ public:
 	void set_scale(float scale) { _sensor_accel_pub.get().scaling = scale; }
 	void set_temperature(float temperature) { _sensor_accel_pub.get().temperature = temperature; }
 
-	void set_sample_rate(unsigned rate) { _sample_rate = rate; _filter.set_cutoff_frequency(_sample_rate, _filter.get_cutoff_freq()); }
+	void set_sample_rate(unsigned rate);
-
-	void configure_filter(float cutoff_freq) { _filter.set_cutoff_frequency(_sample_rate, cutoff_freq); }
 
 	void update(hrt_abstime timestamp, int16_t x, int16_t y, int16_t z);
 
@@ -67,6 +65,8 @@ public:
 
 private:
 
+	void configure_filter(float cutoff_freq) { _filter.set_cutoff_frequency(_sample_rate, cutoff_freq); }
+
 	uORB::Publication<sensor_accel_s>	_sensor_accel_pub;
 
 	math::LowPassFilter2pVector3f _filter{1000, 100};

src/lib/drivers/device/Device.hpp

@@ -130,6 +130,7 @@ public:
 		DeviceBusType_I2C     = 1,
 		DeviceBusType_SPI     = 2,
 		DeviceBusType_UAVCAN  = 3,
+		DeviceBusType_SIMULATION = 4,
 	};
 
 	/**
@@ -160,6 +161,9 @@ public:
 		case DeviceBusType_UAVCAN:
 			return "UAVCAN";
 
+		case DeviceBusType_SIMULATION:
+			return "SIMULATION";
+
 		case DeviceBusType_UNKNOWN:
 		default:
 			return "UNKNOWN";

src/lib/drivers/gyroscope/CMakeLists.txt

@@ -32,3 +32,4 @@
 ############################################################################
 
 px4_add_library(drivers_gyroscope PX4Gyroscope.cpp)
+target_link_libraries(drivers_gyroscope PRIVATE drivers__device)

src/lib/drivers/gyroscope/PX4Gyroscope.cpp

@@ -98,6 +98,12 @@ void PX4Gyroscope::set_device_type(uint8_t devtype)
 	_sensor_gyro_pub.get().device_id = device_id.devid;
 }
 
+void PX4Gyroscope::set_sample_rate(unsigned rate)
+{
+	_sample_rate = rate;
+	_filter.set_cutoff_frequency(_sample_rate, _filter.get_cutoff_freq());
+}
+
 void PX4Gyroscope::update(hrt_abstime timestamp, int16_t x, int16_t y, int16_t z)
 {
 	sensor_gyro_s &report = _sensor_gyro_pub.get();

src/lib/drivers/gyroscope/PX4Gyroscope.hpp

@@ -47,7 +47,7 @@ class PX4Gyroscope : public cdev::CDev, public ModuleParams
 {
 
 public:
-	PX4Gyroscope(uint32_t device_id, uint8_t priority, enum Rotation rotation);
+	PX4Gyroscope(uint32_t device_id, uint8_t priority = ORB_PRIO_DEFAULT, enum Rotation rotation = ROTATION_NONE);
 	~PX4Gyroscope() override;
 
 	int	ioctl(cdev::file_t *filp, int cmd, unsigned long arg) override;
@@ -57,9 +57,7 @@ public:
 	void set_scale(float scale) { _sensor_gyro_pub.get().scaling = scale; }
 	void set_temperature(float temperature) { _sensor_gyro_pub.get().temperature = temperature; }
 
-	void set_sample_rate(unsigned rate) { _sample_rate = rate; _filter.set_cutoff_frequency(_sample_rate, _filter.get_cutoff_freq()); }
+	void set_sample_rate(unsigned rate);
-
-	void configure_filter(float cutoff_freq) { _filter.set_cutoff_frequency(_sample_rate, cutoff_freq); }
 
 	void update(hrt_abstime timestamp, int16_t x, int16_t y, int16_t z);
 
@@ -67,6 +65,8 @@ public:
 
 private:
 
+	void configure_filter(float cutoff_freq) { _filter.set_cutoff_frequency(_sample_rate, cutoff_freq); }
+
 	uORB::Publication<sensor_gyro_s>	_sensor_gyro_pub;
 
 	math::LowPassFilter2pVector3f _filter{1000, 100};

src/modules/simulator/CMakeLists.txt

@@ -73,11 +73,11 @@ px4_add_module(
 		drivers__ledsim
 		git_ecl
 		ecl_geo
+		drivers_accelerometer
+		drivers_barometer
+		drivers_gyroscope
+		drivers_magnetometer
 	)
 target_include_directories(modules__simulator INTERFACE ${PX4_SOURCE_DIR}/mavlink/include/mavlink)
 
-add_subdirectory(accelsim)
-add_subdirectory(airspeedsim)
-add_subdirectory(barosim)
 add_subdirectory(gpssim)
-add_subdirectory(gyrosim)

src/modules/simulator/accelsim/CMakeLists.txt

@@ -1,45 +0,0 @@
-############################################################################
-#
-#   Copyright (c) 2015 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
-# are met:
-#
-# 1. Redistributions of source code must retain the above copyright
-#    notice, this list of conditions and the following disclaimer.
-# 2. Redistributions in binary form must reproduce the above copyright
-#    notice, this list of conditions and the following disclaimer in
-#    the documentation and/or other materials provided with the
-#    distribution.
-# 3. Neither the name PX4 nor the names of its contributors may be
-#    used to endorse or promote products derived from this software
-#    without specific prior written permission.
-#
-# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
-# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
-# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
-# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
-# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
-# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
-# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
-# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
-# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
-# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
-# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
-# POSSIBILITY OF SUCH DAMAGE.
-#
-############################################################################
-
-px4_add_module(
-	MODULE drivers__accelsim
-	MAIN accelsim
-	COMPILE_FLAGS
-		-Wno-double-promotion
-		-Wno-cast-align # TODO: fix and enable
-		-Wno-address-of-packed-member # TODO: fix in c_library_v2
-	SRCS
-		accelsim.cpp
-	DEPENDS
-		modules__simulator
-	)

src/modules/simulator/airspeedsim/CMakeLists.txt

@@ -1,46 +0,0 @@
-############################################################################
-#
-#   Copyright (c) 2015 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
-# are met:
-#
-# 1. Redistributions of source code must retain the above copyright
-#    notice, this list of conditions and the following disclaimer.
-# 2. Redistributions in binary form must reproduce the above copyright
-#    notice, this list of conditions and the following disclaimer in
-#    the documentation and/or other materials provided with the
-#    distribution.
-# 3. Neither the name PX4 nor the names of its contributors may be
-#    used to endorse or promote products derived from this software
-#    without specific prior written permission.
-#
-# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
-# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
-# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
-# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
-# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
-# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
-# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
-# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
-# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
-# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
-# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
-# POSSIBILITY OF SUCH DAMAGE.
-#
-############################################################################
-
-px4_add_module(
-	MODULE drivers__airspeedsim
-	MAIN measairspeedsim
-	COMPILE_FLAGS
-		-Wno-double-promotion
-		-Wno-cast-align # TODO: fix and enable
-		-Wno-address-of-packed-member # TODO: fix in c_library_v2
-	SRCS
-		airspeedsim.cpp
-		meas_airspeed_sim.cpp
-	DEPENDS
-		modules__simulator
-	)

src/modules/simulator/airspeedsim/airspeedsim.cpp

@@ -1,352 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 2013, 2014 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
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-/**
- * @file ets_airspeed.cpp
- * @author Simon Wilks
- * @author Mark Charlebois (simulator)
- * @author Roman Bapst (simulator)
- * Driver for the Simulated AirspeedSim Sensor based on Eagle Tree AirspeedSim V3.
- */
-
-#include <px4_config.h>
-
-#include <sys/types.h>
-#include <stdint.h>
-#include <stdlib.h>
-#include <stdbool.h>
-#include <semaphore.h>
-#include <string.h>
-#include <fcntl.h>
-#include <poll.h>
-#include <errno.h>
-#include <stdio.h>
-#include <math.h>
-#include <unistd.h>
-
-#include <systemlib/err.h>
-#include <parameters/param.h>
-#include <perf/perf_counter.h>
-
-#include <drivers/drv_airspeed.h>
-#include <drivers/drv_hrt.h>
-#include <drivers/device/ringbuffer.h>
-
-#include <uORB/uORB.h>
-#include <uORB/topics/differential_pressure.h>
-
-#include <simulator/simulator.h>
-
-#include "airspeedsim.h"
-
-AirspeedSim::AirspeedSim(int bus, int address, unsigned conversion_interval, const char *path) :
-	CDev(path),
-	_reports(nullptr),
-	_retries(0),
-	_sensor_ok(false),
-	_measure_ticks(0),
-	_collect_phase(false),
-	_diff_pres_offset(0.0f),
-	_airspeed_pub(nullptr),
-	_class_instance(-1),
-	_conversion_interval(conversion_interval),
-	_sample_perf(perf_alloc(PC_ELAPSED, "airspeed_read")),
-	_comms_errors(perf_alloc(PC_COUNT, "airspeed_comms_errors"))
-{
-}
-
-AirspeedSim::~AirspeedSim()
-{
-	/* make sure we are truly inactive */
-	stop();
-
-	if (_class_instance != -1) {
-		unregister_class_devname(AIRSPEED_BASE_DEVICE_PATH, _class_instance);
-	}
-
-	/* free any existing reports */
-	if (_reports != nullptr) {
-		delete _reports;
-	}
-
-	// free perf counters
-	perf_free(_sample_perf);
-	perf_free(_comms_errors);
-}
-
-int
-AirspeedSim::init()
-{
-	int ret = ERROR;
-
-	/* init base class */
-	if (CDev::init() != OK) {
-		PX4_ERR("CDev init failed");
-		goto out;
-	}
-
-	/* allocate basic report buffers */
-	_reports = new ringbuffer::RingBuffer(2, sizeof(differential_pressure_s));
-
-	if (_reports == nullptr) {
-		goto out;
-	}
-
-	/* register alternate interfaces if we have to */
-	_class_instance = register_class_devname(AIRSPEED_BASE_DEVICE_PATH);
-
-	/* publication init */
-	if (_class_instance == 0) {
-
-		/* advertise sensor topic, measure manually to initialize valid report */
-		struct differential_pressure_s arp;
-		measure();
-		_reports->get(&arp);
-
-		/* measurement will have generated a report, publish */
-		_airspeed_pub = orb_advertise(ORB_ID(differential_pressure), &arp);
-
-		if (_airspeed_pub == nullptr) {
-			PX4_WARN("uORB started?");
-		}
-	}
-
-	ret = OK;
-
-out:
-	return ret;
-}
-
-int
-AirspeedSim::probe()
-{
-	/* on initial power up the device may need more than one retry
-	   for detection. Once it is running the number of retries can
-	   be reduced
-	*/
-	_retries = 4;
-	int ret = measure();
-
-	// drop back to 2 retries once initialised
-	_retries = 2;
-	return ret;
-}
-
-int
-AirspeedSim::ioctl(cdev::file_t *filp, int cmd, unsigned long arg)
-{
-	switch (cmd) {
-
-	case SENSORIOCSPOLLRATE: {
-			switch (arg) {
-
-			/* zero would be bad */
-			case 0:
-				return -EINVAL;
-
-			/* set default polling rate */
-			case SENSOR_POLLRATE_DEFAULT: {
-					/* do we need to start internal polling? */
-					bool want_start = (_measure_ticks == 0);
-
-					/* set interval for next measurement to minimum legal value */
-					_measure_ticks = USEC2TICK(_conversion_interval);
-
-					/* if we need to start the poll state machine, do it */
-					if (want_start) {
-						start();
-					}
-
-					return OK;
-				}
-
-			/* adjust to a legal polling interval in Hz */
-			default: {
-					/* do we need to start internal polling? */
-					bool want_start = (_measure_ticks == 0);
-
-					/* convert hz to tick interval via microseconds */
-					int ticks = USEC2TICK(1000000 / arg);
-
-					/* check against maximum rate */
-					if (ticks < USEC2TICK(_conversion_interval)) {
-						return -EINVAL;
-					}
-
-					/* update interval for next measurement */
-					_measure_ticks = ticks;
-
-					/* if we need to start the poll state machine, do it */
-					if (want_start) {
-						start();
-					}
-
-					return OK;
-				}
-			}
-		}
-
-	case AIRSPEEDIOCSSCALE: {
-			struct airspeed_scale *s = (struct airspeed_scale *)arg;
-			_diff_pres_offset = s->offset_pa;
-			return OK;
-		}
-
-	default:
-		/* give it to the superclass */
-		//return I2C::ioctl(filp, cmd, arg);	XXX SIM should be super class
-		return 0;
-	}
-}
-
-ssize_t
-AirspeedSim::read(cdev::file_t *filp, char *buffer, size_t buflen)
-{
-	unsigned count = buflen / sizeof(differential_pressure_s);
-	differential_pressure_s *abuf = reinterpret_cast<differential_pressure_s *>(buffer);
-	int ret = 0;
-
-	/* buffer must be large enough */
-	if (count < 1) {
-		return -ENOSPC;
-	}
-
-	/* if automatic measurement is enabled */
-	if (_measure_ticks > 0) {
-
-		/*
-		 * While there is space in the caller's buffer, and reports, copy them.
-		 * Note that we may be pre-empted by the workq thread while we are doing this;
-		 * we are careful to avoid racing with them.
-		 */
-		while (count--) {
-			if (_reports->get(abuf)) {
-				ret += sizeof(*abuf);
-				abuf++;
-			}
-		}
-
-		/* if there was no data, warn the caller */
-		return ret ? ret : -EAGAIN;
-	}
-
-	/* manual measurement - run one conversion */
-	do {
-		_reports->flush();
-
-		/* trigger a measurement */
-		if (OK != measure()) {
-			ret = -EIO;
-			break;
-		}
-
-		/* wait for it to complete */
-		px4_usleep(_conversion_interval);
-
-		/* run the collection phase */
-		if (OK != collect()) {
-			ret = -EIO;
-			break;
-		}
-
-		/* state machine will have generated a report, copy it out */
-		if (_reports->get(abuf)) {
-			ret = sizeof(*abuf);
-		}
-
-	} while (0);
-
-	return ret;
-}
-
-void
-AirspeedSim::start()
-{
-	/* reset the report ring and state machine */
-	_collect_phase = false;
-	_reports->flush();
-
-	/* schedule a cycle to start things */
-	work_queue(HPWORK, &_work, (worker_t)&AirspeedSim::cycle_trampoline, this, 1);
-}
-
-void
-AirspeedSim::stop()
-{
-	work_cancel(HPWORK, &_work);
-}
-
-void
-AirspeedSim::cycle_trampoline(void *arg)
-{
-	AirspeedSim *dev = (AirspeedSim *)arg;
-
-	dev->cycle();
-}
-
-void
-AirspeedSim::print_info()
-{
-	perf_print_counter(_sample_perf);
-	perf_print_counter(_comms_errors);
-	PX4_INFO("poll interval:  %u ticks", _measure_ticks);
-	_reports->print_info("report queue");
-}
-
-void
-AirspeedSim::new_report(const differential_pressure_s &report)
-{
-	_reports->force(&report);
-}
-
-int
-AirspeedSim::transfer(const uint8_t *send, unsigned send_len, uint8_t *recv, unsigned recv_len)
-{
-	if (recv_len > 0) {
-		// this is equivalent to the collect phase
-		Simulator *sim = Simulator::getInstance();
-
-		if (sim == nullptr) {
-			PX4_ERR("Error BARO_SIM::transfer no simulator");
-			return -ENODEV;
-		}
-
-		PX4_DEBUG("BARO_SIM::transfer getting sample");
-		sim->getAirspeedSample(recv, recv_len);
-
-	} else {
-		// we don't need measure phase
-	}
-
-	return 0;
-}

src/modules/simulator/airspeedsim/airspeedsim.h

@@ -1,172 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 2013, 2014 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
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-/**
- * @file airspeed.h
- * @author Simon Wilks
- *
- * Generic driver for airspeed sensors connected via I2C.
- */
-
-#include <px4_config.h>
-#include <px4_defines.h>
-
-#include <sys/types.h>
-#include <stdint.h>
-#include <stdlib.h>
-#include <stdbool.h>
-#include <semaphore.h>
-#include <string.h>
-#include <fcntl.h>
-#include <poll.h>
-#include <errno.h>
-#include <stdio.h>
-#include <math.h>
-#include <unistd.h>
-
-#include <px4_workqueue.h>
-#include <arch/board/board.h>
-
-#include <systemlib/err.h>
-#include <parameters/param.h>
-#include <perf/perf_counter.h>
-
-#include <drivers/drv_airspeed.h>
-#include <drivers/drv_hrt.h>
-#include <drivers/device/ringbuffer.h>
-#include <lib/cdev/CDev.hpp>
-
-#include <uORB/uORB.h>
-#include <uORB/topics/differential_pressure.h>
-
-/* Default I2C bus */
-#define PX4_I2C_BUS_DEFAULT		PX4_I2C_BUS_EXPANSION
-
-#ifndef CONFIG_SCHED_WORKQUEUE
-# error This requires CONFIG_SCHED_WORKQUEUE.
-#endif
-
-class __EXPORT AirspeedSim : public cdev::CDev
-{
-public:
-	AirspeedSim(int bus, int address, unsigned conversion_interval, const char *path);
-	virtual ~AirspeedSim();
-
-	virtual int	init();
-
-	virtual ssize_t	read(cdev::file_t *filp, char *buffer, size_t buflen);
-	virtual int	ioctl(cdev::file_t *filp, int cmd, unsigned long arg);
-
-	/**
-	 * Diagnostics - print some basic information about the driver.
-	 */
-	virtual void	print_info();
-
-private:
-	ringbuffer::RingBuffer		*_reports;
-
-	unsigned _retries;	// XXX this should come from the SIM class
-
-	/* this class has pointer data members and should not be copied */
-	AirspeedSim(const AirspeedSim &);
-	AirspeedSim &operator=(const AirspeedSim &);
-
-protected:
-	virtual int	probe();
-
-	/**
-	* Perform a poll cycle; collect from the previous measurement
-	* and start a new one.
-	*/
-	virtual void	cycle() = 0;
-	virtual int	measure() = 0;
-	virtual int	collect() = 0;
-
-	virtual int	transfer(const uint8_t *send, unsigned send_len,
-				 uint8_t *recv, unsigned recv_len);
-
-	struct work_s			_work {};
-	bool			_sensor_ok;
-	int			_measure_ticks;
-	bool			_collect_phase;
-	float			_diff_pres_offset;
-
-	orb_advert_t		_airspeed_pub;
-
-	int			_class_instance;
-
-	int		_conversion_interval;
-
-	perf_counter_t		_sample_perf;
-	perf_counter_t		_comms_errors;
-
-
-	/**
-	* Test whether the device supported by the driver is present at a
-	* specific address.
-	*
-	* @param address	The I2C bus address to probe.
-	* @return		True if the device is present.
-	*/
-	int	probe_address(uint8_t address);
-
-	/**
-	* Initialise 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();
-
-	/**
-	* Stop the automatic measurement state machine.
-	*/
-	void	stop();
-
-	/**
-	* Static trampoline from the workq context; because we don't have a
-	* generic workq wrapper yet.
-	*
-	* @param arg		Instance pointer for the driver that is polling.
-	*/
-	static void	cycle_trampoline(void *arg);
-
-	/**
-	* add a new report to the reports queue
-	*
-	* @param report		differential_pressure_s report
-	*/
-	void	new_report(const differential_pressure_s &report);
-};
-
-

src/modules/simulator/airspeedsim/meas_airspeed_sim.cpp

@@ -1,549 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 2013, 2014, 2017 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
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-/**
- * @file meas_airspeed_sim.cpp
- * @author Lorenz Meier <lorenz@px4.io>
- * @author Sarthak Kaingade
- * @author Simon Wilks
- * @author Thomas Gubler
- * @author Roman Bapst
- *
- * Driver for a simulated airspeed sensor.
- *
- */
-
-
-#include <px4_config.h>
-
-#include <sys/types.h>
-#include <stdint.h>
-#include <stdlib.h>
-#include <stdbool.h>
-#include <semaphore.h>
-#include <string.h>
-#include <fcntl.h>
-#include <poll.h>
-#include <errno.h>
-#include <stdio.h>
-#include <math.h>
-#include <unistd.h>
-
-#include <systemlib/err.h>
-#include <parameters/param.h>
-#include <perf/perf_counter.h>
-
-#include <mathlib/math/filter/LowPassFilter2p.hpp>
-
-#include <drivers/drv_airspeed.h>
-#include <drivers/drv_hrt.h>
-
-#include <uORB/uORB.h>
-#include <uORB/topics/differential_pressure.h>
-#include <uORB/topics/system_power.h>
-
-#include "airspeedsim.h"
-
-/* I2C bus address is 1010001x */
-#define I2C_ADDRESS_MS4525DO	0x28	/**< 7-bit address. Depends on the order code (this is for code "I") */
-#define PATH_MS4525		"/dev/ms4525"
-/* The MS5525DSO address is 111011Cx, where C is the complementary value of the pin CSB */
-#define I2C_ADDRESS_MS5525DSO	0x77	//0x77/* 7-bit address, addr. pin pulled low */
-#define PATH_MS5525		"/dev/ms5525"
-
-/* Register address */
-#define ADDR_READ_MR			0x00	/* write to this address to start conversion */
-
-/* Measurement rate is 100Hz */
-#define MEAS_RATE 100
-#define MEAS_DRIVER_FILTER_FREQ 1.2f
-#define CONVERSION_INTERVAL	(1000000 / MEAS_RATE)	/* microseconds */
-
-class MEASAirspeedSim : public AirspeedSim
-{
-public:
-	MEASAirspeedSim(int bus, int address = I2C_ADDRESS_MS4525DO, const char *path = PATH_MS4525);
-
-protected:
-
-	/**
-	* Perform a poll cycle; collect from the previous measurement
-	* and start a new one.
-	*/
-	virtual void	cycle();
-	virtual int	measure();
-	virtual int	collect();
-
-	math::LowPassFilter2p	_filter;
-
-	/**
-	 * Correct for 5V rail voltage variations
-	 */
-	void voltage_correction(float &diff_pres_pa, float &temperature);
-
-	int _t_system_power;
-	struct system_power_s system_power;
-};
-
-/*
- * Driver 'main' command.
- */
-extern "C" __EXPORT int measairspeedsim_main(int argc, char *argv[]);
-
-MEASAirspeedSim::MEASAirspeedSim(int bus, int address, const char *path) : AirspeedSim(bus, address,
-			CONVERSION_INTERVAL, path),
-	_filter(MEAS_RATE, MEAS_DRIVER_FILTER_FREQ),
-	_t_system_power(-1),
-	system_power{}
-{}
-
-int
-MEASAirspeedSim::measure()
-{
-	int ret;
-
-	/*
-	 * Send the command to begin a measurement.
-	 */
-	uint8_t cmd = 0;
-	ret = transfer(&cmd, 1, nullptr, 0);
-
-	if (OK != ret) {
-		perf_count(_comms_errors);
-	}
-
-	return ret;
-}
-
-int
-MEASAirspeedSim::collect()
-{
-	int	ret = -EIO;
-
-	/* read from the sensor */
-#pragma pack(push, 1)
-	struct {
-		float		temperature;
-		float		diff_pressure;
-	} airspeed_report;
-#pragma pack(pop)
-
-
-	perf_begin(_sample_perf);
-
-	ret = transfer(nullptr, 0, (uint8_t *)&airspeed_report, sizeof(airspeed_report));
-
-	if (ret < 0) {
-		perf_count(_comms_errors);
-		perf_end(_sample_perf);
-		return ret;
-	}
-
-	uint8_t status = 0;
-
-	switch (status) {
-	case 0:
-		break;
-
-	case 1:
-
-	/* fallthrough */
-	case 2:
-
-	/* fallthrough */
-	case 3:
-		perf_count(_comms_errors);
-		perf_end(_sample_perf);
-		return -EAGAIN;
-	}
-
-	float temperature = airspeed_report.temperature;
-	float diff_press_pa_raw = airspeed_report.diff_pressure * 100.0f; // convert from millibar to bar
-
-	differential_pressure_s report = {};
-
-	report.timestamp = hrt_absolute_time();
-	report.error_count = perf_event_count(_comms_errors);
-	report.temperature = temperature;
-	report.differential_pressure_filtered_pa = _filter.apply(diff_press_pa_raw);
-	report.differential_pressure_raw_pa = diff_press_pa_raw;
-
-	int instance;
-	orb_publish_auto(ORB_ID(differential_pressure), &_airspeed_pub, &report, &instance, ORB_PRIO_DEFAULT);
-
-	new_report(report);
-
-	/* notify anyone waiting for data */
-	poll_notify(POLLIN);
-
-	ret = OK;
-
-	perf_end(_sample_perf);
-
-	return ret;
-}
-
-void
-MEASAirspeedSim::cycle()
-{
-	int ret;
-
-	/* collection phase? */
-	if (_collect_phase) {
-
-		/* perform collection */
-		ret = collect();
-
-		if (OK != ret) {
-			/* restart the measurement state machine */
-			start();
-			_sensor_ok = false;
-			return;
-		}
-
-		/* next phase is measurement */
-		_collect_phase = false;
-
-		/*
-		 * Is there a collect->measure gap?
-		 */
-		if (_measure_ticks > USEC2TICK(CONVERSION_INTERVAL)) {
-
-			/* schedule a fresh cycle call when we are ready to measure again */
-			work_queue(HPWORK,
-				   &_work,
-				   (worker_t)&AirspeedSim::cycle_trampoline,
-				   this,
-				   _measure_ticks - USEC2TICK(CONVERSION_INTERVAL));
-
-			return;
-		}
-	}
-
-	/* measurement phase */
-	ret = measure();
-
-	if (OK != ret) {
-		PX4_ERR("measure error");
-	}
-
-	_sensor_ok = (ret == OK);
-
-	/* next phase is collection */
-	_collect_phase = true;
-
-	/* schedule a fresh cycle call when the measurement is done */
-	work_queue(HPWORK,
-		   &_work,
-		   (worker_t)&AirspeedSim::cycle_trampoline,
-		   this,
-		   USEC2TICK(CONVERSION_INTERVAL));
-}
-
-/**
- * Local functions in support of the shell command.
- */
-namespace meas_airspeed_sim
-{
-
-MEASAirspeedSim	*g_dev = nullptr;
-
-int	start(int i2c_bus);
-int	stop();
-int test();
-int	reset();
-int	info();
-
-/**
- * Start the driver.
- *
- * This function call only returns once the driver is up and running
- * or failed to detect the sensor.
- */
-int
-start(int i2c_bus)
-{
-	int fd;
-
-	if (g_dev != nullptr) {
-		PX4_WARN("already started");
-	}
-
-	/* create the driver, try the MS4525DO first */
-	g_dev = new MEASAirspeedSim(i2c_bus, I2C_ADDRESS_MS4525DO, PATH_MS4525);
-
-	/* check if the MS4525DO was instantiated */
-	if (g_dev == nullptr) {
-		goto fail;
-	}
-
-	/* try the MS5525DSO next if init fails */
-	if (OK != g_dev->AirspeedSim::init()) {
-		delete g_dev;
-		g_dev = new MEASAirspeedSim(i2c_bus, I2C_ADDRESS_MS5525DSO, PATH_MS5525);
-
-		/* check if the MS5525DSO was instantiated */
-		if (g_dev == nullptr) {
-			goto fail;
-		}
-
-		/* both versions failed if the init for the MS5525DSO fails, give up */
-		if (OK != g_dev->AirspeedSim::init()) {
-			goto fail;
-		}
-	}
-
-	/* set the poll rate to default, starts automatic data collection */
-	fd = px4_open(PATH_MS4525, O_RDONLY);
-
-	if (fd < 0) {
-		goto fail;
-	}
-
-	if (px4_ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) {
-		goto fail;
-	}
-
-	return 0;
-
-fail:
-
-	if (g_dev != nullptr) {
-		delete g_dev;
-		g_dev = nullptr;
-	}
-
-	PX4_ERR("no MS4525 airspeedSim sensor connected");
-	return 1;
-}
-
-/**
- * Stop the driver
- */
-int
-stop()
-{
-	if (g_dev != nullptr) {
-		delete g_dev;
-		g_dev = nullptr;
-
-	} else {
-		PX4_ERR("driver not running");
-	}
-
-	return 0;
-}
-
-/**
- * Perform some basic functional tests on the driver;
- * make sure we can collect data from the sensor in polled
- * and automatic modes.
- */
-int
-test()
-{
-	struct differential_pressure_s report;
-	ssize_t sz;
-	int ret;
-
-	int fd = px4_open(PATH_MS4525, O_RDONLY);
-
-	if (fd < 0) {
-		PX4_ERR("%s open failed (try 'meas_airspeed_sim start' if the driver is not running", PATH_MS4525);
-		return 1;
-	}
-
-	/* do a simple demand read */
-	sz = px4_read(fd, &report, sizeof(report));
-
-	if (sz != sizeof(report)) {
-		PX4_ERR("immediate read failed");
-		return 1;
-	}
-
-	print_message(report);
-
-	/* start the sensor polling at 2Hz */
-	if (OK != px4_ioctl(fd, SENSORIOCSPOLLRATE, 2)) {
-		PX4_WARN("failed to set 2Hz poll rate");
-	}
-
-	/* read the sensor 5x and report each value */
-	for (unsigned i = 0; i < 5; i++) {
-		struct pollfd fds;
-
-		/* wait for data to be ready */
-		fds.fd = fd;
-		fds.events = POLLIN;
-		ret = poll(&fds, 1, 2000);
-
-		if (ret != 1) {
-			warnx("timed out");
-		}
-
-		/* now go get it */
-		sz = read(fd, &report, sizeof(report));
-
-		if (sz != sizeof(report)) {
-			PX4_WARN("periodic read failed");
-		}
-
-		print_message(report);
-	}
-
-	/* reset the sensor polling to its default rate */
-	if (OK != ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT)) {
-		PX4_ERR("failed to set default rate");
-		return 1;
-	}
-
-	PX4_WARN("PASS");
-
-	return 0;
-}
-
-/**
- * Reset the driver.
- */
-int
-reset()
-{
-	int fd = open(PATH_MS4525, O_RDONLY);
-
-	if (fd < 0) {
-		PX4_ERR("failed ");
-		return 1;
-	}
-
-	if (ioctl(fd, SENSORIOCRESET, 0) < 0) {
-		PX4_ERR("driver reset failed");
-		close(fd);
-		return 1;
-	}
-
-	if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) {
-		PX4_ERR("driver poll restart failed");
-		close(fd);
-		return 1;
-	}
-
-	close(fd);
-
-	return 0;
-}
-
-/**
- * Print a little info about the driver.
- */
-int
-info()
-{
-	if (g_dev == nullptr) {
-		PX4_WARN("driver not running");
-		return -1;
-	}
-
-	printf("state @ %p\n", g_dev);
-	g_dev->print_info();
-
-	return 0;
-}
-
-} // namespace
-
-
-static void
-meas_airspeed_usage()
-{
-	PX4_WARN("usage: measairspeedsim command [options]");
-	PX4_WARN("options:");
-	PX4_WARN("\t-b --bus i2cbus (%d)", 1);
-	PX4_WARN("command:");
-	PX4_WARN("\tstart|stop|reset|test|info");
-}
-
-int
-measairspeedsim_main(int argc, char *argv[])
-{
-	int i2c_bus = 1;//PX4_I2C_BUS_DEFAULT;
-
-	int i;
-
-	for (i = 1; i < argc; i++) {
-		if (strcmp(argv[i], "-b") == 0 || strcmp(argv[i], "--bus") == 0) {
-			if (argc > i + 1) {
-				i2c_bus = atoi(argv[i + 1]);
-			}
-		}
-	}
-
-	int ret = 1;
-
-	/*
-	 * Start/load the driver.
-	 */
-	if (!strcmp(argv[1], "start")) {
-		return meas_airspeed_sim::start(i2c_bus);
-	}
-
-	/*
-	 * Stop the driver
-	 */
-	if (!strcmp(argv[1], "stop")) {
-		return meas_airspeed_sim::stop();
-	}
-
-	/*
-	 * Test the driver/device.
-	 */
-	if (!strcmp(argv[1], "test")) {
-		return meas_airspeed_sim::test();
-	}
-
-	/*
-	 * Reset the driver.
-	 */
-	if (!strcmp(argv[1], "reset")) {
-		return meas_airspeed_sim::reset();
-	}
-
-	/*
-	 * Print driver information.
-	 */
-	if (!strcmp(argv[1], "info") || !strcmp(argv[1], "status")) {
-		return meas_airspeed_sim::info();
-	}
-
-	meas_airspeed_usage();
-	return ret;
-}

src/modules/simulator/barosim/CMakeLists.txt

@@ -1,45 +0,0 @@
-############################################################################
-#
-#   Copyright (c) 2015 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
-# are met:
-#
-# 1. Redistributions of source code must retain the above copyright
-#    notice, this list of conditions and the following disclaimer.
-# 2. Redistributions in binary form must reproduce the above copyright
-#    notice, this list of conditions and the following disclaimer in
-#    the documentation and/or other materials provided with the
-#    distribution.
-# 3. Neither the name PX4 nor the names of its contributors may be
-#    used to endorse or promote products derived from this software
-#    without specific prior written permission.
-#
-# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
-# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
-# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
-# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
-# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
-# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
-# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
-# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
-# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
-# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
-# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
-# POSSIBILITY OF SUCH DAMAGE.
-#
-############################################################################
-
-px4_add_module(
-	MODULE drivers__barosim
-	MAIN barosim
-	COMPILE_FLAGS
-		-Wno-double-promotion
-		-Wno-cast-align # TODO: fix and enable
-		-Wno-address-of-packed-member # TODO: fix in c_library_v2
-	SRCS
-		baro.cpp
-	DEPENDS
-		modules__simulator
-	)

src/modules/simulator/barosim/baro.cpp

@@ -1,347 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 2012-2015 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
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-/**
- * @file baro.cpp
- * Driver for the simulated barometric pressure sensor
- */
-
-#include <inttypes.h>
-#include <px4_config.h>
-#include <px4_defines.h>
-#include <px4_time.h>
-#include <px4_getopt.h>
-
-#include <sys/types.h>
-#include <stdint.h>
-#include <stdbool.h>
-#include <stdlib.h>
-#include <semaphore.h>
-#include <string.h>
-#include <fcntl.h>
-#include <errno.h>
-#include <stdio.h>
-#include <math.h>
-#include <unistd.h>
-
-#include <arch/board/board.h>
-#include <board_config.h>
-
-#include <drivers/device/device.h>
-#include <drivers/drv_baro.h>
-#include <drivers/drv_hrt.h>
-#include <drivers/device/ringbuffer.h>
-
-#include <simulator/simulator.h>
-
-#include <perf/perf_counter.h>
-#include <systemlib/err.h>
-
-#include "barosim.h"
-#include "VirtDevObj.hpp"
-
-/* helper macro for arithmetic - returns the square of the argument */
-#define POW2(_x)		((_x) * (_x))
-
-#define BAROSIM_DEV_PATH "/dev/barosim"
-
-#define BAROSIM_MEASURE_INTERVAL_US (10000)
-
-using namespace DriverFramework;
-
-class BAROSIM : public VirtDevObj
-{
-public:
-	BAROSIM(const char *path);
-	virtual ~BAROSIM();
-
-	virtual int init() override;
-
-	virtual int devIOCTL(unsigned long cmd, unsigned long arg) override;
-
-	/**
-	 * Diagnostics - print some basic information about the driver.
-	 */
-	void print_info();
-
-protected:
-
-	ringbuffer::RingBuffer	*_reports;
-
-	/* last report */
-	sensor_baro_s	report;
-
-	orb_advert_t		_baro_topic;
-	int			_orb_class_instance;
-
-	perf_counter_t		_sample_perf;
-	perf_counter_t		_measure_perf;
-	perf_counter_t		_comms_errors;
-	/**
-	 * Get the internal / external state
-	 *
-	 * @return true if the sensor is not on the main MCU board
-	 */
-	bool			is_external() { return (_orb_class_instance == 0); /* XXX put this into the interface class */ }
-
-	virtual void _measure() override;
-
-	/**
-	 * Collect the result of the most recent measurement.
-	 */
-	virtual int		collect();
-};
-
-static BAROSIM *g_barosim = nullptr;
-
-/*
- * Driver 'main' command.
- */
-extern "C" __EXPORT int barosim_main(int argc, char *argv[]);
-
-BAROSIM::BAROSIM(const char *path) :
-	VirtDevObj("BAROSIM", path, BARO_BASE_DEVICE_PATH, BAROSIM_MEASURE_INTERVAL_US),
-	_reports(nullptr),
-	report{},
-	_baro_topic(nullptr),
-	_orb_class_instance(-1),
-	_sample_perf(perf_alloc(PC_ELAPSED, "barosim_read")),
-	_measure_perf(perf_alloc(PC_ELAPSED, "barosim_measure")),
-	_comms_errors(perf_alloc(PC_COUNT, "barosim_comms_errors"))
-{
-}
-
-BAROSIM::~BAROSIM()
-{
-	/* make sure we are truly inactive */
-	stop();
-	setSampleInterval(0);
-
-	/* free any existing reports */
-	if (_reports != nullptr) {
-		delete _reports;
-	}
-
-	// free perf counters
-	perf_free(_sample_perf);
-	perf_free(_measure_perf);
-	perf_free(_comms_errors);
-}
-
-int
-BAROSIM::init()
-{
-	int ret;
-	sensor_baro_s brp = {};
-
-	ret = VirtDevObj::init();
-
-	if (ret != OK) {
-		PX4_ERR("VirtDevObj init failed");
-		goto out;
-	}
-
-	/* allocate basic report buffers */
-	_reports = new ringbuffer::RingBuffer(2, sizeof(sensor_baro_s));
-
-	if (_reports == nullptr) {
-		PX4_ERR("can't get memory for reports");
-		ret = -ENOMEM;
-		goto out;
-	}
-
-	_reports->flush();
-
-	_baro_topic = orb_advertise_multi(ORB_ID(sensor_baro), &brp,
-					  &_orb_class_instance, (is_external()) ? ORB_PRIO_HIGH : ORB_PRIO_DEFAULT);
-
-	if (_baro_topic == nullptr) {
-		PX4_ERR("failed to create sensor_baro publication");
-		return -ENODEV;
-	}
-
-	/* fill report structures */
-	start();
-
-out:
-	return ret;
-}
-
-int
-BAROSIM::devIOCTL(unsigned long cmd, unsigned long arg)
-{
-	/* give it to the bus-specific superclass */
-	// return bus_ioctl(filp, cmd, arg);
-	return VirtDevObj::devIOCTL(cmd, arg);
-}
-
-void
-BAROSIM::_measure()
-{
-	collect();
-}
-
-int
-BAROSIM::collect()
-{
-	bool status;
-
-	simulator::RawBaroData raw_baro;
-
-	perf_begin(_sample_perf);
-
-	report.timestamp = hrt_absolute_time();
-	report.error_count = perf_event_count(_comms_errors);
-
-	/* read requested */
-	Simulator *sim = Simulator::getInstance();
-
-	if (sim == nullptr) {
-		PX4_ERR("Error BAROSIM_DEV::transfer no simulator");
-		return -ENODEV;
-	}
-
-	PX4_DEBUG("BAROSIM_DEV::transfer getting sample");
-	status = sim->getBaroSample((uint8_t *)(&raw_baro), sizeof(raw_baro));
-
-	if (!status) {
-		perf_count(_comms_errors);
-		perf_end(_sample_perf);
-		return -1;
-	}
-
-	report.pressure = raw_baro.pressure;
-	report.temperature = raw_baro.temperature;
-
-	/* fake device ID */
-	report.device_id = 478459;
-
-	int instance;
-	orb_publish_auto(ORB_ID(sensor_baro), &_baro_topic, &report, &instance, ORB_PRIO_DEFAULT);
-
-	_reports->force(&report);
-
-	perf_end(_sample_perf);
-
-	return OK;
-}
-
-void
-BAROSIM::print_info()
-{
-	perf_print_counter(_sample_perf);
-	perf_print_counter(_comms_errors);
-	PX4_INFO("poll interval:  %u usec", m_sample_interval_usecs);
-	_reports->print_info("report queue");
-	PX4_INFO("TEMP:           %f", (double)report.temperature);
-	PX4_INFO("P:              %.3f", (double)report.pressure);
-}
-
-namespace barosim
-{
-
-/**
- * Start the driver.
- *
- * This function call only returns once the driver
- * is either successfully up and running or failed to start.
- */
-static int
-start()
-{
-	g_barosim = new BAROSIM(BAROSIM_DEV_PATH);
-
-	if (g_barosim != nullptr && OK != g_barosim->init()) {
-		delete g_barosim;
-		g_barosim = nullptr;
-		PX4_ERR("bus init failed");
-		return false;
-	}
-
-	return 0;
-}
-
-/**
- * Print a little info about the driver.
- */
-static int
-info()
-{
-	if (g_barosim != nullptr) {
-		PX4_INFO("%s", BAROSIM_DEV_PATH);
-		g_barosim->print_info();
-	}
-
-	return 0;
-}
-
-static void
-usage()
-{
-	PX4_WARN("missing command: try 'start', 'info'");
-}
-
-}; // namespace barosim
-
-int
-barosim_main(int argc, char *argv[])
-{
-	int ret;
-
-	if (argc < 2) {
-		barosim::usage();
-		return 1;
-	}
-
-	const char *verb = argv[1];
-
-	/*
-	 * Start/load the driver.
-	 */
-	if (!strcmp(verb, "start")) {
-		ret = barosim::start();
-	}
-
-	/*
-	 * Print driver information.
-	 */
-	else if (!strcmp(verb, "info")) {
-		ret = barosim::info();
-
-	} else {
-		barosim::usage();
-		return 1;
-	}
-
-	return ret;
-}

src/modules/simulator/barosim/barosim.h

@@ -1,42 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (C) 2012-2013 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
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-/**
- * @file barosim.h
- *
- * A simulated Barometer.
- */
-#pragma once
-
-#include "VirtDevObj.hpp"
-

src/modules/simulator/gyrosim/CMakeLists.txt

@@ -1,46 +0,0 @@
-############################################################################
-#
-#   Copyright (c) 2015 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
-# are met:
-#
-# 1. Redistributions of source code must retain the above copyright
-#    notice, this list of conditions and the following disclaimer.
-# 2. Redistributions in binary form must reproduce the above copyright
-#    notice, this list of conditions and the following disclaimer in
-#    the documentation and/or other materials provided with the
-#    distribution.
-# 3. Neither the name PX4 nor the names of its contributors may be
-#    used to endorse or promote products derived from this software
-#    without specific prior written permission.
-#
-# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
-# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
-# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
-# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
-# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
-# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
-# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
-# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
-# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
-# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
-# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
-# POSSIBILITY OF SUCH DAMAGE.
-#
-############################################################################
-
-px4_add_module(
-	MODULE drivers__gyrosim
-	MAIN gyrosim
-	COMPILE_FLAGS
-		-Wno-double-promotion
-		-Wno-cast-align # TODO: fix and enable
-		-Wno-address-of-packed-member # TODO: fix in c_library_v2
-	STACK_MAIN 1200
-	SRCS
-		gyrosim.cpp
-	DEPENDS
-		modules__simulator
-)

src/modules/simulator/simulator.cpp

@@ -65,66 +65,16 @@ Simulator *Simulator::getInstance()
 	return _instance;
 }
 
-bool Simulator::getMPUReport(uint8_t *buf, int len)
-{
-	return _mpu.copyData(buf, len);
-}
-
-bool Simulator::getRawAccelReport(uint8_t *buf, int len)
-{
-	return _accel.copyData(buf, len);
-}
-
-bool Simulator::getMagReport(uint8_t *buf, int len)
-{
-	return _mag.copyData(buf, len);
-}
-
-bool Simulator::getBaroSample(uint8_t *buf, int len)
-{
-	return _baro.copyData(buf, len);
-}
-
 bool Simulator::getGPSSample(uint8_t *buf, int len)
 {
 	return _gps.copyData(buf, len);
 }
 
-bool Simulator::getAirspeedSample(uint8_t *buf, int len)
-{
-	return _airspeed.copyData(buf, len);
-}
-
-void Simulator::write_MPU_data(void *buf)
-{
-	_mpu.writeData(buf);
-}
-
-void Simulator::write_accel_data(void *buf)
-{
-	_accel.writeData(buf);
-}
-
-void Simulator::write_mag_data(void *buf)
-{
-	_mag.writeData(buf);
-}
-
-void Simulator::write_baro_data(void *buf)
-{
-	_baro.writeData(buf);
-}
-
 void Simulator::write_gps_data(void *buf)
 {
 	_gps.writeData(buf);
 }
 
-void Simulator::write_airspeed_data(void *buf)
-{
-	_airspeed.writeData(buf);
-}
-
 void Simulator::parameters_update(bool force)
 {
 	bool updated;
@@ -149,33 +99,21 @@ int Simulator::start(int argc, char *argv[])
 	if (_instance) {
 		drv_led_start();
 
-		if (argc == 5 && strcmp(argv[3], "-u") == 0) {
+		if (argc == 4 && strcmp(argv[2], "-u") == 0) {
 			_instance->set_ip(InternetProtocol::UDP);
-			_instance->set_port(atoi(argv[4]));
+			_instance->set_port(atoi(argv[3]));
 		}
 
-		if (argc == 5 && strcmp(argv[3], "-c") == 0) {
+		if (argc == 4 && strcmp(argv[2], "-c") == 0) {
 			_instance->set_ip(InternetProtocol::TCP);
-			_instance->set_port(atoi(argv[4]));
+			_instance->set_port(atoi(argv[3]));
 		}
 
-		if (argv[2][1] == 's') {
-			_instance->initialize_sensor_data();
 #ifndef __PX4_QURT
 		// Update sensor data
 		_instance->poll_for_MAVLink_messages();
 #endif
 
-		} else if (argv[2][1] == 'p') {
-			// Update sensor data
-			_instance->set_publish(true);
-			_instance->poll_for_MAVLink_messages();
-
-		} else {
-			_instance->initialize_sensor_data();
-			_instance->_initialized = true;
-		}
-
 		return 0;
 
 	} else {
@@ -197,11 +135,9 @@ void Simulator::set_port(unsigned port)
 static void usage()
 {
 	PX4_WARN("Usage: simulator {start -[spt] [-u udp_port / -c tcp_port] |stop}");
-	PX4_WARN("Simulate raw sensors:     simulator start -s");
+	PX4_WARN("Start simulator:     simulator start");
-	PX4_WARN("Publish sensors combined: simulator start -p");
 	PX4_WARN("Connect using UDP: simulator start -u udp_port");
 	PX4_WARN("Connect using TCP: simulator start -c tcp_port");
-	PX4_WARN("Dummy unit test data:     simulator start -t");
 }
 
 __BEGIN_DECLS
@@ -212,10 +148,7 @@ extern "C" {
 
 	int simulator_main(int argc, char *argv[])
 	{
-		if (argc > 2 && strcmp(argv[1], "start") == 0) {
+		if (argc > 1 && strcmp(argv[1], "start") == 0) {
-			if (strcmp(argv[2], "-s") == 0 ||
-			    strcmp(argv[2], "-p") == 0 ||
-			    strcmp(argv[2], "-t") == 0) {
 
 			if (g_sim_task >= 0) {
 				PX4_WARN("Simulator already started");
@@ -230,7 +163,7 @@ extern "C" {
 							argv);
 
 			while (true) {
-					if (Simulator::getInstance() && Simulator::getInstance()->isInitialized()) {
+				if (Simulator::getInstance()) {
 					break;
 
 				} else {
@@ -238,11 +171,6 @@ extern "C" {
 				}
 			}
 
-			} else {
-				usage();
-				return 1;
-			}
-
 		} else if (argc == 2 && strcmp(argv[1], "stop") == 0) {
 			if (g_sim_task < 0) {
 				PX4_WARN("Simulator not running");

src/modules/simulator/simulator.h

@@ -42,80 +42,38 @@
 
 #pragma once
 
-#include <px4_posix.h>
+#include <battery/battery.h>
-#include <px4_module_params.h>
-#include <uORB/topics/manual_control_setpoint.h>
-#include <uORB/topics/actuator_outputs.h>
-#include <uORB/topics/vehicle_attitude.h>
-#include <uORB/topics/vehicle_local_position.h>
-#include <uORB/topics/vehicle_global_position.h>
-#include <uORB/topics/vehicle_odometry.h>
-#include <uORB/topics/vehicle_status.h>
-#include <uORB/topics/battery_status.h>
-#include <uORB/topics/irlock_report.h>
-#include <uORB/topics/parameter_update.h>
-#include <drivers/drv_accel.h>
-#include <drivers/drv_gyro.h>
-#include <drivers/drv_baro.h>
-#include <drivers/drv_mag.h>
 #include <drivers/drv_hrt.h>
 #include <drivers/drv_rc_input.h>
-#include <perf/perf_counter.h>
+#include <lib/drivers/accelerometer/PX4Accelerometer.hpp>
-#include <battery/battery.h>
+#include <lib/drivers/barometer/PX4Barometer.hpp>
-#include <uORB/uORB.h>
+#include <lib/drivers/gyroscope/PX4Gyroscope.hpp>
-#include <uORB/topics/optical_flow.h>
+#include <lib/drivers/magnetometer/PX4Magnetometer.hpp>
-#include <uORB/topics/distance_sensor.h>
-#include <v2.0/mavlink_types.h>
-#include <v2.0/common/mavlink.h>
 #include <lib/ecl/geo/geo.h>
+#include <perf/perf_counter.h>
+#include <px4_module_params.h>
+#include <px4_posix.h>
+#include <uORB/topics/actuator_outputs.h>
+#include <uORB/topics/battery_status.h>
+#include <uORB/topics/differential_pressure.h>
+#include <uORB/topics/distance_sensor.h>
+#include <uORB/topics/irlock_report.h>
+#include <uORB/topics/manual_control_setpoint.h>
+#include <uORB/topics/optical_flow.h>
+#include <uORB/topics/parameter_update.h>
+#include <uORB/topics/vehicle_attitude.h>
+#include <uORB/topics/vehicle_global_position.h>
+#include <uORB/topics/vehicle_local_position.h>
+#include <uORB/topics/vehicle_odometry.h>
+#include <uORB/topics/vehicle_status.h>
+#include <uORB/uORB.h>
+
+#include <v2.0/common/mavlink.h>
+#include <v2.0/mavlink_types.h>
 
 namespace simulator
 {
 
-#pragma pack(push, 1)
-struct RawAccelData {
-	float temperature;
-	float x;
-	float y;
-	float z;
-};
-#pragma pack(pop)
-
-#pragma pack(push, 1)
-struct RawMagData {
-	float temperature;
-	float x;
-	float y;
-	float z;
-};
-#pragma pack(pop)
-
-#pragma pack(push, 1)
-struct RawMPUData {
-	float	accel_x;
-	float	accel_y;
-	float	accel_z;
-	float	temp;
-	float	gyro_x;
-	float	gyro_y;
-	float	gyro_z;
-};
-#pragma pack(pop)
-
-#pragma pack(push, 1)
-struct RawBaroData {
-	float pressure;
-	float temperature;
-};
-#pragma pack(pop)
-
-#pragma pack(push, 1)
-struct RawAirspeedData {
-	float temperature;
-	float diff_pressure;
-};
-#pragma pack(pop)
-
 #pragma pack(push, 1)
 struct RawGPSData {
 	uint64_t timestamp;
@@ -198,20 +156,6 @@ class Simulator : public ModuleParams
 public:
 	static Simulator *getInstance();
 
-	enum sim_dev_t {
-		SIM_GYRO,
-		SIM_ACCEL,
-		SIM_MAG
-	};
-
-	struct sample {
-		float x;
-		float y;
-		float z;
-		sample() : x(0), y(0), z(0) {}
-		sample(float a, float b, float c) : x(a), y(b), z(c) {}
-	};
-
 	enum class InternetProtocol {
 		TCP,
 		UDP
@@ -219,21 +163,9 @@ public:
 
 	static int start(int argc, char *argv[]);
 
-	bool getAirspeedSample(uint8_t *buf, int len);
-	bool getBaroSample(uint8_t *buf, int len);
 	bool getGPSSample(uint8_t *buf, int len);
-	bool getMagReport(uint8_t *buf, int len);
-	bool getMPUReport(uint8_t *buf, int len);
-	bool getRawAccelReport(uint8_t *buf, int len);
 
-	void write_airspeed_data(void *buf);
-	void write_accel_data(void *buf);
-	void write_baro_data(void *buf);
 	void write_gps_data(void *buf);
-	void write_mag_data(void *buf);
-	void write_MPU_data(void *buf);
-
-	bool isInitialized() { return _initialized; }
 
 	void set_ip(InternetProtocol ip);
 	void set_port(unsigned port);
@@ -251,6 +183,9 @@ private:
 		_param_sub = orb_subscribe(ORB_ID(parameter_update));
 
 		_battery_status.timestamp = hrt_absolute_time();
+
+		_px4_accel.set_sample_rate(250);
+		_px4_gyro.set_sample_rate(250);
 	}
 
 	~Simulator()
@@ -259,22 +194,15 @@ private:
 		orb_unsubscribe(_param_sub);
 
 		// free perf counters
-		perf_free(_perf_accel);
-		perf_free(_perf_airspeed);
-		perf_free(_perf_baro);
 		perf_free(_perf_gps);
-		perf_free(_perf_mag);
-		perf_free(_perf_mpu);
 		perf_free(_perf_sim_delay);
 		perf_free(_perf_sim_interval);
 
-		_instance = NULL;
+		_instance = nullptr;
 	}
 
 	// class methods
-	void initialize_sensor_data();
 
-	int publish_sensor_topics(const mavlink_hil_sensor_t *imu);
 	int publish_flow_topic(const mavlink_hil_optical_flow_t *flow);
 	int publish_odometry_topic(const mavlink_message_t *odom_mavlink);
 	int publish_distance_topic(const mavlink_distance_sensor_t *dist);
@@ -282,31 +210,23 @@ private:
 	static Simulator *_instance;
 
 	// simulated sensor instances
-	simulator::Report<simulator::RawAirspeedData>	_airspeed{1};
+	PX4Accelerometer	_px4_accel{1311244, ORB_PRIO_DEFAULT, ROTATION_NONE}; // 1311244: DRV_ACC_DEVTYPE_ACCELSIM, BUS: 1, ADDR: 1, TYPE: SIMULATION
-	simulator::Report<simulator::RawAccelData>	_accel{1};
+	PX4Gyroscope		_px4_gyro{2294028, ORB_PRIO_DEFAULT, ROTATION_NONE}; // 2294028: DRV_GYR_DEVTYPE_GYROSIM, BUS: 1, ADDR: 2, TYPE: SIMULATION
-	simulator::Report<simulator::RawBaroData>	_baro{1};
+	PX4Magnetometer		_px4_mag{197388, ORB_PRIO_DEFAULT, ROTATION_NONE}; // 197388: DRV_MAG_DEVTYPE_MAGSIM, BUS: 3, ADDR: 1, TYPE: SIMULATION
-	simulator::Report<simulator::RawGPSData>	_gps{1};
+	PX4Barometer		_px4_baro{6620172, ORB_PRIO_DEFAULT}; // 6620172: DRV_BARO_DEVTYPE_BAROSIM, BUS: 1, ADDR: 4, TYPE: SIMULATION
-	simulator::Report<simulator::RawMagData>	_mag{1};
+
-	simulator::Report<simulator::RawMPUData>	_mpu{1};
+	simulator::Report<simulator::RawGPSData>	_gps{1};
 
-	perf_counter_t _perf_accel{perf_alloc_once(PC_ELAPSED, "sim_accel_delay")};
-	perf_counter_t _perf_airspeed{perf_alloc_once(PC_ELAPSED, "sim_airspeed_delay")};
-	perf_counter_t _perf_baro{perf_alloc_once(PC_ELAPSED, "sim_baro_delay")};
 	perf_counter_t _perf_gps{perf_alloc_once(PC_ELAPSED, "sim_gps_delay")};
-	perf_counter_t _perf_mag{perf_alloc_once(PC_ELAPSED, "sim_mag_delay")};
-	perf_counter_t _perf_mpu{perf_alloc_once(PC_ELAPSED, "sim_mpu_delay")};
 	perf_counter_t _perf_sim_delay{perf_alloc_once(PC_ELAPSED, "sim_network_delay")};
 	perf_counter_t _perf_sim_interval{perf_alloc(PC_INTERVAL, "sim_network_interval")};
 
 	// uORB publisher handlers
-	orb_advert_t _accel_pub{nullptr};
-	orb_advert_t _baro_pub{nullptr};
 	orb_advert_t _battery_pub{nullptr};
+	orb_advert_t _differential_pressure_pub{nullptr};
 	orb_advert_t _dist_pub{nullptr};
 	orb_advert_t _flow_pub{nullptr};
-	orb_advert_t _gyro_pub{nullptr};
 	orb_advert_t _irlock_report_pub{nullptr};
-	orb_advert_t _mag_pub{nullptr};
 	orb_advert_t _visual_odometry_pub{nullptr};
 
 	int _param_sub{-1};
@@ -315,8 +235,6 @@ private:
 
 	InternetProtocol _ip{InternetProtocol::UDP};
 
-	bool _initialized{false};
-
 	double _realtime_factor{1.0};		///< How fast the simulation runs in comparison to real system time
 
 	hrt_abstime _last_sim_timestamp{0};
@@ -349,8 +267,7 @@ private:
 	void send_controls();
 	void send_heartbeat();
 	void send_mavlink_message(const mavlink_message_t &aMsg);
-	void set_publish(const bool publish = true);
+	void update_sensors(const hrt_abstime &time, const mavlink_hil_sensor_t &imu);
-	void update_sensors(const mavlink_hil_sensor_t *imu);
 	void update_gps(const mavlink_hil_gps_t *gps_sim);
 
 	static void *sending_trampoline(void *);
@@ -369,7 +286,6 @@ private:
 	struct map_projection_reference_s _hil_local_proj_ref {};
 
 	bool _hil_local_proj_inited{false};
-	bool _publish{false};
 
 	double _hil_ref_lat{0};
 	double _hil_ref_lon{0};

src/modules/simulator/simulator_mavlink.cpp

@@ -220,51 +220,53 @@ static void fill_rc_input_msg(input_rc_s *rc, mavlink_rc_channels_t *rc_channels
 	rc->values[17] = rc_channels->chan18_raw;
 }
 
-void Simulator::update_sensors(const mavlink_hil_sensor_t *imu)
+void Simulator::update_sensors(const hrt_abstime &time, const mavlink_hil_sensor_t &imu)
 {
-	// write sensor data to memory so that drivers can copy data from there
+	if ((imu.fields_updated & 0x1FFF) != 0x1FFF) {
-	RawMPUData mpu = {};
+		PX4_DEBUG("All sensor fields in mavlink HIL_SENSOR packet not updated.  Got %08x", imu.fields_updated);
-	mpu.accel_x = imu->xacc;
+	}
-	mpu.accel_y = imu->yacc;
-	mpu.accel_z = imu->zacc;
-	mpu.temp = imu->temperature;
-	mpu.gyro_x = imu->xgyro;
-	mpu.gyro_y = imu->ygyro;
-	mpu.gyro_z = imu->zgyro;
 
-	write_MPU_data(&mpu);
+	// gyro
-	perf_begin(_perf_mpu);
+	{
+		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);
+	}
 
-	RawAccelData accel = {};
+	// accel
-	accel.x = imu->xacc;
+	{
-	accel.y = imu->yacc;
+		static constexpr float scaling = 1000.0f;
-	accel.z = imu->zacc;
+		_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);
+	}
 
-	write_accel_data(&accel);
+	// magnetometer
-	perf_begin(_perf_accel);
+	{
+		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);
+	}
 
-	RawMagData mag = {};
+	// baro
-	mag.x = imu->xmag;
+	{
-	mag.y = imu->ymag;
+		_px4_baro.set_temperature(imu.temperature);
-	mag.z = imu->zmag;
+		_px4_baro.update(time, imu.abs_pressure);
+	}
 
-	write_mag_data(&mag);
+	// differential pressure
-	perf_begin(_perf_mag);
+	{
+		differential_pressure_s report{};
+		report.timestamp = time;
+		report.temperature = imu.temperature;
+		report.differential_pressure_filtered_pa = imu.diff_pressure * 100.0f; // convert from millibar to bar;
+		report.differential_pressure_raw_pa = imu.diff_pressure * 100.0f; // convert from millibar to bar;
 
-	RawBaroData baro = {};
+		int instance;
-
+		orb_publish_auto(ORB_ID(differential_pressure), &_differential_pressure_pub, &report, &instance, ORB_PRIO_DEFAULT);
-	// Get air pressure and pressure altitude
+	}
-	// valid for troposphere (below 11km AMSL)
-	baro.pressure = imu->abs_pressure;
-	baro.temperature = imu->temperature;
-
-	write_baro_data(&baro);
-
-	RawAirspeedData airspeed = {};
-	airspeed.temperature = imu->temperature;
-	airspeed.diff_pressure = imu->diff_pressure;
-
-	write_airspeed_data(&airspeed);
 }
 
 void Simulator::update_gps(const mavlink_hil_gps_t *gps_sim)
@@ -340,10 +342,6 @@ void Simulator::handle_message_hil_gps(const mavlink_message_t *msg)
 	mavlink_hil_gps_t gps_sim;
 	mavlink_msg_hil_gps_decode(msg, &gps_sim);
 
-	if (_publish) {
-		//PX4_WARN("FIXME:  Need to publish GPS topic.  Not done yet.");
-	}
-
 	update_gps(&gps_sim);
 }
 
@@ -352,9 +350,6 @@ void Simulator::handle_message_hil_sensor(const mavlink_message_t *msg)
 	mavlink_hil_sensor_t imu;
 	mavlink_msg_hil_sensor_decode(msg, &imu);
 
-	// set temperature to a decent value
-	imu.temperature = 32.0f;
-
 	struct timespec ts;
 	abstime_to_ts(&ts, imu.time_usec);
 	px4_clock_settime(CLOCK_MONOTONIC, &ts);
@@ -374,11 +369,7 @@ void Simulator::handle_message_hil_sensor(const mavlink_message_t *msg)
 	last_time = now_us;
 #endif
 
-	if (_publish) {
+	update_sensors(now_us, imu);
-		publish_sensor_topics(&imu);
-	}
-
-	update_sensors(&imu);
 
 	// battery simulation (limit update to 100Hz)
 	if (hrt_elapsed_time(&_battery_status.timestamp) >= 10_ms) {
@@ -540,11 +531,9 @@ void Simulator::handle_message_rc_channels(const mavlink_message_t *msg)
 	fill_rc_input_msg(&_rc_input, &rc_channels);
 
 	// publish message
-	if (_publish) {
 	int rc_multi;
 	orb_publish_auto(ORB_ID(input_rc), &_rc_channels_pub, &_rc_input, &rc_multi, ORB_PRIO_HIGH);
 }
-}
 
 void Simulator::handle_message_vision_position_estimate(const mavlink_message_t *msg)
 {
@@ -652,38 +641,6 @@ void Simulator::send_heartbeat()
 	send_mavlink_message(message);
 }
 
-void Simulator::initialize_sensor_data()
-{
-	// Write sensor data to memory so that drivers can copy data from there.
-	RawMPUData mpu = {};
-	mpu.accel_z = CONSTANTS_ONE_G;
-
-	write_MPU_data(&mpu);
-
-	RawAccelData accel = {};
-	accel.z = CONSTANTS_ONE_G;
-
-	write_accel_data(&accel);
-
-	RawMagData mag = {};
-	mag.x = 0.4f;
-	mag.y = 0.0f;
-	mag.z = 0.6f;
-
-	write_mag_data((void *)&mag);
-
-	RawBaroData baro = {};
-	// calculate air pressure from altitude (valid for low altitude)
-	baro.pressure = 120000.0f;
-	baro.temperature = 25.0f;
-
-	write_baro_data(&baro);
-
-	RawAirspeedData airspeed {};
-
-	write_airspeed_data(&airspeed);
-}
-
 void Simulator::poll_for_MAVLink_messages()
 {
 #ifdef __PX4_DARWIN
@@ -812,8 +769,6 @@ void Simulator::poll_for_MAVLink_messages()
 	// Request HIL_STATE_QUATERNION for ground truth.
 	request_hil_state_quaternion();
 
-	_initialized = true;
-
 	while (true) {
 
 		// wait for new mavlink messages to arrive
@@ -857,11 +812,7 @@ void Simulator::poll_for_MAVLink_messages()
 
 				for (int i = 0; i < len; ++i) {
 					if (mavlink_parse_char(MAVLINK_COMM_1, serial_buf[i], &msg, &serial_status)) {
-						// have a message, handle it
-						bool prev_publish = _publish;
-						set_publish(true);
 						handle_message(&msg);
-						set_publish(prev_publish);
 					}
 				}
 			}
@@ -969,101 +920,6 @@ int openUart(const char *uart_name, int baud)
 }
 #endif
 
-int Simulator::publish_sensor_topics(const mavlink_hil_sensor_t *imu)
-{
-	uint64_t timestamp = hrt_absolute_time();
-
-	if ((imu->fields_updated & 0x1FFF) != 0x1FFF) {
-		PX4_DEBUG("All sensor fields in mavlink HIL_SENSOR packet not updated.  Got %08x", imu->fields_updated);
-	}
-
-	/*
-	  static int count=0;
-	  static uint64_t last_timestamp=0;
-	  count++;
-	  if (!(count % 200)) {
-		PX4_WARN("TIME : %lu,   dt: %lu",
-			 (unsigned long) timestamp,(unsigned long) timestamp - (unsigned long) last_timestamp);
-		PX4_WARN("IMU  : %f %f %f",imu->xgyro,imu->ygyro,imu->zgyro);
-		PX4_WARN("ACCEL: %f %f %f",imu->xacc,imu->yacc,imu->zacc);
-		PX4_WARN("MAG  : %f %f %f",imu->xmag,imu->ymag,imu->zmag);
-		PX4_WARN("BARO : %f %f %f",imu->abs_pressure,imu->pressure_alt,imu->temperature);
-	}
-	last_timestamp = timestamp;
-	*/
-	/* gyro */
-	{
-		sensor_gyro_s gyro = {};
-
-		gyro.timestamp = timestamp;
-		gyro.device_id = 2293768;
-		gyro.x_raw = imu->xgyro * 1000.0f;
-		gyro.y_raw = imu->ygyro * 1000.0f;
-		gyro.z_raw = imu->zgyro * 1000.0f;
-		gyro.x = imu->xgyro;
-		gyro.y = imu->ygyro;
-		gyro.z = imu->zgyro;
-
-		gyro.temperature = imu->temperature;
-
-		int gyro_multi;
-		orb_publish_auto(ORB_ID(sensor_gyro), &_gyro_pub, &gyro, &gyro_multi, ORB_PRIO_HIGH);
-	}
-
-	/* accelerometer */
-	{
-		sensor_accel_s accel = {};
-
-		accel.timestamp = timestamp;
-		accel.device_id = 1376264;
-		accel.x_raw = imu->xacc / (CONSTANTS_ONE_G / 1000.0f);
-		accel.y_raw = imu->yacc / (CONSTANTS_ONE_G / 1000.0f);
-		accel.z_raw = imu->zacc / (CONSTANTS_ONE_G / 1000.0f);
-		accel.x = imu->xacc;
-		accel.y = imu->yacc;
-		accel.z = imu->zacc;
-
-		accel.temperature = imu->temperature;
-
-		int accel_multi;
-		orb_publish_auto(ORB_ID(sensor_accel), &_accel_pub, &accel, &accel_multi, ORB_PRIO_HIGH);
-	}
-
-	/* magnetometer */
-	{
-		struct mag_report mag = {};
-
-		mag.timestamp = timestamp;
-		mag.device_id = 196616;
-		mag.x_raw = imu->xmag * 1000.0f;
-		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.temperature = imu->temperature;
-
-		int mag_multi;
-		orb_publish_auto(ORB_ID(sensor_mag), &_mag_pub, &mag, &mag_multi, ORB_PRIO_HIGH);
-	}
-
-	/* baro */
-	{
-		sensor_baro_s baro = {};
-
-		baro.timestamp = timestamp;
-		baro.device_id = 478459;
-		baro.pressure = imu->abs_pressure;
-		baro.temperature = imu->temperature;
-
-		int baro_multi;
-		orb_publish_auto(ORB_ID(sensor_baro), &_baro_pub, &baro, &baro_multi, ORB_PRIO_HIGH);
-	}
-
-	return OK;
-}
-
 int Simulator::publish_flow_topic(const mavlink_hil_optical_flow_t *flow_mavlink)
 {
 	uint64_t timestamp = hrt_absolute_time();
@@ -1240,8 +1096,3 @@ int Simulator::publish_distance_topic(const mavlink_distance_sensor_t *dist_mavl
 
 	return OK;
 }
-
-void Simulator::set_publish(const bool publish)
-{
-	_publish = publish;
-}