diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 8b406f9c20..69d26b3e21 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -140,7 +140,7 @@ public: /** * Constructor */ - Sensors(); + Sensors(bool hil_enabled); /** * Destructor, also kills the sensors task. @@ -165,8 +165,7 @@ private: bool _task_should_exit; /**< if true, sensor task should exit */ int _sensors_task; /**< task handle for sensor task */ - bool _hil_enabled; /**< if true, HIL is active */ - bool _publishing; /**< if true, we are publishing sensor data (in HIL mode, we don't) */ + const bool _hil_enabled; /**< if true, HIL is active */ bool _armed; /**< arming status of the vehicle */ int _actuator_ctrl_0_sub; /**< attitude controls sub */ @@ -250,14 +249,13 @@ namespace sensors Sensors *g_sensors = nullptr; } -Sensors::Sensors() : +Sensors::Sensors(bool hil_enabled) : _h_adc(), _last_adc(0), _task_should_exit(true), _sensors_task(-1), - _hil_enabled(false), - _publishing(true), + _hil_enabled(hil_enabled), _armed(false), _actuator_ctrl_0_sub(-1), @@ -277,7 +275,7 @@ Sensors::Sensors() : _airspeed_validator(), _rc_update(_parameters), - _voted_sensors_update(_parameters) + _voted_sensors_update(_parameters, hil_enabled) { memset(&_diff_pres, 0, sizeof(_diff_pres)); memset(&_parameters, 0, sizeof(_parameters)); @@ -412,25 +410,12 @@ Sensors::vehicle_control_mode_poll() struct vehicle_control_mode_s vcontrol_mode; bool vcontrol_mode_updated; - /* Check HIL state if vehicle control mode has changed */ orb_check(_vcontrol_mode_sub, &vcontrol_mode_updated); if (vcontrol_mode_updated) { orb_copy(ORB_ID(vehicle_control_mode), _vcontrol_mode_sub, &vcontrol_mode); _armed = vcontrol_mode.flag_armed; - - /* switching from non-HIL to HIL mode */ - if (vcontrol_mode.flag_system_hil_enabled && !_hil_enabled) { - _hil_enabled = true; - _publishing = false; - - /* switching from HIL to non-HIL mode */ - - } else if (!_publishing && !_hil_enabled) { - _hil_enabled = false; - _publishing = true; - } } } @@ -473,8 +458,8 @@ Sensors::parameter_update_poll(bool forced) void Sensors::adc_poll(struct sensor_combined_s &raw) { - /* only read if publishing */ - if (!_publishing) { + /* only read if not in HIL mode */ + if (_hil_enabled) { return; } @@ -569,13 +554,15 @@ Sensors::task_main() /* start individual sensors */ int ret = 0; - /* This calls a sensors_init which can have different implementations on NuttX, POSIX, QURT. */ - ret = sensors_init(); + if (!_hil_enabled) { + /* This calls a sensors_init which can have different implementations on NuttX, POSIX, QURT. */ + ret = sensors_init(); #if !defined(__PX4_QURT) && !defined(__PX4_POSIX_RPI) && !defined(__PX4_POSIX_BEBOP) - // TODO: move adc_init into the sensors_init call. - ret = ret || adc_init(); + // TODO: move adc_init into the sensors_init call. + ret = ret || adc_init(); #endif + } if (ret) { PX4_ERR("sensor initialization failed"); @@ -670,7 +657,7 @@ Sensors::task_main() diff_pres_poll(raw); - if (_publishing && raw.timestamp > 0) { + if (raw.timestamp > 0) { _voted_sensors_update.set_relative_timestamps(raw); @@ -771,7 +758,13 @@ int sensors_main(int argc, char *argv[]) return 0; } - sensors::g_sensors = new Sensors; + bool hil_enabled = false; + + if (argc > 2 && !strcmp(argv[2], "-hil")) { + hil_enabled = true; + } + + sensors::g_sensors = new Sensors(hil_enabled); if (sensors::g_sensors == nullptr) { PX4_ERR("alloc failed"); diff --git a/src/modules/sensors/voted_sensors_update.cpp b/src/modules/sensors/voted_sensors_update.cpp index cd8403e42a..a8d8da7320 100644 --- a/src/modules/sensors/voted_sensors_update.cpp +++ b/src/modules/sensors/voted_sensors_update.cpp @@ -53,8 +53,8 @@ using namespace DriverFramework; const double VotedSensorsUpdate::_msl_pressure = 101.325f; -VotedSensorsUpdate::VotedSensorsUpdate(const Parameters ¶meters) - : _parameters(parameters) +VotedSensorsUpdate::VotedSensorsUpdate(const Parameters ¶meters, bool hil_enabled) + : _parameters(parameters), _hil_enabled(hil_enabled) { memset(&_last_sensor_data, 0, sizeof(_last_sensor_data)); memset(&_last_accel_timestamp, 0, sizeof(_last_accel_timestamp)); @@ -579,9 +579,11 @@ void VotedSensorsUpdate::accel_poll(struct sensor_combined_s &raw) } // handle temperature compensation - if (_temperature_compensation.apply_corrections_accel(uorb_index, accel_data, accel_report.temperature, - offsets[uorb_index], scales[uorb_index]) == 2) { - _corrections_changed = true; + if (!_hil_enabled) { + if (_temperature_compensation.apply_corrections_accel(uorb_index, accel_data, accel_report.temperature, + offsets[uorb_index], scales[uorb_index]) == 2) { + _corrections_changed = true; + } } // rotate corrected measurements from sensor to body frame @@ -677,9 +679,11 @@ void VotedSensorsUpdate::gyro_poll(struct sensor_combined_s &raw) } // handle temperature compensation - if (_temperature_compensation.apply_corrections_gyro(uorb_index, gyro_rate, gyro_report.temperature, - offsets[uorb_index], scales[uorb_index]) == 2) { - _corrections_changed = true; + if (!_hil_enabled) { + if (_temperature_compensation.apply_corrections_gyro(uorb_index, gyro_rate, gyro_report.temperature, + offsets[uorb_index], scales[uorb_index]) == 2) { + _corrections_changed = true; + } } // rotate corrected measurements from sensor to body frame @@ -785,9 +789,11 @@ void VotedSensorsUpdate::baro_poll(struct sensor_combined_s &raw) float corrected_pressure = 100.0f * baro_report.pressure; // handle temperature compensation - if (_temperature_compensation.apply_corrections_baro(uorb_index, corrected_pressure, baro_report.temperature, - offsets[uorb_index], scales[uorb_index]) == 2) { - _corrections_changed = true; + if (!_hil_enabled) { + if (_temperature_compensation.apply_corrections_baro(uorb_index, corrected_pressure, baro_report.temperature, + offsets[uorb_index], scales[uorb_index]) == 2) { + _corrections_changed = true; + } } // First publication with data @@ -1013,7 +1019,7 @@ void VotedSensorsUpdate::sensors_poll(sensor_combined_s &raw) baro_poll(raw); // publish sensor corrections if necessary - if (_corrections_changed) { + if (!_hil_enabled && _corrections_changed) { _corrections.timestamp = hrt_absolute_time(); if (_sensor_correction_pub == nullptr) { diff --git a/src/modules/sensors/voted_sensors_update.h b/src/modules/sensors/voted_sensors_update.h index c71b463ef3..1a3221242f 100644 --- a/src/modules/sensors/voted_sensors_update.h +++ b/src/modules/sensors/voted_sensors_update.h @@ -76,7 +76,7 @@ public: * @param parameters parameter values. These do not have to be initialized when constructing this object. * Only when calling init(), they have to be initialized. */ - VotedSensorsUpdate(const Parameters ¶meters); + VotedSensorsUpdate(const Parameters ¶meters, bool hil_enabled); /** * initialize subscriptions etc. @@ -259,6 +259,7 @@ private: math::Matrix<3, 3> _mag_rotation[SENSOR_COUNT_MAX] = {}; /**< rotation matrix for the orientation that the external mag0 is mounted */ const Parameters &_parameters; + const bool _hil_enabled; /**< is hardware-in-the-loop mode enabled? */ float _accel_diff[3][2]; /**< filtered accel differences between IMU units (m/s/s) */ float _gyro_diff[3][2]; /**< filtered gyro differences between IMU uinits (rad/s) */