Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_InertialSensor: MPU9150: apply correction on each new sample 

These changes are for enabling unified accelerometer vibration and clipping
calculation. For that, we need the values "rotated and corrected" before they
are filtered and the calculation must be called as soon as a new sample arrives
as it takes the sample rate into account.

Thus, move code that applies "corrections" to be executed as soon as accel data
arrive and call _publish_accel() passing rotate_and_correct parameter as false.
Also, do the same for gyro so we can keep it consistent.
This commit is contained in:
Gustavo Jose de Sousa 2015-08-28 10:36:14 -03:00 committed by Andrew Tridgell
parent 487135afa2
commit b844b220c3
1 changed files with 13 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

20
libraries/AP_InertialSensor/AP_InertialSensor_MPU9150.cpp
View File

@ -1059,6 +1059,8 @@ void AP_InertialSensor_MPU9150::_accumulate(void)
// read the samples // read the samples
for (uint16_t i=0; i< fifo_count; i++) { for (uint16_t i=0; i< fifo_count; i++) {
Vector3f accel, gyro;
// read the data // read the data
// TODO check whether it's possible to read all the packages in a single call // TODO check whether it's possible to read all the packages in a single call
hal.i2c->readRegisters(st.hw->addr, st.reg->fifo_r_w, packet_size, data); hal.i2c->readRegisters(st.hw->addr, st.reg->fifo_r_w, packet_size, data);
@ -1086,9 +1088,16 @@ void AP_InertialSensor_MPU9150::_accumulate(void)
// TODO Revisit why AP_InertialSensor_L3G4200D uses a minus sign in the y and z component. Maybe this // TODO Revisit why AP_InertialSensor_L3G4200D uses a minus sign in the y and z component. Maybe this
// is because the sensor is placed in the bottom side of the board? // is because the sensor is placed in the bottom side of the board?
_accel_filtered = _accel_filter.apply(Vector3f(accel_x, accel_y, accel_z));
_gyro_filtered = _gyro_filter.apply(Vector3f(gyro_x, gyro_y, gyro_z)); accel = Vector3f(accel_x, accel_y, accel_z);
accel *= MPU9150_ACCEL_SCALE_2G;
_rotate_and_correct_accel(_accel_instance, accel);
_accel_filtered = _accel_filter.apply(accel);
gyro = Vector3f(gyro_x, gyro_y, gyro_z);
gyro *= MPU9150_GYRO_SCALE_2000;
_rotate_and_correct_gyro(_gyro_instance, gyro);
_gyro_filtered = _gyro_filter.apply(gyro);
_have_sample_available = true; _have_sample_available = true;
} }
@ -1107,11 +1116,8 @@ bool AP_InertialSensor_MPU9150::update(void)
_have_sample_available = false; _have_sample_available = false;
hal.scheduler->resume_timer_procs(); hal.scheduler->resume_timer_procs();
accel *= MPU9150_ACCEL_SCALE_2G; _publish_accel(_accel_instance, accel, false);
_publish_accel(_accel_instance, accel); _publish_gyro(_gyro_instance, gyro, false);
gyro *= MPU9150_GYRO_SCALE_2000;
_publish_gyro(_gyro_instance, gyro);
if (_last_accel_filter_hz != _accel_filter_cutoff()) { if (_last_accel_filter_hz != _accel_filter_cutoff()) {
_set_accel_filter_frequency(_accel_filter_cutoff()); _set_accel_filter_frequency(_accel_filter_cutoff());