diff --git a/src/modules/sensors/temperature_compensation.cpp b/src/modules/sensors/temperature_compensation.cpp index 21eeaf789a..2a07bdefc6 100644 --- a/src/modules/sensors/temperature_compensation.cpp +++ b/src/modules/sensors/temperature_compensation.cpp @@ -315,16 +315,28 @@ bool TemperatureCompensation::calc_thermal_offsets_3D(SensorCalData3D &coef, flo int TemperatureCompensation::set_sensor_id_gyro(uint32_t device_id, int topic_instance) { + if (_parameters.gyro_tc_enable != 1) { + return 0; + } + return set_sensor_id(device_id, topic_instance, _gyro_data, _parameters.gyro_cal_data); } int TemperatureCompensation::set_sensor_id_accel(uint32_t device_id, int topic_instance) { + if (_parameters.accel_tc_enable != 1) { + return 0; + } + return set_sensor_id(device_id, topic_instance, _accel_data, _parameters.accel_cal_data); } int TemperatureCompensation::set_sensor_id_baro(uint32_t device_id, int topic_instance) { + if (_parameters.baro_tc_enable != 1) { + return 0; + } + return set_sensor_id(device_id, topic_instance, _baro_data, _parameters.baro_cal_data); } @@ -346,10 +358,14 @@ int TemperatureCompensation::set_sensor_id(uint32_t device_id, int topic_instanc int TemperatureCompensation::apply_corrections_gyro(int topic_instance, math::Vector<3> &sensor_data, float temperature, float *offsets, float *scales) { + if (_parameters.gyro_tc_enable != 1) { + return 0; + } + uint8_t mapping = _gyro_data.device_mapping[topic_instance]; - if (mapping == 255 || _parameters.gyro_tc_enable != 1) { - return 0; + if (mapping == 255) { + return -1; } calc_thermal_offsets_3D(_parameters.gyro_cal_data[mapping], temperature, offsets); @@ -373,10 +389,14 @@ int TemperatureCompensation::apply_corrections_gyro(int topic_instance, math::Ve int TemperatureCompensation::apply_corrections_accel(int topic_instance, math::Vector<3> &sensor_data, float temperature, float *offsets, float *scales) { + if (_parameters.accel_tc_enable != 1) { + return 0; + } + uint8_t mapping = _accel_data.device_mapping[topic_instance]; - if (mapping == 255 || _parameters.accel_tc_enable != 1) { - return 0; + if (mapping == 255) { + return -1; } calc_thermal_offsets_3D(_parameters.accel_cal_data[mapping], temperature, offsets); @@ -400,10 +420,14 @@ int TemperatureCompensation::apply_corrections_accel(int topic_instance, math::V int TemperatureCompensation::apply_corrections_baro(int topic_instance, float &sensor_data, float temperature, float *offsets, float *scales) { + if (_parameters.baro_tc_enable != 1) { + return 0; + } + uint8_t mapping = _baro_data.device_mapping[topic_instance]; - if (mapping == 255 || _parameters.baro_tc_enable != 1) { - return 0; + if (mapping == 255) { + return -1; } calc_thermal_offsets_1D(_parameters.baro_cal_data[mapping], temperature, *offsets); diff --git a/src/modules/sensors/temperature_compensation.h b/src/modules/sensors/temperature_compensation.h index 1a14c6db02..cfc9b586e4 100644 --- a/src/modules/sensors/temperature_compensation.h +++ b/src/modules/sensors/temperature_compensation.h @@ -78,7 +78,8 @@ public: * @param temperature measured current temperature * @param offsets returns offsets that were applied (length = 3, except for baro), depending on return value * @param scales returns scales that were applied (length = 3), depending on return value - * @return 0: no changes (correction not enabled), + * @return -1: error: correction enabled, but no sensor mapping set (@see set_sendor_id_gyro) + * 0: no changes (correction not enabled), * 1: corrections applied but no changes to offsets & scales, * 2: corrections applied and offsets & scales updated */ diff --git a/src/modules/sensors/voted_sensors_update.cpp b/src/modules/sensors/voted_sensors_update.cpp index 4b6969c284..b9586c95a7 100644 --- a/src/modules/sensors/voted_sensors_update.cpp +++ b/src/modules/sensors/voted_sensors_update.cpp @@ -140,8 +140,56 @@ void VotedSensorsUpdate::parameters_update() _board_rotation = board_rotation_offset * _board_rotation; + /* Load & apply the sensor calibrations. + * IMPORTANT: we assume all sensor drivers are running and published sensor data at this point + */ + + /* temperature compensation */ _temperature_compensation.parameters_update(); + for (unsigned topic_instance = 0; topic_instance < SENSOR_COUNT_MAX; ++topic_instance) { + + /* gyro */ + if (topic_instance < _gyro.subscription_count) { + // valid subscription, so get the driver id by getting the published sensor data + struct gyro_report report; + + if (orb_copy(ORB_ID(sensor_gyro), _gyro.subscription[topic_instance], &report) == 0) { + if (_temperature_compensation.set_sensor_id_gyro(report.device_id, topic_instance) < 0) { + PX4_ERR("gyro temp compensation init: failed to find device ID %u for instance %i", + report.device_id, topic_instance); + } + } + } + + /* accel */ + if (topic_instance < _accel.subscription_count) { + // valid subscription, so get the driver id by getting the published sensor data + struct accel_report report; + + if (orb_copy(ORB_ID(sensor_accel), _accel.subscription[topic_instance], &report) == 0) { + if (_temperature_compensation.set_sensor_id_accel(report.device_id, topic_instance) < 0) { + PX4_ERR("accel temp compensation init: failed to find device ID %u for instance %i", + report.device_id, topic_instance); + } + } + } + + /* baro */ + if (topic_instance < _baro.subscription_count) { + // valid subscription, so get the driver id by getting the published sensor data + struct baro_report report; + + if (orb_copy(ORB_ID(sensor_baro), _baro.subscription[topic_instance], &report) == 0) { + if (_temperature_compensation.set_sensor_id_baro(report.device_id, topic_instance) < 0) { + PX4_ERR("baro temp compensation init: failed to find device ID %u for instance %i", + report.device_id, topic_instance); + } + } + } + } + + /* set offset parameters to new values */ bool failed; char str[30]; @@ -162,8 +210,6 @@ void VotedSensorsUpdate::parameters_update() } uint32_t driver_device_id = h.ioctl(DEVIOCGDEVICEID, 0); - _temperature_compensation.set_sensor_id_gyro(driver_device_id, s); - bool config_ok = false; /* run through all stored calibrations that are applied at the driver level*/ @@ -230,8 +276,6 @@ void VotedSensorsUpdate::parameters_update() } uint32_t driver_device_id = h.ioctl(DEVIOCGDEVICEID, 0); - _temperature_compensation.set_sensor_id_accel(driver_device_id, s); - bool config_ok = false; /* run through all stored calibrations */ @@ -305,7 +349,6 @@ void VotedSensorsUpdate::parameters_update() } uint32_t driver_device_id = h.ioctl(DEVIOCGDEVICEID, 0); - bool config_ok = false; /* run through all stored calibrations */ @@ -421,21 +464,6 @@ void VotedSensorsUpdate::parameters_update() } } - /* run through all baro sensors */ - for (unsigned s = 0; s < SENSOR_COUNT_MAX; s++) { - - (void)sprintf(str, "%s%u", BARO_BASE_DEVICE_PATH, s); - - DevHandle h; - DevMgr::getHandle(str, h); - - if (!h.isValid()) { - continue; - } - - uint32_t driver_device_id = h.ioctl(DEVIOCGDEVICEID, 0); - _temperature_compensation.set_sensor_id_baro(driver_device_id, s); - } } void VotedSensorsUpdate::accel_poll(struct sensor_combined_s &raw) diff --git a/src/modules/sensors/voted_sensors_update.h b/src/modules/sensors/voted_sensors_update.h index 930f17e75f..b649ef9e41 100644 --- a/src/modules/sensors/voted_sensors_update.h +++ b/src/modules/sensors/voted_sensors_update.h @@ -100,7 +100,8 @@ public: float baro_pressure() const { return _last_best_baro_pressure; } /** - * call this whenever parameters got updated + * call this whenever parameters got updated. Make sure to have initialize_sensors() called at least + * once before calling this. */ void parameters_update();