new airframe for sih, HIL_STATE_QUATERION sent through MAVLink

ROMFS/px4fmu_common/init.d/airframes/1100_rc_quad_x_sih.hil

@@ -0,0 +1,19 @@
+#!/bin/sh
+#
+# @name SIH Quadcopter X
+#
+# @type Simulation
+# @class Copter
+#
+# @maintainer Romain Chiappinelli <romain.chiap@gmail.com>
+#
+
+sh /etc/init.d/rc.mc_defaults
+
+set MIXER quad_x
+set PWM_OUT 1234
+
+# set HIL to avoid sensors startup
+param set SYS_HITL 1
+
+param set SYS_SIH 1

ROMFS/px4fmu_common/init.d/airframes/CMakeLists.txt

@@ -38,6 +38,7 @@ px4_add_romfs_files(
 	1000_rc_fw_easystar.hil
 	1001_rc_quad_x.hil
 	1002_standard_vtol.hil
+	1100_rc_quad_x_sih.hil
 	
 	# [2000, 2999] Standard planes"
 	2100_standard_plane

ROMFS/px4fmu_common/init.d/rcS

@@ -337,6 +337,12 @@ else
 		# disable GPS
 		param set GPS_1_CONFIG 0
 
+		# start the SIH if needed
+		if  param compare SYS_SIH 1
+		then
+			sih start
+		fi
+
 	else
 		#
 		# board sensors: rc.sensors

boards/aerotenna/ocpoc/ubuntu.cmake

@@ -49,6 +49,7 @@ px4_add_board(
 		mc_pos_control
 		navigator
 		sensors
+		sih
 		#simulator
 		vmount
 		vtol_att_control

boards/airmind/mindpx-v2/default.cmake

@@ -69,6 +69,7 @@ px4_add_board(
 		mc_pos_control
 		navigator
 		sensors
+		sih
 		vmount
 		vtol_att_control
 		wind_estimator

boards/atlflight/eagle/default.cmake

@@ -84,6 +84,7 @@ px4_add_board(
 		mc_pos_control
 		navigator
 		sensors
+		sih
 		simulator
 		vmount
 		vtol_att_control

boards/atlflight/eagle/qurt-default.cmake

@@ -74,6 +74,7 @@ px4_add_board(
 		mc_att_control
 		mc_pos_control
 		sensors
+		sih
 		vmount
 		vtol_att_control
 		wind_estimator

boards/atlflight/excelsior/default.cmake

@@ -84,6 +84,7 @@ px4_add_board(
 		mc_pos_control
 		navigator
 		sensors
+		sih
 		simulator
 		vmount
 		vtol_att_control

boards/atlflight/excelsior/qurt-default.cmake

@@ -74,6 +74,7 @@ px4_add_board(
 		mc_att_control
 		mc_pos_control
 		sensors
+		sih
 		vmount
 		vtol_att_control
 		wind_estimator

boards/av/x-v1/default.cmake

@@ -77,6 +77,7 @@ px4_add_board(
 		mc_pos_control
 		navigator
 		sensors
+		sih
 		vmount
 		vtol_att_control
 		wind_estimator

boards/beaglebone/blue/cross.cmake

@@ -48,6 +48,7 @@ px4_add_board(
 		mc_pos_control
 		navigator
 		sensors
+		sih
 		vmount
 		vtol_att_control
 		wind_estimator

boards/beaglebone/blue/native.cmake

@@ -46,6 +46,7 @@ px4_add_board(
 		mc_pos_control
 		navigator
 		sensors
+		sih
 		vmount
 		vtol_att_control
 		wind_estimator

boards/bitcraze/crazyflie/default.cmake

@@ -37,6 +37,7 @@ px4_add_board(
 		mc_pos_control
 		navigator
 		sensors
+		sih
 		#vtol_att_control
 		wind_estimator
 

boards/emlid/navio2/cross.cmake

@@ -52,6 +52,7 @@ px4_add_board(
 		mc_pos_control
 		navigator
 		sensors
+		sih
 		#simulator
 		vmount
 		vtol_att_control

boards/emlid/navio2/native.cmake

@@ -50,6 +50,7 @@ px4_add_board(
 		mc_pos_control
 		navigator
 		sensors
+		sih
 		#simulator
 		vmount
 		vtol_att_control

boards/intel/aerofc-v1/default.cmake

@@ -53,6 +53,7 @@ px4_add_board(
 		mc_pos_control
 		navigator
 		sensors
+		sih
 		vmount
 		#vtol_att_control
 		#wind_estimator

boards/intel/aerofc-v1/rtps.cmake

@@ -56,6 +56,7 @@ px4_add_board(
 		micrortps_bridge
 		navigator
 		sensors
+		sih
 		vmount
 		#vtol_att_control
 		#wind_estimator

boards/nxp/fmuk66-v3/default.cmake

@@ -72,6 +72,7 @@ px4_add_board(
 		mc_pos_control
 		navigator
 		sensors
+		sih
 		vmount
 		vtol_att_control
 		wind_estimator

boards/omnibus/f4sd/default.cmake

@@ -65,6 +65,7 @@ px4_add_board(
 		mc_pos_control
 		navigator
 		sensors
+		sih
 		#vmount
 		#vtol_att_control
 		#wind_estimator

boards/parrot/bebop/default.cmake

@@ -40,6 +40,7 @@ px4_add_board(
 		mc_pos_control
 		navigator
 		sensors
+		sih
 		#vtol_att_control
 		wind_estimator
 

boards/px4/fmu-v3/default.cmake

@@ -84,6 +84,7 @@ px4_add_board(
 		mc_pos_control
 		navigator
 		sensors
+		sih
 		vmount
 		vtol_att_control
 		wind_estimator

boards/px4/fmu-v3/rtps.cmake

@@ -84,6 +84,7 @@ px4_add_board(
 		micrortps_bridge
 		navigator
 		sensors
+		sih
 		vmount
 		vtol_att_control
 		wind_estimator

boards/px4/fmu-v3/stackcheck.cmake

@@ -83,6 +83,7 @@ px4_add_board(
 		mc_pos_control
 		navigator
 		sensors
+		sih
 		vmount
 		vtol_att_control
 		wind_estimator

boards/px4/fmu-v4/default.cmake

@@ -69,6 +69,7 @@ px4_add_board(
 		mc_pos_control
 		navigator
 		sensors
+		sih
 		vmount
 		vtol_att_control
 		wind_estimator

boards/px4/fmu-v4/rtps.cmake

@@ -70,6 +70,7 @@ px4_add_board(
 		micrortps_bridge
 		navigator
 		sensors
+		sih
 		vmount
 		vtol_att_control
 		wind_estimator

boards/px4/fmu-v4/stackcheck.cmake

@@ -69,6 +69,7 @@ px4_add_board(
 		mc_pos_control
 		navigator
 		sensors
+		sih
 		vmount
 		vtol_att_control
 		wind_estimator

boards/px4/fmu-v4pro/default.cmake

@@ -82,6 +82,7 @@ px4_add_board(
 		mc_pos_control
 		navigator
 		sensors
+		sih
 		vmount
 		vtol_att_control
 		wind_estimator

boards/px4/fmu-v4pro/rtps.cmake

@@ -82,6 +82,7 @@ px4_add_board(
 		micrortps_bridge
 		navigator
 		sensors
+		sih
 		vmount
 		vtol_att_control
 		wind_estimator

boards/px4/fmu-v5/default.cmake

@@ -83,6 +83,7 @@ px4_add_board(
 		mc_pos_control
 		navigator
 		sensors
+		sih
 		vmount
 		vtol_att_control
 		wind_estimator

boards/px4/fmu-v5/multicopter.cmake

@@ -66,6 +66,7 @@ px4_add_board(
 		mc_pos_control
 		navigator
 		sensors
+		sih
 		vmount
 		wind_estimator
 

boards/px4/fmu-v5/rtps.cmake

@@ -83,6 +83,7 @@ px4_add_board(
 		micrortps_bridge
 		navigator
 		sensors
+		sih
 		vmount
 		vtol_att_control
 		wind_estimator

boards/px4/fmu-v5/stackcheck.cmake

@@ -83,6 +83,7 @@ px4_add_board(
 		#micrortps_bridge
 		navigator
 		sensors
+		sih
 		vmount
 		vtol_att_control
 		wind_estimator

boards/px4/raspberrypi/cross.cmake

@@ -45,6 +45,7 @@ px4_add_board(
 		mc_pos_control
 		navigator
 		sensors
+		sih
 		vmount
 		vtol_att_control
 		wind_estimator

boards/px4/raspberrypi/native.cmake

@@ -43,6 +43,7 @@ px4_add_board(
 		mc_pos_control
 		navigator
 		sensors
+		sih
 		#simulator
 		vmount
 		vtol_att_control

msg/sih.msg

@@ -7,3 +7,5 @@ float32[3] 	omega_b			# body rates in body frame [rad/s]
 float32[3]  p_i_local 		# local inertial position [m]
 float32[3]  v_i 			# inertial velocity [m]
 float32[4]  u 				# motor signals [0;1]
+uint32     te_us 			# execution time [us]
+uint32     td_us 			# delay time [us]

src/lib/systemlib/system_params.c

@@ -76,6 +76,19 @@ PARAM_DEFINE_INT32(SYS_AUTOCONFIG, 0);
  */
 PARAM_DEFINE_INT32(SYS_HITL, 0);
 
+/**
+ * Enable SIH mode on next boot
+ *
+ * The simulation in hardware (SIH) will enable a quad simulator to run on the autopilot.
+ * When disabled the same vehicle can be normally flown outdoors.
+ *
+ * @boolean
+ * @reboot_required true
+ *
+ * @group System
+ */
+PARAM_DEFINE_INT32(SYS_SIH, 0);
+
 /**
  * Set restart type
  *

src/modules/mavlink/mavlink_main.cpp

@@ -765,12 +765,16 @@ Mavlink::set_hil_enabled(bool hil_enabled)
 	if (hil_enabled && !_hil_enabled && _datarate > 5000) {
 		_hil_enabled = true;
 		ret = configure_stream("HIL_ACTUATOR_CONTROLS", 200.0f);
+
+		configure_stream("GROUND_TRUTH", 25.0f); 	// HIL_STATE_QUATERNION to display the SIH
 	}
 
 	/* disable HIL */
 	if (!hil_enabled && _hil_enabled) {
 		_hil_enabled = false;
 		ret = configure_stream("HIL_ACTUATOR_CONTROLS", 0.0f);
+
+		configure_stream("GROUND_TRUTH", 0.0f);
 	}
 
 	return ret;

src/modules/mavlink/mavlink_messages.cpp

@@ -4666,12 +4666,13 @@ protected:
 			msg.yawspeed = att.yawspeed;
 
 			// vehicle_global_position -> hil_state_quaternion
-			msg.lat = gpos.lat;
-			msg.lon = gpos.lon;
-			msg.alt = gpos.alt;
-			msg.vx = gpos.vel_n;
-			msg.vy = gpos.vel_e;
-			msg.vz = gpos.vel_d;
+			// same units as defined in mavlink/common.xml
+			msg.lat = gpos.lat * 1e7;
+			msg.lon = gpos.lon * 1e7;
+			msg.alt = gpos.alt * 1e3f;
+			msg.vx = gpos.vel_n * 1e2f;
+			msg.vy = gpos.vel_e * 1e2f;
+			msg.vz = gpos.vel_d * 1e2f;
 			msg.ind_airspeed = 0;
 			msg.true_airspeed = 0;
 			msg.xacc = 0;

src/modules/sih/CMakeLists.txt

@@ -1,6 +1,6 @@
 ############################################################################
 #
-#   Copyright (c) 2015 PX4 Development Team. All rights reserved.
+#   Copyright (c) 2019 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

src/modules/sih/sih.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2018 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2019 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
@@ -123,7 +123,7 @@ int Sih::task_spawn(int argc, char *argv[])
 {
 	_task_id = px4_task_spawn_cmd("sih",
 				      SCHED_DEFAULT,
-				      SCHED_PRIORITY_DEFAULT,
+				      SCHED_PRIORITY_MAX, //SCHED_PRIORITY_DEFAULT
 				      4096,
 				      (px4_main_t)&run_trampoline,
 				      (char *const *)argv);
@@ -188,108 +188,104 @@ Sih::Sih(int example_param, bool example_flag)
 
 void Sih::run()
 {
-	
-	// to subscribe to (read) the actuators_out pwm
-	int actuator_out_sub = orb_subscribe(ORB_ID(actuator_outputs));
 
-	int vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status));
+	// to subscribe to (read) the actuators_out pwm
+	_actuator_out_sub = orb_subscribe(ORB_ID(actuator_outputs));
 
 	// initialize parameters
-	int parameter_update_sub = orb_subscribe(ORB_ID(parameter_update));
-	parameters_update_poll(parameter_update_sub);
-
+	_parameter_update_sub = orb_subscribe(ORB_ID(parameter_update));
+	parameters_update_poll();
 
 	init_variables();
  	init_sensors();	
 	// on the AUAVX21: "/dev/ttyS2/" is TELEM2 UART3 --- "/dev/ttyS5/" is Debug UART7 --- "/dev/ttyS4/" is OSD UART8
-	int serial_fd=init_serial_port(); 	 	// init and open the serial port
+	// int serial_fd=init_serial_port(); 	 	// init and open the serial port
 
 	const hrt_abstime task_start = hrt_absolute_time();
-	hrt_abstime last_run = task_start;
-	hrt_abstime gps_time = task_start;
-	hrt_abstime serial_time = task_start;
-	hrt_abstime now;
+	_last_run = task_start;
+	_gps_time = task_start;
+	_serial_time = task_start;
 
-	while (!should_exit() && is_HIL_running(vehicle_status_sub)) {
+	// hrt_call_every(&_timer_call, LOOP_INTERVAL, LOOP_INTERVAL, timer_callback, this);
 
-		now = hrt_absolute_time();
-		_dt = (now - last_run) * 1e-6f;
-		last_run = now;
+	while (!should_exit())
+	{
+		inner_loop();
 		
-		read_motors(actuator_out_sub);
-
-		generate_force_and_torques();
-
-		equations_of_motion();
-
-		reconstruct_sensors_signals();
-
-		send_IMU(now);
-
-		if (now - gps_time > 50000) 	// gps published at 20Hz
-		{
-			gps_time=now;
-			send_gps(gps_time);				
-		}		
-		
-		// send uart message every 40 ms
-		if (now - serial_time > 40000)
-		{
-			serial_time=now;
-
-			publish_sih(); 	// publish _sih message for debug purpose
-
-			send_serial_msg(serial_fd, (int64_t)(now - task_start)/1000); 	
-
-			parameters_update_poll(parameter_update_sub); 	// update the parameters if needed
-		}
-		// else if (loop_count==5)
-		// {
-		// 	tcflush(serial_fd, TCOFLUSH); 	// flush output data
-		// 	tcdrain(serial_fd);
-		// }
-
-		usleep(1000); 	// sleeping time us
-
+		usleep(1000);
 	}
 
-	orb_unsubscribe(actuator_out_sub);
-	orb_unsubscribe(parameter_update_sub); 
-	orb_unsubscribe(vehicle_status_sub);
-	close(serial_fd);
-	
+	// hrt_cancel(&_timer_call); 	// close the periodic timer interruption
+	orb_unsubscribe(_actuator_out_sub);
+	orb_unsubscribe(_parameter_update_sub);
+	// close(serial_fd);
+
 }
 
-void Sih::parameters_update_poll(int parameter_update_sub)
+// timer_callback() is used as a trampoline to inner_loop()
+void Sih::timer_callback(void *arg)
+{
+	(reinterpret_cast<Sih *>(arg))->inner_loop();
+}
+
+// This is the main execution called periodically by the timer callback
+void Sih::inner_loop()
+{
+	_now = hrt_absolute_time();
+	_dt = (_now - _last_run) * 1e-6f;
+	_last_run = _now;
+
+	read_motors();
+
+	generate_force_and_torques();
+
+	equations_of_motion();
+
+	reconstruct_sensors_signals();
+
+	send_IMU();
+
+	if (_now - _gps_time >= 50000) 	// gps published at 20Hz
+	{
+		_gps_time=_now;
+		send_gps();
+	}
+
+	// send uart message every 40 ms
+	if (_now - _serial_time >= 40000)
+	{
+		_serial_time=_now;
+
+		publish_sih(); 	// publish _sih message for debug purpose
+
+		// send_serial_msg(serial_fd, (int64_t)(now - task_start)/1000);
+
+		parameters_update_poll(); 	// update the parameters if needed
+	}
+	// else if (loop_count==5)
+	// {
+	// 	tcflush(serial_fd, TCOFLUSH); 	// flush output data
+	// 	tcdrain(serial_fd);
+	// }
+
+	_sih.te_us=hrt_absolute_time()-_now; 	// execution time (without delay)
+
+}
+
+void Sih::parameters_update_poll()
 {
 	bool updated;
 	struct parameter_update_s param_upd;
 
-	orb_check(parameter_update_sub, &updated);
+	orb_check(_parameter_update_sub, &updated);
 
 	if (updated) {
-		orb_copy(ORB_ID(parameter_update), parameter_update_sub, &param_upd);
+		orb_copy(ORB_ID(parameter_update), _parameter_update_sub, &param_upd);
 		updateParams();
-		parameters_updated();		
+		parameters_updated();
 	}
 }
 
-uint8_t Sih::is_HIL_running(int vehicle_status_sub)
-{
-	bool updated;
-	struct vehicle_status_s vehicle_status;
-	static uint8_t running=false;
-
-	orb_check(vehicle_status_sub, &updated);
-
-	if (updated) {
-		orb_copy(ORB_ID(vehicle_status), vehicle_status_sub, &vehicle_status);
-		running=vehicle_status.hil_state;
-	}	
-
-	return running;
-}
-
 // store the parameters in a more convenient form
 void Sih::parameters_updated()
 {
@@ -395,16 +391,16 @@ int Sih::init_serial_port()
 }
 
 // read the motor signals outputted from the mixer
-void Sih::read_motors(const int actuator_out_sub)
+void Sih::read_motors()
 {
 	struct actuator_outputs_s actuators_out {};
 
 	// read the actuator outputs
 	bool updated;
-	orb_check(actuator_out_sub, &updated);
+	orb_check(_actuator_out_sub, &updated);
 
 	if (updated) {
-		orb_copy(ORB_ID(actuator_outputs), actuator_out_sub, &actuators_out);
+		orb_copy(ORB_ID(actuator_outputs), _actuator_out_sub, &actuators_out);
 		for (int i=0; i<NB_MOTORS; i++) 	// saturate the motor signals
 			_u[i]=constrain((actuators_out.output[i]-PWM_DEFAULT_MIN)/(PWM_DEFAULT_MAX-PWM_DEFAULT_MIN),0.0f, 1.0f);
 	}
@@ -482,9 +478,9 @@ void Sih::reconstruct_sensors_signals()
 	_gps_vel=_v_I+noiseGauss3f(0.06f,0.077f,0.158f);
 }
 
-void Sih::send_IMU(hrt_abstime now)
+void Sih::send_IMU()
 {
-	_sensor_accel.timestamp=now;
+	_sensor_accel.timestamp=_now;
 	_sensor_accel.x=_acc(0);
 	_sensor_accel.y=_acc(1);	
 	_sensor_accel.z=_acc(2);
@@ -494,7 +490,7 @@ void Sih::send_IMU(hrt_abstime now)
 		_sensor_accel_pub = orb_advertise(ORB_ID(sensor_accel), &_sensor_accel);
 	}
 
-	_sensor_gyro.timestamp=now;
+	_sensor_gyro.timestamp=_now;
 	_sensor_gyro.x=_gyro(0);
 	_sensor_gyro.y=_gyro(1);	
 	_sensor_gyro.z=_gyro(2);
@@ -504,7 +500,7 @@ void Sih::send_IMU(hrt_abstime now)
 		_sensor_gyro_pub = orb_advertise(ORB_ID(sensor_gyro), &_sensor_gyro);
 	}
 
-	_sensor_mag.timestamp=now;
+	_sensor_mag.timestamp=_now;
 	_sensor_mag.x=_mag(0);
 	_sensor_mag.y=_mag(1);
 	_sensor_mag.z=_mag(2);
@@ -514,7 +510,7 @@ void Sih::send_IMU(hrt_abstime now)
 		_sensor_mag_pub = orb_advertise(ORB_ID(sensor_mag), &_sensor_mag);
 	}
 
-	_sensor_baro.timestamp=now;
+	_sensor_baro.timestamp=_now;
 	_sensor_baro.pressure=_baro_p_mBar;
 	_sensor_baro.temperature=_baro_temp_c;
 	if (_sensor_baro_pub != nullptr) {
@@ -524,9 +520,9 @@ void Sih::send_IMU(hrt_abstime now)
 	}
 }
 
-void Sih::send_gps(hrt_abstime now)
+void Sih::send_gps()
 {
-	_vehicle_gps_pos.timestamp=now; 			
+	_vehicle_gps_pos.timestamp=_now;
 	_vehicle_gps_pos.lat=(int32_t)(_gps_lat*1e7); 			// Latitude in 1E-7 degrees	
 	_vehicle_gps_pos.lon=(int32_t)(_gps_lon*1e7);	// Longitude in 1E-7 degrees
 	_vehicle_gps_pos.alt=(int32_t)(_gps_alt*1000.0f); 	// Altitude in 1E-3 meters above MSL, (millimetres)
@@ -547,6 +543,35 @@ void Sih::send_gps(hrt_abstime now)
 void Sih::publish_sih()
 {
 
+
+	_gpos_gt.lat=_gps_lat_noiseless;
+	_gpos_gt.lon=_gps_lon_noiseless;
+	_gpos_gt.alt=_gps_alt_noiseless;
+	_gpos_gt.vel_n=_v_I(0);
+	_gpos_gt.vel_e=_v_I(1);
+	_gpos_gt.vel_d=_v_I(2);
+
+	if (_gpos_gt_sub != nullptr) {
+		orb_publish(ORB_ID(vehicle_global_position_groundtruth), _gpos_gt_sub, &_gpos_gt);
+	} else {
+		_gpos_gt_sub = orb_advertise(ORB_ID(vehicle_global_position_groundtruth), &_gpos_gt);
+	}
+
+	// publish attitude groundtruth
+	_att_gt.timestamp=hrt_absolute_time();
+	_att_gt.q[0]=_q(0);
+	_att_gt.q[1]=_q(1);
+	_att_gt.q[2]=_q(2);
+	_att_gt.q[3]=_q(3);
+	_att_gt.rollspeed=_w_B(0);
+	_att_gt.pitchspeed=_w_B(1);
+	_att_gt.yawspeed=_w_B(2);
+	if (_att_gt_sub != nullptr) {
+		orb_publish(ORB_ID(vehicle_attitude_groundtruth), _att_gt_sub, &_att_gt);
+	} else {
+		_att_gt_sub = orb_advertise(ORB_ID(vehicle_attitude_groundtruth), &_att_gt);
+	}
+
 	Eulerf Euler(_q);
 	_sih.timestamp=hrt_absolute_time();
 	_sih.dt_us=(uint32_t)(_dt*1e6f);

src/modules/sih/sih.hpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2018 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2019 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
@@ -40,6 +40,7 @@
 #include <conversion/rotation.h> 	// math::radians, 
 #include <ecl/geo/geo.h> 			// to get the physical constants
 #include <drivers/drv_hrt.h> 		// to get the real time
+// #include <mathlib/math/filter/LowPassFilter2p.hpp>
 
 #include <uORB/topics/parameter_update.h>
 #include <uORB/topics/actuator_outputs.h>
@@ -49,6 +50,8 @@
 #include <uORB/topics/sensor_mag.h>
 #include <uORB/topics/vehicle_gps_position.h>
 #include <uORB/topics/sih.h>
+#include <uORB/topics/vehicle_global_position.h> 	// to publish groundtruth
+#include <uORB/topics/vehicle_attitude.h> 			// to publish groundtruth
 
 using namespace matrix;
 
@@ -84,6 +87,8 @@ public:
 	// generate white Gaussian noise sample as a 3D vector with specified std
 	static Vector3f noiseGauss3f(float stdx, float stdy, float stdz);
 
+	static void timer_callback(void *arg);
+
 	// static int pack_float(char* uart_msg, int index, void *value); 	// pack a float to a IEEE754
 private:
 
@@ -92,11 +97,9 @@ private:
 	 * @param parameter_update_sub uorb subscription to parameter_update
 	 * @param force for a parameter update
 	 */
-	void parameters_update_poll(int parameter_update_sub);
+	void parameters_update_poll();
 	void parameters_updated();
 
-	uint8_t is_HIL_running(int vehicle_status_sub);
-
 	// to publish the simulator states
 	struct sih_s 					_sih {};
 	orb_advert_t    				_sih_pub{nullptr};
@@ -115,6 +118,15 @@ private:
 	// to publish the gps position
 	struct vehicle_gps_position_s 	_vehicle_gps_pos {};
 	orb_advert_t 					_vehicle_gps_pos_pub{nullptr};
+	// attitude groundtruth
+	struct vehicle_global_position_s 	_gpos_gt {};
+	orb_advert_t 						_gpos_gt_sub{nullptr};
+	// global position groundtruth
+	struct vehicle_attitude_s			_att_gt {};
+	orb_advert_t 						_att_gt_sub{nullptr};
+
+	int _parameter_update_sub {-1};
+	int _actuator_out_sub {-1};
 
 	// hard constants
 	static constexpr uint16_t NB_MOTORS = 4;
@@ -122,19 +134,27 @@ private:
 	static constexpr float T1_K = T1_C - CONSTANTS_ABSOLUTE_NULL_CELSIUS;	// ground temperature in Kelvin
 	static constexpr float TEMP_GRADIENT  = -6.5f / 1000.0f;	// temperature gradient in degrees per metre
 	static constexpr uint32_t BAUDS_RATE = 57600; 			// bauds rate of the serial port
+	static constexpr hrt_abstime LOOP_INTERVAL = 10000; 		// 250 Hz real time
 
 	void init_variables();
 	void init_sensors();
 	int  init_serial_port();
-	void read_motors(const int actuator_out_sub);
+	void read_motors();
 	void generate_force_and_torques();
 	void equations_of_motion();
 	void reconstruct_sensors_signals();
-	void send_IMU(hrt_abstime now);
-	void send_gps(hrt_abstime now);
+	void send_IMU();
+	void send_gps();
 	void publish_sih();
 	void send_serial_msg(int serial_fd, int64_t t_ms);
+	void inner_loop();
 
+	int32_t 	_counter = 0;
+	hrt_call	_timer_call;
+	hrt_abstime _last_run;
+	hrt_abstime _gps_time;
+	hrt_abstime _serial_time;
+	hrt_abstime _now;
 	float  		_dt; 			// sampling time [s]
 
 	char _uart_name[12] = "/dev/ttyS5/"; 					// serial port name

src/modules/sih/sih_params.c

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2013-2015 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2013-2019 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