45 #include <mathlib/mathlib.h> 49 float max_start_temperature)
78 for (
unsigned s = 0; s < 3; s++) {
90 bool finished = data.hot_soaked;
96 return finished ? 0 : 1;
109 data.sensor_sample_filt[0] = accel_data.
x;
110 data.sensor_sample_filt[1] = accel_data.
y;
111 data.sensor_sample_filt[2] = accel_data.
z;
112 data.sensor_sample_filt[3] = accel_data.
temperature;
119 if (!data.cold_soaked) {
127 data.cold_soaked =
true;
128 data.low_temp = data.sensor_sample_filt[3];
129 data.high_temp = data.low_temp;
140 if (data.sensor_sample_filt[3] > data.high_temp) {
141 data.high_temp = data.sensor_sample_filt[3];
142 data.hot_soak_sat = 0;
150 data.hot_soaked =
true;
154 TC_DEBUG(
"\nAccel: %.20f,%.20f,%.20f,%.20f, %.6f, %.6f, %.6f\n\n", (
double)data.sensor_sample_filt[0],
155 (
double)data.sensor_sample_filt[1],
156 (
double)data.sensor_sample_filt[2], (
double)data.sensor_sample_filt[3], (
double)data.low_temp, (
double)data.high_temp,
157 (
double)(data.high_temp - data.low_temp));
161 double relative_temperature = (double)data.sensor_sample_filt[3] - (
double)data.ref_temp;
162 data.P[0].update(relative_temperature, (
double)data.sensor_sample_filt[0]);
163 data.P[1].update(relative_temperature, (
double)data.sensor_sample_filt[1]);
164 data.P[2].update(relative_temperature, (
double)data.sensor_sample_filt[2]);
178 if (result != PX4_OK) {
179 PX4_ERR(
"unable to reset TC_A_ENABLE (%i)", result);
187 if (!data.hot_soaked || data.tempcal_complete) {
191 double res[3][4] = {};
192 data.P[0].fit(res[0]);
194 PX4_INFO(
"Result Accel %d Axis 0: %.20f %.20f %.20f %.20f", sensor_index, (
double)res[0][0], (
double)res[0][1],
197 data.P[1].fit(res[1]);
199 PX4_INFO(
"Result Accel %d Axis 1: %.20f %.20f %.20f %.20f", sensor_index, (
double)res[1][0], (
double)res[1][1],
202 data.P[2].fit(res[2]);
204 PX4_INFO(
"Result Accel %d Axis 2: %.20f %.20f %.20f %.20f", sensor_index, (
double)res[2][0], (
double)res[2][1],
207 data.tempcal_complete =
true;
215 for (
unsigned axis_index = 0; axis_index < 3; axis_index++) {
216 for (
unsigned coef_index = 0; coef_index <= 3; coef_index++) {
217 sprintf(str,
"TC_A%d_X%d_%d", sensor_index, 3 - coef_index, axis_index);
218 param = (float)res[axis_index][coef_index];
221 if (result != PX4_OK) {
222 PX4_ERR(
"unable to reset %s", str);
int _sensor_subs[SENSOR_COUNT_MAX]
virtual int update_sensor_instance(PerSensorData &data, int sensor_sub)
update a single sensor instance
int orb_copy(const struct orb_metadata *meta, int handle, void *buffer)
__EXPORT int param_set_no_notification(param_t param, const void *val)
Set the value of a parameter, but do not notify the system about the change.
float _min_temperature_rise
minimum difference in temperature before the process finishes
#define TC_ERROR_INITIAL_TEMP_TOO_HIGH
starting temperature was above the configured allowed temperature
class TemperatureCalibrationCommon Common base class for all sensor types, contains shared code & dat...
unsigned _num_sensor_instances
High-resolution timer with callouts and timekeeping.
int set_parameter(const char *format_str, unsigned index, const void *value)
set a system parameter (without system notification) and print an error if it fails ...
#define ORB_ID(_name)
Generates a pointer to the uORB metadata structure for a given topic.
TemperatureCalibrationAccel(float min_temperature_rise, float min_start_temperature, float max_start_temperature)
int orb_unsubscribe(int handle)
int orb_group_count(const struct orb_metadata *meta)
Get the number of published instances of a topic group.
__EXPORT param_t param_find(const char *name)
Look up a parameter by name.
virtual ~TemperatureCalibrationAccel()
int orb_check(int handle, bool *updated)
int orb_subscribe_multi(const struct orb_metadata *meta, unsigned instance)
float _max_start_temperature
maximum temperature above which the process does not start and an error is declared ...
float _min_start_temperature
minimum temperature before the process starts
int finish_sensor_instance(PerSensorData &data, int sensor_index)
#define TC_DEBUG(fmt,...)
__EXPORT hrt_abstime hrt_absolute_time(void)
Get absolute time in [us] (does not wrap).
PerSensorData _data[SENSOR_COUNT_MAX]