Browse Source

sensors/vehicle_imu: on calibration change don't resume in flight calibration learning immediately

master
Daniel Agar 3 years ago
parent
commit
548b7d5ece
  1. 8
      src/modules/sensors/vehicle_imu/VehicleIMU.cpp
  2. 6
      src/modules/sensors/vehicle_imu/VehicleIMU.hpp

8
src/modules/sensors/vehicle_imu/VehicleIMU.cpp

@ -128,7 +128,7 @@ bool VehicleIMU::ParametersUpdate(bool force) @@ -128,7 +128,7 @@ bool VehicleIMU::ParametersUpdate(bool force)
if (accel_calibration_count != _accel_calibration.calibration_count()) {
// if calibration changed reset any existing learned calibration
_accel_cal_available = false;
_in_flight_calibration_check_timestamp_last = hrt_absolute_time();
_in_flight_calibration_check_timestamp_last = hrt_absolute_time() + INFLIGHT_CALIBRATION_QUIET_PERIOD_US;
for (auto &learned_cal : _accel_learned_calibration) {
learned_cal = {};
@ -138,7 +138,7 @@ bool VehicleIMU::ParametersUpdate(bool force) @@ -138,7 +138,7 @@ bool VehicleIMU::ParametersUpdate(bool force)
if (gyro_calibration_count != _gyro_calibration.calibration_count()) {
// if calibration changed reset any existing learned calibration
_gyro_cal_available = false;
_in_flight_calibration_check_timestamp_last = hrt_absolute_time();
_in_flight_calibration_check_timestamp_last = hrt_absolute_time() + INFLIGHT_CALIBRATION_QUIET_PERIOD_US;
for (auto &learned_cal : _gyro_learned_calibration) {
learned_cal = {};
@ -797,6 +797,8 @@ void VehicleIMU::SensorCalibrationSaveAccel() @@ -797,6 +797,8 @@ void VehicleIMU::SensorCalibrationSaveAccel()
if (_accel_calibration.ParametersSave(_sensor_accel_sub.get_instance())) {
param_notify_changes();
}
_in_flight_calibration_check_timestamp_last = hrt_absolute_time() + INFLIGHT_CALIBRATION_QUIET_PERIOD_US;
}
}
@ -847,6 +849,8 @@ void VehicleIMU::SensorCalibrationSaveGyro() @@ -847,6 +849,8 @@ void VehicleIMU::SensorCalibrationSaveGyro()
if (_gyro_calibration.ParametersSave(_sensor_gyro_sub.get_instance())) {
param_notify_changes();
}
_in_flight_calibration_check_timestamp_last = hrt_absolute_time() + INFLIGHT_CALIBRATION_QUIET_PERIOD_US;
}
}

6
src/modules/sensors/vehicle_imu/VehicleIMU.hpp

@ -116,8 +116,6 @@ private: @@ -116,8 +116,6 @@ private:
hrt_abstime _gyro_timestamp_sample_last{0};
hrt_abstime _gyro_timestamp_last{0};
hrt_abstime _in_flight_calibration_check_timestamp_last{0};
math::WelfordMean<matrix::Vector3f> _raw_accel_mean{};
math::WelfordMean<matrix::Vector3f> _raw_gyro_mean{};
@ -175,6 +173,10 @@ private: @@ -175,6 +173,10 @@ private:
InFlightCalibration _accel_learned_calibration[ORB_MULTI_MAX_INSTANCES] {};
InFlightCalibration _gyro_learned_calibration[ORB_MULTI_MAX_INSTANCES] {};
static constexpr hrt_abstime INFLIGHT_CALIBRATION_QUIET_PERIOD_US{30_s};
hrt_abstime _in_flight_calibration_check_timestamp_last{0};
perf_counter_t _accel_generation_gap_perf{perf_alloc(PC_COUNT, MODULE_NAME": accel data gap")};
perf_counter_t _gyro_generation_gap_perf{perf_alloc(PC_COUNT, MODULE_NAME": gyro data gap")};

Loading…
Cancel
Save