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

AP_InertialSensor: call gyro_sample and accel_sample AP_Module hooks

Browse Source
This commit is contained in:
Andrew Tridgell 2016-07-12 21:50:46 +10:00
parent 6b6a02e67e
commit 33ce1213a2
1 changed files with 11 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp
View File

@ -4,6 +4,7 @@
#include "AP_InertialSensor.h"
#include "AP_InertialSensor_Backend.h"
#include <DataFlash/DataFlash.h>
#include <AP_Module/AP_Module.h>
const extern AP_HAL::HAL& hal;
@ -70,6 +71,11 @@ void AP_InertialSensor_Backend::_notify_new_gyro_raw_sample(uint8_t instance,
dt = 1.0f / _imu._gyro_raw_sample_rates[instance];
#if AP_MODULE_SUPPORTED
// call gyro_sample hook if any
AP_Module::call_hook_gyro_sample(instance, dt, gyro);
#endif
// compute delta angle
Vector3f delta_angle = (gyro + _imu._last_raw_gyro[instance]) * 0.5f * dt;
@ -165,6 +171,11 @@ void AP_InertialSensor_Backend::_notify_new_accel_raw_sample(uint8_t instance,
dt = 1.0f / _imu._accel_raw_sample_rates[instance];
#if AP_MODULE_SUPPORTED
// call gyro_sample hook if any
AP_Module::call_hook_accel_sample(instance, dt, accel);
#endif
_imu.calc_vibration_and_clipping(instance, accel, dt);
// delta velocity