fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-21 03:33:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

sensors: preserve all valid calibration parameters even if sensor currently missing

Browse Source 
- mark calibration slots active for first N sensors found
 - calibration procedure don't bother resetting unused slots
This commit is contained in:
Daniel Agar
2021-06-07 08:50:32 -04:00
parent 2ccd86102b
commit 4f850c7cd0
7 changed files with 125 additions and 63 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/sensors/sensors.cpp
+78 -6
View File
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2012-2018 PX4 Development Team. All rights reserved.
* Copyright (c) 2012-2021 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@@ -48,6 +48,7 @@
#include <lib/mathlib/mathlib.h>
#include <lib/parameters/param.h>
#include <lib/perf/perf_counter.h>
#include <lib/sensor_calibration/Utilities.hpp>
#include <px4_platform_common/getopt.h>
#include <px4_platform_common/module.h>
#include <px4_platform_common/module_params.h>
@@ -179,6 +180,12 @@ private:
VehicleIMU *_vehicle_imu_list[MAX_SENSOR_COUNT] {};
uint8_t _n_accel{0};
uint8_t _n_baro{0};
uint8_t _n_gps{0};
uint8_t _n_gyro{0};
uint8_t _n_mag{0};
/**
* Update our local parameter cache.
*/
@@ -308,6 +315,54 @@ int Sensors::parameters_update()
_voted_sensors_update.parametersUpdate();
// mark all existing sensor calibrations active even if sensor is missing
// this preserves the calibration in the event of a parameter export while the sensor is missing
for (int i = 0; i < MAX_SENSOR_COUNT; i++) {
uint32_t device_id_accel = calibration::GetCalibrationParam("ACC", "ID", i);
uint32_t device_id_gyro = calibration::GetCalibrationParam("GYRO", "ID", i);
uint32_t device_id_mag = calibration::GetCalibrationParam("MAG", "ID", i);
if (device_id_accel != 0) {
bool external_accel = (calibration::GetCalibrationParam("ACC", "ROT", i) >= 0);
calibration::Accelerometer accel_cal(device_id_accel, external_accel);
}
if (device_id_gyro != 0) {
bool external_gyro = (calibration::GetCalibrationParam("GYRO", "ROT", i) >= 0);
calibration::Gyroscope gyro_cal(device_id_gyro, external_gyro);
}
if (device_id_mag != 0) {
bool external_mag = (calibration::GetCalibrationParam("MAG", "ROT", i) >= 0);
calibration::Magnetometer mag_cal(device_id_mag, external_mag);
}
}
// ensure calibration slots are active for the number of sensors currently available
// this to done to eliminate differences in the active set of parameters before and after sensor calibration
for (int i = 0; i < MAX_SENSOR_COUNT; i++) {
if (orb_exists(ORB_ID(sensor_accel), i) == PX4_OK) {
bool external = (calibration::GetCalibrationParam("ACC", "ROT", i) >= 0);
calibration::Accelerometer cal{0, external};
cal.set_calibration_index(i);
cal.ParametersUpdate();
}
if (orb_exists(ORB_ID(sensor_gyro), i) == PX4_OK) {
bool external = (calibration::GetCalibrationParam("GYRO", "ROT", i) >= 0);
calibration::Gyroscope cal{0, external};
cal.set_calibration_index(i);
cal.ParametersUpdate();
}
if (orb_exists(ORB_ID(sensor_mag), i) == PX4_OK) {
bool external = (calibration::GetCalibrationParam("MAG", "ROT", i) >= 0);
calibration::Magnetometer cal{0, external};
cal.set_calibration_index(i);
cal.ParametersUpdate();
}
}
return PX4_OK;
}
@@ -611,11 +666,28 @@ void Sensors::Run()
// keep adding sensors as long as we are not armed,
// when not adding sensors poll for param updates
if (!_armed && hrt_elapsed_time(&_last_config_update) > 500_ms) {
_voted_sensors_update.initializeSensors();
InitializeVehicleAirData();
InitializeVehicleIMU();
InitializeVehicleGPSPosition();
InitializeVehicleMagnetometer();
const int n_accel = orb_group_count(ORB_ID(sensor_accel));
const int n_baro = orb_group_count(ORB_ID(sensor_baro));
const int n_gps = orb_group_count(ORB_ID(sensor_gps));
const int n_gyro = orb_group_count(ORB_ID(sensor_gyro));
const int n_mag = orb_group_count(ORB_ID(sensor_mag));
if ((n_accel != _n_accel) || (n_baro != _n_baro) || (n_gps != _n_gps) || (n_gyro != _n_gyro) || (n_mag != _n_mag)) {
_n_accel = n_accel;
_n_baro = n_baro;
_n_gps = n_gps;
_n_gyro = n_gyro;
_n_mag = n_mag;
parameters_update();
_voted_sensors_update.initializeSensors();
InitializeVehicleAirData();
InitializeVehicleIMU();
InitializeVehicleGPSPosition();
InitializeVehicleMagnetometer();
}
_last_config_update = hrt_absolute_time();
} else {