From f150e1e7aadf4685037d6552b601ba0dcf9cd651 Mon Sep 17 00:00:00 2001 From: romain-chiap Date: Fri, 12 Mar 2021 12:04:14 -0500 Subject: [PATCH] sih: time constant added on thrusters, and minor cleanup --- src/modules/sih/sih.cpp | 17 ++++++++++------- src/modules/sih/sih.hpp | 7 ++++--- src/modules/sih/sih_params.c | 10 ++++++++++ 3 files changed, 24 insertions(+), 10 deletions(-) diff --git a/src/modules/sih/sih.cpp b/src/modules/sih/sih.cpp index 987687c43c..d43da83b4b 100644 --- a/src/modules/sih/sih.cpp +++ b/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 - if (_now - _serial_time >= 40_ms) { - _serial_time = _now; + // send groundtruth message every 40 ms + if (_now - _gt_time >= 40_ms) { + _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; diff --git a/src/modules/sih/sih.hpp b/src/modules/sih/sih.hpp index d5be2ba5e4..2b6e4779fd 100644 --- a/src/modules/sih/sih.hpp +++ b/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) _sih_mag_offset_z, (ParamFloat) _sih_distance_snsr_min, (ParamFloat) _sih_distance_snsr_max, - (ParamFloat) _sih_distance_snsr_override + (ParamFloat) _sih_distance_snsr_override, + (ParamFloat) _sih_thrust_tau ) }; diff --git a/src/modules/sih/sih_params.c b/src/modules/sih/sih_params.c index 83177478c6..a5e9c9a60f 100644 --- a/src/modules/sih/sih_params.c +++ b/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);