fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-01 02:40:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

accel and gyro calibration refactor and cleanup

Browse Source 
- remove all remaining IOCTLs for accel and gyro and handle all calibration entirely in sensors module with parameters
 - sensor_accel and sensor_gyro are now always raw sensor data
 - calibration procedures no longer need to first clear existing values before starting
 - temperature calibration (TC) remove all scale (SCL) parameters
    - gyro and baro scale are completely unused
    - regular accel calibration scale can be used (CAL_ACC*_xSCALE) instead of TC scale
This commit is contained in:
Daniel Agar
2020-06-17 22:50:09 -04:00
committed by GitHub
parent 588d551098
commit f55ed0992c
99 changed files with 933 additions and 2229 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/temperature_compensation/TemperatureCompensationModule.cpp
+18 -41
View File
@@ -48,19 +48,6 @@ TemperatureCompensationModule::TemperatureCompensationModule() :
ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::lp_default),
_loop_perf(perf_alloc(PC_ELAPSED, "temperature_compensation"))
{
// Initialize the publication variables
for (unsigned i = 0; i < 3; i++) {
_corrections.gyro_scale_0[i] = 1.0f;
_corrections.accel_scale_0[i] = 1.0f;
_corrections.gyro_scale_1[i] = 1.0f;
_corrections.accel_scale_1[i] = 1.0f;
_corrections.gyro_scale_2[i] = 1.0f;
_corrections.accel_scale_2[i] = 1.0f;
}
_corrections.baro_scale_0 = 1.0f;
_corrections.baro_scale_1 = 1.0f;
_corrections.baro_scale_2 = 1.0f;
}
TemperatureCompensationModule::~TemperatureCompensationModule()
@@ -81,11 +68,9 @@ void TemperatureCompensationModule::parameters_update()
if (temp < 0) {
PX4_ERR("%s init: failed to find device ID %u for instance %i", "gyro", report.device_id, uorb_index);
_corrections.gyro_mapping[uorb_index] = 0;
_corrections.gyro_device_ids[uorb_index] = 0;
} else {
_corrections.gyro_mapping[uorb_index] = temp;
_corrections.gyro_device_ids[uorb_index] = report.device_id;
}
}
@@ -101,11 +86,9 @@ void TemperatureCompensationModule::parameters_update()
if (temp < 0) {
PX4_ERR("%s init: failed to find device ID %u for instance %i", "accel", report.device_id, uorb_index);
_corrections.accel_mapping[uorb_index] = 0;
_corrections.accel_device_ids[uorb_index] = 0;
} else {
_corrections.accel_mapping[uorb_index] = temp;
_corrections.accel_device_ids[uorb_index] = report.device_id;
}
}
@@ -120,13 +103,10 @@ void TemperatureCompensationModule::parameters_update()
if (temp < 0) {
PX4_ERR("%s init: failed to find device ID %u for instance %i", "baro", report.device_id, uorb_index);
_corrections.baro_mapping[uorb_index] = 0;
_corrections.baro_device_ids[uorb_index] = 0;
} else {
_corrections.baro_mapping[uorb_index] = temp;
_corrections.baro_device_ids[uorb_index] = temp;
}
}
}
@@ -135,7 +115,6 @@ void TemperatureCompensationModule::parameters_update()
void TemperatureCompensationModule::accelPoll()
{
float *offsets[] = {_corrections.accel_offset_0, _corrections.accel_offset_1, _corrections.accel_offset_2 };
float *scales[] = {_corrections.accel_scale_0, _corrections.accel_scale_1, _corrections.accel_scale_2 };
// For each accel instance
for (uint8_t uorb_index = 0; uorb_index < ACCEL_COUNT_MAX; uorb_index++) {
@@ -143,13 +122,13 @@ void TemperatureCompensationModule::accelPoll()
// Grab temperature from report
if (_accel_subs[uorb_index].update(&report)) {
if (PX4_ISFINITE(report.temperature)) {
// Update the offsets and mark for publication if they've changed
if (_temperature_compensation.update_offsets_accel(uorb_index, report.temperature, offsets[uorb_index]) == 2) {
// Update the scales and offsets and mark for publication if they've changed
if (_temperature_compensation.update_scales_and_offsets_accel(uorb_index, report.temperature, offsets[uorb_index],
scales[uorb_index]) == 2) {
_corrections.accel_device_ids[uorb_index] = report.device_id;
_corrections_changed = true;
_corrections.accel_device_ids[uorb_index] = report.device_id;
_corrections_changed = true;
}
}
}
}
@@ -158,7 +137,6 @@ void TemperatureCompensationModule::accelPoll()
void TemperatureCompensationModule::gyroPoll()
{
float *offsets[] = {_corrections.gyro_offset_0, _corrections.gyro_offset_1, _corrections.gyro_offset_2 };
float *scales[] = {_corrections.gyro_scale_0, _corrections.gyro_scale_1, _corrections.gyro_scale_2 };
// For each gyro instance
for (uint8_t uorb_index = 0; uorb_index < GYRO_COUNT_MAX; uorb_index++) {
@@ -166,13 +144,13 @@ void TemperatureCompensationModule::gyroPoll()
// Grab temperature from report
if (_gyro_subs[uorb_index].update(&report)) {
if (PX4_ISFINITE(report.temperature)) {
// Update the offsets and mark for publication if they've changed
if (_temperature_compensation.update_offsets_gyro(uorb_index, report.temperature, offsets[uorb_index]) == 2) {
// Update the scales and offsets and mark for publication if they've changed
if (_temperature_compensation.update_scales_and_offsets_gyro(uorb_index, report.temperature, offsets[uorb_index],
scales[uorb_index]) == 2) {
_corrections.gyro_device_ids[uorb_index] = report.device_id;
_corrections_changed = true;
_corrections.gyro_device_ids[uorb_index] = report.device_id;
_corrections_changed = true;
}
}
}
}
@@ -181,7 +159,6 @@ void TemperatureCompensationModule::gyroPoll()
void TemperatureCompensationModule::baroPoll()
{
float *offsets[] = {&_corrections.baro_offset_0, &_corrections.baro_offset_1, &_corrections.baro_offset_2 };
float *scales[] = {&_corrections.baro_scale_0, &_corrections.baro_scale_1, &_corrections.baro_scale_2 };
// For each baro instance
for (uint8_t uorb_index = 0; uorb_index < BARO_COUNT_MAX; uorb_index++) {
@@ -189,13 +166,13 @@ void TemperatureCompensationModule::baroPoll()
// Grab temperature from report
if (_baro_subs[uorb_index].update(&report)) {
if (PX4_ISFINITE(report.temperature)) {
// Update the offsets and mark for publication if they've changed
if (_temperature_compensation.update_offsets_baro(uorb_index, report.temperature, offsets[uorb_index]) == 2) {
// Update the scales and offsets and mark for publication if they've changed
if (_temperature_compensation.update_scales_and_offsets_baro(uorb_index, report.temperature,
offsets[uorb_index], scales[uorb_index]) == 2) {
_corrections.baro_device_ids[uorb_index] = report.device_id;
_corrections_changed = true;
_corrections.baro_device_ids[uorb_index] = report.device_id;
_corrections_changed = true;
}
}
}
}