Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

fix thermal calibration if a sensor does not have temperature sensor #15671

Merged
merged 2 commits into from
Sep 5, 2020
Merged

fix thermal calibration if a sensor does not have temperature sensor #15671

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
42adbd0
thermal calibrtation: do not calibrate sensor without temperature sensor
NicolasM0 Sep 2, 2020
db88179
temperature compensation: change sensor id not found message
NicolasM0 Sep 2, 2020
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
7 changes: 3 additions & 4 deletions src/modules/temperature_compensation/TemperatureCompensationModule.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -67,7 +67,7 @@ void TemperatureCompensationModule::parameters_update()
int temp = _temperature_compensation.set_sensor_id_gyro(report.device_id, uorb_index);

if (temp < 0) {
PX4_ERR("%s init: failed to find device ID %u for instance %i", "gyro", report.device_id, uorb_index);
PX4_INFO("No temperature calibration available for gyro %i (device id %u)", uorb_index, report.device_id);
_corrections.gyro_device_ids[uorb_index] = 0;

} else {
Expand All @@ -84,8 +84,7 @@ void TemperatureCompensationModule::parameters_update()
int temp = _temperature_compensation.set_sensor_id_accel(report.device_id, uorb_index);

if (temp < 0) {
PX4_ERR("%s init: failed to find device ID %u for instance %i", "accel", report.device_id, uorb_index);

PX4_INFO("No temperature calibration available for accel %i (device id %u)", uorb_index, report.device_id);
_corrections.accel_device_ids[uorb_index] = 0;

} else {
Expand All @@ -102,7 +101,7 @@ void TemperatureCompensationModule::parameters_update()
int temp = _temperature_compensation.set_sensor_id_baro(report.device_id, uorb_index);

if (temp < 0) {
PX4_ERR("%s init: failed to find device ID %u for instance %i", "baro", report.device_id, uorb_index);
PX4_INFO("No temperature calibration available for baro %i (device id %u)", uorb_index, report.device_id);
_corrections.baro_device_ids[uorb_index] = 0;

} else {
Expand Down
15 changes: 15 additions & 0 deletions src/modules/temperature_compensation/temperature_calibration/accel.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down Expand Up @@ -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, &param);
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

This makes sense, but worries me slightly. We know about the cases that don't have temperature at all (now reporting NAN), but I'm not absolutely certain it's impossible for other sensors to never publish NAN (eg sporadic read error, etc).

What do you think?

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

You are right, better to be sure on that point. I think I can reverse the logic : initialize with no temperature sensors and change that when a temperature measure is received. Calibration will exit immediately if the first temperature measure of all sensors is nan ( I hope it is not the case juste afetthe boot ...)

Copy link
Member

@dagar dagar Sep 5, 2020 •
edited
Loading

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Calibration will exit immediately if the first temperature measure of all sensors is nan ( I hope it is not the case juste afetthe boot ...)

Briefly thinking about this I'd say it's unlikely, but also not impossible.

The structure of many of the newer IMU drivers is that the temperature is read AFTER the FIFO. This doesn't apply to the sensors like the ICM20602 that have the temperature in the FIFO. https://github.com/PX4/Firmware/blob/7e5e8259f3bc644185569cb2292fd5ebd6bb532f/src/drivers/imu/invensense/icm20689/ICM20689.cpp#L265-L269

That means technically the very first sensor_{accel, gyro} publications with have a temperature of NAN. Now, these drivers start much earlier than the temperature_compensation module and run at a much higher priority, but some of them do have fairly long initial reset periods (30-100 ms) before that first publication.

Let's move forward with this change and keep an eye on it. I wouldn't be opposed to restructuring the drivers slightly so that we go out of our way to never publish NAN as a temperature unless there's no sensor or something is actually busted.

return 0;
}

if (!data.hot_soaked || data.tempcal_complete) {
return 0;
}
Expand Down
15 changes: 15 additions & 0 deletions src/modules/temperature_compensation/temperature_calibration/baro.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down Expand Up @@ -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, &param);
return 0;
}

if (!data.hot_soaked || data.tempcal_complete) {
return 0;
}
Expand Down
5 changes: 5 additions & 0 deletions src/modules/temperature_compensation/temperature_calibration/common.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -146,6 +146,10 @@ class TemperatureCalibrationCommon : public TemperatureCalibrationBase
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) {
Expand All @@ -171,6 +175,7 @@ class TemperatureCalibrationCommon : public TemperatureCalibrationBase
/// 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
Expand Down
16 changes: 16 additions & 0 deletions src/modules/temperature_compensation/temperature_calibration/gyro.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down Expand Up @@ -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, &param);
return 0;
}

if (!data.hot_soaked || data.tempcal_complete) {
return 0;
}
Expand Down