int TemperatureCalibrationBaro::update_sensor_instance(PerSensorData &data, int sensor_sub)
{
bool finished = data.hot_soaked;
bool updated;
orb_check(sensor_sub, &updated);
if (!updated) {
return finished ? 0 : 1;
}
sensor_baro_s baro_data;
orb_copy(ORB_ID(sensor_baro), sensor_sub, &baro_data);
if (finished) {
// if we're done, return, but we need to return after orb_copy because of poll()
return 0;
}
data.device_id = baro_data.device_id;
data.sensor_sample_filt[0] = 100.0f * baro_data.pressure; // convert from hPA to Pa
data.sensor_sample_filt[1] = baro_data.temperature;
if (!data.cold_soaked) {
data.cold_soaked = true;
data.low_temp = data.sensor_sample_filt[1]; //Record the low temperature
data.ref_temp = data.sensor_sample_filt[1] + 0.5f * _min_temperature_rise;
}
// check if temperature increased
if (data.sensor_sample_filt[1] > data.high_temp) {
data.high_temp = data.sensor_sample_filt[1];
data.hot_soak_sat = 0;
} else {
return 1;
}
//TODO: Detect when temperature has stopped rising for more than TBD seconds
if (data.hot_soak_sat == 10 || (data.high_temp - data.low_temp) > _min_temperature_rise) {
data.hot_soaked = true;
}
if (sensor_sub == _sensor_subs[0]) { // debug output, but only for the first sensor
TC_DEBUG("\nBaro: %.20f,%.20f,%.20f,%.20f, %.6f, %.6f, %.6f\n\n", (double)data.sensor_sample_filt[0],
(double)data.sensor_sample_filt[1], (double)data.low_temp, (double)data.high_temp,
(double)(data.high_temp - data.low_temp));
}
//update linear fit matrices
data.sensor_sample_filt[1] -= data.ref_temp;
data.P[0].update((double)data.sensor_sample_filt[1], (double)data.sensor_sample_filt[0]);
return 1;
}
int TemperatureCalibrationAccel::update_sensor_instance(PerSensorData &data, int sensor_sub)
{
bool finished = data.hot_soaked;
bool updated;
orb_check(sensor_sub, &updated);
if (!updated) {
return finished ? 0 : 1;
}
sensor_accel_s accel_data;
orb_copy(ORB_ID(sensor_accel), sensor_sub, &accel_data);
if (finished) {
// if we're done, return, but we need to return after orb_copy because of poll()
return 0;
}
data.device_id = accel_data.device_id;
data.sensor_sample_filt[0] = accel_data.x;
data.sensor_sample_filt[1] = accel_data.y;
data.sensor_sample_filt[2] = accel_data.z;
data.sensor_sample_filt[3] = accel_data.temperature;
// wait for min start temp to be reached before starting calibration
if (data.sensor_sample_filt[3] < _min_start_temperature) {
return 1;
}
if (!data.cold_soaked) {
// allow time for sensors and filters to settle
if (hrt_absolute_time() > 10E6) {
// If intial temperature exceeds maximum declare an error condition and exit
if (data.sensor_sample_filt[3] > _max_start_temperature) {
return -TC_ERROR_INITIAL_TEMP_TOO_HIGH;
} else {
data.cold_soaked = true;
data.low_temp = data.sensor_sample_filt[3]; // Record the low temperature
data.high_temp = data.low_temp; // Initialise the high temperature to the initial temperature
data.ref_temp = data.sensor_sample_filt[3] + 0.5f * _min_temperature_rise;
return 1;
}
} else {
return 1;
}
}
// check if temperature increased
if (data.sensor_sample_filt[3] > data.high_temp) {
data.high_temp = data.sensor_sample_filt[3];
data.hot_soak_sat = 0;
} else {
return 1;
}
//TODO: Detect when temperature has stopped rising for more than TBD seconds
if (data.hot_soak_sat == 10 || (data.high_temp - data.low_temp) > _min_temperature_rise) {
data.hot_soaked = true;
}
if (sensor_sub == _sensor_subs[0]) { // debug output, but only for the first sensor
TC_DEBUG("\nAccel: %.20f,%.20f,%.20f,%.20f, %.6f, %.6f, %.6f\n\n", (double)data.sensor_sample_filt[0],
(double)data.sensor_sample_filt[1],
(double)data.sensor_sample_filt[2], (double)data.sensor_sample_filt[3], (double)data.low_temp, (double)data.high_temp,
(double)(data.high_temp - data.low_temp));
}
//update linear fit matrices
double relative_temperature = (double)data.sensor_sample_filt[3] - (double)data.ref_temp;
data.P[0].update(relative_temperature, (double)data.sensor_sample_filt[0]);
data.P[1].update(relative_temperature, (double)data.sensor_sample_filt[1]);
data.P[2].update(relative_temperature, (double)data.sensor_sample_filt[2]);
return 1;
}
