diff --git a/src/modules/temperature_compensation/temperature_calibration/accel.cpp b/src/modules/temperature_compensation/temperature_calibration/accel.cpp index 4d87c3249c..d9d4ef2ae6 100644 --- a/src/modules/temperature_compensation/temperature_calibration/accel.cpp +++ b/src/modules/temperature_compensation/temperature_calibration/accel.cpp @@ -87,6 +87,13 @@ int TemperatureCalibrationAccel::update_sensor_instance(PerSensorData &data, int return 0; } + if (PX4_ISFINITE(accel_data.temperature)) { + data.has_valid_temperature = true; + + } else { + return 0; + } + data.device_id = accel_data.device_id; data.sensor_sample_filt[0] = accel_data.x; @@ -167,6 +174,14 @@ int TemperatureCalibrationAccel::finish() int TemperatureCalibrationAccel::finish_sensor_instance(PerSensorData &data, int sensor_index) { + if (!data.has_valid_temperature) { + PX4_WARN("Result Accel %d does not have a valid temperature sensor", sensor_index); + + uint32_t param = 0; + set_parameter("TC_A%d_ID", sensor_index, ¶m); + return 0; + } + if (!data.hot_soaked || data.tempcal_complete) { return 0; } diff --git a/src/modules/temperature_compensation/temperature_calibration/baro.cpp b/src/modules/temperature_compensation/temperature_calibration/baro.cpp index 10f6889ff0..bd535e3432 100644 --- a/src/modules/temperature_compensation/temperature_calibration/baro.cpp +++ b/src/modules/temperature_compensation/temperature_calibration/baro.cpp @@ -87,6 +87,13 @@ int TemperatureCalibrationBaro::update_sensor_instance(PerSensorData &data, int return 0; } + if (PX4_ISFINITE(baro_data.temperature)) { + data.has_valid_temperature = true; + + } else { + return 0; + } + data.device_id = baro_data.device_id; data.sensor_sample_filt[0] = 100.0f * baro_data.pressure; // convert from hPA to Pa @@ -163,6 +170,14 @@ int TemperatureCalibrationBaro::finish() int TemperatureCalibrationBaro::finish_sensor_instance(PerSensorData &data, int sensor_index) { + if (!data.has_valid_temperature) { + PX4_WARN("Result baro %d does not have a valid temperature sensor", sensor_index); + + uint32_t param = 0; + set_parameter("TC_B%d_ID", sensor_index, ¶m); + return 0; + } + if (!data.hot_soaked || data.tempcal_complete) { return 0; } diff --git a/src/modules/temperature_compensation/temperature_calibration/common.h b/src/modules/temperature_compensation/temperature_calibration/common.h index f45e7597d0..442ed01af3 100644 --- a/src/modules/temperature_compensation/temperature_calibration/common.h +++ b/src/modules/temperature_compensation/temperature_calibration/common.h @@ -146,6 +146,10 @@ public: float min_diff = _min_temperature_rise; for (unsigned uorb_index = 0; uorb_index < _num_sensor_instances; uorb_index++) { + if (!_data[uorb_index].has_valid_temperature) { + return 110; + } + float cur_diff = _data[uorb_index].high_temp - _data[uorb_index].low_temp; if (cur_diff < min_diff) { @@ -171,6 +175,7 @@ protected: /// verified and the starting temperature set bool hot_soaked = false; ///< true when the sensor has achieved the specified temperature increase bool tempcal_complete = false; ///< true when the calibration has been completed + bool has_valid_temperature = false; ///< true if this sensor has temperature sensor float low_temp = 0.f; ///< low temperature recorded at start of calibration (deg C) float high_temp = 0.f; ///< highest temperature recorded during calibration (deg C) float ref_temp = 0.f; /**< calibration reference temperature, nominally in the middle of the diff --git a/src/modules/temperature_compensation/temperature_calibration/gyro.cpp b/src/modules/temperature_compensation/temperature_calibration/gyro.cpp index 714837862c..22cb0f1fc0 100644 --- a/src/modules/temperature_compensation/temperature_calibration/gyro.cpp +++ b/src/modules/temperature_compensation/temperature_calibration/gyro.cpp @@ -74,6 +74,13 @@ int TemperatureCalibrationGyro::update_sensor_instance(PerSensorData &data, int return 0; } + if (PX4_ISFINITE(gyro_data.temperature)) { + data.has_valid_temperature = true; + + } else { + return 0; + } + data.device_id = gyro_data.device_id; data.sensor_sample_filt[0] = gyro_data.x; @@ -154,6 +161,15 @@ int TemperatureCalibrationGyro::finish() int TemperatureCalibrationGyro::finish_sensor_instance(PerSensorData &data, int sensor_index) { + if (!data.has_valid_temperature) { + PX4_WARN("Result Gyro %d does not have a valid temperature sensor", sensor_index); + data.tempcal_complete = true; + + uint32_t param = 0; + set_parameter("TC_G%d_ID", sensor_index, ¶m); + return 0; + } + if (!data.hot_soaked || data.tempcal_complete) { return 0; }