simulator move to PX4Accelerometer, PX4Gyroscope, PX4Magnetometer, PX…

…4Barometer helpers (#12081)
PX4 · May 31, 2019 · 43e3fc7 · 43e3fc7
1 parent 4a4d323
commit 43e3fc7
Show file tree

Hide file tree

Showing 30 changed files with 149 additions and 4,453 deletions.
diff --git a/ROMFS/px4fmu_common/init.d-posix/rcS b/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_ACC1_ID 1310728
-	param set CAL_ACC_PRIME 1376264
+	param set CAL_ACC0_ID 1311244
+	param set CAL_ACC_PRIME 1311244
 
-	param set CAL_GYRO0_ID 2293768
-	param set CAL_GYRO_PRIME 2293768
+	param set CAL_GYRO0_ID 2294028
+	param set CAL_GYRO_PRIME 2294028
 
-	param set CAL_MAG0_ID 196616
-	param set CAL_MAG_PRIME 196616
+	param set CAL_MAG0_ID 197388
+	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.

diff --git a/posix-configs/SITL/init/test/cmd_template.in b/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

diff --git a/posix-configs/SITL/init/test/test_mavlink b/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

diff --git a/posix-configs/SITL/init/test/test_shutdown b/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

diff --git a/posix-configs/SITL/init/test/test_template.in b/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

diff --git a/posix-configs/eagle/hil/mainapphil.config b/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
diff --git a/src/drivers/drv_sensor.h b/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

diff --git a/src/lib/drivers/CMakeLists.txt b/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)

diff --git a/src/lib/drivers/accelerometer/CMakeLists.txt b/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)
diff --git a/src/lib/drivers/accelerometer/PX4Accelerometer.cpp b/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();

diff --git a/src/lib/drivers/accelerometer/PX4Accelerometer.hpp b/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,16 +57,16 @@ class PX4Accelerometer : public cdev::CDev, public ModuleParams
 	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 configure_filter(float cutoff_freq) { _filter.set_cutoff_frequency(_sample_rate, cutoff_freq); }
+	void set_sample_rate(unsigned rate);
 
 	void update(hrt_abstime timestamp, int16_t x, int16_t y, int16_t z);
 
 	void print_status();
 
 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};
@@ -79,7 +79,7 @@ class PX4Accelerometer : public cdev::CDev, public ModuleParams
 
 	int			_class_device_instance{-1};
 
-	unsigned	_sample_rate{1000};
+	unsigned		_sample_rate{1000};
 
 	DEFINE_PARAMETERS(
 		(ParamFloat<px4::params::IMU_ACCEL_CUTOFF>) _param_imu_accel_cutoff

diff --git a/src/lib/drivers/device/Device.hpp b/src/lib/drivers/device/Device.hpp
@@ -130,6 +130,7 @@ class __EXPORT Device
 		DeviceBusType_I2C     = 1,
 		DeviceBusType_SPI     = 2,
 		DeviceBusType_UAVCAN  = 3,
+		DeviceBusType_SIMULATION = 4,
 	};
 
 	/**
@@ -160,6 +161,9 @@ class __EXPORT Device
 		case DeviceBusType_UAVCAN:
 			return "UAVCAN";
 
+		case DeviceBusType_SIMULATION:
+			return "SIMULATION";
+
 		case DeviceBusType_UNKNOWN:
 		default:
 			return "UNKNOWN";

diff --git a/src/lib/drivers/gyroscope/CMakeLists.txt b/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)
diff --git a/src/lib/drivers/gyroscope/PX4Gyroscope.cpp b/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();

diff --git a/src/lib/drivers/gyroscope/PX4Gyroscope.hpp b/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,16 +57,16 @@ class PX4Gyroscope : public cdev::CDev, public ModuleParams
 	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 configure_filter(float cutoff_freq) { _filter.set_cutoff_frequency(_sample_rate, cutoff_freq); }
+	void set_sample_rate(unsigned rate);
 
 	void update(hrt_abstime timestamp, int16_t x, int16_t y, int16_t z);
 
 	void print_status();
 
 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};
@@ -79,7 +79,7 @@ class PX4Gyroscope : public cdev::CDev, public ModuleParams
 
 	int			_class_device_instance{-1};
 
-	unsigned	_sample_rate{1000};
+	unsigned		_sample_rate{1000};
 
 	DEFINE_PARAMETERS(
 		(ParamFloat<px4::params::IMU_GYRO_CUTOFF>) _param_imu_gyro_cutoff

diff --git a/src/modules/simulator/CMakeLists.txt b/src/modules/simulator/CMakeLists.txt
@@ -41,7 +41,7 @@ if(ENABLE_UART_RC_INPUT)
 	elseif (WIN32)
 		set(PIXHAWK_DEVICE "COM3")
 	endif()
-		
+
 	set(PIXHAWK_DEVICE_BAUD 115200)
 endif()
 configure_file(simulator_config.h.in simulator_config.h @ONLY)
@@ -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)
diff --git a/src/modules/simulator/accelsim/CMakeLists.txt b/src/modules/simulator/accelsim/CMakeLists.txt
Original file line number	Diff line number	Diff line change
Expand Up		@@ -32,3 +32,4 @@
		############################################################################

		px4_add_library(drivers_accelerometer PX4Accelerometer.cpp)
		target_link_libraries(drivers_accelerometer PRIVATE drivers__device)