Simulator: modified -p to publish individual sensor data

The simulator was changed to publish the sensor data that is read
by the sensors module when the -p flag is passed.

Signed-off-by: Mark Charlebois <charlebm@gmail.com>

msg/hil_sensor.msg

@@ -0,0 +1,100 @@
+# Definition of the hil_sensor uORB topic.
+
+int32 MAGNETOMETER_MODE_NORMAL = 0
+int32 MAGNETOMETER_MODE_POSITIVE_BIAS = 1
+int32 MAGNETOMETER_MODE_NEGATIVE_BIAS = 2
+
+# Sensor readings in raw and SI-unit form.
+#
+# These values are read from the sensors. Raw values are in sensor-specific units,
+# the scaled values are in SI-units, as visible from the ending of the variable
+# or the comments. The use of the SI fields is in general advised, as these fields
+# are scaled and offset-compensated where possible and do not change with board
+# revisions and sensor updates.
+#
+# Actual data, this is specific to the type of data which is stored in this struct
+# A line containing L0GME will be added by the Python logging code generator to the logged dataset.
+#
+# NOTE: Ordering of fields optimized to align to 32 bit / 4 bytes Change with consideration only
+
+uint64 timestamp			# Timestamp in microseconds since boot, from gyro
+#
+int16[3] gyro_raw			# Raw sensor values of angular velocity
+float32[3] gyro_rad_s			# Angular velocity in radian per seconds
+uint32 gyro_errcount			# Error counter for gyro 0
+float32 gyro_temp			# Temperature of gyro 0
+
+int16[3] accelerometer_raw		# Raw acceleration in NED body frame
+float32[3] accelerometer_m_s2		# Acceleration in NED body frame, in m/s^2
+int16 accelerometer_mode			# Accelerometer measurement mode
+float32 accelerometer_range_m_s2		# Accelerometer measurement range in m/s^2
+uint64 accelerometer_timestamp	# Accelerometer timestamp
+uint32 accelerometer_errcount	# Error counter for accel 0
+float32 accelerometer_temp		# Temperature of accel 0
+
+int16[3] magnetometer_raw		# Raw magnetic field in NED body frame
+float32[3] magnetometer_ga		# Magnetic field in NED body frame, in Gauss
+int16 magnetometer_mode			# Magnetometer measurement mode
+float32 magnetometer_range_ga		# measurement range in Gauss
+float32 magnetometer_cuttoff_freq_hz	# Internal analog low pass frequency of sensor
+uint64 magnetometer_timestamp	# Magnetometer timestamp
+uint32 magnetometer_errcount		# Error counter for mag 0
+float32 magnetometer_temp		# Temperature of mag 0
+
+int16[3] gyro1_raw			# Raw sensor values of angular velocity
+float32[3] gyro1_rad_s			# Angular velocity in radian per seconds
+uint64 gyro1_timestamp		# Gyro timestamp
+uint32 gyro1_errcount		# Error counter for gyro 1
+float32 gyro1_temp			# Temperature of gyro 1
+
+int16[3] accelerometer1_raw		# Raw acceleration in NED body frame
+float32[3] accelerometer1_m_s2		# Acceleration in NED body frame, in m/s^2
+uint64 accelerometer1_timestamp	# Accelerometer timestamp
+uint32 accelerometer1_errcount	# Error counter for accel 1
+float32 accelerometer1_temp		# Temperature of accel 1
+
+int16[3] magnetometer1_raw		# Raw magnetic field in NED body frame
+float32[3] magnetometer1_ga		# Magnetic field in NED body frame, in Gauss
+uint64 magnetometer1_timestamp	# Magnetometer timestamp
+uint32 magnetometer1_errcount	# Error counter for mag 1
+float32 magnetometer1_temp		# Temperature of mag 1
+
+int16[3] gyro2_raw			# Raw sensor values of angular velocity
+float32[3] gyro2_rad_s			# Angular velocity in radian per seconds
+uint64 gyro2_timestamp		# Gyro timestamp
+uint32 gyro2_errcount		# Error counter for gyro 1
+float32 gyro2_temp			# Temperature of gyro 1
+
+int16[3] accelerometer2_raw		# Raw acceleration in NED body frame
+float32[3] accelerometer2_m_s2		# Acceleration in NED body frame, in m/s^2
+uint64 accelerometer2_timestamp	# Accelerometer timestamp
+uint32 accelerometer2_errcount	# Error counter for accel 2
+float32 accelerometer2_temp		# Temperature of accel 2
+
+int16[3] magnetometer2_raw		# Raw magnetic field in NED body frame
+float32[3] magnetometer2_ga		# Magnetic field in NED body frame, in Gauss
+uint64 magnetometer2_timestamp	# Magnetometer timestamp
+uint32 magnetometer2_errcount	# Error counter for mag 2
+float32 magnetometer2_temp		# Temperature of mag 2
+
+float32 baro_pres_mbar			# Barometric pressure, already temp. comp.
+float32 baro_alt_meter			# Altitude, already temp. comp.
+float32 baro_temp_celcius		# Temperature in degrees celsius
+uint64 baro_timestamp		# Barometer timestamp
+
+float32 baro1_pres_mbar			# Barometric pressure, already temp. comp.
+float32 baro1_alt_meter			# Altitude, already temp. comp.
+float32 baro1_temp_celcius		# Temperature in degrees celsius
+uint64 baro1_timestamp		# Barometer timestamp
+
+float32[10] adc_voltage_v		# ADC voltages of ADC Chan 10/11/12/13 or -1
+uint16[10] adc_mapping		# Channel indices of each of these values
+float32 mcu_temp_celcius			# Internal temperature measurement of MCU
+
+float32 differential_pressure_pa			# Airspeed sensor differential pressure
+uint64 differential_pressure_timestamp	# Last measurement timestamp
+float32 differential_pressure_filtered_pa	# Low pass filtered airspeed sensor differential pressure reading
+
+float32 differential_pressure1_pa			# Airspeed sensor differential pressure
+uint64 differential_pressure1_timestamp	# Last measurement timestamp
+float32 differential_pressure1_filtered_pa	# Low pass filtered airspeed sensor differential pressure reading

src/modules/simulator/simulator.cpp

@@ -114,11 +114,14 @@ int Simulator::start(int argc, char *argv[])
 		PX4_INFO("Simulator started");
 		drv_led_start();
 		if (argv[2][1] == 's') {
+			_instance->initializeSensorData();
 #ifndef __PX4_QURT
-			_instance->updateSamples();
+			// Update sensor data
+			_instance->pollForMAVLinkMessages(false);
 #endif
 		} else {
-			_instance->publishSensorsCombined();
+			// Update sensor data
+			_instance->pollForMAVLinkMessages(true);
 		}
 	}
 	else {
@@ -128,71 +131,6 @@ int Simulator::start(int argc, char *argv[])
 	return ret;
 }
 
-void Simulator::publishSensorsCombined() {
-
-	struct baro_report baro;
-	memset(&baro,0,sizeof(baro));
-	baro.pressure = 120000.0f;
-
-	// acceleration report
-	struct accel_report accel;
-	memset(&accel,0,sizeof(accel));
-	accel.z = 9.81f;
-	accel.range_m_s2 = 80.0f;
-
-	// gyro report
-	struct gyro_report gyro;
-	memset(&gyro, 0 ,sizeof(gyro));
-
-	// mag report
-	struct mag_report mag;
-	memset(&mag, 0 ,sizeof(mag));
-	// init publishers
-	_baro_pub = orb_advertise(ORB_ID(sensor_baro), &baro);
-	_accel_pub = orb_advertise(ORB_ID(sensor_accel), &accel);
-	_gyro_pub = orb_advertise(ORB_ID(sensor_gyro), &gyro);
-	_mag_pub = orb_advertise(ORB_ID(sensor_mag), &mag);
-
-	struct sensor_combined_s sensors;
-	memset(&sensors, 0, sizeof(sensors));
-	// fill sensors with some data
-	sensors.accelerometer_m_s2[2] = 9.81f;
-	sensors.magnetometer_ga[0] = 0.2f;
-	sensors.timestamp = hrt_absolute_time();
-	sensors.accelerometer_timestamp = hrt_absolute_time();
-	sensors.magnetometer_timestamp = hrt_absolute_time();
-	sensors.baro_timestamp = hrt_absolute_time();
-	// advertise
-	_sensor_combined_pub = orb_advertise(ORB_ID(sensor_combined), &sensors);
-
-	hrt_abstime time_last = hrt_absolute_time();
-	uint64_t delta;
-	for(;;) {
-		delta = hrt_absolute_time() - time_last;
-		if(delta > (uint64_t)1000000) {
-			time_last = hrt_absolute_time();
-			sensors.timestamp = time_last;
-			sensors.accelerometer_timestamp = time_last;
-			sensors.magnetometer_timestamp = time_last;
-			sensors.baro_timestamp = time_last;
-			baro.timestamp = time_last;
-			accel.timestamp = time_last;
-			gyro.timestamp = time_last;
-			mag.timestamp = time_last;
-			// publish the sensor values
-			//PX4_DEBUG("Publishing SensorsCombined\n");
-			orb_publish(ORB_ID(sensor_combined), _sensor_combined_pub, &sensors);
-			orb_publish(ORB_ID(sensor_baro), _baro_pub, &baro);
-			orb_publish(ORB_ID(sensor_accel), _accel_pub, &baro);
-			orb_publish(ORB_ID(sensor_gyro), _gyro_pub, &baro);
-			orb_publish(ORB_ID(sensor_mag), _mag_pub, &mag);
-		}
-		else {
-			usleep(1000000-delta);
-		}
-	}
-}
-
 static void usage()
 {
 	PX4_WARN("Usage: simulator {start -[sc] |stop}");

src/modules/simulator/simulator.h

@@ -39,7 +39,7 @@
 #pragma once
 
 #include <semaphore.h>
-#include <uORB/topics/sensor_combined.h>
+#include <uORB/topics/hil_sensor.h>
 #include <uORB/topics/manual_control_setpoint.h>
 #include <uORB/topics/actuator_outputs.h>
 #include <uORB/topics/vehicle_attitude.h>
@@ -52,11 +52,6 @@
 #include <uORB/uORB.h>
 #include <v1.0/mavlink_types.h>
 #include <v1.0/common/mavlink.h>
-#ifndef __PX4_QURT
-#include <sys/socket.h>
-#include <netinet/in.h>
-#endif
-
 namespace simulator {
 
 // FIXME - what is the endianness of these on actual device?
@@ -118,11 +113,11 @@ struct RawGPSData {
 template <typename RType> class Report {
 public:
 	Report(int readers) :
-			_readidx(0),
-        	_max_readers(readers),
-        	_report_len(sizeof(RType))
+		_readidx(0),
+		_max_readers(readers),
+		_report_len(sizeof(RType))
 	{
-        	sem_init(&_lock, 0, _max_readers);
+		sem_init(&_lock, 0, _max_readers);
 	}
 
 	~Report() {};
@@ -148,11 +143,11 @@ public:
 protected:
 	void read_lock() { sem_wait(&_lock); }
 	void read_unlock() { sem_post(&_lock); }
-	void write_lock() 
+	void write_lock()
 	{
 		for (int i=0; i<_max_readers; i++) {
-                	sem_wait(&_lock);
-        	}
+			sem_wait(&_lock);
+		}
 	}
 	void write_unlock()
 	{
@@ -209,14 +204,15 @@ private:
 	_baro(1),
 	_mag(1),
 	_gps(1),
-	_sensor_combined_pub(nullptr)
 #ifndef __PX4_QURT
-	,
+	_accel_pub(nullptr),
+	_baro_pub(nullptr),
+	_gyro_pub(nullptr),
+	_mag_pub(nullptr),
 	_rc_channels_pub(nullptr),
 	_actuator_outputs_sub(-1),
 	_vehicle_attitude_sub(-1),
 	_manual_sub(-1),
-	_sensor{},
 	_rc_input{},
 	_actuators{},
 	_attitude{},
@@ -225,17 +221,15 @@ private:
 	{}
 	~Simulator() { _instance=NULL; }
 
-#ifndef __PX4_QURT
-	void updateSamples();
-#endif
+	void initializeSensorData();
 
 	static Simulator *_instance;
 
 	// simulated sensor instances
-	simulator::Report<simulator::RawAccelData> 	_accel;
+	simulator::Report<simulator::RawAccelData>	_accel;
 	simulator::Report<simulator::RawMPUData>	_mpu;
 	simulator::Report<simulator::RawBaroData>	_baro;
-	simulator::Report<simulator::RawMagData> 	_mag;
+	simulator::Report<simulator::RawMagData>	_mag;
 	simulator::Report<simulator::RawGPSData>	_gps;
 
 	// uORB publisher handlers
@@ -243,10 +237,9 @@ private:
 	orb_advert_t _baro_pub;
 	orb_advert_t _gyro_pub;
 	orb_advert_t _mag_pub;
-	orb_advert_t _sensor_combined_pub;
 
 	// class methods
-	void publishSensorsCombined();
+	int publish_sensor_topics(mavlink_hil_sensor_t *imu);
 
 #ifndef __PX4_QURT
 	// uORB publisher handlers
@@ -258,23 +251,19 @@ private:
 	int _manual_sub;
 
 	// uORB data containers
-	struct sensor_combined_s _sensor;
 	struct rc_input_values _rc_input;
 	struct actuator_outputs_s _actuators;
 	struct vehicle_attitude_s _attitude;
 	struct manual_control_setpoint_s _manual;
 
-	int _fd;
-	unsigned char _buf[200];
-	struct sockaddr_in _srcaddr;
-	socklen_t _addrlen = sizeof(_srcaddr);
-
 	void poll_actuators();
-	void handle_message(mavlink_message_t *msg);
+	void handle_message(mavlink_message_t *msg, bool publish);
 	void send_controls();
+	void pollForMAVLinkMessages(bool publish);
+
 	void pack_actuator_message(mavlink_hil_controls_t &actuator_msg);
 	void send_mavlink_message(const uint8_t msgid, const void *msg, uint8_t component_ID);
-	void update_sensors(struct sensor_combined_s *sensor, mavlink_hil_sensor_t *imu);
+	void update_sensors(mavlink_hil_sensor_t *imu);
 	void update_gps(mavlink_hil_gps_t *gps_sim);
 	static void *sending_trampoline(void *);
 	void send();

src/modules/simulator/simulator_mavlink.cpp

@@ -35,9 +35,10 @@
 #include <px4_time.h>
 #include "simulator.h"
 #include "errno.h"
+#include <geo/geo.h>
 #include <drivers/drv_pwm_output.h>
-
-using namespace simulator;
+#include <sys/socket.h>
+#include <netinet/in.h>
 
 #define SEND_INTERVAL 	20
 #define UDP_PORT 	14560
@@ -49,9 +50,17 @@ using namespace simulator;
 
 static const uint8_t mavlink_message_lengths[256] = MAVLINK_MESSAGE_LENGTHS;
 static const uint8_t mavlink_message_crcs[256] = MAVLINK_MESSAGE_CRCS;
+static const float mg2ms2 = CONSTANTS_ONE_G / 1000.0f;
 
 static int openUart(const char *uart_name, int baud);
 
+static int _fd;
+static unsigned char _buf[200];
+sockaddr_in _srcaddr;
+static socklen_t _addrlen = sizeof(_srcaddr);
+
+using namespace simulator;
+
 void Simulator::pack_actuator_message(mavlink_hil_controls_t &actuator_msg) {
 	float out[8];
 
@@ -93,6 +102,7 @@ void Simulator::pack_actuator_message(mavlink_hil_controls_t &actuator_msg) {
 void Simulator::send_controls() {
 	mavlink_hil_controls_t msg;
 	pack_actuator_message(msg);
+	//PX4_WARN("Sending HIL_CONTROLS msg");
 	send_mavlink_message(MAVLINK_MSG_ID_HIL_CONTROLS, &msg, 200);
 }
 
@@ -102,6 +112,17 @@ static void fill_rc_input_msg(struct rc_input_values *rc, mavlink_rc_channels_t
 	rc->channel_count = rc_channels->chancount;
 	rc->rssi = rc_channels->rssi;
 
+/*	PX4_WARN("RC: %d, %d, %d, %d, %d, %d, %d, %d",
+		 rc_channels->chan1_raw,
+		 rc_channels->chan2_raw,
+		 rc_channels->chan3_raw,
+		 rc_channels->chan4_raw,
+		 rc_channels->chan5_raw,
+		 rc_channels->chan6_raw,
+		 rc_channels->chan7_raw,
+		 rc_channels->chan8_raw);
+*/
+
 	rc->values[0] = rc_channels->chan1_raw;
 	rc->values[1] = rc_channels->chan2_raw;
 	rc->values[2] = rc_channels->chan3_raw;
@@ -122,7 +143,7 @@ static void fill_rc_input_msg(struct rc_input_values *rc, mavlink_rc_channels_t
 	rc->values[17] = rc_channels->chan18_raw;
 }
 
-void Simulator::update_sensors(struct sensor_combined_s *sensor, mavlink_hil_sensor_t *imu) {
+void Simulator::update_sensors(mavlink_hil_sensor_t *imu) {
 	// write sensor data to memory so that drivers can copy data from there
 	RawMPUData mpu;
 	mpu.accel_x = imu->xacc;
@@ -174,36 +195,42 @@ void Simulator::update_gps(mavlink_hil_gps_t *gps_sim) {
 	gps.satellites_visible = gps_sim->satellites_visible;
 
 	write_gps_data((void *)&gps);
-
 }
 
-void Simulator::handle_message(mavlink_message_t *msg) {
+void Simulator::handle_message(mavlink_message_t *msg, bool publish) {
 	switch(msg->msgid) {
-		case MAVLINK_MSG_ID_HIL_SENSOR:
-			mavlink_hil_sensor_t imu;
-			mavlink_msg_hil_sensor_decode(msg, &imu);
-			update_sensors(&_sensor, &imu);
-			break;
-
-		case MAVLINK_MSG_ID_HIL_GPS:
-			mavlink_hil_gps_t gps_sim;
-			mavlink_msg_hil_gps_decode(msg, &gps_sim);
-			update_gps(&gps_sim);
-			break;
-
-		case MAVLINK_MSG_ID_RC_CHANNELS:
+	case MAVLINK_MSG_ID_HIL_SENSOR:
+		mavlink_hil_sensor_t imu;
+		mavlink_msg_hil_sensor_decode(msg, &imu);
+		if (publish) {
+			publish_sensor_topics(&imu);
+		}
+		update_sensors(&imu);
+		break;
+
+	case MAVLINK_MSG_ID_HIL_GPS:
+		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);
+		break;
 
-			mavlink_rc_channels_t rc_channels;
-			mavlink_msg_rc_channels_decode(msg, &rc_channels);
-			fill_rc_input_msg(&_rc_input, &rc_channels);
+	case MAVLINK_MSG_ID_RC_CHANNELS:
+		mavlink_rc_channels_t rc_channels;
+		mavlink_msg_rc_channels_decode(msg, &rc_channels);
+		fill_rc_input_msg(&_rc_input, &rc_channels);
 
-			// publish message
+		// publish message
+		if (publish) {
 			if(_rc_channels_pub == nullptr) {
 				_rc_channels_pub = orb_advertise(ORB_ID(input_rc), &_rc_input);
 			} else {
 				orb_publish(ORB_ID(input_rc), _rc_channels_pub, &_rc_input);
 			}
-			break;
+		}
+		break;
 	}
 }
 
@@ -246,6 +273,7 @@ void Simulator::poll_actuators() {
 	bool updated;
 	orb_check(_actuator_outputs_sub, &updated);
 	if(updated) {
+	  //PX4_WARN("Received actuator_output0 orb_topic");
 		orb_copy(ORB_ID(actuator_outputs), _actuator_outputs_sub, &_actuators);
 	}
 }
@@ -286,12 +314,8 @@ void Simulator::send() {
 	}
 }
 
-void Simulator::updateSamples()
+void Simulator::initializeSensorData()
 {
-	// udp socket data
-	struct sockaddr_in _myaddr;
-	const int _port = UDP_PORT;
-
 	struct baro_report baro;
 	memset(&baro,0,sizeof(baro));
 	baro.pressure = 120000.0f;
@@ -309,6 +333,13 @@ void Simulator::updateSamples()
 	// mag report
 	struct mag_report mag;
 	memset(&mag, 0 ,sizeof(mag));
+}
+
+void Simulator::pollForMAVLinkMessages(bool publish)
+{
+	// udp socket data
+	struct sockaddr_in _myaddr;
+	const int _port = UDP_PORT;
 
 	// try to setup udp socket for communcation with simulator
 	memset((char *)&_myaddr, 0, sizeof(_myaddr));
@@ -372,9 +403,11 @@ void Simulator::updateSamples()
 	while (pret <= 0) {
 		pret = ::poll(&fds[0], (sizeof(fds[0])/sizeof(fds[0])), 100);
 	}
+	PX4_WARN("Found initial message, pret = %d",pret);
 
 	if (fds[0].revents & POLLIN) {
 		len = recvfrom(_fd, _buf, sizeof(_buf), 0, (struct sockaddr *)&_srcaddr, &_addrlen);
+		PX4_WARN("Sending initial controls message to jMAVSim.");
 		send_controls();
 	}
 
@@ -413,7 +446,7 @@ void Simulator::updateSamples()
 					if (mavlink_parse_char(MAVLINK_COMM_0, _buf[i], &msg, &status))
 					{
 						// have a message, handle it
-						handle_message(&msg);
+						handle_message(&msg, publish);
 					}
 				}
 			}
@@ -430,7 +463,7 @@ void Simulator::updateSamples()
 					if (mavlink_parse_char(MAVLINK_COMM_0, serial_buf[i], &msg, &status))
 					{
 						// have a message, handle it
-						handle_message(&msg);
+						handle_message(&msg, publish);
 					}
 				}
 			}
@@ -528,8 +561,8 @@ int openUart(const char *uart_name, int baud)
 
 	}
 
-    // Make raw
-    cfmakeraw(&uart_config);
+	// Make raw
+	cfmakeraw(&uart_config);
 
 	if ((termios_state = tcsetattr(uart_fd, TCSANOW, &uart_config)) < 0) {
 		warnx("ERR SET CONF %s\n", uart_name);
@@ -537,5 +570,113 @@ int openUart(const char *uart_name, int baud)
 		return -1;
 	}
 
-    return uart_fd;
+	return uart_fd;
+}
+
+int Simulator::publish_sensor_topics(mavlink_hil_sensor_t *imu) {
+
+
+        //uint64_t timestamp = imu->time_usec;
+        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 */
+	{
+		struct gyro_report gyro;
+		memset(&gyro, 0, sizeof(gyro));
+
+		gyro.timestamp = timestamp;
+		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;
+
+		if (_gyro_pub == nullptr) {
+			_gyro_pub = orb_advertise(ORB_ID(sensor_gyro), &gyro);
+
+		} else {
+			orb_publish(ORB_ID(sensor_gyro), _gyro_pub, &gyro);
+		}
+	}
+
+	/* accelerometer */
+	{
+		struct accel_report accel;
+		memset(&accel, 0, sizeof(accel));
+
+		accel.timestamp = timestamp;
+		accel.x_raw = imu->xacc / mg2ms2;
+		accel.y_raw = imu->yacc / mg2ms2;
+		accel.z_raw = imu->zacc / mg2ms2;
+		accel.x = imu->xacc;
+		accel.y = imu->yacc;
+		accel.z = imu->zacc;
+
+		if (_accel_pub == nullptr) {
+			_accel_pub = orb_advertise(ORB_ID(sensor_accel), &accel);
+
+		} else {
+			orb_publish(ORB_ID(sensor_accel), _accel_pub, &accel);
+		}
+	}
+
+	/* magnetometer */
+	{
+		struct mag_report mag;
+		memset(&mag, 0, sizeof(mag));
+
+		mag.timestamp = timestamp;
+		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;
+
+		if (_mag_pub == nullptr) {
+			/* publish to the first mag topic */
+			_mag_pub = orb_advertise(ORB_ID(sensor_mag), &mag);
+
+		} else {
+			orb_publish(ORB_ID(sensor_mag), _mag_pub, &mag);
+		}
+	}
+
+	/* baro */
+	{
+		struct baro_report baro;
+		memset(&baro, 0, sizeof(baro));
+
+		baro.timestamp = timestamp;
+		baro.pressure = imu->abs_pressure;
+		baro.altitude = imu->pressure_alt;
+		baro.temperature = imu->temperature;
+
+		if (_baro_pub == nullptr) {
+			_baro_pub = orb_advertise(ORB_ID(sensor_baro), &baro);
+
+		} else {
+			orb_publish(ORB_ID(sensor_baro), _baro_pub, &baro);
+		}
+	}
+
+        return OK;
 }