From c34a5055764d95976cef01f15645e3d053649db2 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 21 Jun 2015 18:50:53 +0200 Subject: [PATCH] MPU6K driver: Add delta angles / velocities including coning correction --- src/drivers/mpu6000/mpu6000.cpp | 38 +++++++++++++++++++++++++++++---- 1 file changed, 34 insertions(+), 4 deletions(-) diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp index 7c7bc43d3e..194fdf1669 100644 --- a/src/drivers/mpu6000/mpu6000.cpp +++ b/src/drivers/mpu6000/mpu6000.cpp @@ -69,6 +69,7 @@ #include #include +#include #include #include #include @@ -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); }