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

sensors: automatically set initial accel/gyro calibration if stable bias available

Browse Source
This commit is contained in:
Daniel Agar 2022-01-07 09:45:31 -05:00
parent e731fcdbc0
commit d9e7315420
3 changed files with 94 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

82
src/modules/sensors/vehicle_imu/VehicleIMU.cpp
View File

@ -259,8 +259,8 @@ void VehicleIMU::Run()
}
}
if (!parameters_updated) {
if (_armed) {
if (_param_sens_imu_autocal.get() && !parameters_updated) {
if (_armed || !_accel_calibration.calibrated() || !_gyro_calibration.calibrated()) {
if (now_us > _in_flight_calibration_check_timestamp_last + 1_s) {
SensorCalibrationUpdate();
_in_flight_calibration_check_timestamp_last = now_us;
@ -792,6 +792,45 @@ void VehicleIMU::SensorCalibrationSaveAccel()
(double)cal_orig(0), (double)cal_orig(1), (double)cal_orig(2),
(double)offset_estimate(0), (double)offset_estimate(1), (double)offset_estimate(2));
// find appropriate calibration slot if not already set
if (_accel_calibration.calibration_index() < 0) {
uint32_t cal_device_ids[calibration::Accelerometer::MAX_SENSOR_COUNT] {};
bool cal_slot_match = false;
for (unsigned cal_index = 0; cal_index < calibration::Accelerometer::MAX_SENSOR_COUNT; cal_index++) {
char str[20] {};
sprintf(str, "CAL_%s%u_ID", "ACC", cal_index);
int32_t device_id_val = 0;
if (param_get(param_find_no_notification(str), &device_id_val) == PX4_OK) {
cal_device_ids[cal_index] = device_id_val;
if (cal_device_ids[cal_index] == _accel_calibration.device_id()) {
cal_slot_match = true;
_accel_calibration.set_calibration_index(cal_index);
break;
}
}
}
if (!cal_slot_match) {
// prefer slot that matches sensor instance
int accel_index = _sensor_accel_sub.get_instance();
if (cal_device_ids[accel_index] == 0) {
_accel_calibration.set_calibration_index(accel_index);
} else {
for (int cal_index = 0; cal_index < calibration::Accelerometer::MAX_SENSOR_COUNT; cal_index++) {
if (cal_device_ids[accel_index] == 0) {
_accel_calibration.set_calibration_index(cal_index);
break;
}
}
}
}
}
if (_accel_calibration.ParametersSave()) {
param_notify_changes();
}
@ -841,6 +880,45 @@ void VehicleIMU::SensorCalibrationSaveGyro()
(double)cal_orig(0), (double)cal_orig(1), (double)cal_orig(2),
(double)offset_estimate(0), (double)offset_estimate(1), (double)offset_estimate(2));
// find appropriate calibration slot if not already set
if (_gyro_calibration.calibration_index() < 0) {
uint32_t cal_device_ids[calibration::Gyroscope::MAX_SENSOR_COUNT] {};
bool cal_slot_match = false;
for (unsigned cal_index = 0; cal_index < calibration::Gyroscope::MAX_SENSOR_COUNT; cal_index++) {
char str[20] {};
sprintf(str, "CAL_%s%u_ID", "GYRO", cal_index);
int32_t device_id_val = 0;
if (param_get(param_find_no_notification(str), &device_id_val) == PX4_OK) {
cal_device_ids[cal_index] = device_id_val;
if (cal_device_ids[cal_index] == _gyro_calibration.device_id()) {
cal_slot_match = true;
_gyro_calibration.set_calibration_index(cal_index);
break;
}
}
}
if (!cal_slot_match) {
// prefer slot that matches sensor instance
int gyro_index = _sensor_gyro_sub.get_instance();
if (cal_device_ids[gyro_index] == 0) {
_gyro_calibration.set_calibration_index(gyro_index);
} else {
for (int cal_index = 0; cal_index < calibration::Gyroscope::MAX_SENSOR_COUNT; cal_index++) {
if (cal_device_ids[gyro_index] == 0) {
_gyro_calibration.set_calibration_index(cal_index);
break;
}
}
}
}
}
if (_gyro_calibration.ParametersSave()) {
param_notify_changes();
}

3
src/modules/sensors/vehicle_imu/VehicleIMU.hpp
View File

@ -180,7 +180,8 @@ private:
DEFINE_PARAMETERS(
(ParamInt<px4::params::IMU_INTEG_RATE>) _param_imu_integ_rate,
(ParamInt<px4::params::IMU_GYRO_RATEMAX>) _param_imu_gyro_ratemax
(ParamInt<px4::params::IMU_GYRO_RATEMAX>) _param_imu_gyro_ratemax,
(ParamBool<px4::params::SENS_IMU_AUTOCAL>) _param_sens_imu_autocal
)
};

12
src/modules/sensors/vehicle_imu/imu_parameters.c
View File

@ -48,3 +48,15 @@
* @group Sensors
*/
PARAM_DEFINE_INT32(IMU_INTEG_RATE, 200);
/**
* IMU auto calibration
*
* Automatically initialize IMU (accel/gyro) calibration from bias estimates if available.
*
* @boolean
*
* @category system
* @group Sensors
*/
PARAM_DEFINE_INT32(SENS_IMU_AUTOCAL, 1);