diff --git a/ROMFS/px4fmu_common/init.d/airframes/1100_rc_quad_x_sih.hil b/ROMFS/px4fmu_common/init.d/airframes/1100_rc_quad_x_sih.hil
new file mode 100644
index 0000000000000000000000000000000000000000..63fea2122b65ba18415eae6a7ffb16484ef27375
--- /dev/null
+++ b/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
diff --git a/ROMFS/px4fmu_common/init.d/airframes/CMakeLists.txt b/ROMFS/px4fmu_common/init.d/airframes/CMakeLists.txt
index 66972e2023961d9f4a07a2a9d1078d1f9bde1654..fca7f6e7094f21df7d2144991c9402377033de13 100644
--- a/ROMFS/px4fmu_common/init.d/airframes/CMakeLists.txt
+++ b/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
diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS
index 9dc58d3650850d19060bfe6bb38aae9020787ed9..ef938cef916039432aace07477fdaac7d253778b 100644
--- a/ROMFS/px4fmu_common/init.d/rcS
+++ b/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
diff --git a/boards/aerotenna/ocpoc/ubuntu.cmake b/boards/aerotenna/ocpoc/ubuntu.cmake
index 50ee37fa0a9fe0cf1cbf7c3bbf7d07bdbd4fbfda..12f8a093e603372520f31f9de0f6d4114426d94a 100644
--- a/boards/aerotenna/ocpoc/ubuntu.cmake
+++ b/boards/aerotenna/ocpoc/ubuntu.cmake
@@ -49,6 +49,7 @@ px4_add_board(
mc_pos_control
navigator
sensors
+ sih
#simulator
vmount
vtol_att_control
diff --git a/boards/airmind/mindpx-v2/default.cmake b/boards/airmind/mindpx-v2/default.cmake
index 2858d338f1d9b7c80f92338eaec8911a69796b0b..807733606736faf3273fea0a957faf48f01fb15d 100644
--- a/boards/airmind/mindpx-v2/default.cmake
+++ b/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
diff --git a/boards/atlflight/eagle/default.cmake b/boards/atlflight/eagle/default.cmake
index f398822f69abacdebbabcf3f8b5138bd73d1186a..3b55e894cb1dc73f16e10f031d941f1473da54e2 100644
--- a/boards/atlflight/eagle/default.cmake
+++ b/boards/atlflight/eagle/default.cmake
@@ -84,6 +84,7 @@ px4_add_board(
mc_pos_control
navigator
sensors
+ sih
simulator
vmount
vtol_att_control
diff --git a/boards/atlflight/eagle/qurt-default.cmake b/boards/atlflight/eagle/qurt-default.cmake
index 3a04541198db98f3c82bb324fe1bc65bf4a5f1f8..04bbbae4cbe857874af2bce1f69e296376479ce5 100644
--- a/boards/atlflight/eagle/qurt-default.cmake
+++ b/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
diff --git a/boards/atlflight/excelsior/default.cmake b/boards/atlflight/excelsior/default.cmake
index 9ab0e9cdf0e3a2f2be50b64200fe9b46eed3d89d..4ad315e86c42b76e5f4c70bdf329cf836cbb9aed 100644
--- a/boards/atlflight/excelsior/default.cmake
+++ b/boards/atlflight/excelsior/default.cmake
@@ -84,6 +84,7 @@ px4_add_board(
mc_pos_control
navigator
sensors
+ sih
simulator
vmount
vtol_att_control
diff --git a/boards/atlflight/excelsior/qurt-default.cmake b/boards/atlflight/excelsior/qurt-default.cmake
index 18c7e35701be43f14d974abc5a09305a8614968b..7934fece4d32e5bec209dad6cf4aef1559bdf344 100644
--- a/boards/atlflight/excelsior/qurt-default.cmake
+++ b/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
diff --git a/boards/av/x-v1/default.cmake b/boards/av/x-v1/default.cmake
index e19b04e58e7171012d50d76859ebfe053a26d57e..a62cc1bac74988c76437ebbb9fe22651dce92252 100644
--- a/boards/av/x-v1/default.cmake
+++ b/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
diff --git a/boards/beaglebone/blue/cross.cmake b/boards/beaglebone/blue/cross.cmake
index 782769e5f92d3eb05f57a3d5dcea26d7ec07b724..0ceebaf32c0bda370a3cd9fe3b18d183cb435120 100644
--- a/boards/beaglebone/blue/cross.cmake
+++ b/boards/beaglebone/blue/cross.cmake
@@ -48,6 +48,7 @@ px4_add_board(
mc_pos_control
navigator
sensors
+ sih
vmount
vtol_att_control
wind_estimator
diff --git a/boards/beaglebone/blue/native.cmake b/boards/beaglebone/blue/native.cmake
index 314e274e9898f68e385f377b70c10e20e307eac9..a6e921156fae79050e5168800041ce89a44f6799 100644
--- a/boards/beaglebone/blue/native.cmake
+++ b/boards/beaglebone/blue/native.cmake
@@ -46,6 +46,7 @@ px4_add_board(
mc_pos_control
navigator
sensors
+ sih
vmount
vtol_att_control
wind_estimator
diff --git a/boards/bitcraze/crazyflie/default.cmake b/boards/bitcraze/crazyflie/default.cmake
index 3cf7b87cd14927c5326113eefd69326335f4ef58..1d454255c70f1a7ed39e1e99532ffe173810f08d 100644
--- a/boards/bitcraze/crazyflie/default.cmake
+++ b/boards/bitcraze/crazyflie/default.cmake
@@ -37,6 +37,7 @@ px4_add_board(
mc_pos_control
navigator
sensors
+ sih
#vtol_att_control
wind_estimator
diff --git a/boards/emlid/navio2/cross.cmake b/boards/emlid/navio2/cross.cmake
index 282b7f6c765552aba53e2374f66d719681fdd070..8a66ae5fa92f9f66b4cb78ad3c2595f0d66b904b 100644
--- a/boards/emlid/navio2/cross.cmake
+++ b/boards/emlid/navio2/cross.cmake
@@ -52,6 +52,7 @@ px4_add_board(
mc_pos_control
navigator
sensors
+ sih
#simulator
vmount
vtol_att_control
diff --git a/boards/emlid/navio2/native.cmake b/boards/emlid/navio2/native.cmake
index cfd2dbb81c8f360e91949c46016d2a0dfdd37cd2..96340c0e7b7f83902ec3c4fbdfc9e177b8ca9c29 100644
--- a/boards/emlid/navio2/native.cmake
+++ b/boards/emlid/navio2/native.cmake
@@ -50,6 +50,7 @@ px4_add_board(
mc_pos_control
navigator
sensors
+ sih
#simulator
vmount
vtol_att_control
diff --git a/boards/intel/aerofc-v1/default.cmake b/boards/intel/aerofc-v1/default.cmake
index 1d6115485e5862d7ea179fec7fedfde8ad421cfd..5c48c5764bdf0ed007a85e0a0f3d492c32d98b97 100644
--- a/boards/intel/aerofc-v1/default.cmake
+++ b/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
diff --git a/boards/intel/aerofc-v1/rtps.cmake b/boards/intel/aerofc-v1/rtps.cmake
index 7bca9069c7d9d6603fa3f2c596eb480d449c788d..f8710b8e5194c888bab023ce154dee9b4197fbe0 100644
--- a/boards/intel/aerofc-v1/rtps.cmake
+++ b/boards/intel/aerofc-v1/rtps.cmake
@@ -56,6 +56,7 @@ px4_add_board(
micrortps_bridge
navigator
sensors
+ sih
vmount
#vtol_att_control
#wind_estimator
diff --git a/boards/nxp/fmuk66-v3/default.cmake b/boards/nxp/fmuk66-v3/default.cmake
index 857b8849226c51cda8346735ea35a8205f663dcc..dc7dc960d7b5880db63871e4ea808b8ea0dfc1ba 100644
--- a/boards/nxp/fmuk66-v3/default.cmake
+++ b/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
diff --git a/boards/omnibus/f4sd/default.cmake b/boards/omnibus/f4sd/default.cmake
index fc9c77640abf7140873a4a5aaf34946f162897a4..25ecce55436cf271825e584d7228075fdf0ffae7 100644
--- a/boards/omnibus/f4sd/default.cmake
+++ b/boards/omnibus/f4sd/default.cmake
@@ -65,6 +65,7 @@ px4_add_board(
mc_pos_control
navigator
sensors
+ sih
#vmount
#vtol_att_control
#wind_estimator
diff --git a/boards/parrot/bebop/default.cmake b/boards/parrot/bebop/default.cmake
index 5f38f34443befb2e5fe3f07f71b5fc37c1c02b59..525277ae283a0001f04c2d6f6e477e7ca8f1ee40 100644
--- a/boards/parrot/bebop/default.cmake
+++ b/boards/parrot/bebop/default.cmake
@@ -40,6 +40,7 @@ px4_add_board(
mc_pos_control
navigator
sensors
+ sih
#vtol_att_control
wind_estimator
diff --git a/boards/px4/fmu-v3/default.cmake b/boards/px4/fmu-v3/default.cmake
index a360022a34f1af4c0fbe37e7b3fd9ac39586aab9..6b81d64819d6d1285a1dc21a7fde5f1cb4a16022 100644
--- a/boards/px4/fmu-v3/default.cmake
+++ b/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
diff --git a/boards/px4/fmu-v3/rtps.cmake b/boards/px4/fmu-v3/rtps.cmake
index 41205759da3eafa96cefb3698a8072d4d74693d3..cb3a8e6b4b4afca5e12ffdb4cf3472d91b33d397 100644
--- a/boards/px4/fmu-v3/rtps.cmake
+++ b/boards/px4/fmu-v3/rtps.cmake
@@ -84,6 +84,7 @@ px4_add_board(
micrortps_bridge
navigator
sensors
+ sih
vmount
vtol_att_control
wind_estimator
diff --git a/boards/px4/fmu-v3/stackcheck.cmake b/boards/px4/fmu-v3/stackcheck.cmake
index e6d78488c056ed15b3acc0b52912633686ec533f..eebec26e93f338f5915e9de773060e004b8cd70a 100644
--- a/boards/px4/fmu-v3/stackcheck.cmake
+++ b/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
diff --git a/boards/px4/fmu-v4/default.cmake b/boards/px4/fmu-v4/default.cmake
index d83beb27d397859bf26fc3809d1123612530c5dd..a3b0cc02c3567b2b1b150d4841cd5cfc65475c85 100644
--- a/boards/px4/fmu-v4/default.cmake
+++ b/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
diff --git a/boards/px4/fmu-v4/rtps.cmake b/boards/px4/fmu-v4/rtps.cmake
index 72730122a85ae394886e3dba3a12f79af2382167..966e66cfc931a43d24dd04a83c1304a544f78078 100644
--- a/boards/px4/fmu-v4/rtps.cmake
+++ b/boards/px4/fmu-v4/rtps.cmake
@@ -70,6 +70,7 @@ px4_add_board(
micrortps_bridge
navigator
sensors
+ sih
vmount
vtol_att_control
wind_estimator
diff --git a/boards/px4/fmu-v4/stackcheck.cmake b/boards/px4/fmu-v4/stackcheck.cmake
index 913db65cf4b6236b40e780e02635ae35937b7321..856fb4ffdc88ce84003d42d653d54f953d7a29fe 100644
--- a/boards/px4/fmu-v4/stackcheck.cmake
+++ b/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
diff --git a/boards/px4/fmu-v4pro/default.cmake b/boards/px4/fmu-v4pro/default.cmake
index 4d46cd82f0168ad86bdb4e3a79e28625a46be34b..2d515b20d3316deb4847e21480f682465ffe6be3 100644
--- a/boards/px4/fmu-v4pro/default.cmake
+++ b/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
diff --git a/boards/px4/fmu-v4pro/rtps.cmake b/boards/px4/fmu-v4pro/rtps.cmake
index 881ecf6986d53f8234e303f17ac83b0651e7ad49..388e827dce033de58abc7399f8f16a5b126969d8 100644
--- a/boards/px4/fmu-v4pro/rtps.cmake
+++ b/boards/px4/fmu-v4pro/rtps.cmake
@@ -82,6 +82,7 @@ px4_add_board(
micrortps_bridge
navigator
sensors
+ sih
vmount
vtol_att_control
wind_estimator
diff --git a/boards/px4/fmu-v5/default.cmake b/boards/px4/fmu-v5/default.cmake
index 56203b6961099e22da4baeb2c51567370444d604..7fef685fd0bd0cb0518f21431db5fbe00aff8040 100644
--- a/boards/px4/fmu-v5/default.cmake
+++ b/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
diff --git a/boards/px4/fmu-v5/multicopter.cmake b/boards/px4/fmu-v5/multicopter.cmake
index c13bae7f16ca47f6b5de40b0c7170e20bd4ec2d6..2b2d2a386ccd5a0a5e0edfb6c9af1efc5391a568 100644
--- a/boards/px4/fmu-v5/multicopter.cmake
+++ b/boards/px4/fmu-v5/multicopter.cmake
@@ -66,6 +66,7 @@ px4_add_board(
mc_pos_control
navigator
sensors
+ sih
vmount
wind_estimator
diff --git a/boards/px4/fmu-v5/rtps.cmake b/boards/px4/fmu-v5/rtps.cmake
index 57660e82e900dcc7fb47d9abb81dc12e99f70793..527ea2114fd3f0be219c452f2f8bd307ad5acbfc 100644
--- a/boards/px4/fmu-v5/rtps.cmake
+++ b/boards/px4/fmu-v5/rtps.cmake
@@ -83,6 +83,7 @@ px4_add_board(
micrortps_bridge
navigator
sensors
+ sih
vmount
vtol_att_control
wind_estimator
diff --git a/boards/px4/fmu-v5/stackcheck.cmake b/boards/px4/fmu-v5/stackcheck.cmake
index 85ac683482b5d746ee5d46de55f868fabd6fb9a9..9728d25bc63cc428c6c1a48599277af90b124e33 100644
--- a/boards/px4/fmu-v5/stackcheck.cmake
+++ b/boards/px4/fmu-v5/stackcheck.cmake
@@ -83,6 +83,7 @@ px4_add_board(
#micrortps_bridge
navigator
sensors
+ sih
vmount
vtol_att_control
wind_estimator
diff --git a/boards/px4/raspberrypi/cross.cmake b/boards/px4/raspberrypi/cross.cmake
index ba93f231e978e0ad56311ae3603b48e27cecdf00..606e2f89d3f61c5ca3e47650ff987387c08da810 100644
--- a/boards/px4/raspberrypi/cross.cmake
+++ b/boards/px4/raspberrypi/cross.cmake
@@ -45,6 +45,7 @@ px4_add_board(
mc_pos_control
navigator
sensors
+ sih
vmount
vtol_att_control
wind_estimator
diff --git a/boards/px4/raspberrypi/native.cmake b/boards/px4/raspberrypi/native.cmake
index 1778aed980dc390aa419fe879cd535d3d43dee9d..548b2fc27f8ae11407f77203ec205a75f222e5dc 100644
--- a/boards/px4/raspberrypi/native.cmake
+++ b/boards/px4/raspberrypi/native.cmake
@@ -43,6 +43,7 @@ px4_add_board(
mc_pos_control
navigator
sensors
+ sih
#simulator
vmount
vtol_att_control
diff --git a/msg/sih.msg b/msg/sih.msg
index f685139ed1be63e87e92c44f0b03cfb70889c96b..c1fe8a3a935b55cd5038f50846ae5f721677bc6d 100644
--- a/msg/sih.msg
+++ b/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]
diff --git a/src/lib/systemlib/system_params.c b/src/lib/systemlib/system_params.c
index a066997c5f4f6dba7ea9700247651934805d2519..8ffa55f2ffc9c543e484bed3904669ff17750ae9 100644
--- a/src/lib/systemlib/system_params.c
+++ b/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
*
diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp
index 794934a42b9f1f1ffa16330ad511d41fe28b1adc..b758ee9a9d42cb43674fd368d785090befd1ca27 100644
--- a/src/modules/mavlink/mavlink_main.cpp
+++ b/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;
diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp
index 6744841cb4f91f533e95655b866c829c59fa8543..afbf0b9095337f3cc360cbdff675209832f78bf0 100644
--- a/src/modules/mavlink/mavlink_messages.cpp
+++ b/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;
diff --git a/src/modules/sih/CMakeLists.txt b/src/modules/sih/CMakeLists.txt
index c44498611ee83080aab7910fff1531020a9eca8a..ab77255e284b9dcaa04ae097e83fa9c2868949ca 100644
--- a/src/modules/sih/CMakeLists.txt
+++ b/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
diff --git a/src/modules/sih/sih.cpp b/src/modules/sih/sih.cpp
index 84ad9c43c74db42dd2ed38d14ab561f4a1c1c85e..b8d5a598f4e3a92b306c17fce9cb885a4b16e72a 100644
--- a/src/modules/sih/sih.cpp
+++ b/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,106 +188,102 @@ 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);
+ usleep(1000);
+ }
- generate_force_and_torques();
+ // hrt_cancel(&_timer_call); // close the periodic timer interruption
+ orb_unsubscribe(_actuator_out_sub);
+ orb_unsubscribe(_parameter_update_sub);
+ // close(serial_fd);
- equations_of_motion();
+}
- reconstruct_sensors_signals();
+// timer_callback() is used as a trampoline to inner_loop()
+void Sih::timer_callback(void *arg)
+{
+ (reinterpret_cast<Sih *>(arg))->inner_loop();
+}
- send_IMU(now);
+// 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;
- 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;
+ read_motors();
- publish_sih(); // publish _sih message for debug purpose
+ generate_force_and_torques();
- send_serial_msg(serial_fd, (int64_t)(now - task_start)/1000);
+ equations_of_motion();
- 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);
- // }
+ reconstruct_sensors_signals();
- usleep(1000); // sleeping time us
+ send_IMU();
+ if (_now - _gps_time >= 50000) // gps published at 20Hz
+ {
+ _gps_time=_now;
+ send_gps();
}
- orb_unsubscribe(actuator_out_sub);
- orb_unsubscribe(parameter_update_sub);
- orb_unsubscribe(vehicle_status_sub);
- close(serial_fd);
-
-}
+ // send uart message every 40 ms
+ if (_now - _serial_time >= 40000)
+ {
+ _serial_time=_now;
-void Sih::parameters_update_poll(int parameter_update_sub)
-{
- bool updated;
- struct parameter_update_s param_upd;
+ publish_sih(); // publish _sih message for debug purpose
- orb_check(parameter_update_sub, &updated);
+ // send_serial_msg(serial_fd, (int64_t)(now - task_start)/1000);
- if (updated) {
- orb_copy(ORB_ID(parameter_update), parameter_update_sub, ¶m_upd);
- updateParams();
- parameters_updated();
+ 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)
+
}
-uint8_t Sih::is_HIL_running(int vehicle_status_sub)
+void Sih::parameters_update_poll()
{
bool updated;
- struct vehicle_status_s vehicle_status;
- static uint8_t running=false;
+ struct parameter_update_s param_upd;
- orb_check(vehicle_status_sub, &updated);
+ orb_check(_parameter_update_sub, &updated);
if (updated) {
- orb_copy(ORB_ID(vehicle_status), vehicle_status_sub, &vehicle_status);
- running=vehicle_status.hil_state;
- }
-
- return running;
+ orb_copy(ORB_ID(parameter_update), _parameter_update_sub, ¶m_upd);
+ updateParams();
+ parameters_updated();
+ }
}
// store the parameters in a more convenient form
@@ -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);
diff --git a/src/modules/sih/sih.hpp b/src/modules/sih/sih.hpp
index 1c8de14612f71c0ddf8ba57c99cafa5b980e5de6..55b08f1f09f1d1d762bec15cc4bab5c3c5ec104b 100644
--- a/src/modules/sih/sih.hpp
+++ b/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
diff --git a/src/modules/sih/sih_params.c b/src/modules/sih/sih_params.c
index 30485c9f781e9786f5422c022bfd7dd51b253ff9..91f18c1b94b0256dbd66fe5d8a363d6441cedbc5 100644
--- a/src/modules/sih/sih_params.c
+++ b/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