aboutsummaryrefslogtreecommitdiff
path: root/src/drivers/mpu6000/mpu6000.cpp
diff options
context:
space:
mode:
Diffstat (limited to 'src/drivers/mpu6000/mpu6000.cpp')
-rw-r--r--src/drivers/mpu6000/mpu6000.cpp219
1 files changed, 140 insertions, 79 deletions
diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp
index c4e331a30..70359110e 100644
--- a/src/drivers/mpu6000/mpu6000.cpp
+++ b/src/drivers/mpu6000/mpu6000.cpp
@@ -63,7 +63,7 @@
#include <nuttx/arch.h>
#include <nuttx/clock.h>
-#include <arch/board/board.h>
+#include <board_config.h>
#include <drivers/drv_hrt.h>
#include <drivers/device/spi.h>
@@ -149,6 +149,17 @@
#define MPU6000_REV_D9 0x59
#define MPU6000_REV_D10 0x5A
+#define MPU6000_ACCEL_DEFAULT_RANGE_G 8
+#define MPU6000_ACCEL_DEFAULT_RATE 1000
+#define MPU6000_ACCEL_DEFAULT_DRIVER_FILTER_FREQ 30
+
+#define MPU6000_GYRO_DEFAULT_RANGE_G 8
+#define MPU6000_GYRO_DEFAULT_RATE 1000
+#define MPU6000_GYRO_DEFAULT_DRIVER_FILTER_FREQ 30
+
+#define MPU6000_DEFAULT_ONCHIP_FILTER_FREQ 42
+
+#define MPU6000_ONE_G 9.80665f
class MPU6000_gyro;
@@ -183,16 +194,14 @@ private:
struct hrt_call _call;
unsigned _call_interval;
- typedef RingBuffer<accel_report> AccelReportBuffer;
- AccelReportBuffer *_accel_reports;
+ RingBuffer *_accel_reports;
struct accel_scale _accel_scale;
float _accel_range_scale;
float _accel_range_m_s2;
orb_advert_t _accel_topic;
- typedef RingBuffer<gyro_report> GyroReportBuffer;
- GyroReportBuffer *_gyro_reports;
+ RingBuffer *_gyro_reports;
struct gyro_scale _gyro_scale;
float _gyro_range_scale;
@@ -285,12 +294,26 @@ private:
uint16_t swap16(uint16_t val) { return (val >> 8) | (val << 8); }
/**
- * Self test
+ * Measurement self test
*
* @return 0 on success, 1 on failure
*/
int self_test();
+ /**
+ * Accel self test
+ *
+ * @return 0 on success, 1 on failure
+ */
+ int accel_self_test();
+
+ /**
+ * Gyro self test
+ *
+ * @return 0 on success, 1 on failure
+ */
+ int gyro_self_test();
+
/*
set low pass filter frequency
*/
@@ -321,6 +344,7 @@ protected:
void parent_poll_notify();
private:
MPU6000 *_parent;
+
};
/** driver 'main' command */
@@ -342,12 +366,12 @@ MPU6000::MPU6000(int bus, spi_dev_e device) :
_reads(0),
_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)
+ _accel_filter_x(MPU6000_ACCEL_DEFAULT_RATE, MPU6000_ACCEL_DEFAULT_DRIVER_FILTER_FREQ),
+ _accel_filter_y(MPU6000_ACCEL_DEFAULT_RATE, MPU6000_ACCEL_DEFAULT_DRIVER_FILTER_FREQ),
+ _accel_filter_z(MPU6000_ACCEL_DEFAULT_RATE, MPU6000_ACCEL_DEFAULT_DRIVER_FILTER_FREQ),
+ _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)
{
// disable debug() calls
_debug_enabled = false;
@@ -405,11 +429,11 @@ MPU6000::init()
}
/* allocate basic report buffers */
- _accel_reports = new AccelReportBuffer(2);
+ _accel_reports = new RingBuffer(2, sizeof(accel_report));
if (_accel_reports == nullptr)
goto out;
- _gyro_reports = new GyroReportBuffer(2);
+ _gyro_reports = new RingBuffer(2, sizeof(gyro_report));
if (_gyro_reports == nullptr)
goto out;
@@ -440,14 +464,14 @@ MPU6000::init()
_gyro_topic = -1;
} else {
gyro_report gr;
- _gyro_reports->get(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_reports->get(&ar);
_accel_topic = orb_advertise(ORB_ID(sensor_accel), &ar);
out:
@@ -470,14 +494,13 @@ void MPU6000::reset()
up_udelay(1000);
// SAMPLE RATE
- //write_reg(MPUREG_SMPLRT_DIV, 0x04); // Sample rate = 200Hz Fsample= 1Khz/(4+1) = 200Hz
- _set_sample_rate(_sample_rate); // default sample rate = 200Hz
+ _set_sample_rate(_sample_rate);
usleep(1000);
// 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(42);
+ _set_dlpf_filter(MPU6000_DEFAULT_ONCHIP_FILTER_FREQ);
usleep(1000);
// Gyro scale 2000 deg/s ()
write_reg(MPUREG_GYRO_CONFIG, BITS_FS_2000DPS);
@@ -520,8 +543,8 @@ void MPU6000::reset()
// Correct accel scale factors of 4096 LSB/g
// scale to m/s^2 ( 1g = 9.81 m/s^2)
- _accel_range_scale = (9.81f / 4096.0f);
- _accel_range_m_s2 = 8.0f * 9.81f;
+ _accel_range_scale = (MPU6000_ONE_G / 4096.0f);
+ _accel_range_m_s2 = 8.0f * MPU6000_ONE_G;
usleep(1000);
@@ -633,9 +656,10 @@ MPU6000::read(struct file *filp, char *buffer, size_t buflen)
accel_report *arp = reinterpret_cast<accel_report *>(buffer);
int transferred = 0;
while (count--) {
- if (!_accel_reports->get(*arp++))
+ if (!_accel_reports->get(arp))
break;
transferred++;
+ arp++;
}
/* return the number of bytes transferred */
@@ -653,6 +677,56 @@ MPU6000::self_test()
return (_reads > 0) ? 0 : 1;
}
+int
+MPU6000::accel_self_test()
+{
+ if (self_test())
+ return 1;
+
+ /* inspect accel offsets */
+ if (fabsf(_accel_scale.x_offset) < 0.000001f)
+ return 1;
+ if (fabsf(_accel_scale.x_scale - 1.0f) > 0.4f || fabsf(_accel_scale.x_scale - 1.0f) < 0.000001f)
+ return 1;
+
+ if (fabsf(_accel_scale.y_offset) < 0.000001f)
+ return 1;
+ if (fabsf(_accel_scale.y_scale - 1.0f) > 0.4f || fabsf(_accel_scale.y_scale - 1.0f) < 0.000001f)
+ return 1;
+
+ if (fabsf(_accel_scale.z_offset) < 0.000001f)
+ return 1;
+ if (fabsf(_accel_scale.z_scale - 1.0f) > 0.4f || fabsf(_accel_scale.z_scale - 1.0f) < 0.000001f)
+ return 1;
+
+ return 0;
+}
+
+int
+MPU6000::gyro_self_test()
+{
+ if (self_test())
+ return 1;
+
+ /* evaluate gyro offsets, complain if offset -> zero or larger than 6 dps */
+ if (fabsf(_gyro_scale.x_offset) > 0.1f || fabsf(_gyro_scale.x_offset) < 0.000001f)
+ return 1;
+ if (fabsf(_gyro_scale.x_scale - 1.0f) > 0.3f)
+ return 1;
+
+ if (fabsf(_gyro_scale.y_offset) > 0.1f || fabsf(_gyro_scale.y_offset) < 0.000001f)
+ return 1;
+ if (fabsf(_gyro_scale.y_scale - 1.0f) > 0.3f)
+ return 1;
+
+ if (fabsf(_gyro_scale.z_offset) > 0.1f || fabsf(_gyro_scale.z_offset) < 0.000001f)
+ return 1;
+ if (fabsf(_gyro_scale.z_scale - 1.0f) > 0.3f)
+ return 1;
+
+ return 0;
+}
+
ssize_t
MPU6000::gyro_read(struct file *filp, char *buffer, size_t buflen)
{
@@ -673,12 +747,13 @@ MPU6000::gyro_read(struct file *filp, char *buffer, size_t buflen)
return -EAGAIN;
/* copy reports out of our buffer to the caller */
- gyro_report *arp = reinterpret_cast<gyro_report *>(buffer);
+ gyro_report *grp = reinterpret_cast<gyro_report *>(buffer);
int transferred = 0;
while (count--) {
- if (!_gyro_reports->get(*arp++))
+ if (!_gyro_reports->get(grp))
break;
transferred++;
+ grp++;
}
/* return the number of bytes transferred */
@@ -712,9 +787,10 @@ MPU6000::ioctl(struct file *filp, int cmd, unsigned long arg)
/* set default/max polling rate */
case SENSOR_POLLRATE_MAX:
+ return ioctl(filp, SENSORIOCSPOLLRATE, 1000);
+
case SENSOR_POLLRATE_DEFAULT:
- /* set to same as sample rate per default */
- return ioctl(filp, SENSORIOCSPOLLRATE, _sample_rate);
+ return ioctl(filp, SENSORIOCSPOLLRATE, MPU6000_ACCEL_DEFAULT_RATE);
/* adjust to a legal polling interval in Hz */
default: {
@@ -761,28 +837,19 @@ MPU6000::ioctl(struct file *filp, int cmd, unsigned long arg)
return 1000000 / _call_interval;
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;
+ /* lower bound is mandatory, upper bound is a sanity check */
+ if ((arg < 1) || (arg > 100))
+ return -EINVAL;
+
+ irqstate_t flags = irqsave();
+ if (!_accel_reports->resize(arg)) {
+ irqrestore(flags);
+ return -ENOMEM;
}
+ irqrestore(flags);
+
+ return OK;
+ }
case SENSORIOCGQUEUEDEPTH:
return _accel_reports->size();
@@ -802,9 +869,9 @@ MPU6000::ioctl(struct file *filp, int cmd, unsigned long 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);
+ _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:
@@ -832,10 +899,10 @@ MPU6000::ioctl(struct file *filp, int cmd, unsigned long arg)
// _accel_range_m_s2 = 8.0f * 9.81f;
return -EINVAL;
case ACCELIOCGRANGE:
- return _accel_range_m_s2;
+ return (unsigned long)((_accel_range_m_s2)/MPU6000_ONE_G + 0.5f);
case ACCELIOCSELFTEST:
- return self_test();
+ return accel_self_test();
default:
/* give it to the superclass */
@@ -855,28 +922,19 @@ MPU6000::gyro_ioctl(struct file *filp, int cmd, unsigned long arg)
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;
+ /* lower bound is mandatory, upper bound is a sanity check */
+ if ((arg < 1) || (arg > 100))
+ return -EINVAL;
+
+ irqstate_t flags = irqsave();
+ if (!_gyro_reports->resize(arg)) {
+ irqrestore(flags);
+ return -ENOMEM;
}
+ irqrestore(flags);
+
+ return OK;
+ }
case SENSORIOCGQUEUEDEPTH:
return _gyro_reports->size();
@@ -915,10 +973,10 @@ MPU6000::gyro_ioctl(struct file *filp, int cmd, unsigned long arg)
// _gyro_range_rad_s = xx
return -EINVAL;
case GYROIOCGRANGE:
- return _gyro_range_rad_s;
+ return (unsigned long)(_gyro_range_rad_s * 180.0f / M_PI_F + 0.5f);
case GYROIOCSELFTEST:
- return self_test();
+ return gyro_self_test();
default:
/* give it to the superclass */
@@ -1128,7 +1186,7 @@ MPU6000::measure()
* Adjust and scale results to m/s^2.
*/
grb.timestamp = arb.timestamp = hrt_absolute_time();
-
+ grb.error_count = arb.error_count = 0; // not reported
/*
* 1) Scale raw value to SI units using scaling from datasheet.
@@ -1184,8 +1242,8 @@ MPU6000::measure()
grb.temperature_raw = report.temp;
grb.temperature = (report.temp) / 361.0f + 35.0f;
- _accel_reports->put(arb);
- _gyro_reports->put(grb);
+ _accel_reports->force(&arb);
+ _gyro_reports->force(&grb);
/* notify anyone waiting for data */
poll_notify(POLLIN);
@@ -1204,7 +1262,10 @@ MPU6000::measure()
void
MPU6000::print_info()
{
+ perf_print_counter(_sample_perf);
printf("reads: %u\n", _reads);
+ _accel_reports->print_info("accel queue");
+ _gyro_reports->print_info("gyro queue");
}
MPU6000_gyro::MPU6000_gyro(MPU6000 *parent) :
@@ -1335,7 +1396,7 @@ test()
warnx("acc y: \t%d\traw 0x%0x", (short)a_report.y_raw, (unsigned short)a_report.y_raw);
warnx("acc z: \t%d\traw 0x%0x", (short)a_report.z_raw, (unsigned short)a_report.z_raw);
warnx("acc range: %8.4f m/s^2 (%8.4f g)", (double)a_report.range_m_s2,
- (double)(a_report.range_m_s2 / 9.81f));
+ (double)(a_report.range_m_s2 / MPU6000_ONE_G));
/* do a simple demand read */
sz = read(fd_gyro, &g_report, sizeof(g_report));
generated by cgit v1.2.3 (git 2.39.1) at 2024-07-06 05:58:48 +0000