aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_InertialSensor: fixed locking issue in delta accumulation

Browse Source 
this fixes a bug in calculating delta velocity and delta acceleration
between threads.
This commit is contained in:
Andrew Tridgell 2021-08-30 16:47:54 +10:00
parent 657fcb7f59
commit 4fdc1ebc30
2 changed files with 5 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
libraries/AP_InertialSensor/AP_InertialSensor.cpp
View File

@ -1495,14 +1495,6 @@ void AP_InertialSensor::update(void)
_backends[i]->update();
}
// clear accumulators
for (uint8_t i = 0; i < INS_MAX_INSTANCES; i++) {
_delta_velocity_acc[i].zero();
_delta_velocity_acc_dt[i] = 0;
_delta_angle_acc[i].zero();
_delta_angle_acc_dt[i] = 0;
}
if (!_startup_error_counts_set) {
for (uint8_t i=0; i<INS_MAX_INSTANCES; i++) {
_accel_startup_error_count[i] = _accel_error_count[i];

5
libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp
View File

@ -175,6 +175,9 @@ void AP_InertialSensor_Backend::_publish_gyro(uint8_t instance, const Vector3f &
_imu._delta_angle[instance] = _imu._delta_angle_acc[instance];
_imu._delta_angle_dt[instance] = _imu._delta_angle_acc_dt[instance];
_imu._delta_angle_valid[instance] = true;
_imu._delta_angle_acc[instance].zero();
_imu._delta_angle_acc_dt[instance] = 0;
}
void AP_InertialSensor_Backend::_notify_new_gyro_raw_sample(uint8_t instance,
@ -334,6 +337,8 @@ void AP_InertialSensor_Backend::_publish_accel(uint8_t instance, const Vector3f
_imu._delta_velocity_dt[instance] = _imu._delta_velocity_acc_dt[instance];
_imu._delta_velocity_valid[instance] = true;
_imu._delta_velocity_acc[instance].zero();
_imu._delta_velocity_acc_dt[instance] = 0;
if (_imu._accel_calibrator != nullptr && _imu._accel_calibrator[instance].get_status() == ACCEL_CAL_COLLECTING_SAMPLE) {
Vector3f cal_sample = _imu._delta_velocity[instance];