fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-14 06:17:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

sensor calibration delete temperature (CAL_ACCx_TEMP, CAL_GYROx_TEMP, CAL_MAGx_TEMP)

Browse Source 
- this was an experiment to casually monitor sensor offsets relative to temperature, but now that all calibration offsets can be adjusted post-flight the stored temperature can be misleading
 - deleting to save a little bit of flash (and storing the temperature wasn't useful)
This commit is contained in:
Daniel Agar
2022-03-12 11:38:49 -05:00
committed by GitHub
parent aa64789792
commit 3d54d25867
15 changed files with 11 additions and 196 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/accelerometer_calibration.cpp
+2 -29
View File
@@ -157,15 +157,13 @@ 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],
float (&accel_temperature_avg)[MAX_ACCEL_SENS], unsigned orient, unsigned samples_num)
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;
@@ -217,14 +215,6 @@ 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;
}
}
}
@@ -248,8 +238,6 @@ 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;
@@ -263,7 +251,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, worker_data->accel_temperature_ref, orientation, samples_num);
read_accelerometer_avg(worker_data->accel_ref, orientation, samples_num);
// check accel
for (unsigned accel_index = 0; accel_index < MAX_ACCEL_SENS; accel_index++) {
@@ -414,8 +402,6 @@ 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();
@@ -490,7 +476,6 @@ 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)) {
@@ -526,21 +511,11 @@ 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;
}
}
@@ -550,7 +525,6 @@ 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};
@@ -593,7 +567,6 @@ int do_accel_calibration_quick(orb_advert_t *mavlink_log_pub)
} else {
calibration.set_offset(offset);
calibration.set_temperature(temperature_avg);
if (calibration.ParametersSave(accel_index)) {
calibration.PrintStatus();
src/modules/commander/gyro_calibration.cpp
-12
View File
@@ -71,7 +71,6 @@ struct gyro_worker_data_t {
calibration::Gyroscope calibrations[MAX_GYROS] {};
Vector3f offset[MAX_GYROS] {};
float temperature[MAX_GYROS] {NAN, NAN, NAN, NAN};
math::MedianFilter<float, 9> filter[3] {};
};
@@ -119,14 +118,6 @@ static calibrate_return gyro_calibration_worker(gyro_worker_data_t &worker_data)
calibration_counter[gyro_index]++;
if (!PX4_ISFINITE(worker_data.temperature[gyro_index])) {
// set first valid value
worker_data.temperature[gyro_index] = gyro_report.temperature * calibration_counter[gyro_index];
} else {
worker_data.temperature[gyro_index] += gyro_report.temperature;
}
if (gyro_index == 0) {
worker_data.filter[0].insert(gyro_report.x - thermal_offset(0));
worker_data.filter[1].insert(gyro_report.y - thermal_offset(1));
@@ -169,7 +160,6 @@ static calibrate_return gyro_calibration_worker(gyro_worker_data_t &worker_data)
}
worker_data.offset[s] /= calibration_counter[s];
worker_data.temperature[s] /= calibration_counter[s];
}
return calibrate_return_ok;
@@ -269,8 +259,6 @@ int do_gyro_calibration(orb_advert_t *mavlink_log_pub)
if (calibration.device_id() != 0) {
calibration.set_offset(worker_data.offset[uorb_index]);
calibration.set_temperature(worker_data.temperature[uorb_index]);
calibration.PrintStatus();
if (calibration.ParametersSave(uorb_index, true)) {
src/modules/commander/mag_calibration.cpp
-15
View File
@@ -94,8 +94,6 @@ struct mag_worker_data_t {
float *y[MAX_MAGS];
float *z[MAX_MAGS];
float temperature[MAX_MAGS] {NAN, NAN, NAN, NAN};
calibration::Magnetometer calibration[MAX_MAGS] {};
};
@@ -342,7 +340,6 @@ static calibrate_return mag_calibration_worker(detect_orientation_return orienta
if (mag_sub[0].updatedBlocking(1000_ms)) {
bool rejected = false;
Vector3f new_samples[MAX_MAGS] {};
float new_temperature[MAX_MAGS] {NAN, NAN, NAN, NAN};
for (uint8_t cur_mag = 0; cur_mag < MAX_MAGS; cur_mag++) {
if (worker_data->calibration[cur_mag].device_id() != 0) {
@@ -371,7 +368,6 @@ static calibrate_return mag_calibration_worker(detect_orientation_return orienta
if (!reject) {
new_samples[cur_mag] = Vector3f{mag.x, mag.y, mag.z};
new_temperature[cur_mag] = mag.temperature;
updated = true;
break;
}
@@ -392,14 +388,6 @@ static calibrate_return mag_calibration_worker(detect_orientation_return orienta
worker_data->y[cur_mag][worker_data->calibration_counter_total[cur_mag]] = new_samples[cur_mag](1);
worker_data->z[cur_mag][worker_data->calibration_counter_total[cur_mag]] = new_samples[cur_mag](2);
if (!PX4_ISFINITE(worker_data->temperature[cur_mag])) {
// set first valid value
worker_data->temperature[cur_mag] = new_temperature[cur_mag];
} else {
worker_data->temperature[cur_mag] = 0.5f * (worker_data->temperature[cur_mag] + new_temperature[cur_mag]);
}
worker_data->calibration_counter_total[cur_mag]++;
}
}
@@ -912,8 +900,6 @@ calibrate_return mag_calibrate_all(orb_advert_t *mavlink_log_pub, int32_t cal_ma
current_cal.set_offdiagonal(offdiag[cur_mag]);
}
current_cal.set_temperature(worker_data.temperature[cur_mag]);
current_cal.PrintStatus();
if (current_cal.ParametersSave(cur_mag, true)) {
@@ -1019,7 +1005,6 @@ int do_mag_calibration_quick(orb_advert_t *mavlink_log_pub, float heading_radian
// use any existing scale and store the offset to the expected earth field
const Vector3f offset = Vector3f{mag.x, mag.y, mag.z} - (cal.scale().I() * cal.rotation().transpose() * expected_field);
cal.set_offset(offset);
cal.set_temperature(mag.temperature);
// save new calibration
if (cal.ParametersSave(cur_mag)) {
src/modules/gyro_calibration/GyroCalibration.cpp
-1
View File
@@ -246,7 +246,6 @@ void GyroCalibration::Run()
const Vector3f old_offset{_gyro_calibration[gyro].offset()};
if (_gyro_calibration[gyro].set_offset(_gyro_mean[gyro].mean())) {
_gyro_calibration[gyro].set_temperature(_temperature[gyro]);
calibration_updated = true;
src/modules/sensors/module.yaml
-36
View File
@@ -159,18 +159,6 @@ parameters:
num_instances: *max_num_sensor_instances
instance_start: 0
CAL_ACC${i}_TEMP:
description:
short: Accelerometer ${i} calibration temperature
long: Temperature during last calibration.
category: System
type: float
default: -1000.
unit: celcius
volatile: true
num_instances: *max_num_sensor_instances
instance_start: 0
# Gyroscope calibration
CAL_GYRO${i}_ID:
description:
@@ -288,18 +276,6 @@ parameters:
num_instances: *max_num_sensor_instances
instance_start: 0
CAL_GYRO${i}_TEMP:
description:
short: Gyroscope ${i} calibration temperature
long: Temperature during last calibration.
category: System
type: float
default: -1000.
unit: celcius
volatile: true
num_instances: *max_num_sensor_instances
instance_start: 0
# Magnetometer calibration
CAL_MAG${i}_ID:
description:
@@ -530,15 +506,3 @@ parameters:
volatile: true
num_instances: *max_num_sensor_instances
instance_start: 0
CAL_MAG${i}_TEMP:
description:
short: Magnetometer ${i} calibration temperature
long: Temperature during last calibration.
category: System
type: float
default: -1000.
unit: celcius
volatile: true
num_instances: *max_num_sensor_instances
instance_start: 0
src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.cpp
-4
View File
@@ -194,7 +194,6 @@ void VehicleMagnetometer::UpdateMagBiasEstimate()
const Vector3f offset = _calibration[mag_index].BiasCorrectedSensorOffset(_calibration_estimator_bias[mag_index]);
if (_calibration[mag_index].set_offset(offset)) {
_calibration[mag_index].set_temperature(_last_data[mag_index].temperature);
// save parameters with preferred calibration slot to current sensor index
_calibration[mag_index].ParametersSave(mag_index);
@@ -253,7 +252,6 @@ void VehicleMagnetometer::UpdateMagCalibration()
_calibration_estimator_bias[mag_index];
_mag_cal[i].variance = bias_variance;
_mag_cal[i].temperature = _last_data[mag_index].temperature;
_in_flight_mag_cal_available = true;
break;
@@ -298,8 +296,6 @@ void VehicleMagnetometer::UpdateMagCalibration()
(double)mag_cal_offset(0), (double)mag_cal_offset(1), (double)mag_cal_offset(2),
(double)_mag_cal[i].offset(0), (double)_mag_cal[i].offset(1), (double)_mag_cal[i].offset(2));
_calibration[mag_index].set_temperature(_last_data[mag_index].temperature);
_calibration[mag_index].ParametersSave();
calibration_param_save_needed = true;
src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.hpp
-1
View File
@@ -119,7 +119,6 @@ private:
uint32_t device_id{0};
matrix::Vector3f offset{};
matrix::Vector3f variance{};
float temperature{NAN};
} _mag_cal[ORB_MULTI_MAX_INSTANCES] {};
uORB::SubscriptionCallbackWorkItem _sensor_sub[MAX_SENSOR_COUNT] {