send MAVLink GROUND_TRUTH at 25 Hz, only in SIH mode. And minor cleanup

src/modules/mavlink/mavlink_main.cpp

@@ -766,7 +766,12 @@ Mavlink::set_hil_enabled(bool hil_enabled)
 		_hil_enabled = true;
 		ret = configure_stream("HIL_ACTUATOR_CONTROLS", 200.0f);
 
-		configure_stream("GROUND_TRUTH", 200.0f); 	// HIL_STATE_QUATERNION to display the SIH
+		if (_param_sys_hitl.get() == 2) {		// Simulation in Hardware enabled ?
+			configure_stream("GROUND_TRUTH", 25.0f); 	// HIL_STATE_QUATERNION to display the SIH
+
+		} else {
+			configure_stream("GROUND_TRUTH", 0.0f);
+		}
 	}
 
 	/* disable HIL */

src/modules/mavlink/mavlink_main.h

@@ -656,7 +656,8 @@ private:
 		(ParamInt<px4::params::MAV_BROADCAST>) _param_mav_broadcast,
 		(ParamBool<px4::params::MAV_HASH_CHK_EN>) _param_mav_hash_chk_en,
 		(ParamBool<px4::params::MAV_HB_FORW_EN>) _param_mav_hb_forw_en,
-		(ParamBool<px4::params::MAV_ODOM_LP>) _param_mav_odom_lp
+		(ParamBool<px4::params::MAV_ODOM_LP>) _param_mav_odom_lp,
+		(ParamInt<px4::params::SYS_HITL>) _param_sys_hitl
 	)
 
 	perf_counter_t		_loop_perf;			/**< loop performance counter */

src/modules/sih/sih.cpp

@@ -48,12 +48,13 @@
 #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 <unistd.h>
-#include <string.h>    			//
+#include <string.h>
-#include <fcntl.h> 				//
+#include <fcntl.h>
-#include <termios.h> 			//
+#include <termios.h>
 
 using namespace math;
+using namespace matrix;
 
 int Sih::print_usage(const char *reason)
 {
@@ -63,7 +64,7 @@ int Sih::print_usage(const char *reason)
 
 	PRINT_MODULE_DESCRIPTION(
 		R"DESCR_STR(
-### Simulator in Hardware
+### Description
 This module provide a simulator for quadrotors running fully
 inside the hardware autopilot.
 
@@ -84,7 +85,6 @@ Most of the variables are declared global in the .hpp file to avoid stack overfl
 
     PRINT_MODULE_USAGE_NAME("sih", "simulation");
     PRINT_MODULE_USAGE_COMMAND("start");
-	PRINT_MODULE_USAGE_PARAM_FLAG('f', "Optional example flag", true);
     PRINT_MODULE_USAGE_PARAM_INT('p', 0, 0, 1024, "Optional example parameter", true);
     PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
 
@@ -94,26 +94,11 @@ Most of the variables are declared global in the .hpp file to avoid stack overfl
 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");
 }
 
@@ -122,7 +107,7 @@ int Sih::task_spawn(int argc, char *argv[])
 {
     _task_id = px4_task_spawn_cmd("sih",
                       SCHED_DEFAULT,
-				      SCHED_PRIORITY_MAX, //SCHED_PRIORITY_DEFAULT
+                      SCHED_PRIORITY_MAX,
                       1024,
                       (px4_main_t)&run_trampoline,
                       (char *const *)argv);
@@ -137,41 +122,7 @@ int Sih::task_spawn(int argc, char *argv[])
 
 Sih *Sih::instantiate(int argc, char *argv[])
 {
-	int example_param = 0;
+    Sih *instance = new Sih(0, false);
-	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");
@@ -225,8 +176,8 @@ void Sih::run()
         perf_end(_loop_perf);
     }
 
-    px4_sem_destroy(&_data_semaphore);
     hrt_cancel(&_timer_call);   // close the periodic timer interruption
+    px4_sem_destroy(&_data_semaphore);
     orb_unsubscribe(_actuator_out_sub);
     orb_unsubscribe(_parameter_update_sub);
 
@@ -579,10 +530,11 @@ float Sih::generate_wgn() 	// generate white Gaussian noise sample with std=1
     return X;
 }
 
-Vector3f Sih::noiseGauss3f(float stdx,float stdy, float stdz) 	// generate white Gaussian noise sample with specified std
+// generate white Gaussian noise sample vector with specified std
+Vector3f Sih::noiseGauss3f(float stdx,float stdy, float stdz)
 {
     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[])
 {

src/modules/sih/sih.hpp

@@ -53,8 +53,6 @@
 #include <uORB/topics/vehicle_global_position.h>    // to publish groundtruth
 #include <uORB/topics/vehicle_attitude.h>           // to publish groundtruth
 
-using namespace matrix;
-
 extern "C" __EXPORT int sih_main(int argc, char *argv[]);
 
 class Sih : public ModuleBase<Sih>, public ModuleParams
@@ -85,12 +83,11 @@ public:
 	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 matrix::Vector3f noiseGauss3f(float stdx, float stdy, float stdz);
 
 	// timer called periodically to post the semaphore
 	static void timer_callback(void *sem);
 
-	// static int pack_float(char* uart_msg, int index, void *value); 	// pack a float to a IEEE754
 private:
 
 	/**
@@ -157,28 +154,28 @@ private:
 	hrt_abstime _now;
 	float       _dt;            // sampling time [s]
 
-	Vector3f 	_T_B; 			// thrust force in body frame [N]
+	matrix::Vector3f    _T_B;           // thrust force in body frame [N]
-	Vector3f 	_Fa_I; 			// aerodynamic force in inertial frame [N]
+	matrix::Vector3f    _Fa_I;          // aerodynamic force in inertial frame [N]
-	Vector3f 	_Mt_B; 			// thruster moments in the body frame [Nm]
+	matrix::Vector3f    _Mt_B;          // thruster moments in the body frame [Nm]
-	Vector3f 	_Ma_B; 			// aerodynamic moments in the body frame [Nm]
+	matrix::Vector3f    _Ma_B;          // aerodynamic moments in the body frame [Nm]
-	Vector3f 	_p_I; 			// inertial position [m]
+	matrix::Vector3f    _p_I;           // inertial position [m]
-	Vector3f 	_v_I; 			// inertial velocity [m/s]
+	matrix::Vector3f    _v_I;           // inertial velocity [m/s]
-	Vector3f 	_v_B; 			// body frame velocity [m/s]
+	matrix::Vector3f    _v_B;           // body frame velocity [m/s]
-	Vector3f 	_p_I_dot; 		// inertial position differential
+	matrix::Vector3f    _p_I_dot;       // inertial position differential
-	Vector3f 	_v_I_dot; 		// inertial velocity differential
+	matrix::Vector3f    _v_I_dot;       // inertial velocity differential
-	Quatf 		_q; 			// quaternion attitude
+	matrix::Quatf       _q;             // quaternion attitude
-	Dcmf 		_C_IB; 			// body to inertial transformation
+	matrix::Dcmf        _C_IB;          // body to inertial transformation
-	Vector3f 	_w_B; 			// body rates in body frame [rad/s]
+	matrix::Vector3f    _w_B;           // body rates in body frame [rad/s]
-	Quatf 		_q_dot;			// quaternion differential
+	matrix::Quatf       _q_dot;         // quaternion differential
-	Vector3f 	_w_B_dot; 		// body rates differential
+	matrix::Vector3f    _w_B_dot;       // body rates differential
 	float       _u[NB_MOTORS];  // thruster signals
 
 
 	// sensors reconstruction
-	Vector3f 	_acc;
+	matrix::Vector3f    _acc;
-	Vector3f 	_mag;
+	matrix::Vector3f    _mag;
-	Vector3f 	_gyro;
+	matrix::Vector3f    _gyro;
-	Vector3f 	_gps_vel;
+	matrix::Vector3f    _gps_vel;
 	double      _gps_lat, _gps_lat_noiseless;
 	double      _gps_lon, _gps_lon_noiseless;
 	float       _gps_alt, _gps_alt_noiseless;
@@ -188,10 +185,10 @@ private:
 	// 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]
+	matrix::Vector3f _W_I;  // weight of the vehicle in inertial frame [N]
-	Matrix3f _I; 	// vehicle inertia matrix
+	matrix::Matrix3f _I;    // vehicle inertia matrix
-	Matrix3f _Im1; 	// inverse of the intertia matrix
+	matrix::Matrix3f _Im1;  // inverse of the intertia matrix
-	Vector3f _mu_I;	// NED magnetic field in inertial frame [G]
+	matrix::Vector3f _mu_I; // NED magnetic field in inertial frame [G]
 
 	// parameters defined in sih_params.c
 	DEFINE_PARAMETERS(
@@ -208,11 +205,11 @@ private:
 		(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_LOC_LAT0>) _sih_lat0,
-		(ParamInt<px4::params::SIH__LON0>) _sih_lon0,
+		(ParamInt<px4::params::SIH_LOC_LON0>) _sih_lon0,
-		(ParamFloat<px4::params::SIH__H0>) _sih_h0,
+		(ParamFloat<px4::params::SIH_LOC_H0>) _sih_h0,
-		(ParamFloat<px4::params::SIH__MU_X>) _sih_mu_x,
+		(ParamFloat<px4::params::SIH_LOC_MU_X>) _sih_mu_x,
-		(ParamFloat<px4::params::SIH__MU_Y>) _sih_mu_y,
+		(ParamFloat<px4::params::SIH_LOC_MU_Y>) _sih_mu_y,
-		(ParamFloat<px4::params::SIH__MU_Z>) _sih_mu_z
+		(ParamFloat<px4::params::SIH_LOC_MU_Z>) _sih_mu_z
 	)
 };

src/modules/sih/sih_params.c

@@ -245,7 +245,7 @@ PARAM_DEFINE_FLOAT(SIH_KDW, 0.025f);
  * @max  850000000
  * @group Simulation In Hardware
  */
-PARAM_DEFINE_INT32(SIH__LAT0, 454671160);
+PARAM_DEFINE_INT32(SIH_LOC_LAT0, 454671160);
 
 /**
  * Initial geodetic longitude
@@ -261,7 +261,7 @@ PARAM_DEFINE_INT32(SIH__LAT0, 454671160);
  * @max  1800000000
  * @group Simulation In Hardware
  */
-PARAM_DEFINE_INT32(SIH__LON0, -737578370);
+PARAM_DEFINE_INT32(SIH_LOC_LON0, -737578370);
 
 /**
  * Initial AMSL ground altitude
@@ -282,7 +282,7 @@ PARAM_DEFINE_INT32(SIH__LON0, -737578370);
  * @increment 0.01
  * @group Simulation In Hardware
  */
-PARAM_DEFINE_FLOAT(SIH__H0, 32.34f);
+PARAM_DEFINE_FLOAT(SIH_LOC_H0, 32.34f);
 
 /**
  * North magnetic field at the initial location
@@ -302,7 +302,7 @@ PARAM_DEFINE_FLOAT(SIH__H0, 32.34f);
  * @increment 0.001
  * @group Simulation In Hardware
  */
-PARAM_DEFINE_FLOAT(SIH__MU_X,  0.179f);
+PARAM_DEFINE_FLOAT(SIH_LOC_MU_X,  0.179f);
 
 /**
  * East magnetic field at the initial location
@@ -322,7 +322,7 @@ PARAM_DEFINE_FLOAT(SIH__MU_X,  0.179f);
  * @increment 0.001
  * @group Simulation In Hardware
  */
-PARAM_DEFINE_FLOAT(SIH__MU_Y, -0.045f);
+PARAM_DEFINE_FLOAT(SIH_LOC_MU_Y, -0.045f);
 
 /**
  * Down magnetic field at the initial location
@@ -342,4 +342,4 @@ PARAM_DEFINE_FLOAT(SIH__MU_Y, -0.045f);
  * @increment 0.001
  * @group Simulation In Hardware
  */
-PARAM_DEFINE_FLOAT(SIH__MU_Z,  0.504f);
+PARAM_DEFINE_FLOAT(SIH_LOC_MU_Z,  0.504f);