fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-06 04:30:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

sensor calibration: save temperature at calibration time for monitoring

Browse Source
This commit is contained in:
Daniel Agar
2021-04-01 11:24:53 -04:00
parent a76bcd3e01
commit ff39e27e2d
29 changed files with 295 additions and 41 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/accelerometer_calibration.cpp
+30 -2
View File
@@ -157,13 +157,15 @@ struct accel_worker_data_s {
orb_advert_t *mavlink_log_pub{nullptr};
unsigned done_count{0};
float accel_ref[MAX_ACCEL_SENS][detect_orientation_side_count][3] {};
float accel_temperature_ref[MAX_ACCEL_SENS] {NAN, NAN, NAN, NAN};
};
// Read specified number of accelerometer samples, calculate average and dispersion.
static calibrate_return read_accelerometer_avg(float (&accel_avg)[MAX_ACCEL_SENS][detect_orientation_side_count][3],
unsigned orient, unsigned samples_num)
float (&accel_temperature_avg)[MAX_ACCEL_SENS], unsigned orient, unsigned samples_num)
{
Vector3f accel_sum[MAX_ACCEL_SENS] {};
float temperature_sum[MAX_ACCEL_SENS] {NAN, NAN, NAN, NAN};
unsigned counts[MAX_ACCEL_SENS] {};
unsigned errcount = 0;
@@ -213,7 +215,16 @@ static calibrate_return read_accelerometer_avg(float (&accel_avg)[MAX_ACCEL_SENS
}
accel_sum[accel_index] += Vector3f{arp.x, arp.y, arp.z} - offset;
counts[accel_index]++;
if (!PX4_ISFINITE(temperature_sum[accel_index])) {
// set first valid value
temperature_sum[accel_index] = (arp.temperature * counts[accel_index]);
} else {
temperature_sum[accel_index] += arp.temperature;
}
}
}
@@ -237,6 +248,8 @@ static calibrate_return read_accelerometer_avg(float (&accel_avg)[MAX_ACCEL_SENS
for (unsigned s = 0; s < MAX_ACCEL_SENS; s++) {
const Vector3f avg{accel_sum[s] / counts[s]};
avg.copyTo(accel_avg[s][orient]);
accel_temperature_avg[s] = temperature_sum[s] / counts[s];
}
return calibrate_return_ok;
@@ -250,7 +263,7 @@ static calibrate_return accel_calibration_worker(detect_orientation_return orien
calibration_log_info(worker_data->mavlink_log_pub, "[cal] Hold still, measuring %s side",
detect_orientation_str(orientation));
read_accelerometer_avg(worker_data->accel_ref, orientation, samples_num);
read_accelerometer_avg(worker_data->accel_ref, worker_data->accel_temperature_ref, orientation, samples_num);
// check accel
for (unsigned accel_index = 0; accel_index < MAX_ACCEL_SENS; accel_index++) {
@@ -404,6 +417,8 @@ int do_accel_calibration(orb_advert_t *mavlink_log_pub)
const Matrix3f accel_T_rotated{board_rotation_t *accel_T * board_rotation};
calibrations[i].set_scale(accel_T_rotated.diag());
calibrations[i].set_temperature(worker_data.accel_temperature_ref[i]);
#if defined(DEBUD_BUILD)
PX4_INFO("accel %d: offset", i);
offset.print();
@@ -478,6 +493,7 @@ int do_accel_calibration_quick(orb_advert_t *mavlink_log_pub)
for (unsigned accel_index = 0; accel_index < MAX_ACCEL_SENS; accel_index++) {
sensor_accel_s arp{};
Vector3f accel_sum{};
float temperature_sum{NAN};
unsigned count = 0;
while (accel_subs[accel_index].update(&arp)) {
@@ -513,11 +529,21 @@ int do_accel_calibration_quick(orb_advert_t *mavlink_log_pub)
if (diff.norm() < 1.f) {
accel_sum += Vector3f{arp.x, arp.y, arp.z} - offset;
count++;
if (!PX4_ISFINITE(temperature_sum)) {
// set first valid value
temperature_sum = (arp.temperature * count);
} else {
temperature_sum += arp.temperature;
}
}
} else {
accel_sum = accel;
temperature_sum = arp.temperature;
count = 1;
}
}
@@ -527,6 +553,7 @@ int do_accel_calibration_quick(orb_advert_t *mavlink_log_pub)
bool calibrated = false;
const Vector3f accel_avg = accel_sum / count;
const float temperature_avg = temperature_sum / count;
Vector3f offset{0.f, 0.f, 0.f};
@@ -572,6 +599,7 @@ int do_accel_calibration_quick(orb_advert_t *mavlink_log_pub)
} else {
calibration.set_offset(offset);
calibration.set_temperature(temperature_avg);
if (calibration.ParametersSave()) {
calibration.PrintStatus();