From 80cac6561f8024f6dcce68b19ea3a44b62e8bc67 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 24 Jul 2018 13:40:03 +0200 Subject: [PATCH] sensors: no temperature compensation in HITL Temperature compensation is not supported in HITL. Therefore, don't try and fail. This basically removes the confusing error message that we get in HITL. --- src/modules/sensors/sensors.cpp | 20 +++---------- .../sensors/temperature_compensation.cpp | 23 ++++++++++++--- .../sensors/temperature_compensation.h | 2 +- src/modules/sensors/voted_sensors_update.cpp | 28 ++++++++----------- 4 files changed, 35 insertions(+), 38 deletions(-) diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 74cc737ee4..3fbea6b538 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -95,6 +95,7 @@ using namespace DriverFramework; using namespace sensors; +using namespace time_literals; /** * Analog layout: @@ -584,13 +585,11 @@ Sensors::run() } sensor_combined_s raw = {}; + sensor_preflight_s preflt = {}; vehicle_air_data_s airdata = {}; vehicle_magnetometer_s magnetometer = {}; - struct sensor_preflight_s preflt = {}; - _rc_update.init(); - _voted_sensors_update.init(raw); /* (re)load params and calibration */ @@ -600,11 +599,8 @@ Sensors::run() * do subscriptions */ _diff_pres_sub = orb_subscribe(ORB_ID(differential_pressure)); - _vcontrol_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); - _params_sub = orb_subscribe(ORB_ID(parameter_update)); - _actuator_ctrl_0_sub = orb_subscribe(ORB_ID(actuator_controls_0)); /* get a set of initial values */ @@ -614,18 +610,10 @@ Sensors::run() _rc_update.rc_parameter_map_poll(_parameter_handles, true /* forced */); - /* advertise the sensor_preflight topic and make the initial publication */ - preflt.accel_inconsistency_m_s_s = 0.0f; - - preflt.gyro_inconsistency_rad_s = 0.0f; - - preflt.mag_inconsistency_ga = 0.0f; - _sensor_preflight = orb_advertise(ORB_ID(sensor_preflight), &preflt); /* wakeup source */ px4_pollfd_struct_t poll_fds = {}; - poll_fds.events = POLLIN; uint64_t last_config_update = hrt_absolute_time(); @@ -704,7 +692,7 @@ Sensors::run() /* keep adding sensors as long as we are not armed, * when not adding sensors poll for param updates */ - if (!_armed && hrt_elapsed_time(&last_config_update) > 500 * 1000) { + if (!_armed && hrt_elapsed_time(&last_config_update) > 500_ms) { _voted_sensors_update.initialize_sensors(); last_config_update = hrt_absolute_time(); @@ -846,7 +834,7 @@ Sensors *Sensors::instantiate(int argc, char *argv[]) return nullptr; } - return new Sensors(hil_enabled);; + return new Sensors(hil_enabled); } int sensors_main(int argc, char *argv[]) diff --git a/src/modules/sensors/temperature_compensation.cpp b/src/modules/sensors/temperature_compensation.cpp index cb272b1cf3..dea5b65d6f 100644 --- a/src/modules/sensors/temperature_compensation.cpp +++ b/src/modules/sensors/temperature_compensation.cpp @@ -151,7 +151,7 @@ int TemperatureCompensation::initialize_parameter_handles(ParameterHandles ¶ return PX4_OK; } -int TemperatureCompensation::parameters_update() +int TemperatureCompensation::parameters_update(bool hil_enabled) { int ret = 0; @@ -163,7 +163,12 @@ int TemperatureCompensation::parameters_update() } /* rate gyro calibration parameters */ - param_get(parameter_handles.gyro_tc_enable, &(_parameters.gyro_tc_enable)); + if (!hil_enabled) { + param_get(parameter_handles.gyro_tc_enable, &_parameters.gyro_tc_enable); + + } else { + _parameters.gyro_tc_enable = 0; + } if (_parameters.gyro_tc_enable == 1) { for (unsigned j = 0; j < GYRO_COUNT_MAX; j++) { @@ -196,7 +201,12 @@ int TemperatureCompensation::parameters_update() } /* accelerometer calibration parameters */ - param_get(parameter_handles.accel_tc_enable, &(_parameters.accel_tc_enable)); + if (!hil_enabled) { + param_get(parameter_handles.accel_tc_enable, &_parameters.accel_tc_enable); + + } else { + _parameters.accel_tc_enable = 0; + } if (_parameters.accel_tc_enable == 1) { for (unsigned j = 0; j < ACCEL_COUNT_MAX; j++) { @@ -229,7 +239,12 @@ int TemperatureCompensation::parameters_update() } /* barometer calibration parameters */ - param_get(parameter_handles.baro_tc_enable, &(_parameters.baro_tc_enable)); + if (!hil_enabled) { + param_get(parameter_handles.baro_tc_enable, &_parameters.baro_tc_enable); + + } else { + _parameters.baro_tc_enable = 0; + } if (_parameters.baro_tc_enable == 1) { for (unsigned j = 0; j < BARO_COUNT_MAX; j++) { diff --git a/src/modules/sensors/temperature_compensation.h b/src/modules/sensors/temperature_compensation.h index 00a07b59ea..69ab49afec 100644 --- a/src/modules/sensors/temperature_compensation.h +++ b/src/modules/sensors/temperature_compensation.h @@ -68,7 +68,7 @@ class TemperatureCompensation public: /** (re)load the parameters. Make sure to call this on startup as well */ - int parameters_update(); + int parameters_update(bool hil_enabled = false); /** supply information which device_id matches a specific uORB topic_instance * (needed if a system has multiple sensors of the same type) diff --git a/src/modules/sensors/voted_sensors_update.cpp b/src/modules/sensors/voted_sensors_update.cpp index b6bb84fff5..b16c84e03b 100644 --- a/src/modules/sensors/voted_sensors_update.cpp +++ b/src/modules/sensors/voted_sensors_update.cpp @@ -148,7 +148,7 @@ void VotedSensorsUpdate::parameters_update() */ /* temperature compensation */ - _temperature_compensation.parameters_update(); + _temperature_compensation.parameters_update(_hil_enabled); /* gyro */ for (int topic_instance = 0; topic_instance < GYRO_COUNT_MAX; ++topic_instance) { @@ -602,11 +602,9 @@ void VotedSensorsUpdate::accel_poll(struct sensor_combined_s &raw) } // handle temperature compensation - 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; - } + 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 @@ -711,11 +709,9 @@ void VotedSensorsUpdate::gyro_poll(struct sensor_combined_s &raw) } // handle temperature compensation - 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; - } + 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 @@ -829,11 +825,9 @@ void VotedSensorsUpdate::baro_poll(vehicle_air_data_s &airdata) float corrected_pressure = 100.0f * baro_report.pressure; // handle temperature compensation - 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; - } + 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 @@ -1080,7 +1074,7 @@ void VotedSensorsUpdate::sensors_poll(sensor_combined_s &raw, vehicle_air_data_s baro_poll(airdata); // publish sensor corrections if necessary - if (!_hil_enabled && _corrections_changed) { + if (_corrections_changed) { _corrections.timestamp = hrt_absolute_time(); if (_sensor_correction_pub == nullptr) {