sih: time constant added on thrusters, and minor cleanup

src/modules/sih/sih.cpp

@@ -66,7 +66,7 @@ Sih::Sih() :
 	const hrt_abstime task_start = hrt_absolute_time();
 	_last_run = task_start;
 	_gps_time = task_start;
-	_serial_time = task_start;
+	_gt_time = task_start;
 	_dist_snsr_time = task_start;
 }
 
@@ -142,7 +142,6 @@ void Sih::Run()
 		_px4_baro.update(_now, _baro_p_mBar);
 	}
 
-
 	// gps published at 20Hz
 	if (_now - _gps_time >= 50_ms) {
 		_gps_time = _now;
@@ -156,9 +155,9 @@ void Sih::Run()
 		send_dist_snsr();
 	}
 
-	// send uart message every 40 ms
+	// send groundtruth message every 40 ms
-	if (_now - _serial_time >= 40_ms) {
+	if (_now - _gt_time >= 40_ms) {
-		_serial_time = _now;
+		_gt_time = _now;
 
 		publish_sih();  // publish _sih message for debug purpose
 	}
@@ -203,6 +202,8 @@ void Sih::parameters_updated()
 	_distance_snsr_min = _sih_distance_snsr_min.get();
 	_distance_snsr_max = _sih_distance_snsr_max.get();
 	_distance_snsr_override = _sih_distance_snsr_override.get();
+
+	_T_TAU = _sih_thrust_tau.get();
 }
 
 // initialization of the variables for the simulator
@@ -254,7 +255,8 @@ void Sih::read_motors()
 
 	if (_actuator_out_sub.update(&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);
+			float u_sp = constrain((actuators_out.output[i] - PWM_DEFAULT_MIN) / (PWM_DEFAULT_MAX - PWM_DEFAULT_MIN), 0.0f, 1.0f);
+			_u[i] = _u[i] + _dt / _T_TAU * (u_sp - _u[i]); // first order transfer function with time constant tau
 		}
 	}
 }
@@ -382,7 +384,7 @@ void Sih::send_dist_snsr()
 		_distance_snsr.current_distance = -_p_I(2) / _C_IB(2, 2);
 
 		if (_distance_snsr.current_distance > _distance_snsr_max) {
-			// this is based on lightware lw20 behavior
+			// this is based on lightware lw20 behaviour
 			_distance_snsr.current_distance = UINT16_MAX / 100.f;
 
 		}
@@ -410,6 +412,7 @@ void Sih::publish_sih()
 
 	_att_gt_pub.publish(_att_gt);
 
+	// publish position groundtruth
 	_gpos_gt.timestamp = hrt_absolute_time();
 	_gpos_gt.lat = _gps_lat_noiseless;
 	_gpos_gt.lon = _gps_lon_noiseless;

src/modules/sih/sih.hpp

@@ -140,7 +140,7 @@ private:
 	hrt_abstime _baro_time{0};
 	hrt_abstime _gps_time{0};
 	hrt_abstime _mag_time{0};
-	hrt_abstime _serial_time{0};
+	hrt_abstime _gt_time{0};
 	hrt_abstime _dist_snsr_time{0};
 	hrt_abstime _now{0};
 	float       _dt{0};         // sampling time [s]
@@ -175,7 +175,7 @@ private:
 	float       _baro_temp_c;   // reconstructed (simulated) barometer temperature in celcius
 
 	// parameters
-	float _MASS, _T_MAX, _Q_MAX, _L_ROLL, _L_PITCH, _KDV, _KDW, _H0;
+	float _MASS, _T_MAX, _Q_MAX, _L_ROLL, _L_PITCH, _KDV, _KDW, _H0, _T_TAU;
 	double _LAT0, _LON0, _COS_LAT0;
 	matrix::Vector3f _W_I;  // weight of the vehicle in inertial frame [N]
 	matrix::Matrix3f _I;    // vehicle inertia matrix
@@ -216,6 +216,7 @@ private:
 		(ParamFloat<px4::params::SIH_MAG_OFFSET_Z>) _sih_mag_offset_z,
 		(ParamFloat<px4::params::SIH_DISTSNSR_MIN>) _sih_distance_snsr_min,
 		(ParamFloat<px4::params::SIH_DISTSNSR_MAX>) _sih_distance_snsr_max,
-		(ParamFloat<px4::params::SIH_DISTSNSR_OVR>) _sih_distance_snsr_override
+		(ParamFloat<px4::params::SIH_DISTSNSR_OVR>) _sih_distance_snsr_override,
+		(ParamFloat<px4::params::SIH_T_TAU>) _sih_thrust_tau
 	)
 };

src/modules/sih/sih_params.c

@@ -424,3 +424,13 @@ PARAM_DEFINE_FLOAT(SIH_DISTSNSR_MAX, 100.0f);
  * @group Simulation In Hardware
  */
 PARAM_DEFINE_FLOAT(SIH_DISTSNSR_OVR, -1.0f);
+
+/**
+ * thruster time constant tau
+ *
+ * the time taken for the thruster to step from 0 to 100% should be about 4 times tau
+ *
+ * @unit s
+ * @group Simulation In Hardware
+ */
+PARAM_DEFINE_FLOAT(SIH_T_TAU, 0.05f);