diff options
author | Anton Babushkin <anton.babushkin@me.com> | 2013-08-15 10:33:45 +0200 |
---|---|---|
committer | Anton Babushkin <anton.babushkin@me.com> | 2013-08-15 10:33:45 +0200 |
commit | 7476b03543f879df5ea29d44be147ff4926f8216 (patch) | |
tree | e1c056fa3e12b2c2c3ae601ae2835ac086b60338 /src/drivers/mpu6000 | |
parent | 39ae01dd07d53e3509826ae3737fc6a509adec34 (diff) | |
parent | d2f19c7d84030ad6ed1f6c17538fa96864c5dcef (diff) | |
download | px4-firmware-7476b03543f879df5ea29d44be147ff4926f8216.tar.gz px4-firmware-7476b03543f879df5ea29d44be147ff4926f8216.tar.bz2 px4-firmware-7476b03543f879df5ea29d44be147ff4926f8216.zip |
Merge branch 'master' into new_state_machine_drton
Diffstat (limited to 'src/drivers/mpu6000')
-rw-r--r-- | src/drivers/mpu6000/mpu6000.cpp | 402 |
1 files changed, 299 insertions, 103 deletions
diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp index 8d9054a38..c4e331a30 100644 --- a/src/drivers/mpu6000/mpu6000.cpp +++ b/src/drivers/mpu6000/mpu6000.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * 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 @@ -35,6 +35,9 @@ * @file mpu6000.cpp * * Driver for the Invensense MPU6000 connected via SPI. + * + * @author Andrew Tridgell + * @author Pat Hickey */ #include <nuttx/config.h> @@ -64,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 @@ -178,21 +183,33 @@ 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; orb_advert_t _gyro_topic; unsigned _reads; + 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. */ @@ -204,6 +221,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. * @@ -215,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(); @@ -307,14 +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_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; @@ -335,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)); } @@ -348,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); } @@ -356,6 +393,7 @@ int MPU6000::init() { int ret; + int gyro_ret; /* do SPI init (and probe) first */ ret = SPI::init(); @@ -366,9 +404,58 @@ MPU6000::init() return ret; } - /* advertise sensor topics */ - _accel_topic = orb_advertise(ORB_ID(sensor_accel), &_accel_report); - _gyro_topic = orb_advertise(ORB_ID(sensor_gyro), &_gyro_report); + /* 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); @@ -384,13 +471,13 @@ MPU6000::init() // SAMPLE RATE //write_reg(MPUREG_SMPLRT_DIV, 0x04); // Sample rate = 200Hz Fsample= 1Khz/(4+1) = 200Hz - _set_sample_rate(200); // default sample rate = 200Hz + _set_sample_rate(_sample_rate); // default sample rate = 200Hz 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(20); + _set_dlpf_filter(42); usleep(1000); // Gyro scale 2000 deg/s () write_reg(MPUREG_GYRO_CONFIG, BITS_FS_2000DPS); @@ -401,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; @@ -439,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; @@ -460,14 +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(); - - if (gyro_ret != OK) { - _gyro_topic = -1; - } - - return ret; } int @@ -509,6 +576,7 @@ MPU6000::_set_sample_rate(uint16_t desired_sample_rate_hz) if(div>200) div=200; if(div<1) div=1; write_reg(MPUREG_SMPLRT_DIV, div-1); + _sample_rate = 1000 / div; } /* @@ -545,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; + + /* 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 ret; + /* return the number of bytes transferred */ + return (transferred * sizeof(accel_report)); } int @@ -576,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; + + /* 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 ret; + /* return the number of bytes transferred */ + return (transferred * sizeof(gyro_report)); } int @@ -598,6 +690,10 @@ MPU6000::ioctl(struct file *filp, int cmd, unsigned long arg) { switch (cmd) { + case SENSORIOCRESET: + reset(); + return OK; + case SENSORIOCSPOLLRATE: { switch (arg) { @@ -617,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: { @@ -632,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; @@ -651,23 +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: - case ACCELIOCGSAMPLERATE: - _set_sample_rate(arg); - return OK; + _set_sample_rate(arg); + return OK; - case ACCELIOCSLOWPASS: case ACCELIOCGLOWPASS: - _set_dlpf_filter((uint16_t)arg); + return _accel_filter_x.get_cutoff_freq(); + + case ACCELIOCSLOWPASS: + + // 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: @@ -689,12 +826,13 @@ MPU6000::ioctl(struct file *filp, int cmd, unsigned long arg) return OK; case ACCELIOCSRANGE: - case ACCELIOCGRANGE: /* XXX not implemented */ // XXX change these two values on set: // _accel_range_scale = (9.81f / 4096.0f); - // _accel_range_rad_s = 8.0f * 9.81f; + // _accel_range_m_s2 = 8.0f * 9.81f; return -EINVAL; + case ACCELIOCGRANGE: + return _accel_range_m_s2; case ACCELIOCSELFTEST: return self_test(); @@ -713,19 +851,51 @@ 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 GYROIOCSSAMPLERATE: + 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: - _set_sample_rate(arg); - return OK; + return _sample_rate; + + case GYROIOCSSAMPLERATE: + _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: @@ -739,12 +909,13 @@ MPU6000::gyro_ioctl(struct file *filp, int cmd, unsigned long arg) return OK; case GYROIOCSRANGE: - case GYROIOCGRANGE: /* XXX not implemented */ // XXX change these two values on set: // _gyro_range_scale = xx - // _gyro_range_m_s2 = xx + // _gyro_range_rad_s = xx return -EINVAL; + case GYROIOCGRANGE: + return _gyro_range_rad_s; case GYROIOCSELFTEST: return self_test(); @@ -849,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); } @@ -862,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(); @@ -944,9 +1119,15 @@ MPU6000::measure() 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(); /* @@ -967,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 */ @@ -1103,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); @@ -1129,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); @@ -1146,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); |