forked from Archive/PX4-Autopilot
temperature_calibration: refactor variable names: remove preceding _
This commit is contained in:
parent
b5b6fb24e3
commit
c829e27a02
|
@ -140,7 +140,7 @@ void Tempcal::task_main()
|
|||
float gyro_sample_filt[SENSOR_COUNT_MAX][4];
|
||||
polyfitter<4> P[SENSOR_COUNT_MAX][3];
|
||||
px4_pollfd_struct_t fds[SENSOR_COUNT_MAX] = {};
|
||||
unsigned _hot_soak_sat[SENSOR_COUNT_MAX] = {};
|
||||
unsigned hot_soak_sat[SENSOR_COUNT_MAX] = {};
|
||||
unsigned num_gyro = orb_group_count(ORB_ID(sensor_gyro));
|
||||
unsigned num_samples[SENSOR_COUNT_MAX] = {0};
|
||||
uint32_t device_ids[SENSOR_COUNT_MAX] = {};
|
||||
|
@ -151,12 +151,12 @@ void Tempcal::task_main()
|
|||
num_gyro = SENSOR_COUNT_MAX;
|
||||
}
|
||||
|
||||
bool _cold_soaked[SENSOR_COUNT_MAX] = {false};
|
||||
bool _hot_soaked[SENSOR_COUNT_MAX] = {false};
|
||||
bool _tempcal_complete[SENSOR_COUNT_MAX] = {false};
|
||||
float _low_temp[SENSOR_COUNT_MAX];
|
||||
float _high_temp[SENSOR_COUNT_MAX] = {0};
|
||||
float _ref_temp[SENSOR_COUNT_MAX];
|
||||
bool cold_soaked[SENSOR_COUNT_MAX] = {};
|
||||
bool hot_soaked[SENSOR_COUNT_MAX] = {};
|
||||
bool tempcal_complete[SENSOR_COUNT_MAX] = {};
|
||||
float low_temp[SENSOR_COUNT_MAX];
|
||||
float high_temp[SENSOR_COUNT_MAX] = {};
|
||||
float ref_temp[SENSOR_COUNT_MAX];
|
||||
|
||||
for (unsigned i = 0; i < num_gyro; i++) {
|
||||
gyro_sub[i] = orb_subscribe_multi(ORB_ID(sensor_gyro), i);
|
||||
|
@ -183,7 +183,7 @@ void Tempcal::task_main()
|
|||
}
|
||||
|
||||
for (unsigned i = 0; i < num_gyro; i++) {
|
||||
if (_hot_soaked[i]) {
|
||||
if (hot_soaked[i]) {
|
||||
continue;
|
||||
}
|
||||
|
||||
|
@ -197,10 +197,10 @@ void Tempcal::task_main()
|
|||
gyro_sample_filt[i][2] = gyro_data.z;
|
||||
gyro_sample_filt[i][3] = gyro_data.temperature;
|
||||
|
||||
if (!_cold_soaked[i]) {
|
||||
_cold_soaked[i] = true;
|
||||
_low_temp[i] = gyro_sample_filt[i][3]; //Record the low temperature
|
||||
_ref_temp[i] = gyro_sample_filt[i][3] + 12.0f;
|
||||
if (!cold_soaked[i]) {
|
||||
cold_soaked[i] = true;
|
||||
low_temp[i] = gyro_sample_filt[i][3]; //Record the low temperature
|
||||
ref_temp[i] = gyro_sample_filt[i][3] + 12.0f;
|
||||
}
|
||||
|
||||
num_samples[i]++;
|
||||
|
@ -208,32 +208,32 @@ void Tempcal::task_main()
|
|||
}
|
||||
|
||||
for (unsigned i = 0; i < num_gyro; i++) {
|
||||
if (_hot_soaked[i]) {
|
||||
if (hot_soaked[i]) {
|
||||
continue;
|
||||
}
|
||||
|
||||
if (gyro_sample_filt[i][3] > _high_temp[i]) {
|
||||
_high_temp[i] = gyro_sample_filt[i][3];
|
||||
_hot_soak_sat[i] = 0;
|
||||
if (gyro_sample_filt[i][3] > high_temp[i]) {
|
||||
high_temp[i] = gyro_sample_filt[i][3];
|
||||
hot_soak_sat[i] = 0;
|
||||
|
||||
} else {
|
||||
continue;
|
||||
}
|
||||
|
||||
//TODO: Hot Soak Saturation
|
||||
if (_hot_soak_sat[i] == 10 || (_high_temp[i] - _low_temp[i]) > 24.0f) {
|
||||
_hot_soaked[i] = true;
|
||||
if (hot_soak_sat[i] == 10 || (high_temp[i] - low_temp[i]) > 24.0f) {
|
||||
hot_soaked[i] = true;
|
||||
}
|
||||
|
||||
if (i == 0) {
|
||||
TC_DEBUG("\n%.20f,%.20f,%.20f,%.20f, %.6f, %.6f, %.6f\n\n", (double)gyro_sample_filt[i][0],
|
||||
(double)gyro_sample_filt[i][1],
|
||||
(double)gyro_sample_filt[i][2], (double)gyro_sample_filt[i][3], (double)_low_temp[i], (double)_high_temp[i],
|
||||
(double)(_high_temp[i] - _low_temp[i]));
|
||||
(double)gyro_sample_filt[i][2], (double)gyro_sample_filt[i][3], (double)low_temp[i], (double)high_temp[i],
|
||||
(double)(high_temp[i] - low_temp[i]));
|
||||
}
|
||||
|
||||
//update linear fit matrices
|
||||
gyro_sample_filt[i][3] -= _ref_temp[i];
|
||||
gyro_sample_filt[i][3] -= ref_temp[i];
|
||||
P[i][0].update((double)gyro_sample_filt[i][3], (double)gyro_sample_filt[i][0]);
|
||||
P[i][1].update((double)gyro_sample_filt[i][3], (double)gyro_sample_filt[i][1]);
|
||||
P[i][2].update((double)gyro_sample_filt[i][3], (double)gyro_sample_filt[i][2]);
|
||||
|
@ -241,7 +241,7 @@ void Tempcal::task_main()
|
|||
}
|
||||
|
||||
for (unsigned i = 0; i < num_gyro; i++) {
|
||||
if (_hot_soaked[i] && !_tempcal_complete[i]) {
|
||||
if (hot_soaked[i] && !tempcal_complete[i]) {
|
||||
double res[3][4] = {0.0f};
|
||||
P[i][0].fit(res[0]);
|
||||
PX4_WARN("Result Gyro %d Axis 0: %.20f %.20f %.20f %.20f", i, (double)res[0][0], (double)res[0][1], (double)res[0][2],
|
||||
|
@ -252,7 +252,7 @@ void Tempcal::task_main()
|
|||
P[i][2].fit(res[2]);
|
||||
PX4_WARN("Result Gyro %d Axis 2: %.20f %.20f %.20f %.20f", i, (double)res[2][0], (double)res[2][1], (double)res[2][2],
|
||||
(double)res[2][3]);
|
||||
_tempcal_complete[i] = true;
|
||||
tempcal_complete[i] = true;
|
||||
++num_completed;
|
||||
|
||||
char str[30];
|
||||
|
@ -279,7 +279,7 @@ void Tempcal::task_main()
|
|||
}
|
||||
|
||||
sprintf(str, "TC_G%d_TMAX", i);
|
||||
param = _high_temp[i];
|
||||
param = high_temp[i];
|
||||
result = param_set_no_notification(param_find(str), ¶m);
|
||||
|
||||
if (result != PX4_OK) {
|
||||
|
@ -287,7 +287,7 @@ void Tempcal::task_main()
|
|||
}
|
||||
|
||||
sprintf(str, "TC_G%d_TMIN", i);
|
||||
param = _low_temp[i];
|
||||
param = low_temp[i];
|
||||
result = param_set_no_notification(param_find(str), ¶m);
|
||||
|
||||
if (result != PX4_OK) {
|
||||
|
@ -295,7 +295,7 @@ void Tempcal::task_main()
|
|||
}
|
||||
|
||||
sprintf(str, "TC_G%d_TREF", i);
|
||||
param = _ref_temp[i];
|
||||
param = ref_temp[i];
|
||||
result = param_set_no_notification(param_find(str), ¶m);
|
||||
|
||||
if (result != PX4_OK) {
|
||||
|
|
Loading…
Reference in New Issue