fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-26 06:13:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

L3GD20 driver: Add delta angle support

Browse Source
This commit is contained in:
Lorenz Meier
2015-08-19 10:18:53 +02:00
parent fa62841ff7
commit 808d8520b5
1 changed files with 27 additions and 12 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/l3gd20/l3gd20.cpp
+27 -12
View File
@@ -66,6 +66,7 @@
#include <drivers/device/spi.h> #include <drivers/device/spi.h>
#include <drivers/drv_gyro.h> #include <drivers/drv_gyro.h>
#include <drivers/device/ringbuffer.h> #include <drivers/device/ringbuffer.h>
#include <drivers/device/integrator.h>
#include <board_config.h> #include <board_config.h>
#include <mathlib/math/filter/LowPassFilter2p.hpp> #include <mathlib/math/filter/LowPassFilter2p.hpp>
@@ -175,6 +176,7 @@ static const int ERROR = -1;
#define L3GD20_DEFAULT_RATE 760 #define L3GD20_DEFAULT_RATE 760
#define L3G4200D_DEFAULT_RATE 800 #define L3G4200D_DEFAULT_RATE 800
#define L3GD20_MAX_OUTPUT_RATE 280
#define L3GD20_DEFAULT_RANGE_DPS 2000 #define L3GD20_DEFAULT_RANGE_DPS 2000
#define L3GD20_DEFAULT_FILTER_FREQ 30 #define L3GD20_DEFAULT_FILTER_FREQ 30
#define L3GD20_TEMP_OFFSET_CELSIUS 40 #define L3GD20_TEMP_OFFSET_CELSIUS 40
@@ -256,6 +258,8 @@ private:
math::LowPassFilter2p _gyro_filter_y; math::LowPassFilter2p _gyro_filter_y;
math::LowPassFilter2p _gyro_filter_z; math::LowPassFilter2p _gyro_filter_z;
Integrator _gyro_int;
/* true if an L3G4200D is detected */ /* true if an L3G4200D is detected */
bool _is_l3g4200d; bool _is_l3g4200d;
@@ -427,6 +431,7 @@ L3GD20::L3GD20(int bus, const char* path, spi_dev_e device, enum Rotation rotati
_gyro_filter_x(L3GD20_DEFAULT_RATE, L3GD20_DEFAULT_FILTER_FREQ), _gyro_filter_x(L3GD20_DEFAULT_RATE, L3GD20_DEFAULT_FILTER_FREQ),
_gyro_filter_y(L3GD20_DEFAULT_RATE, L3GD20_DEFAULT_FILTER_FREQ), _gyro_filter_y(L3GD20_DEFAULT_RATE, L3GD20_DEFAULT_FILTER_FREQ),
_gyro_filter_z(L3GD20_DEFAULT_RATE, L3GD20_DEFAULT_FILTER_FREQ), _gyro_filter_z(L3GD20_DEFAULT_RATE, L3GD20_DEFAULT_FILTER_FREQ),
_gyro_int(1000000 / L3GD20_MAX_OUTPUT_RATE, true),
_is_l3g4200d(false), _is_l3g4200d(false),
_rotation(rotation), _rotation(rotation),
_checked_next(0) _checked_next(0)
@@ -1029,13 +1034,21 @@ L3GD20::measure()
// apply user specified rotation // apply user specified rotation
rotate_3f(_rotation, xraw_f, yraw_f, zraw_f); rotate_3f(_rotation, xraw_f, yraw_f, zraw_f);
report.x = ((xraw_f * _gyro_range_scale) - _gyro_scale.x_offset) * _gyro_scale.x_scale; float xin = ((xraw_f * _gyro_range_scale) - _gyro_scale.x_offset) * _gyro_scale.x_scale;
report.y = ((yraw_f * _gyro_range_scale) - _gyro_scale.y_offset) * _gyro_scale.y_scale; float yin = ((yraw_f * _gyro_range_scale) - _gyro_scale.y_offset) * _gyro_scale.y_scale;
report.z = ((zraw_f * _gyro_range_scale) - _gyro_scale.z_offset) * _gyro_scale.z_scale; float zin = ((zraw_f * _gyro_range_scale) - _gyro_scale.z_offset) * _gyro_scale.z_scale;
report.x = _gyro_filter_x.apply(report.x); report.x = _gyro_filter_x.apply(xin);
report.y = _gyro_filter_y.apply(report.y); report.y = _gyro_filter_y.apply(yin);
report.z = _gyro_filter_z.apply(report.z); report.z = _gyro_filter_z.apply(zin);
math::Vector<3> gval(xin, yin, zin);
math::Vector<3> gval_integrated;
bool gyro_notify = _gyro_int.put(report.timestamp, gval, gval_integrated, report.integral_dt);
report.x_integral = gval_integrated(0);
report.y_integral = gval_integrated(1);
report.z_integral = gval_integrated(2);
report.temperature = L3GD20_TEMP_OFFSET_CELSIUS - raw_report.temp; report.temperature = L3GD20_TEMP_OFFSET_CELSIUS - raw_report.temp;
@@ -1044,13 +1057,15 @@ L3GD20::measure()
_reports->force(&report); _reports->force(&report);
/* notify anyone waiting for data */ if (gyro_notify) {
poll_notify(POLLIN); /* notify anyone waiting for data */
poll_notify(POLLIN);
/* publish for subscribers */ /* publish for subscribers */
if (!(_pub_blocked)) { if (!(_pub_blocked)) {
/* publish it */ /* publish it */
orb_publish(ORB_ID(sensor_gyro), _gyro_topic, &report); orb_publish(ORB_ID(sensor_gyro), _gyro_topic, &report);
}
} }
_read++; _read++;