forked from Archive/PX4-Autopilot
Merge branch 'mpu6k_queue' of github.com:PX4/Firmware
This commit is contained in:
commit
ab90030a0b
|
@ -94,6 +94,7 @@ MODULES += modules/sdlog2
|
|||
MODULES += modules/systemlib
|
||||
MODULES += modules/systemlib/mixer
|
||||
MODULES += modules/mathlib
|
||||
MODULES += modules/mathlib/math/filter
|
||||
MODULES += modules/controllib
|
||||
MODULES += modules/uORB
|
||||
|
||||
|
|
|
@ -67,8 +67,10 @@
|
|||
#include <drivers/drv_hrt.h>
|
||||
|
||||
#include <drivers/device/spi.h>
|
||||
#include <drivers/device/ringbuffer.h>
|
||||
#include <drivers/drv_accel.h>
|
||||
#include <drivers/drv_gyro.h>
|
||||
#include <mathlib/math/filter/LowPassFilter2p.hpp>
|
||||
|
||||
#define DIR_READ 0x80
|
||||
#define DIR_WRITE 0x00
|
||||
|
@ -181,13 +183,17 @@ private:
|
|||
struct hrt_call _call;
|
||||
unsigned _call_interval;
|
||||
|
||||
struct accel_report _accel_report;
|
||||
typedef RingBuffer<accel_report> AccelReportBuffer;
|
||||
AccelReportBuffer *_accel_reports;
|
||||
|
||||
struct accel_scale _accel_scale;
|
||||
float _accel_range_scale;
|
||||
float _accel_range_m_s2;
|
||||
orb_advert_t _accel_topic;
|
||||
|
||||
struct gyro_report _gyro_report;
|
||||
typedef RingBuffer<gyro_report> GyroReportBuffer;
|
||||
GyroReportBuffer *_gyro_reports;
|
||||
|
||||
struct gyro_scale _gyro_scale;
|
||||
float _gyro_range_scale;
|
||||
float _gyro_range_rad_s;
|
||||
|
@ -197,6 +203,13 @@ private:
|
|||
unsigned _sample_rate;
|
||||
perf_counter_t _sample_perf;
|
||||
|
||||
math::LowPassFilter2p _accel_filter_x;
|
||||
math::LowPassFilter2p _accel_filter_y;
|
||||
math::LowPassFilter2p _accel_filter_z;
|
||||
math::LowPassFilter2p _gyro_filter_x;
|
||||
math::LowPassFilter2p _gyro_filter_y;
|
||||
math::LowPassFilter2p _gyro_filter_z;
|
||||
|
||||
/**
|
||||
* Start automatic measurement.
|
||||
*/
|
||||
|
@ -207,6 +220,13 @@ private:
|
|||
*/
|
||||
void stop();
|
||||
|
||||
/**
|
||||
* Reset chip.
|
||||
*
|
||||
* Resets the chip and measurements ranges, but not scale and offset.
|
||||
*/
|
||||
void reset();
|
||||
|
||||
/**
|
||||
* Static trampoline from the hrt_call context; because we don't have a
|
||||
* generic hrt wrapper yet.
|
||||
|
@ -219,7 +239,7 @@ private:
|
|||
static void measure_trampoline(void *arg);
|
||||
|
||||
/**
|
||||
* Fetch measurements from the sensor and update the report ring.
|
||||
* Fetch measurements from the sensor and update the report buffers.
|
||||
*/
|
||||
void measure();
|
||||
|
||||
|
@ -311,15 +331,23 @@ MPU6000::MPU6000(int bus, spi_dev_e device) :
|
|||
_gyro(new MPU6000_gyro(this)),
|
||||
_product(0),
|
||||
_call_interval(0),
|
||||
_accel_reports(nullptr),
|
||||
_accel_range_scale(0.0f),
|
||||
_accel_range_m_s2(0.0f),
|
||||
_accel_topic(-1),
|
||||
_gyro_reports(nullptr),
|
||||
_gyro_range_scale(0.0f),
|
||||
_gyro_range_rad_s(0.0f),
|
||||
_gyro_topic(-1),
|
||||
_reads(0),
|
||||
_sample_rate(500),
|
||||
_sample_perf(perf_alloc(PC_ELAPSED, "mpu6000_read"))
|
||||
_sample_rate(1000),
|
||||
_sample_perf(perf_alloc(PC_ELAPSED, "mpu6000_read")),
|
||||
_accel_filter_x(1000, 30),
|
||||
_accel_filter_y(1000, 30),
|
||||
_accel_filter_z(1000, 30),
|
||||
_gyro_filter_x(1000, 30),
|
||||
_gyro_filter_y(1000, 30),
|
||||
_gyro_filter_z(1000, 30)
|
||||
{
|
||||
// disable debug() calls
|
||||
_debug_enabled = false;
|
||||
|
@ -340,8 +368,6 @@ MPU6000::MPU6000(int bus, spi_dev_e device) :
|
|||
_gyro_scale.z_offset = 0;
|
||||
_gyro_scale.z_scale = 1.0f;
|
||||
|
||||
memset(&_accel_report, 0, sizeof(_accel_report));
|
||||
memset(&_gyro_report, 0, sizeof(_gyro_report));
|
||||
memset(&_call, 0, sizeof(_call));
|
||||
}
|
||||
|
||||
|
@ -353,6 +379,12 @@ MPU6000::~MPU6000()
|
|||
/* delete the gyro subdriver */
|
||||
delete _gyro;
|
||||
|
||||
/* free any existing reports */
|
||||
if (_accel_reports != nullptr)
|
||||
delete _accel_reports;
|
||||
if (_gyro_reports != nullptr)
|
||||
delete _gyro_reports;
|
||||
|
||||
/* delete the perf counter */
|
||||
perf_free(_sample_perf);
|
||||
}
|
||||
|
@ -361,6 +393,7 @@ int
|
|||
MPU6000::init()
|
||||
{
|
||||
int ret;
|
||||
int gyro_ret;
|
||||
|
||||
/* do SPI init (and probe) first */
|
||||
ret = SPI::init();
|
||||
|
@ -371,6 +404,59 @@ MPU6000::init()
|
|||
return ret;
|
||||
}
|
||||
|
||||
/* allocate basic report buffers */
|
||||
_accel_reports = new AccelReportBuffer(2);
|
||||
if (_accel_reports == nullptr)
|
||||
goto out;
|
||||
|
||||
_gyro_reports = new GyroReportBuffer(2);
|
||||
if (_gyro_reports == nullptr)
|
||||
goto out;
|
||||
|
||||
reset();
|
||||
|
||||
/* Initialize offsets and scales */
|
||||
_accel_scale.x_offset = 0;
|
||||
_accel_scale.x_scale = 1.0f;
|
||||
_accel_scale.y_offset = 0;
|
||||
_accel_scale.y_scale = 1.0f;
|
||||
_accel_scale.z_offset = 0;
|
||||
_accel_scale.z_scale = 1.0f;
|
||||
|
||||
_gyro_scale.x_offset = 0;
|
||||
_gyro_scale.x_scale = 1.0f;
|
||||
_gyro_scale.y_offset = 0;
|
||||
_gyro_scale.y_scale = 1.0f;
|
||||
_gyro_scale.z_offset = 0;
|
||||
_gyro_scale.z_scale = 1.0f;
|
||||
|
||||
/* do CDev init for the gyro device node, keep it optional */
|
||||
gyro_ret = _gyro->init();
|
||||
|
||||
/* fetch an initial set of measurements for advertisement */
|
||||
measure();
|
||||
|
||||
if (gyro_ret != OK) {
|
||||
_gyro_topic = -1;
|
||||
} else {
|
||||
gyro_report gr;
|
||||
_gyro_reports->get(gr);
|
||||
|
||||
_gyro_topic = orb_advertise(ORB_ID(sensor_gyro), &gr);
|
||||
}
|
||||
|
||||
/* advertise accel topic */
|
||||
accel_report ar;
|
||||
_accel_reports->get(ar);
|
||||
_accel_topic = orb_advertise(ORB_ID(sensor_accel), &ar);
|
||||
|
||||
out:
|
||||
return ret;
|
||||
}
|
||||
|
||||
void MPU6000::reset()
|
||||
{
|
||||
|
||||
// Chip reset
|
||||
write_reg(MPUREG_PWR_MGMT_1, BIT_H_RESET);
|
||||
up_udelay(10000);
|
||||
|
@ -391,7 +477,7 @@ MPU6000::init()
|
|||
// FS & DLPF FS=2000 deg/s, DLPF = 20Hz (low pass filter)
|
||||
// was 90 Hz, but this ruins quality and does not improve the
|
||||
// system response
|
||||
_set_dlpf_filter(20);
|
||||
_set_dlpf_filter(42);
|
||||
usleep(1000);
|
||||
// Gyro scale 2000 deg/s ()
|
||||
write_reg(MPUREG_GYRO_CONFIG, BITS_FS_2000DPS);
|
||||
|
@ -402,12 +488,6 @@ MPU6000::init()
|
|||
// 2000 deg/s = (2000/180)*PI = 34.906585 rad/s
|
||||
// scaling factor:
|
||||
// 1/(2^15)*(2000/180)*PI
|
||||
_gyro_scale.x_offset = 0;
|
||||
_gyro_scale.x_scale = 1.0f;
|
||||
_gyro_scale.y_offset = 0;
|
||||
_gyro_scale.y_scale = 1.0f;
|
||||
_gyro_scale.z_offset = 0;
|
||||
_gyro_scale.z_scale = 1.0f;
|
||||
_gyro_range_scale = (0.0174532 / 16.4);//1.0f / (32768.0f * (2000.0f / 180.0f) * M_PI_F);
|
||||
_gyro_range_rad_s = (2000.0f / 180.0f) * M_PI_F;
|
||||
|
||||
|
@ -440,12 +520,6 @@ MPU6000::init()
|
|||
|
||||
// Correct accel scale factors of 4096 LSB/g
|
||||
// scale to m/s^2 ( 1g = 9.81 m/s^2)
|
||||
_accel_scale.x_offset = 0;
|
||||
_accel_scale.x_scale = 1.0f;
|
||||
_accel_scale.y_offset = 0;
|
||||
_accel_scale.y_scale = 1.0f;
|
||||
_accel_scale.z_offset = 0;
|
||||
_accel_scale.z_scale = 1.0f;
|
||||
_accel_range_scale = (9.81f / 4096.0f);
|
||||
_accel_range_m_s2 = 8.0f * 9.81f;
|
||||
|
||||
|
@ -461,22 +535,6 @@ MPU6000::init()
|
|||
// write_reg(MPUREG_PWR_MGMT_1,MPU_CLK_SEL_PLLGYROZ);
|
||||
usleep(1000);
|
||||
|
||||
/* do CDev init for the gyro device node, keep it optional */
|
||||
int gyro_ret = _gyro->init();
|
||||
|
||||
/* ensure we got real values to share */
|
||||
measure();
|
||||
|
||||
if (gyro_ret != OK) {
|
||||
_gyro_topic = -1;
|
||||
} else {
|
||||
_gyro_topic = orb_advertise(ORB_ID(sensor_gyro), &_gyro_report);
|
||||
}
|
||||
|
||||
/* advertise sensor topics */
|
||||
_accel_topic = orb_advertise(ORB_ID(sensor_accel), &_accel_report);
|
||||
|
||||
return ret;
|
||||
}
|
||||
|
||||
int
|
||||
|
@ -555,21 +613,33 @@ MPU6000::_set_dlpf_filter(uint16_t frequency_hz)
|
|||
ssize_t
|
||||
MPU6000::read(struct file *filp, char *buffer, size_t buflen)
|
||||
{
|
||||
int ret = 0;
|
||||
unsigned count = buflen / sizeof(accel_report);
|
||||
|
||||
/* buffer must be large enough */
|
||||
if (buflen < sizeof(_accel_report))
|
||||
if (count < 1)
|
||||
return -ENOSPC;
|
||||
|
||||
/* if automatic measurement is not enabled */
|
||||
if (_call_interval == 0)
|
||||
/* if automatic measurement is not enabled, get a fresh measurement into the buffer */
|
||||
if (_call_interval == 0) {
|
||||
_accel_reports->flush();
|
||||
measure();
|
||||
}
|
||||
|
||||
/* copy out the latest reports */
|
||||
memcpy(buffer, &_accel_report, sizeof(_accel_report));
|
||||
ret = sizeof(_accel_report);
|
||||
/* if no data, error (we could block here) */
|
||||
if (_accel_reports->empty())
|
||||
return -EAGAIN;
|
||||
|
||||
return ret;
|
||||
/* copy reports out of our buffer to the caller */
|
||||
accel_report *arp = reinterpret_cast<accel_report *>(buffer);
|
||||
int transferred = 0;
|
||||
while (count--) {
|
||||
if (!_accel_reports->get(*arp++))
|
||||
break;
|
||||
transferred++;
|
||||
}
|
||||
|
||||
/* return the number of bytes transferred */
|
||||
return (transferred * sizeof(accel_report));
|
||||
}
|
||||
|
||||
int
|
||||
|
@ -586,21 +656,33 @@ MPU6000::self_test()
|
|||
ssize_t
|
||||
MPU6000::gyro_read(struct file *filp, char *buffer, size_t buflen)
|
||||
{
|
||||
int ret = 0;
|
||||
unsigned count = buflen / sizeof(gyro_report);
|
||||
|
||||
/* buffer must be large enough */
|
||||
if (buflen < sizeof(_gyro_report))
|
||||
if (count < 1)
|
||||
return -ENOSPC;
|
||||
|
||||
/* if automatic measurement is not enabled */
|
||||
if (_call_interval == 0)
|
||||
/* if automatic measurement is not enabled, get a fresh measurement into the buffer */
|
||||
if (_call_interval == 0) {
|
||||
_gyro_reports->flush();
|
||||
measure();
|
||||
}
|
||||
|
||||
/* copy out the latest report */
|
||||
memcpy(buffer, &_gyro_report, sizeof(_gyro_report));
|
||||
ret = sizeof(_gyro_report);
|
||||
/* if no data, error (we could block here) */
|
||||
if (_gyro_reports->empty())
|
||||
return -EAGAIN;
|
||||
|
||||
return ret;
|
||||
/* copy reports out of our buffer to the caller */
|
||||
gyro_report *arp = reinterpret_cast<gyro_report *>(buffer);
|
||||
int transferred = 0;
|
||||
while (count--) {
|
||||
if (!_gyro_reports->get(*arp++))
|
||||
break;
|
||||
transferred++;
|
||||
}
|
||||
|
||||
/* return the number of bytes transferred */
|
||||
return (transferred * sizeof(gyro_report));
|
||||
}
|
||||
|
||||
int
|
||||
|
@ -608,6 +690,10 @@ MPU6000::ioctl(struct file *filp, int cmd, unsigned long arg)
|
|||
{
|
||||
switch (cmd) {
|
||||
|
||||
case SENSORIOCRESET:
|
||||
reset();
|
||||
return OK;
|
||||
|
||||
case SENSORIOCSPOLLRATE: {
|
||||
switch (arg) {
|
||||
|
||||
|
@ -627,8 +713,8 @@ MPU6000::ioctl(struct file *filp, int cmd, unsigned long arg)
|
|||
/* set default/max polling rate */
|
||||
case SENSOR_POLLRATE_MAX:
|
||||
case SENSOR_POLLRATE_DEFAULT:
|
||||
/* XXX 500Hz is just a wild guess */
|
||||
return ioctl(filp, SENSORIOCSPOLLRATE, 500);
|
||||
/* set to same as sample rate per default */
|
||||
return ioctl(filp, SENSORIOCSPOLLRATE, _sample_rate);
|
||||
|
||||
/* adjust to a legal polling interval in Hz */
|
||||
default: {
|
||||
|
@ -642,6 +728,19 @@ MPU6000::ioctl(struct file *filp, int cmd, unsigned long arg)
|
|||
if (ticks < 1000)
|
||||
return -EINVAL;
|
||||
|
||||
// adjust filters
|
||||
float cutoff_freq_hz = _accel_filter_x.get_cutoff_freq();
|
||||
float sample_rate = 1.0e6f/ticks;
|
||||
_accel_filter_x.set_cutoff_frequency(sample_rate, cutoff_freq_hz);
|
||||
_accel_filter_y.set_cutoff_frequency(sample_rate, cutoff_freq_hz);
|
||||
_accel_filter_z.set_cutoff_frequency(sample_rate, cutoff_freq_hz);
|
||||
|
||||
|
||||
float cutoff_freq_hz_gyro = _gyro_filter_x.get_cutoff_freq();
|
||||
_gyro_filter_x.set_cutoff_frequency(sample_rate, cutoff_freq_hz_gyro);
|
||||
_gyro_filter_y.set_cutoff_frequency(sample_rate, cutoff_freq_hz_gyro);
|
||||
_gyro_filter_z.set_cutoff_frequency(sample_rate, cutoff_freq_hz_gyro);
|
||||
|
||||
/* update interval for next measurement */
|
||||
/* XXX this is a bit shady, but no other way to adjust... */
|
||||
_call.period = _call_interval = ticks;
|
||||
|
@ -661,25 +760,51 @@ MPU6000::ioctl(struct file *filp, int cmd, unsigned long arg)
|
|||
|
||||
return 1000000 / _call_interval;
|
||||
|
||||
case SENSORIOCSQUEUEDEPTH:
|
||||
/* XXX not implemented */
|
||||
return -EINVAL;
|
||||
case SENSORIOCSQUEUEDEPTH: {
|
||||
/* lower bound is mandatory, upper bound is a sanity check */
|
||||
if ((arg < 1) || (arg > 100))
|
||||
return -EINVAL;
|
||||
|
||||
/* allocate new buffer */
|
||||
AccelReportBuffer *buf = new AccelReportBuffer(arg);
|
||||
|
||||
if (nullptr == buf)
|
||||
return -ENOMEM;
|
||||
if (buf->size() == 0) {
|
||||
delete buf;
|
||||
return -ENOMEM;
|
||||
}
|
||||
|
||||
/* reset the measurement state machine with the new buffer, free the old */
|
||||
stop();
|
||||
delete _accel_reports;
|
||||
_accel_reports = buf;
|
||||
start();
|
||||
|
||||
return OK;
|
||||
}
|
||||
|
||||
case SENSORIOCGQUEUEDEPTH:
|
||||
/* XXX not implemented */
|
||||
return -EINVAL;
|
||||
|
||||
return _accel_reports->size();
|
||||
|
||||
case ACCELIOCGSAMPLERATE:
|
||||
return _sample_rate;
|
||||
|
||||
case ACCELIOCSSAMPLERATE:
|
||||
_set_sample_rate(arg);
|
||||
return OK;
|
||||
_set_sample_rate(arg);
|
||||
return OK;
|
||||
|
||||
case ACCELIOCGLOWPASS:
|
||||
return _accel_filter_x.get_cutoff_freq();
|
||||
|
||||
case ACCELIOCSLOWPASS:
|
||||
case ACCELIOCGLOWPASS:
|
||||
_set_dlpf_filter((uint16_t)arg);
|
||||
|
||||
// XXX decide on relationship of both filters
|
||||
// i.e. disable the on-chip filter
|
||||
//_set_dlpf_filter((uint16_t)arg);
|
||||
_accel_filter_x.set_cutoff_frequency(1.0e6f / _call_interval, arg);
|
||||
_accel_filter_y.set_cutoff_frequency(1.0e6f / _call_interval, arg);
|
||||
_accel_filter_z.set_cutoff_frequency(1.0e6f / _call_interval, arg);
|
||||
return OK;
|
||||
|
||||
case ACCELIOCSSCALE:
|
||||
|
@ -726,11 +851,36 @@ MPU6000::gyro_ioctl(struct file *filp, int cmd, unsigned long arg)
|
|||
/* these are shared with the accel side */
|
||||
case SENSORIOCSPOLLRATE:
|
||||
case SENSORIOCGPOLLRATE:
|
||||
case SENSORIOCSQUEUEDEPTH:
|
||||
case SENSORIOCGQUEUEDEPTH:
|
||||
case SENSORIOCRESET:
|
||||
return ioctl(filp, cmd, arg);
|
||||
|
||||
case SENSORIOCSQUEUEDEPTH: {
|
||||
/* lower bound is mandatory, upper bound is a sanity check */
|
||||
if ((arg < 1) || (arg > 100))
|
||||
return -EINVAL;
|
||||
|
||||
/* allocate new buffer */
|
||||
GyroReportBuffer *buf = new GyroReportBuffer(arg);
|
||||
|
||||
if (nullptr == buf)
|
||||
return -ENOMEM;
|
||||
if (buf->size() == 0) {
|
||||
delete buf;
|
||||
return -ENOMEM;
|
||||
}
|
||||
|
||||
/* reset the measurement state machine with the new buffer, free the old */
|
||||
stop();
|
||||
delete _gyro_reports;
|
||||
_gyro_reports = buf;
|
||||
start();
|
||||
|
||||
return OK;
|
||||
}
|
||||
|
||||
case SENSORIOCGQUEUEDEPTH:
|
||||
return _gyro_reports->size();
|
||||
|
||||
case GYROIOCGSAMPLERATE:
|
||||
return _sample_rate;
|
||||
|
||||
|
@ -738,9 +888,14 @@ MPU6000::gyro_ioctl(struct file *filp, int cmd, unsigned long arg)
|
|||
_set_sample_rate(arg);
|
||||
return OK;
|
||||
|
||||
case GYROIOCSLOWPASS:
|
||||
case GYROIOCGLOWPASS:
|
||||
_set_dlpf_filter((uint16_t)arg);
|
||||
return _gyro_filter_x.get_cutoff_freq();
|
||||
case GYROIOCSLOWPASS:
|
||||
_gyro_filter_x.set_cutoff_frequency(1.0e6f / _call_interval, arg);
|
||||
_gyro_filter_y.set_cutoff_frequency(1.0e6f / _call_interval, arg);
|
||||
_gyro_filter_z.set_cutoff_frequency(1.0e6f / _call_interval, arg);
|
||||
// XXX check relation to the internal lowpass
|
||||
//_set_dlpf_filter((uint16_t)arg);
|
||||
return OK;
|
||||
|
||||
case GYROIOCSSCALE:
|
||||
|
@ -865,6 +1020,10 @@ MPU6000::start()
|
|||
/* make sure we are stopped first */
|
||||
stop();
|
||||
|
||||
/* discard any stale data in the buffers */
|
||||
_accel_reports->flush();
|
||||
_gyro_reports->flush();
|
||||
|
||||
/* start polling at the specified rate */
|
||||
hrt_call_every(&_call, 1000, _call_interval, (hrt_callout)&MPU6000::measure_trampoline, this);
|
||||
}
|
||||
|
@ -878,7 +1037,7 @@ MPU6000::stop()
|
|||
void
|
||||
MPU6000::measure_trampoline(void *arg)
|
||||
{
|
||||
MPU6000 *dev = (MPU6000 *)arg;
|
||||
MPU6000 *dev = reinterpret_cast<MPU6000 *>(arg);
|
||||
|
||||
/* make another measurement */
|
||||
dev->measure();
|
||||
|
@ -959,10 +1118,16 @@ MPU6000::measure()
|
|||
report.gyro_x = gyro_xt;
|
||||
report.gyro_y = gyro_yt;
|
||||
|
||||
/*
|
||||
* Report buffers.
|
||||
*/
|
||||
accel_report arb;
|
||||
gyro_report grb;
|
||||
|
||||
/*
|
||||
* Adjust and scale results to m/s^2.
|
||||
*/
|
||||
_gyro_report.timestamp = _accel_report.timestamp = hrt_absolute_time();
|
||||
grb.timestamp = arb.timestamp = hrt_absolute_time();
|
||||
|
||||
|
||||
/*
|
||||
|
@ -983,40 +1148,53 @@ MPU6000::measure()
|
|||
|
||||
/* NOTE: Axes have been swapped to match the board a few lines above. */
|
||||
|
||||
_accel_report.x_raw = report.accel_x;
|
||||
_accel_report.y_raw = report.accel_y;
|
||||
_accel_report.z_raw = report.accel_z;
|
||||
arb.x_raw = report.accel_x;
|
||||
arb.y_raw = report.accel_y;
|
||||
arb.z_raw = report.accel_z;
|
||||
|
||||
_accel_report.x = ((report.accel_x * _accel_range_scale) - _accel_scale.x_offset) * _accel_scale.x_scale;
|
||||
_accel_report.y = ((report.accel_y * _accel_range_scale) - _accel_scale.y_offset) * _accel_scale.y_scale;
|
||||
_accel_report.z = ((report.accel_z * _accel_range_scale) - _accel_scale.z_offset) * _accel_scale.z_scale;
|
||||
_accel_report.scaling = _accel_range_scale;
|
||||
_accel_report.range_m_s2 = _accel_range_m_s2;
|
||||
float x_in_new = ((report.accel_x * _accel_range_scale) - _accel_scale.x_offset) * _accel_scale.x_scale;
|
||||
float y_in_new = ((report.accel_y * _accel_range_scale) - _accel_scale.y_offset) * _accel_scale.y_scale;
|
||||
float z_in_new = ((report.accel_z * _accel_range_scale) - _accel_scale.z_offset) * _accel_scale.z_scale;
|
||||
|
||||
arb.x = _accel_filter_x.apply(x_in_new);
|
||||
arb.y = _accel_filter_y.apply(y_in_new);
|
||||
arb.z = _accel_filter_z.apply(z_in_new);
|
||||
|
||||
_accel_report.temperature_raw = report.temp;
|
||||
_accel_report.temperature = (report.temp) / 361.0f + 35.0f;
|
||||
arb.scaling = _accel_range_scale;
|
||||
arb.range_m_s2 = _accel_range_m_s2;
|
||||
|
||||
_gyro_report.x_raw = report.gyro_x;
|
||||
_gyro_report.y_raw = report.gyro_y;
|
||||
_gyro_report.z_raw = report.gyro_z;
|
||||
arb.temperature_raw = report.temp;
|
||||
arb.temperature = (report.temp) / 361.0f + 35.0f;
|
||||
|
||||
_gyro_report.x = ((report.gyro_x * _gyro_range_scale) - _gyro_scale.x_offset) * _gyro_scale.x_scale;
|
||||
_gyro_report.y = ((report.gyro_y * _gyro_range_scale) - _gyro_scale.y_offset) * _gyro_scale.y_scale;
|
||||
_gyro_report.z = ((report.gyro_z * _gyro_range_scale) - _gyro_scale.z_offset) * _gyro_scale.z_scale;
|
||||
_gyro_report.scaling = _gyro_range_scale;
|
||||
_gyro_report.range_rad_s = _gyro_range_rad_s;
|
||||
grb.x_raw = report.gyro_x;
|
||||
grb.y_raw = report.gyro_y;
|
||||
grb.z_raw = report.gyro_z;
|
||||
|
||||
_gyro_report.temperature_raw = report.temp;
|
||||
_gyro_report.temperature = (report.temp) / 361.0f + 35.0f;
|
||||
float x_gyro_in_new = ((report.gyro_x * _gyro_range_scale) - _gyro_scale.x_offset) * _gyro_scale.x_scale;
|
||||
float y_gyro_in_new = ((report.gyro_y * _gyro_range_scale) - _gyro_scale.y_offset) * _gyro_scale.y_scale;
|
||||
float z_gyro_in_new = ((report.gyro_z * _gyro_range_scale) - _gyro_scale.z_offset) * _gyro_scale.z_scale;
|
||||
|
||||
grb.x = _gyro_filter_x.apply(x_gyro_in_new);
|
||||
grb.y = _gyro_filter_y.apply(y_gyro_in_new);
|
||||
grb.z = _gyro_filter_z.apply(z_gyro_in_new);
|
||||
|
||||
grb.scaling = _gyro_range_scale;
|
||||
grb.range_rad_s = _gyro_range_rad_s;
|
||||
|
||||
grb.temperature_raw = report.temp;
|
||||
grb.temperature = (report.temp) / 361.0f + 35.0f;
|
||||
|
||||
_accel_reports->put(arb);
|
||||
_gyro_reports->put(grb);
|
||||
|
||||
/* notify anyone waiting for data */
|
||||
poll_notify(POLLIN);
|
||||
_gyro->parent_poll_notify();
|
||||
|
||||
/* and publish for subscribers */
|
||||
orb_publish(ORB_ID(sensor_accel), _accel_topic, &_accel_report);
|
||||
orb_publish(ORB_ID(sensor_accel), _accel_topic, &arb);
|
||||
if (_gyro_topic != -1) {
|
||||
orb_publish(ORB_ID(sensor_gyro), _gyro_topic, &_gyro_report);
|
||||
orb_publish(ORB_ID(sensor_gyro), _gyro_topic, &grb);
|
||||
}
|
||||
|
||||
/* stop measuring */
|
||||
|
@ -1119,21 +1297,19 @@ fail:
|
|||
void
|
||||
test()
|
||||
{
|
||||
int fd = -1;
|
||||
int fd_gyro = -1;
|
||||
struct accel_report a_report;
|
||||
struct gyro_report g_report;
|
||||
accel_report a_report;
|
||||
gyro_report g_report;
|
||||
ssize_t sz;
|
||||
|
||||
/* get the driver */
|
||||
fd = open(ACCEL_DEVICE_PATH, O_RDONLY);
|
||||
int fd = open(ACCEL_DEVICE_PATH, O_RDONLY);
|
||||
|
||||
if (fd < 0)
|
||||
err(1, "%s open failed (try 'mpu6000 start' if the driver is not running)",
|
||||
ACCEL_DEVICE_PATH);
|
||||
|
||||
/* get the driver */
|
||||
fd_gyro = open(GYRO_DEVICE_PATH, O_RDONLY);
|
||||
int fd_gyro = open(GYRO_DEVICE_PATH, O_RDONLY);
|
||||
|
||||
if (fd_gyro < 0)
|
||||
err(1, "%s open failed", GYRO_DEVICE_PATH);
|
||||
|
@ -1145,8 +1321,10 @@ test()
|
|||
/* do a simple demand read */
|
||||
sz = read(fd, &a_report, sizeof(a_report));
|
||||
|
||||
if (sz != sizeof(a_report))
|
||||
if (sz != sizeof(a_report)) {
|
||||
warnx("ret: %d, expected: %d", sz, sizeof(a_report));
|
||||
err(1, "immediate acc read failed");
|
||||
}
|
||||
|
||||
warnx("single read");
|
||||
warnx("time: %lld", a_report.timestamp);
|
||||
|
@ -1162,8 +1340,10 @@ test()
|
|||
/* do a simple demand read */
|
||||
sz = read(fd_gyro, &g_report, sizeof(g_report));
|
||||
|
||||
if (sz != sizeof(g_report))
|
||||
if (sz != sizeof(g_report)) {
|
||||
warnx("ret: %d, expected: %d", sz, sizeof(g_report));
|
||||
err(1, "immediate gyro read failed");
|
||||
}
|
||||
|
||||
warnx("gyro x: \t% 9.5f\trad/s", (double)g_report.x);
|
||||
warnx("gyro y: \t% 9.5f\trad/s", (double)g_report.y);
|
||||
|
|
|
@ -0,0 +1,77 @@
|
|||
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
||||
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/// @file LowPassFilter.cpp
|
||||
/// @brief A class to implement a second order low pass filter
|
||||
/// Author: Leonard Hall <LeonardTHall@gmail.com>
|
||||
|
||||
#include "LowPassFilter2p.hpp"
|
||||
#include "math.h"
|
||||
|
||||
namespace math
|
||||
{
|
||||
|
||||
void LowPassFilter2p::set_cutoff_frequency(float sample_freq, float cutoff_freq)
|
||||
{
|
||||
_cutoff_freq = cutoff_freq;
|
||||
float fr = sample_freq/_cutoff_freq;
|
||||
float ohm = tanf(M_PI_F/fr);
|
||||
float c = 1.0f+2.0f*cosf(M_PI_F/4.0f)*ohm + ohm*ohm;
|
||||
_b0 = ohm*ohm/c;
|
||||
_b1 = 2.0f*_b0;
|
||||
_b2 = _b0;
|
||||
_a1 = 2.0f*(ohm*ohm-1.0f)/c;
|
||||
_a2 = (1.0f-2.0f*cosf(M_PI_F/4.0f)*ohm+ohm*ohm)/c;
|
||||
}
|
||||
|
||||
float LowPassFilter2p::apply(float sample)
|
||||
{
|
||||
// do the filtering
|
||||
float delay_element_0 = sample - _delay_element_1 * _a1 - _delay_element_2 * _a2;
|
||||
if (isnan(delay_element_0) || isinf(delay_element_0)) {
|
||||
// don't allow bad values to propogate via the filter
|
||||
delay_element_0 = sample;
|
||||
}
|
||||
float output = delay_element_0 * _b0 + _delay_element_1 * _b1 + _delay_element_2 * _b2;
|
||||
|
||||
_delay_element_2 = _delay_element_1;
|
||||
_delay_element_1 = delay_element_0;
|
||||
|
||||
// return the value. Should be no need to check limits
|
||||
return output;
|
||||
}
|
||||
|
||||
} // namespace math
|
||||
|
|
@ -0,0 +1,78 @@
|
|||
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
||||
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2012 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/// @file LowPassFilter.h
|
||||
/// @brief A class to implement a second order low pass filter
|
||||
/// Author: Leonard Hall <LeonardTHall@gmail.com>
|
||||
/// Adapted for PX4 by Andrew Tridgell
|
||||
|
||||
#pragma once
|
||||
|
||||
namespace math
|
||||
{
|
||||
class __EXPORT LowPassFilter2p
|
||||
{
|
||||
public:
|
||||
// constructor
|
||||
LowPassFilter2p(float sample_freq, float cutoff_freq) {
|
||||
// set initial parameters
|
||||
set_cutoff_frequency(sample_freq, cutoff_freq);
|
||||
_delay_element_1 = _delay_element_2 = 0;
|
||||
}
|
||||
|
||||
// change parameters
|
||||
void set_cutoff_frequency(float sample_freq, float cutoff_freq);
|
||||
|
||||
// apply - Add a new raw value to the filter
|
||||
// and retrieve the filtered result
|
||||
float apply(float sample);
|
||||
|
||||
// return the cutoff frequency
|
||||
float get_cutoff_freq(void) const {
|
||||
return _cutoff_freq;
|
||||
}
|
||||
|
||||
private:
|
||||
float _cutoff_freq;
|
||||
float _a1;
|
||||
float _a2;
|
||||
float _b0;
|
||||
float _b1;
|
||||
float _b2;
|
||||
float _delay_element_1; // buffered sample -1
|
||||
float _delay_element_2; // buffered sample -2
|
||||
};
|
||||
|
||||
} // namespace math
|
|
@ -0,0 +1,44 @@
|
|||
############################################################################
|
||||
#
|
||||
# Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
|
||||
#
|
||||
# Redistribution and use in source and binary forms, with or without
|
||||
# modification, are permitted provided that the following conditions
|
||||
# are met:
|
||||
#
|
||||
# 1. Redistributions of source code must retain the above copyright
|
||||
# notice, this list of conditions and the following disclaimer.
|
||||
# 2. Redistributions in binary form must reproduce the above copyright
|
||||
# notice, this list of conditions and the following disclaimer in
|
||||
# the documentation and/or other materials provided with the
|
||||
# distribution.
|
||||
# 3. Neither the name PX4 nor the names of its contributors may be
|
||||
# used to endorse or promote products derived from this software
|
||||
# without specific prior written permission.
|
||||
#
|
||||
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
# POSSIBILITY OF SUCH DAMAGE.
|
||||
#
|
||||
############################################################################
|
||||
|
||||
#
|
||||
# filter library
|
||||
#
|
||||
SRCS = LowPassFilter2p.cpp
|
||||
|
||||
#
|
||||
# In order to include .config we first have to save off the
|
||||
# current makefile name, since app.mk needs it.
|
||||
#
|
||||
APP_MAKEFILE := $(lastword $(MAKEFILE_LIST))
|
||||
-include $(TOPDIR)/.config
|
|
@ -92,8 +92,35 @@
|
|||
#define BARO_HEALTH_COUNTER_LIMIT_OK 5
|
||||
#define ADC_HEALTH_COUNTER_LIMIT_OK 5
|
||||
|
||||
#define ADC_BATTERY_VOLTAGE_CHANNEL 10
|
||||
#define ADC_AIRSPEED_VOLTAGE_CHANNEL 11
|
||||
/**
|
||||
* Analog layout:
|
||||
* FMU:
|
||||
* IN2 - battery voltage
|
||||
* IN3 - battery current
|
||||
* IN4 - 5V sense
|
||||
* IN10 - spare (we could actually trim these from the set)
|
||||
* IN11 - spare (we could actually trim these from the set)
|
||||
* IN12 - spare (we could actually trim these from the set)
|
||||
* IN13 - aux1
|
||||
* IN14 - aux2
|
||||
* IN15 - pressure sensor
|
||||
*
|
||||
* IO:
|
||||
* IN4 - servo supply rail
|
||||
* IN5 - analog RSSI
|
||||
*/
|
||||
|
||||
#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
|
||||
#define ADC_BATTERY_VOLTAGE_CHANNEL 10
|
||||
#define ADC_AIRSPEED_VOLTAGE_CHANNEL 11
|
||||
#endif
|
||||
|
||||
#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2
|
||||
#define ADC_BATTERY_VOLTAGE_CHANNEL 2
|
||||
#define ADC_BATTERY_CURRENT_CHANNEL 3
|
||||
#define ADC_5V_RAIL_SENSE 4
|
||||
#define ADC_AIRSPEED_VOLTAGE_CHANNEL 15
|
||||
#endif
|
||||
|
||||
#define BAT_VOL_INITIAL 0.f
|
||||
#define BAT_VOL_LOWPASS_1 0.99f
|
||||
|
@ -731,11 +758,26 @@ Sensors::accel_init()
|
|||
errx(1, "FATAL: no accelerometer found");
|
||||
|
||||
} else {
|
||||
/* set the accel internal sampling rate up to at leat 500Hz */
|
||||
ioctl(fd, ACCELIOCSSAMPLERATE, 500);
|
||||
|
||||
/* set the driver to poll at 500Hz */
|
||||
ioctl(fd, SENSORIOCSPOLLRATE, 500);
|
||||
// XXX do the check more elegantly
|
||||
|
||||
#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
|
||||
|
||||
/* set the accel internal sampling rate up to at leat 1000Hz */
|
||||
ioctl(fd, ACCELIOCSSAMPLERATE, 1000);
|
||||
|
||||
/* set the driver to poll at 1000Hz */
|
||||
ioctl(fd, SENSORIOCSPOLLRATE, 1000);
|
||||
|
||||
#else
|
||||
|
||||
/* set the accel internal sampling rate up to at leat 800Hz */
|
||||
ioctl(fd, ACCELIOCSSAMPLERATE, 800);
|
||||
|
||||
/* set the driver to poll at 800Hz */
|
||||
ioctl(fd, SENSORIOCSPOLLRATE, 800);
|
||||
|
||||
#endif
|
||||
|
||||
warnx("using system accel");
|
||||
close(fd);
|
||||
|
@ -754,11 +796,28 @@ Sensors::gyro_init()
|
|||
errx(1, "FATAL: no gyro found");
|
||||
|
||||
} else {
|
||||
/* set the gyro internal sampling rate up to at leat 500Hz */
|
||||
ioctl(fd, GYROIOCSSAMPLERATE, 500);
|
||||
|
||||
/* set the driver to poll at 500Hz */
|
||||
ioctl(fd, SENSORIOCSPOLLRATE, 500);
|
||||
// XXX do the check more elegantly
|
||||
|
||||
#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
|
||||
|
||||
/* set the gyro internal sampling rate up to at least 1000Hz */
|
||||
if (ioctl(fd, GYROIOCSSAMPLERATE, 1000) != OK)
|
||||
ioctl(fd, GYROIOCSSAMPLERATE, 800);
|
||||
|
||||
/* set the driver to poll at 1000Hz */
|
||||
if (ioctl(fd, SENSORIOCSPOLLRATE, 1000) != OK)
|
||||
ioctl(fd, SENSORIOCSPOLLRATE, 800);
|
||||
|
||||
#else
|
||||
|
||||
/* set the gyro internal sampling rate up to at leat 800Hz */
|
||||
ioctl(fd, GYROIOCSSAMPLERATE, 800);
|
||||
|
||||
/* set the driver to poll at 800Hz */
|
||||
ioctl(fd, SENSORIOCSPOLLRATE, 800);
|
||||
|
||||
#endif
|
||||
|
||||
warnx("using system gyro");
|
||||
close(fd);
|
||||
|
@ -1360,6 +1419,9 @@ Sensors::task_main()
|
|||
/* rate limit vehicle status updates to 5Hz */
|
||||
orb_set_interval(_vstatus_sub, 200);
|
||||
|
||||
/* rate limit gyro to 250 Hz (the gyro signal is lowpassed accordingly earlier) */
|
||||
orb_set_interval(_gyro_sub, 4);
|
||||
|
||||
/*
|
||||
* do advertisements
|
||||
*/
|
||||
|
@ -1395,7 +1457,7 @@ Sensors::task_main()
|
|||
|
||||
while (!_task_should_exit) {
|
||||
|
||||
/* wait for up to 500ms for data */
|
||||
/* wait for up to 100ms for data */
|
||||
int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100);
|
||||
|
||||
/* timed out - periodic check for _task_should_exit, etc. */
|
||||
|
|
Loading…
Reference in New Issue