simulator in hardware, new module added that allows to run a simulator in the hardware autopilot, for more documentation visit https://github.com/romain-chiap/PX4_SIH_QuadX

boards/auav/x21/default.cmake

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

msg/CMakeLists.txt

@@ -107,6 +107,7 @@ set(msg_files
 	sensor_preflight.msg
 	sensor_selection.msg
 	servorail_status.msg
+	sih.msg
 	subsystem_info.msg
 	system_power.msg
 	task_stack_info.msg

msg/sih.msg

@@ -0,0 +1,9 @@
+# simulator in Hardware - Romain Chiappinelli - Jan 8, 2019
+uint64 timestamp				# time since system start (microseconds)
+
+uint32      dt_us 			# simulator sampling time [us]
+float32[3]  euler_rpy 		# euler angles (roll-pitch-yaw) [deg]
+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]

src/modules/sih/CMakeLists.txt

@@ -0,0 +1,44 @@
+############################################################################
+#
+#   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 modules__sih
+	MAIN sih
+	STACK_MAIN 1200
+	STACK_MAX 3500
+	COMPILE_FLAGS
+	SRCS
+		sih.cpp
+	DEPENDS
+		mathlib
+	)

src/modules/sih/sih.cpp

@@ -0,0 +1,632 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2018 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 sih.cpp
+ * Simulator in Hardware
+ *
+ * @author Romain Chiappinelli		<romain.chiap@gmail.com>
+ *
+ * Coriolis g Corporation - January 2019
+ */
+
+#include "sih.hpp"
+
+#include <px4_getopt.h>
+#include <px4_log.h>
+#include <px4_posix.h>
+
+#include <drivers/drv_pwm_output.h>			// to get PWM flags
+#include <uORB/topics/vehicle_status.h> 	// to get the HIL status
+
+#include <unistd.h> 			//
+#include <string.h>    			//
+#include <fcntl.h> 				//
+#include <termios.h> 			//
+
+using namespace math;
+
+int Sih::print_usage(const char *reason)
+{
+	if (reason) {
+		PX4_WARN("%s\n", reason);
+	}
+
+	PRINT_MODULE_DESCRIPTION(
+		R"DESCR_STR(
+### Simulator in Hardware
+This module provide a simulator for quadrotors running fully 
+inside the hardware autopilot.
+
+This simulator subscribes to "actuator_outputs" which are the actuator pwm
+signals given by the mixer.
+
+This simulator publishes the sensors signals corrupted with realistic noise
+in order to incorporate the state estimator in the loop.
+
+### Implementation
+The simulator implements the equations of motion using matrix algebra. 
+Quaternion representation is used for the attitude.
+Forward Euler is used for integration.
+Most of the variables are declared global in the .hpp file to avoid stack overflow.
+
+
+)DESCR_STR");
+
+	PRINT_MODULE_USAGE_NAME("sih", "sih");
+	PRINT_MODULE_USAGE_COMMAND("start");
+	PRINT_MODULE_USAGE_PARAM_FLAG('f', "Optional example flag", true);
+	PRINT_MODULE_USAGE_PARAM_INT('p', 0, 0, 4096, "Optional example parameter", true);
+	PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
+
+	return 0;
+}
+
+int Sih::print_status()
+{
+	PX4_INFO("Running");
+	// TODO: print additional runtime information about the state of the module
+
+	return 0;
+}
+
+int Sih::custom_command(int argc, char *argv[])
+{
+	/*
+	if (!is_running()) {
+		print_usage("not running");
+		return 1;
+	}
+
+	// additional custom commands can be handled like this:
+	if (!strcmp(argv[0], "do-something")) {
+		get_instance()->do_something();
+		return 0;
+	}
+	 */
+
+	return print_usage("unknown command");
+}
+
+
+int Sih::task_spawn(int argc, char *argv[])
+{
+	_task_id = px4_task_spawn_cmd("sih",
+				      SCHED_DEFAULT,
+				      SCHED_PRIORITY_DEFAULT,
+				      4096,
+				      (px4_main_t)&run_trampoline,
+				      (char *const *)argv);
+
+	if (_task_id < 0) {
+		_task_id = -1;
+		return -errno;
+	}
+
+	return 0;
+}
+
+Sih *Sih::instantiate(int argc, char *argv[])
+{
+	int example_param = 0;
+	bool example_flag = false;
+	bool error_flag = false;
+
+	int myoptind = 1;
+	int ch;
+	const char *myoptarg = nullptr;
+
+	// parse CLI arguments
+	while ((ch = px4_getopt(argc, argv, "p:f", &myoptind, &myoptarg)) != EOF) {
+		switch (ch) {
+		case 'p':
+			example_param = (int)strtol(myoptarg, nullptr, 10);
+			break;
+
+		case 'f':
+			example_flag = true;
+			break;
+
+		case '?':
+			error_flag = true;
+			break;
+
+		default:
+			PX4_WARN("unrecognized flag");
+			error_flag = true;
+			break;
+		}
+	}
+
+	if (error_flag) {
+		return nullptr;
+	}
+
+	Sih *instance = new Sih(example_param, example_flag);
+
+	if (instance == nullptr) {
+		PX4_ERR("alloc failed");
+	}
+
+	return instance;
+}
+
+Sih::Sih(int example_param, bool example_flag)
+	: ModuleParams(nullptr)
+{
+}
+
+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));
+
+	// initialize parameters
+	int parameter_update_sub = orb_subscribe(ORB_ID(parameter_update));
+	parameters_update_poll(parameter_update_sub);
+
+
+	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
+
+	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;
+
+	while (!should_exit() && is_HIL_running(vehicle_status_sub)) {
+
+		now = hrt_absolute_time();
+		_dt = (now - last_run) * 1e-6f;
+		last_run = now;
+		
+		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
+
+	}
+
+	orb_unsubscribe(actuator_out_sub);
+	orb_unsubscribe(parameter_update_sub); 
+	orb_unsubscribe(vehicle_status_sub);
+	close(serial_fd);
+	
+}
+
+void Sih::parameters_update_poll(int parameter_update_sub)
+{
+	bool updated;
+	struct parameter_update_s param_upd;
+
+	orb_check(parameter_update_sub, &updated);
+
+	if (updated) {
+		orb_copy(ORB_ID(parameter_update), parameter_update_sub, &param_upd);
+		updateParams();
+		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()
+{
+
+	_T_MAX = _sih_t_max.get();
+	_Q_MAX = _sih_q_max.get();
+	_L_ROLL = _sih_l_roll.get();
+	_L_PITCH = _sih_l_pitch.get();
+	_KDV = _sih_kdv.get();
+	_KDW = _sih_kdw.get();
+	_H0 = _sih_h0.get();
+
+	_LAT0 = (double)_sih_lat0.get()*1.0e-7;
+	_LON0 = (double)_sih_lon0.get()*1.0e-7;
+	_COS_LAT0=cosl(radians(_LAT0)); 
+
+	_MASS=_sih_mass.get();
+
+	_W_I=Vector3f(0.0f,0.0f,_MASS*CONSTANTS_ONE_G);
+	
+	_I=diag(Vector3f(_sih_ixx.get(),_sih_iyy.get(),_sih_izz.get()));
+	_I(0,1)=_I(1,0)=_sih_ixy.get();
+	_I(0,2)=_I(2,0)=_sih_ixz.get();
+	_I(1,2)=_I(2,1)=_sih_iyz.get();
+
+	_Im1=inv(_I);
+
+	_mu_I=Vector3f(_sih_mu_x.get(), _sih_mu_y.get(), _sih_mu_z.get());
+
+}
+
+// initialization of the variables for the simulator
+void Sih::init_variables()
+{
+	srand(1234); 	// initialize the random seed once before calling generate_wgn()
+
+	_p_I=Vector3f(0.0f,0.0f,0.0f);
+	_v_I=Vector3f(0.0f,0.0f,0.0f);
+	_q=Quatf(1.0f,0.0f,0.0f,0.0f);
+	_w_B=Vector3f(0.0f,0.0f,0.0f);
+
+	_u[0]=_u[1]=_u[2]=_u[3]=0.0f;
+
+}
+
+void Sih::init_sensors()
+{
+
+	_sensor_accel.device_id=1;
+	_sensor_accel.error_count=0;		
+	_sensor_accel.integral_dt=0;
+	_sensor_accel.temperature=T1_C;
+	_sensor_accel.scaling=0.0f;
+
+	_sensor_gyro.device_id=1;
+	_sensor_gyro.error_count=0;		
+	_sensor_gyro.integral_dt=0;
+	_sensor_gyro.temperature=T1_C;
+	_sensor_gyro.scaling=0.0f;
+
+	_sensor_mag.device_id=1;
+	_sensor_mag.error_count=0;		
+	_sensor_mag.temperature=T1_C;
+	_sensor_mag.scaling=0.0f;		
+	_sensor_mag.is_external=false;
+
+	_sensor_baro.error_count=0;
+	_sensor_baro.device_id=1;
+
+	_vehicle_gps_pos.fix_type=3; 	// 3D fix
+	_vehicle_gps_pos.satellites_used=8;
+	_vehicle_gps_pos.heading=NAN;
+	_vehicle_gps_pos.heading_offset=NAN;
+	_vehicle_gps_pos.s_variance_m_s = 0.5f;
+	_vehicle_gps_pos.c_variance_rad = 0.1f;
+	_vehicle_gps_pos.eph = 0.9f;
+	_vehicle_gps_pos.epv = 1.78f; 
+	_vehicle_gps_pos.hdop = 0.7f;
+	_vehicle_gps_pos.vdop = 1.1f;	
+}
+
+int Sih::init_serial_port()
+{
+	struct termios uart_config;
+	int serial_fd = open(_uart_name, O_WRONLY | O_NONBLOCK | O_NOCTTY);
+
+	if (serial_fd < 0) {
+		PX4_ERR("failed to open port: %s", _uart_name);
+	}
+
+	tcgetattr(serial_fd, &uart_config); // read configuration
+
+	uart_config.c_oflag |= ONLCR;
+	// try to set Bauds rate
+	if (cfsetispeed(&uart_config, BAUDS_RATE) < 0 || cfsetospeed(&uart_config, BAUDS_RATE) < 0) {
+		PX4_WARN("ERR SET BAUD %s\n", _uart_name);
+		close(serial_fd);
+	}	
+
+	tcsetattr(serial_fd, TCSANOW, &uart_config); 	// set config
+
+	return serial_fd;
+}
+
+// read the motor signals outputted from the mixer
+void Sih::read_motors(const int actuator_out_sub)
+{
+	struct actuator_outputs_s actuators_out {};
+
+	// read the actuator outputs
+	bool updated;
+	orb_check(actuator_out_sub, &updated);
+
+	if (updated) {
+		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);
+	}
+}
+
+// generate the motors thrust and torque in the body frame
+void Sih::generate_force_and_torques()
+{
+	_T_B=Vector3f(0.0f,0.0f,-_T_MAX*(+_u[0]+_u[1]+_u[2]+_u[3]));
+	_Mt_B=Vector3f(	_L_ROLL*_T_MAX* (-_u[0]+_u[1]+_u[2]-_u[3]),
+					_L_PITCH*_T_MAX*(+_u[0]-_u[1]+_u[2]-_u[3]),
+						   _Q_MAX * (+_u[0]+_u[1]-_u[2]-_u[3]));
+
+	_Fa_I=-_KDV*_v_I; 		// first order drag to slow down the aircraft
+	_Ma_B=-_KDW*_w_B; 		// first order angular damper
+}
+
+// apply the equations of motion of a rigid body and integrate one step
+void Sih::equations_of_motion()
+{
+	_C_IB=_q.to_dcm(); 	// body to inertial transformation 
+
+	// Equations of motion of a rigid body
+	_p_I_dot=_v_I; 							// position differential
+	_v_I_dot=(_W_I+_Fa_I+_C_IB*_T_B)/_MASS; 			// conservation of linear momentum
+	_q_dot=_q.derivative1(_w_B); 				// attitude differential
+	_w_B_dot=_Im1*(_Mt_B+_Ma_B-_w_B.cross(_I*_w_B));	// conservation of angular momentum
+
+	// fake ground, avoid free fall
+	if(_p_I(2)>0.0f && (_v_I_dot(2)>0.0f || _v_I(2)>0.0f))
+	{
+		_v_I.setZero();
+		_w_B.setZero(); 
+		_v_I_dot.setZero();
+	}
+	else
+	{
+		// integration: Euler forward
+		_p_I = _p_I + _p_I_dot*_dt;
+		_v_I = _v_I + _v_I_dot*_dt;
+		_q = _q+_q_dot*_dt; 	// as given in attitude_estimator_q_main.cpp
+		_q.normalize(); 	
+		_w_B = _w_B + _w_B_dot*_dt;
+
+	}
+}
+
+// reconstruct the noisy sensor signals
+void Sih::reconstruct_sensors_signals()
+{
+
+	// The sensor signals reconstruction and noise levels are from
+	// Bulka, Eitan, and Meyer Nahon. "Autonomous fixed-wing aerobatics: from theory to flight." 
+	// In 2018 IEEE International Conference on Robotics and Automation (ICRA), pp. 6573-6580. IEEE, 2018.
+
+	// IMU
+	_acc=_C_IB.transpose()*(_v_I_dot-Vector3f(0.0f,0.0f,CONSTANTS_ONE_G))+noiseGauss3f(0.5f,1.7f,1.4f);
+	_gyro=_w_B+noiseGauss3f(0.14f,0.07f,0.03f);
+	_mag=_C_IB.transpose()*_mu_I+noiseGauss3f(0.02f,0.02f,0.03f);
+
+	// barometer
+	float altitude=(_H0-_p_I(2))+generate_wgn()*0.14f; 	// altitude with noise
+	_baro_p_mBar=CONSTANTS_STD_PRESSURE_MBAR* 			//  reconstructed pressure in mBar
+			powf((1.0f+altitude*TEMP_GRADIENT/T1_K),-CONSTANTS_ONE_G/(TEMP_GRADIENT*CONSTANTS_AIR_GAS_CONST)); 	
+	_baro_temp_c=T1_K+CONSTANTS_ABSOLUTE_NULL_CELSIUS+TEMP_GRADIENT*altitude; 	// reconstructed temperture in celcius
+
+	// GPS
+	_gps_lat_noiseless=_LAT0+degrees((double)_p_I(0)/CONSTANTS_RADIUS_OF_EARTH);
+	_gps_lon_noiseless=_LON0+degrees((double)_p_I(1)/CONSTANTS_RADIUS_OF_EARTH)/_COS_LAT0;
+	_gps_alt_noiseless=_H0-_p_I(2);
+
+	_gps_lat=_gps_lat_noiseless+(double)(generate_wgn()*7.2e-6f);  			// latitude in degrees
+	_gps_lon=_gps_lon_noiseless+(double)(generate_wgn()*1.75e-5f); 	// longitude in degrees
+	_gps_alt=_gps_alt_noiseless+generate_wgn()*1.78f;
+	_gps_vel=_v_I+noiseGauss3f(0.06f,0.077f,0.158f);
+}
+
+void Sih::send_IMU(hrt_abstime now)
+{
+	_sensor_accel.timestamp=now;
+	_sensor_accel.x=_acc(0);
+	_sensor_accel.y=_acc(1);	
+	_sensor_accel.z=_acc(2);
+	if (_sensor_accel_pub != nullptr) {
+		orb_publish(ORB_ID(sensor_accel), _sensor_accel_pub, &_sensor_accel);
+	} else {
+		_sensor_accel_pub = orb_advertise(ORB_ID(sensor_accel), &_sensor_accel);
+	}
+
+	_sensor_gyro.timestamp=now;
+	_sensor_gyro.x=_gyro(0);
+	_sensor_gyro.y=_gyro(1);	
+	_sensor_gyro.z=_gyro(2);
+	if (_sensor_gyro_pub != nullptr) {
+		orb_publish(ORB_ID(sensor_gyro), _sensor_gyro_pub, &_sensor_gyro);
+	} else {
+		_sensor_gyro_pub = orb_advertise(ORB_ID(sensor_gyro), &_sensor_gyro);
+	}
+
+	_sensor_mag.timestamp=now;
+	_sensor_mag.x=_mag(0);
+	_sensor_mag.y=_mag(1);
+	_sensor_mag.z=_mag(2);
+	if (_sensor_mag_pub != nullptr) {
+		orb_publish(ORB_ID(sensor_mag), _sensor_mag_pub, &_sensor_mag);
+	} else {
+		_sensor_mag_pub = orb_advertise(ORB_ID(sensor_mag), &_sensor_mag);
+	}
+
+	_sensor_baro.timestamp=now;
+	_sensor_baro.pressure=_baro_p_mBar;
+	_sensor_baro.temperature=_baro_temp_c;
+	if (_sensor_baro_pub != nullptr) {
+		orb_publish(ORB_ID(sensor_baro), _sensor_baro_pub, &_sensor_baro);
+	} else {
+		_sensor_baro_pub = orb_advertise(ORB_ID(sensor_baro), &_sensor_baro);
+	}
+}
+
+void Sih::send_gps(hrt_abstime 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)
+	_vehicle_gps_pos.alt_ellipsoid = (int32_t)(_gps_alt*1000); 	// Altitude in 1E-3 meters bove Ellipsoid, (millimetres)
+	_vehicle_gps_pos.vel_ned_valid=true;				// True if NED velocity is valid
+	_vehicle_gps_pos.vel_m_s=sqrtf(_gps_vel(0)*_gps_vel(0)+_gps_vel(1)*_gps_vel(1));	// GPS ground speed, (metres/sec)
+	_vehicle_gps_pos.vel_n_m_s=_gps_vel(0);				// GPS North velocity, (metres/sec)
+	_vehicle_gps_pos.vel_e_m_s=_gps_vel(1);				// GPS East velocity, (metres/sec)
+	_vehicle_gps_pos.vel_d_m_s=_gps_vel(2);				// GPS Down velocity, (metres/sec)
+	_vehicle_gps_pos.cog_rad=atan2(_gps_vel(1),_gps_vel(0));	// Course over ground (NOT heading, but direction of movement), -PI..PI, (radians)
+	if (_vehicle_gps_pos_pub != nullptr) {
+		orb_publish(ORB_ID(vehicle_gps_position), _vehicle_gps_pos_pub, &_vehicle_gps_pos);
+	} else {
+		_vehicle_gps_pos_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_vehicle_gps_pos);
+	}
+}
+
+void Sih::publish_sih()
+{
+
+	Eulerf Euler(_q);
+	_sih.timestamp=hrt_absolute_time();
+	_sih.dt_us=(uint32_t)(_dt*1e6f);
+	_sih.euler_rpy[0]=degrees(Euler(0));
+	_sih.euler_rpy[1]=degrees(Euler(1));
+	_sih.euler_rpy[2]=degrees(Euler(2));
+	_sih.omega_b[0]=_w_B(0); 	// wing body rates in body frame
+	_sih.omega_b[1]=_w_B(1);
+	_sih.omega_b[2]=_w_B(2);
+	_sih.p_i_local[0]=_p_I(0); 	// local inertial position
+	_sih.p_i_local[1]=_p_I(1);
+	_sih.p_i_local[2]=_p_I(2);
+	_sih.v_i[0]=_v_I(0); 	// inertial velocity
+	_sih.v_i[1]=_v_I(1);
+	_sih.v_i[2]=_v_I(2);
+	_sih.u[0]=_u[0];
+	_sih.u[1]=_u[1];
+	_sih.u[2]=_u[2];
+	_sih.u[3]=_u[3];
+	if (_sih_pub != nullptr) {
+		orb_publish(ORB_ID(sih), _sih_pub, &_sih);
+	} else {
+		_sih_pub = orb_advertise(ORB_ID(sih), &_sih);
+	}
+} 
+
+void Sih::send_serial_msg(int serial_fd, int64_t t_ms)
+{
+
+	char uart_msg[100];
+
+	uint8_t n;
+	int32_t GPS_pos[3]; 	// latitude, longitude in 10^-7 degrees, altitude AMSL in mm
+	int32_t EA_deci_deg[3]; // Euler angles in deci degrees integers to send to serial
+	int32_t throttles[4]; 	// throttles from 0 to 99
+
+	GPS_pos[0]=(int32_t)(_gps_lat_noiseless*1e7); 		// Latitude in 1E-7 degrees	
+	GPS_pos[1]=(int32_t)(_gps_lon_noiseless*1e7);			// Longitude in 1E-7 degrees
+	GPS_pos[2]=(int32_t)(_gps_alt_noiseless*1000.0f); 	// Altitude in 1E-3 meters above MSL, (millimetres)
+	Eulerf Euler(_q);
+	EA_deci_deg[0]=(int32_t)degrees(Euler(0)*10.0f); 	// decidegrees
+	EA_deci_deg[1]=(int32_t)degrees(Euler(1)*10.0f);
+	EA_deci_deg[2]=(int32_t)degrees(Euler(2)*10.0f);
+	throttles[0]=(int32_t)(_u[0]*99.0f); 	
+	throttles[1]=(int32_t)(_u[1]*99.0f);
+	throttles[2]=(int32_t)(_u[2]*99.0f);
+	throttles[3]=(int32_t)(_u[3]*99.0f);
+
+	n = sprintf(uart_msg, "T%07lld,P%+010d%+010d%+08d,A%+05d%+05d%+05d,U%+03d%+03d%+03d%+03d\n", 
+		t_ms,GPS_pos[0],GPS_pos[1],GPS_pos[2],
+		EA_deci_deg[0],EA_deci_deg[1],EA_deci_deg[2],
+		throttles[0],throttles[1],throttles[2],throttles[3]);
+	write(serial_fd, uart_msg, n);
+}
+
+float Sih::generate_wgn() 	// generate white Gaussian noise sample with std=1
+{
+	float temp=((float)(rand()+1))/(((float)RAND_MAX+1.0f));
+
+	return sqrtf(-2.0f*logf(temp))*cosf(2.0f*M_PI_F*rand()/RAND_MAX);	
+}
+
+Vector3f Sih::noiseGauss3f(float stdx,float stdy, float stdz) 	// generate white Gaussian noise sample with specified std
+{
+	return Vector3f(generate_wgn()*stdx,generate_wgn()*stdy,generate_wgn()*stdz);	
+} // there is another wgn algorithm in BlockRandGauss.hpp
+
+int sih_main(int argc, char *argv[])
+{
+	return Sih::main(argc, argv);
+}
+
+// int Sih::pack_float(char* uart_msg, int index, void *value)
+// {
+// 	uint32_t value_raw=(uint32_t)(value*);
+
+// 	for (int i=3; i>=0; i=i-1) {
+// 		buffer[index+i]=(char)(value_raw&0xFF);
+// 		value_raw=value_raw>>8;
+// 	}
+
+// 	return index+4;	// points to the index for the next value
+// }

src/modules/sih/sih.hpp

@@ -0,0 +1,200 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2018 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.
+ *
+ ****************************************************************************/
+
+#pragma once
+
+#include <px4_module.h>
+#include <px4_module_params.h>
+
+#include <matrix/matrix/math.hpp> 	// matrix, vectors, dcm, quaterions
+#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 <uORB/topics/parameter_update.h>
+#include <uORB/topics/actuator_outputs.h>
+#include <uORB/topics/sensor_baro.h>
+#include <uORB/topics/sensor_gyro.h>
+#include <uORB/topics/sensor_accel.h>
+#include <uORB/topics/sensor_mag.h>
+#include <uORB/topics/vehicle_gps_position.h>
+#include <uORB/topics/sih.h>
+
+using namespace matrix;
+
+extern "C" __EXPORT int sih_main(int argc, char *argv[]);
+
+class Sih : public ModuleBase<Sih>, public ModuleParams
+{
+public:
+	Sih(int example_param, bool example_flag);
+
+	virtual ~Sih() = default;
+
+	/** @see ModuleBase */
+	static int task_spawn(int argc, char *argv[]);
+
+	/** @see ModuleBase */
+	static Sih *instantiate(int argc, char *argv[]);
+
+	/** @see ModuleBase */
+	static int custom_command(int argc, char *argv[]);
+
+	/** @see ModuleBase */
+	static int print_usage(const char *reason = nullptr);
+
+	/** @see ModuleBase::run() */
+	void run() override;
+
+	/** @see ModuleBase::print_status() */
+	int print_status() override;
+
+	static float generate_wgn(); 	// generate white Gaussian noise sample
+
+	// generate white Gaussian noise sample as a 3D vector with specified std
+	static Vector3f noiseGauss3f(float stdx, float stdy, float stdz);
+
+	// static int pack_float(char* uart_msg, int index, void *value); 	// pack a float to a IEEE754
+private:
+
+	/**
+	 * Check for parameter changes and update them if needed.
+	 * @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_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};
+	// to publish the sensor baro
+	struct sensor_baro_s 			_sensor_baro {};
+	orb_advert_t    				_sensor_baro_pub{nullptr};
+	// to publish the sensor mag
+	struct sensor_mag_s 			_sensor_mag {};
+	orb_advert_t    				_sensor_mag_pub{nullptr};
+	// to publish the sensor gyroscope
+	struct sensor_gyro_s 			_sensor_gyro {};
+	orb_advert_t    				_sensor_gyro_pub{nullptr};
+	// to publish the sensor accelerometer
+	struct sensor_accel_s 			_sensor_accel {};
+	orb_advert_t    				_sensor_accel_pub{nullptr};
+	// to publish the gps position
+	struct vehicle_gps_position_s 	_vehicle_gps_pos {};
+	orb_advert_t 					_vehicle_gps_pos_pub{nullptr};
+
+	// hard constants
+	static constexpr uint16_t NB_MOTORS = 4;
+	static constexpr float T1_C = 15.0f; 						// ground temperature in celcius
+	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
+
+	void init_variables();
+	void init_sensors();
+	int  init_serial_port();
+	void read_motors(const int actuator_out_sub);
+	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 publish_sih();
+	void send_serial_msg(int serial_fd, int64_t t_ms);
+
+	float  		_dt; 			// sampling time [s]
+
+	char _uart_name[12] = "/dev/ttyS5/"; 					// serial port name
+
+	Vector3f 	_T_B; 			// thrust force in body frame [N]
+	Vector3f 	_Fa_I; 			// aerodynamic force in inertial frame [N]
+	Vector3f 	_Mt_B; 			// thruster moments in the body frame [Nm]
+	Vector3f 	_Ma_B; 			// aerodynamic moments in the body frame [Nm]
+	Vector3f 	_p_I; 			// inertial position [m]
+	Vector3f 	_v_I; 			// inertial velocity [m/s]
+	Vector3f 	_v_B; 			// body frame velocity [m/s]
+	Vector3f 	_p_I_dot; 		// inertial position differential
+	Vector3f 	_v_I_dot; 		// inertial velocity differential
+	Quatf 		_q; 			// quaternion attitude
+	Dcmf 		_C_IB; 			// body to inertial transformation
+	Vector3f 	_w_B; 			// body rates in body frame [rad/s]
+	Quatf 		_q_dot;			// quaternion differential
+	Vector3f 	_w_B_dot; 		// body rates differential
+	float 		_u[NB_MOTORS];	// thruster signals
+
+
+	// sensors reconstruction
+	Vector3f 	_acc;
+	Vector3f 	_mag;
+	Vector3f 	_gyro;
+	Vector3f 	_gps_vel;
+	double 		_gps_lat, _gps_lat_noiseless;
+	double 		_gps_lon, _gps_lon_noiseless;
+	float 		_gps_alt, _gps_alt_noiseless;
+	float 		_baro_p_mBar; 	// reconstructed (simulated) pressure in mBar
+	float 		_baro_temp_c; 	// reconstructed (simulated) barometer temperature in celcius
+
+	// parameters
+	float _MASS, _T_MAX, _Q_MAX, _L_ROLL, _L_PITCH, _KDV, _KDW, _H0;
+	double _LAT0, _LON0, _COS_LAT0;
+	Vector3f _W_I; 	// weight of the vehicle in inertial frame [N]
+	Matrix3f _I; 	// vehicle inertia matrix
+	Matrix3f _Im1; 	// inverse of the intertia matrix
+	Vector3f _mu_I;	// NED magnetic field in inertial frame [G]
+
+	// parameters defined in sih_params.c
+	DEFINE_PARAMETERS(
+		(ParamFloat<px4::params::SIH_MASS>) _sih_mass,
+		(ParamFloat<px4::params::SIH_IXX>) _sih_ixx,
+		(ParamFloat<px4::params::SIH_IYY>) _sih_iyy,
+		(ParamFloat<px4::params::SIH_IZZ>) _sih_izz,
+		(ParamFloat<px4::params::SIH_IXY>) _sih_ixy,
+		(ParamFloat<px4::params::SIH_IXZ>) _sih_ixz,
+		(ParamFloat<px4::params::SIH_IYZ>) _sih_iyz,
+		(ParamFloat<px4::params::SIH_T_MAX>) _sih_t_max,
+		(ParamFloat<px4::params::SIH_Q_MAX>) _sih_q_max,
+		(ParamFloat<px4::params::SIH_L_ROLL>) _sih_l_roll,
+		(ParamFloat<px4::params::SIH_L_PITCH>) _sih_l_pitch,
+		(ParamFloat<px4::params::SIH_KDV>) _sih_kdv,
+		(ParamFloat<px4::params::SIH_KDW>) _sih_kdw,
+		(ParamInt<px4::params::SIH_LAT0>) _sih_lat0,
+		(ParamInt<px4::params::SIH_LON0>) _sih_lon0,
+		(ParamFloat<px4::params::SIH_H0>) _sih_h0,
+		(ParamFloat<px4::params::SIH_MU_X>) _sih_mu_x,
+		(ParamFloat<px4::params::SIH_MU_Y>) _sih_mu_y,
+		(ParamFloat<px4::params::SIH_MU_Z>) _sih_mu_z
+	)
+};

src/modules/sih/sih_params.c

@@ -0,0 +1,345 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013-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 sih_params.c
+ * Parameters for quadcopter X simulator in hardware.
+ *
+ * @author Romain Chiappinelli <romain.chiap@gmail.com>
+ * February 2019
+ */
+
+/**
+ * Vehicle mass
+ *
+ * This value can be measured by weighting the quad on a scale.
+ *
+ * @unit kg
+ * @min 0.0
+ * @decimal 2
+ * @increment 0.1
+ * @group SIH
+ */
+PARAM_DEFINE_FLOAT(SIH_MASS, 1.0f);
+
+/**
+ * Vehicle inertia about X axis
+ *
+ * The intertia is a 3 by 3 symmetric matrix.
+ * It represents the difficulty of the vehicle to modify its angular rate.
+ *
+ * @unit kg*m*m
+ * @min 0.0
+ * @decimal 3
+ * @increment 0.005
+ * @group SIH
+ */
+PARAM_DEFINE_FLOAT(SIH_IXX, 0.025f);
+
+/**
+ * Vehicle inertia about Y axis
+ *
+ * The intertia is a 3 by 3 symmetric matrix.
+ * It represents the difficulty of the vehicle to modify its angular rate.
+ *
+ * @unit kg*m*m
+ * @min 0.0
+ * @decimal 3
+ * @increment 0.005
+ * @group SIH
+ */
+PARAM_DEFINE_FLOAT(SIH_IYY, 0.025f);
+
+/**
+ * Vehicle inertia about Z axis
+ *
+ * The intertia is a 3 by 3 symmetric matrix.
+ * It represents the difficulty of the vehicle to modify its angular rate.
+ *
+ * @unit kg*m*m
+ * @min 0.0
+ * @decimal 3
+ * @increment 0.005
+ * @group SIH
+ */
+PARAM_DEFINE_FLOAT(SIH_IZZ, 0.030f);
+
+/**
+ * Vehicle cross term inertia xy
+ *
+ * The intertia is a 3 by 3 symmetric matrix.
+ * This value can be set to 0 for a quad symmetric about its center of mass.
+ *
+ * @unit kg*m*m
+ * @decimal 3
+ * @increment 0.005
+ * @group SIH
+ */
+PARAM_DEFINE_FLOAT(SIH_IXY, 0.0f);
+
+/**
+ * Vehicle cross term inertia xz
+ *
+ * The intertia is a 3 by 3 symmetric matrix.
+ * This value can be set to 0 for a quad symmetric about its center of mass.
+ *
+ * @unit kg*m*m
+ * @decimal 3
+ * @increment 0.005
+ * @group SIH
+ */
+PARAM_DEFINE_FLOAT(SIH_IXZ, 0.0f);
+
+/**
+ * Vehicle cross term inertia yz
+ *
+ * The intertia is a 3 by 3 symmetric matrix.
+ * This value can be set to 0 for a quad symmetric about its center of mass.
+ *
+ * @unit kg*m*m
+ * @decimal 3
+ * @increment 0.005
+ * @group SIH
+ */
+PARAM_DEFINE_FLOAT(SIH_IYZ, 0.0f);
+
+/**
+ * Max propeller thrust force
+ *
+ * This is the maximum force delivered by one propeller
+ * when the motor is running at full speed.
+ *
+ * This value is usually about 5 times the mass of the quadrotor.
+ *
+ * @unit N
+ * @min 0.0
+ * @decimal 2
+ * @increment 0.5
+ * @group SIH
+ */
+PARAM_DEFINE_FLOAT(SIH_T_MAX, 5.0f);
+
+/**
+ * Max propeller torque
+ *
+ * This is the maximum torque delivered by one propeller
+ * when the motor is running at full speed.
+ *
+ * This value is usually about few percent of the maximum thrust force.
+ *
+ * @unit Nm
+ * @min 0.0
+ * @decimal 3
+ * @increment 0.05
+ * @group SIH
+ */
+PARAM_DEFINE_FLOAT(SIH_Q_MAX, 0.1f);
+
+/**
+ * Roll arm length
+ *
+ * This is the arm length generating the rolling moment
+ *
+ * This value can be measured with a ruler.
+ * This corresponds to half the distance between the left and right motors.
+ *
+ * @unit m
+ * @min 0.0
+ * @decimal 2
+ * @increment 0.05
+ * @group SIH
+ */
+PARAM_DEFINE_FLOAT(SIH_L_ROLL, 0.2f);
+
+/**
+ * Pitch arm length
+ *
+ * This is the arm length generating the pitching moment
+ *
+ * This value can be measured with a ruler.
+ * This corresponds to half the distance between the front and rear motors.
+ *
+ * @unit m
+ * @min 0.0
+ * @decimal 2
+ * @increment 0.05
+ * @group SIH
+ */
+PARAM_DEFINE_FLOAT(SIH_L_PITCH, 0.2f);
+
+/**
+ * First order drag coefficient
+ *
+ * Physical coefficient representing the friction with air particules.
+ * The greater this value, the slower the quad will move.
+ *
+ * Drag force function of velocity: D=-KDV*V.
+ * The maximum freefall velocity can be computed as V=10*MASS/KDV [m/s]
+ *
+ * @unit N/(m/s)
+ * @min 0.0
+ * @decimal 2
+ * @increment 0.05
+ * @group SIH
+ */
+PARAM_DEFINE_FLOAT(SIH_KDV, 1.0f);
+
+/**
+ * First order angular damper coefficient
+ *
+ * Physical coefficient representing the friction with air particules during rotations.
+ * The greater this value, the slower the quad will rotate.
+ *
+ * Aerodynamic moment function of body rate: Ma=-KDW*W_B.
+ * This value can be set to 0 if unknown.
+ *
+ * @unit Nm/(rad/s)
+ * @min 0.0
+ * @decimal 3
+ * @increment 0.005
+ * @group SIH
+ */
+PARAM_DEFINE_FLOAT(SIH_KDW, 0.025f);
+
+/**
+ * Initial geodetic latitude
+ *
+ * This value represents the North-South location on Earth where the simulation begins.
+ * A value of 45 deg should be written 450000000.
+ *
+ * LAT0, LON0, H0, MU_X, MU_Y, and MU_Z should ideally be consistent among each others
+ * to represent a physical ground location on Earth.
+ *
+ * @unit 1e-7 deg
+ * @min -850000000
+ * @max  850000000
+ * @group SIH
+ */
+PARAM_DEFINE_INT32(SIH_LAT0, 454671160);
+
+/**
+ * Initial geodetic longitude
+ *
+ * This value represents the East-West location on Earth where the simulation begins.
+ * A value of 45 deg should be written 450000000.
+ *
+ * LAT0, LON0, H0, MU_X, MU_Y, and MU_Z should ideally be consistent among each others
+ * to represent a physical ground location on Earth.
+ *
+ * @unit 1e-7 deg
+ * @min -1800000000
+ * @max  1800000000
+ * @group SIH
+ */
+PARAM_DEFINE_INT32(SIH_LON0, -737578370);
+
+/**
+ * Initial AMSL ground altitude
+ *
+ * This value represents the Above Mean Sea Level (AMSL) altitude where the simulation begins.
+ *
+ * If using FlightGear as a visual animation,
+ * this value can be tweaked such that the vehicle lies on the ground at takeoff.
+ *
+ * LAT0, LON0, H0, MU_X, MU_Y, and MU_Z should ideally be consistent among each others
+ * to represent a physical ground location on Earth.
+ *
+ *
+ * @unit m
+ * @min -420.0
+ * @max 8848.0
+ * @decimal 2
+ * @increment 0.01
+ * @group SIH
+ */
+PARAM_DEFINE_FLOAT(SIH_H0, 32.34f);
+
+/**
+ * North magnetic field at the initial location
+ *
+ * This value represents the North magnetic field at the initial location.
+ *
+ * A magnetic field calculator can be found on the NOAA website
+ * Note, the values need to be converted from nano Tesla to Gauss
+ *
+ * LAT0, LON0, H0, MU_X, MU_Y, and MU_Z should ideally be consistent among each others
+ * to represent a physical ground location on Earth.
+ *
+ * @unit Gauss
+ * @min -1.0
+ * @max  1.0
+ * @decimal 2
+ * @increment 0.001
+ * @group SIH
+ */
+PARAM_DEFINE_FLOAT(SIH_MU_X,  0.179f);
+
+/**
+ * East magnetic field at the initial location
+ *
+ * This value represents the East magnetic field at the initial location.
+ *
+ * A magnetic field calculator can be found on the NOAA website
+ * Note, the values need to be converted from nano Tesla to Gauss
+ *
+ * LAT0, LON0, H0, MU_X, MU_Y, and MU_Z should ideally be consistent among each others
+ * to represent a physical ground location on Earth.
+ *
+ * @unit Gauss
+ * @min -1.0
+ * @max  1.0
+ * @decimal 2
+ * @increment 0.001
+ * @group SIH
+ */
+PARAM_DEFINE_FLOAT(SIH_MU_Y, -0.045f);
+
+/**
+ * Down magnetic field at the initial location
+ *
+ * This value represents the Down magnetic field at the initial location.
+ *
+ * A magnetic field calculator can be found on the NOAA website
+ * Note, the values need to be converted from nano Tesla to Gauss
+ *
+ * LAT0, LON0, H0, MU_X, MU_Y, and MU_Z should ideally be consistent among each others
+ * to represent a physical ground location on Earth.
+ *
+ * @unit Gauss
+ * @min -1.0
+ * @max  1.0
+ * @decimal 2
+ * @increment 0.001
+ * @group SIH
+ */
+PARAM_DEFINE_FLOAT(SIH_MU_Z,  0.504f);