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

MPU6K driver: Add delta angles / velocities including coning correction

Browse Source
This commit is contained in:
Lorenz Meier
2015-08-19 10:16:35 +02:00
parent 62f37204be
commit c34a505576
1 changed files with 34 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/mpu6000/mpu6000.cpp
+34 -4
View File
@@ -69,6 +69,7 @@
#include <drivers/device/spi.h>
#include <drivers/device/ringbuffer.h>
#include <drivers/device/integrator.h>
#include <drivers/drv_accel.h>
#include <drivers/drv_gyro.h>
#include <mathlib/math/filter/LowPassFilter2p.hpp>
@@ -167,10 +168,13 @@
#define MPU6000_ACCEL_DEFAULT_RANGE_G 8
#define MPU6000_ACCEL_DEFAULT_RATE 1000
#define MPU6000_ACCEL_MAX_OUTPUT_RATE 280
#define MPU6000_ACCEL_DEFAULT_DRIVER_FILTER_FREQ 30
#define MPU6000_GYRO_DEFAULT_RANGE_G 8
#define MPU6000_GYRO_DEFAULT_RATE 1000
/* rates need to be the same between accel and gyro */
#define MPU6000_GYRO_MAX_OUTPUT_RATE MPU6000_ACCEL_MAX_OUTPUT_RATE
#define MPU6000_GYRO_DEFAULT_DRIVER_FILTER_FREQ 30
#define MPU6000_DEFAULT_ONCHIP_FILTER_FREQ 42
@@ -281,6 +285,9 @@ private:
math::LowPassFilter2p _gyro_filter_y;
math::LowPassFilter2p _gyro_filter_z;
Integrator _accel_int;
Integrator _gyro_int;
enum Rotation _rotation;
// this is used to support runtime checking of key
@@ -535,6 +542,8 @@ MPU6000::MPU6000(int bus, const char *path_accel, const char *path_gyro, spi_dev
_gyro_filter_x(MPU6000_GYRO_DEFAULT_RATE, MPU6000_GYRO_DEFAULT_DRIVER_FILTER_FREQ),
_gyro_filter_y(MPU6000_GYRO_DEFAULT_RATE, MPU6000_GYRO_DEFAULT_DRIVER_FILTER_FREQ),
_gyro_filter_z(MPU6000_GYRO_DEFAULT_RATE, MPU6000_GYRO_DEFAULT_DRIVER_FILTER_FREQ),
_accel_int(1000000 / MPU6000_ACCEL_MAX_OUTPUT_RATE),
_gyro_int(1000000 / MPU6000_GYRO_MAX_OUTPUT_RATE, true),
_rotation(rotation),
_checked_next(0),
_in_factory_test(false),
@@ -1759,6 +1768,14 @@ MPU6000::measure()
arb.y = _accel_filter_y.apply(y_in_new);
arb.z = _accel_filter_z.apply(z_in_new);
math::Vector<3> aval(x_in_new, y_in_new, z_in_new);
math::Vector<3> aval_integrated;
bool accel_notify = _accel_int.put(arb.timestamp, aval, aval_integrated, arb.integral_dt);
arb.x_integral = aval_integrated(0);
arb.y_integral = aval_integrated(1);
arb.z_integral = aval_integrated(2);
arb.scaling = _accel_range_scale;
arb.range_m_s2 = _accel_range_m_s2;
@@ -1786,6 +1803,14 @@ MPU6000::measure()
grb.y = _gyro_filter_y.apply(y_gyro_in_new);
grb.z = _gyro_filter_z.apply(z_gyro_in_new);
math::Vector<3> gval(x_gyro_in_new, y_gyro_in_new, z_gyro_in_new);
math::Vector<3> gval_integrated;
bool gyro_notify = _gyro_int.put(arb.timestamp, gval, gval_integrated, grb.integral_dt);
grb.x_integral = gval_integrated(0);
grb.y_integral = gval_integrated(1);
grb.z_integral = gval_integrated(2);
grb.scaling = _gyro_range_scale;
grb.range_rad_s = _gyro_range_rad_s;
@@ -1796,10 +1821,15 @@ MPU6000::measure()
_gyro_reports->force(&grb);
/* notify anyone waiting for data */
poll_notify(POLLIN);
_gyro->parent_poll_notify();
if (accel_notify) {
poll_notify(POLLIN);
}
if (!(_pub_blocked)) {
if (gyro_notify) {
_gyro->parent_poll_notify();
}
if (accel_notify && !(_pub_blocked)) {
/* log the time of this report */
perf_begin(_controller_latency_perf);
perf_begin(_system_latency_perf);
@@ -1807,7 +1837,7 @@ MPU6000::measure()
orb_publish(ORB_ID(sensor_accel), _accel_topic, &arb);
}
if (!(_pub_blocked)) {
if (gyro_notify && !(_pub_blocked)) {
/* publish it */
orb_publish(ORB_ID(sensor_gyro), _gyro->_gyro_topic, &grb);
}