From 9a97001cc849038223aa5eda6a76ac0fc6f1094f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 4 Aug 2013 01:50:58 +0200 Subject: Added queue to mpu6k driver --- src/drivers/mpu6000/mpu6000.cpp | 254 +++++++++++++++++++++++++++++++--------- 1 file changed, 201 insertions(+), 53 deletions(-) (limited to 'src/drivers/mpu6000') diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp index 1fd6cb17e..d37d39a7a 100644 --- a/src/drivers/mpu6000/mpu6000.cpp +++ b/src/drivers/mpu6000/mpu6000.cpp @@ -181,13 +181,21 @@ private: struct hrt_call _call; unsigned _call_interval; - struct accel_report _accel_report; + unsigned _num_accel_reports; + volatile unsigned _next_accel_report; + volatile unsigned _oldest_accel_report; + struct accel_report *_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; + unsigned _num_gyro_reports; + volatile unsigned _next_gyro_report; + volatile unsigned _oldest_gyro_report; + struct gyro_report *_gyro_reports; + struct gyro_scale _gyro_scale; float _gyro_range_scale; float _gyro_range_rad_s; @@ -306,14 +314,25 @@ private: /** driver 'main' command */ extern "C" { __EXPORT int mpu6000_main(int argc, char *argv[]); } +/* helper macro for handling report buffer indices */ +#define INCREMENT(_x, _lim) do { _x++; if (_x >= _lim) _x = 0; } while(0) + MPU6000::MPU6000(int bus, spi_dev_e device) : SPI("MPU6000", ACCEL_DEVICE_PATH, bus, device, SPIDEV_MODE3, 10000000), _gyro(new MPU6000_gyro(this)), _product(0), _call_interval(0), + _num_accel_reports(0), + _next_accel_report(0), + _oldest_accel_report(0), + _accel_reports(nullptr), _accel_range_scale(0.0f), _accel_range_m_s2(0.0f), _accel_topic(-1), + _num_gyro_reports(0), + _next_gyro_report(0), + _oldest_gyro_report(0), + _gyro_reports(nullptr), _gyro_range_scale(0.0f), _gyro_range_rad_s(0.0f), _gyro_topic(-1), @@ -340,8 +359,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 +370,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 +384,7 @@ int MPU6000::init() { int ret; + int gyro_ret; /* do SPI init (and probe) first */ ret = SPI::init(); @@ -371,6 +395,21 @@ MPU6000::init() return ret; } + /* allocate basic report buffers */ + _num_accel_reports = 2; + _oldest_accel_report = _next_accel_report = 0; + _accel_reports = new struct accel_report[_num_accel_reports]; + + if (_accel_reports == nullptr) + goto out; + + _num_gyro_reports = 2; + _oldest_gyro_report = _next_gyro_report = 0; + _gyro_reports = new struct gyro_report[_num_gyro_reports]; + + if (_gyro_reports == nullptr) + goto out; + // Chip reset write_reg(MPUREG_PWR_MGMT_1, BIT_H_RESET); up_udelay(10000); @@ -462,7 +501,10 @@ MPU6000::init() usleep(1000); /* do CDev init for the gyro device node, keep it optional */ - int gyro_ret = _gyro->init(); + gyro_ret = _gyro->init(); + + memset(&_accel_reports[0], 0, sizeof(_accel_reports[0])); + memset(&_gyro_reports[0], 0, sizeof(_gyro_reports[0])); /* ensure we got real values to share */ measure(); @@ -470,12 +512,13 @@ MPU6000::init() if (gyro_ret != OK) { _gyro_topic = -1; } else { - _gyro_topic = orb_advertise(ORB_ID(sensor_gyro), &_gyro_report); + _gyro_topic = orb_advertise(ORB_ID(sensor_gyro), &_gyro_reports[0]); } - /* advertise sensor topics */ - _accel_topic = orb_advertise(ORB_ID(sensor_accel), &_accel_report); + /* advertise accel topic */ + _accel_topic = orb_advertise(ORB_ID(sensor_accel), &_accel_reports[0]); +out: return ret; } @@ -555,19 +598,40 @@ MPU6000::_set_dlpf_filter(uint16_t frequency_hz) ssize_t MPU6000::read(struct file *filp, char *buffer, size_t buflen) { + unsigned count = buflen / sizeof(struct accel_report); int ret = 0; /* 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) - measure(); + /* if automatic measurement is enabled */ + if (_call_interval > 0) { + + /* + * While there is space in the caller's buffer, and reports, copy them. + * Note that we may be pre-empted by the measurement code while we are doing this; + * we are careful to avoid racing with it. + */ + while (count--) { + if (_oldest_accel_report != _next_accel_report) { + memcpy(buffer, _accel_reports + _oldest_accel_report, sizeof(*_accel_reports)); + ret += sizeof(_accel_reports[0]); + INCREMENT(_oldest_accel_report, _num_accel_reports); + } + } - /* copy out the latest reports */ - memcpy(buffer, &_accel_report, sizeof(_accel_report)); - ret = sizeof(_accel_report); + /* if there was no data, warn the caller */ + return ret ? ret : -EAGAIN; + } + + /* manual measurement */ + _oldest_accel_report = _next_accel_report = 0; + measure(); + + /* measurement will have generated a report, copy it out */ + memcpy(buffer, _accel_reports, sizeof(*_accel_reports)); + ret = sizeof(*_accel_reports); return ret; } @@ -586,19 +650,40 @@ MPU6000::self_test() ssize_t MPU6000::gyro_read(struct file *filp, char *buffer, size_t buflen) { + unsigned count = buflen / sizeof(struct gyro_report); int ret = 0; /* 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) - measure(); + /* if automatic measurement is enabled */ + if (_call_interval > 0) { + + /* + * While there is space in the caller's buffer, and reports, copy them. + * Note that we may be pre-empted by the measurement code while we are doing this; + * we are careful to avoid racing with it. + */ + while (count--) { + if (_oldest_gyro_report != _next_gyro_report) { + memcpy(buffer, _gyro_reports + _oldest_gyro_report, sizeof(*_gyro_reports)); + ret += sizeof(_gyro_reports[0]); + INCREMENT(_oldest_gyro_report, _num_gyro_reports); + } + } - /* copy out the latest report */ - memcpy(buffer, &_gyro_report, sizeof(_gyro_report)); - ret = sizeof(_gyro_report); + /* if there was no data, warn the caller */ + return ret ? ret : -EAGAIN; + } + + /* manual measurement */ + _oldest_gyro_report = _next_gyro_report = 0; + measure(); + + /* measurement will have generated a report, copy it out */ + memcpy(buffer, _gyro_reports, sizeof(*_gyro_reports)); + ret = sizeof(*_gyro_reports); return ret; } @@ -661,14 +746,32 @@ MPU6000::ioctl(struct file *filp, int cmd, unsigned long arg) return 1000000 / _call_interval; - case SENSORIOCSQUEUEDEPTH: - /* XXX not implemented */ - return -EINVAL; + case SENSORIOCSQUEUEDEPTH: { + /* account for sentinel in the ring */ + arg++; - case SENSORIOCGQUEUEDEPTH: - /* XXX not implemented */ - return -EINVAL; + /* lower bound is mandatory, upper bound is a sanity check */ + if ((arg < 2) || (arg > 100)) + return -EINVAL; + + /* allocate new buffer */ + struct accel_report *buf = new struct accel_report[arg]; + + if (nullptr == buf) + return -ENOMEM; + /* reset the measurement state machine with the new buffer, free the old */ + stop(); + delete[] _accel_reports; + _num_accel_reports = arg; + _accel_reports = buf; + start(); + + return OK; + } + + case SENSORIOCGQUEUEDEPTH: + return _num_accel_reports - 1; case ACCELIOCGSAMPLERATE: return _sample_rate; @@ -726,11 +829,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: { + /* account for sentinel in the ring */ + arg++; + + /* lower bound is mandatory, upper bound is a sanity check */ + if ((arg < 2) || (arg > 100)) + return -EINVAL; + + /* allocate new buffer */ + struct gyro_report *buf = new struct gyro_report[arg]; + + if (nullptr == buf) + return -ENOMEM; + + /* reset the measurement state machine with the new buffer, free the old */ + stop(); + delete[] _gyro_reports; + _num_gyro_reports = arg; + _gyro_reports = buf; + start(); + + return OK; + } + + case SENSORIOCGQUEUEDEPTH: + return _num_gyro_reports - 1; + case GYROIOCGSAMPLERATE: return _sample_rate; @@ -959,10 +1087,16 @@ MPU6000::measure() report.gyro_x = gyro_xt; report.gyro_y = gyro_yt; + /* + * Get references to the current reports + */ + accel_report *accel_report = &_accel_reports[_next_accel_report]; + gyro_report *gyro_report = &_gyro_reports[_next_gyro_report]; + /* * Adjust and scale results to m/s^2. */ - _gyro_report.timestamp = _accel_report.timestamp = hrt_absolute_time(); + gyro_report->timestamp = accel_report->timestamp = hrt_absolute_time(); /* @@ -983,40 +1117,54 @@ 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; + accel_report->x_raw = report.accel_x; + accel_report->y_raw = report.accel_y; + accel_report->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; + + accel_report->temperature_raw = report.temp; + accel_report->temperature = (report.temp) / 361.0f + 35.0f; + + gyro_report->x_raw = report.gyro_x; + gyro_report->y_raw = report.gyro_y; + gyro_report->z_raw = report.gyro_z; + + 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; - _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; + gyro_report->temperature_raw = report.temp; + gyro_report->temperature = (report.temp) / 361.0f + 35.0f; - _accel_report.temperature_raw = report.temp; - _accel_report.temperature = (report.temp) / 361.0f + 35.0f; + /* ACCEL: post a report to the ring - note, not locked */ + INCREMENT(_next_accel_report, _num_accel_reports); - _gyro_report.x_raw = report.gyro_x; - _gyro_report.y_raw = report.gyro_y; - _gyro_report.z_raw = report.gyro_z; + /* ACCEL: if we are running up against the oldest report, fix it */ + if (_next_accel_report == _oldest_accel_report) + INCREMENT(_oldest_accel_report, _num_accel_reports); - _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; + /* GYRO: post a report to the ring - note, not locked */ + INCREMENT(_next_gyro_report, _num_gyro_reports); - _gyro_report.temperature_raw = report.temp; - _gyro_report.temperature = (report.temp) / 361.0f + 35.0f; + /* GYRO: if we are running up against the oldest report, fix it */ + if (_next_gyro_report == _oldest_gyro_report) + INCREMENT(_oldest_gyro_report, _num_gyro_reports); /* 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, accel_report); if (_gyro_topic != -1) { - orb_publish(ORB_ID(sensor_gyro), _gyro_topic, &_gyro_report); + orb_publish(ORB_ID(sensor_gyro), _gyro_topic, gyro_report); } /* stop measuring */ -- cgit v1.2.3