dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

mpu6000: use a wrapper struct to avoid a linker error 

the linker doesn't cope with us having multiple modules implementing
RingBuffer<gyro_report>

this also switches to use force() instead of put(), so we discard old
entries when the buffer overflows
This commit is contained in:
Andrew Tridgell 2013-09-09 10:55:22 +10:00 committed by Lorenz Meier
parent 3c45261117
commit 37d09f0944
1 changed files with 64 additions and 69 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

117
src/drivers/mpu6000/mpu6000.cpp
View File

@ -194,7 +194,14 @@ private:
struct hrt_call _call; struct hrt_call _call;
unsigned _call_interval; unsigned _call_interval;
typedef RingBuffer<accel_report> AccelReportBuffer; /*
these wrapper types are needed to avoid a linker error for
RingBuffer instances which appear in two drivers.
*/
struct _accel_report {
struct accel_report r;
};
typedef RingBuffer<_accel_report> AccelReportBuffer;
AccelReportBuffer *_accel_reports; AccelReportBuffer *_accel_reports;
struct accel_scale _accel_scale; struct accel_scale _accel_scale;
@ -202,7 +209,10 @@ private:
float _accel_range_m_s2; float _accel_range_m_s2;
orb_advert_t _accel_topic; orb_advert_t _accel_topic;
typedef RingBuffer<gyro_report> GyroReportBuffer; struct _gyro_report {
struct gyro_report r;
};
typedef RingBuffer<_gyro_report> GyroReportBuffer;
GyroReportBuffer *_gyro_reports; GyroReportBuffer *_gyro_reports;
struct gyro_scale _gyro_scale; struct gyro_scale _gyro_scale;
@ -465,16 +475,16 @@ MPU6000::init()
if (gyro_ret != OK) { if (gyro_ret != OK) {
_gyro_topic = -1; _gyro_topic = -1;
} else { } else {
gyro_report gr; _gyro_report gr;
_gyro_reports->get(gr); _gyro_reports->get(gr);
_gyro_topic = orb_advertise(ORB_ID(sensor_gyro), &gr); _gyro_topic = orb_advertise(ORB_ID(sensor_gyro), &gr.r);
} }
/* advertise accel topic */ /* advertise accel topic */
accel_report ar; _accel_report ar;
_accel_reports->get(ar); _accel_reports->get(ar);
_accel_topic = orb_advertise(ORB_ID(sensor_accel), &ar); _accel_topic = orb_advertise(ORB_ID(sensor_accel), &ar.r);
out: out:
return ret; return ret;
@ -655,7 +665,7 @@ MPU6000::read(struct file *filp, char *buffer, size_t buflen)
return -EAGAIN; return -EAGAIN;
/* copy reports out of our buffer to the caller */ /* copy reports out of our buffer to the caller */
accel_report *arp = reinterpret_cast<accel_report *>(buffer); _accel_report *arp = reinterpret_cast<_accel_report *>(buffer);
int transferred = 0; int transferred = 0;
while (count--) { while (count--) {
if (!_accel_reports->get(*arp++)) if (!_accel_reports->get(*arp++))
@ -748,7 +758,7 @@ MPU6000::gyro_read(struct file *filp, char *buffer, size_t buflen)
return -EAGAIN; return -EAGAIN;
/* copy reports out of our buffer to the caller */ /* copy reports out of our buffer to the caller */
gyro_report *arp = reinterpret_cast<gyro_report *>(buffer); _gyro_report *arp = reinterpret_cast<_gyro_report *>(buffer);
int transferred = 0; int transferred = 0;
while (count--) { while (count--) {
if (!_gyro_reports->get(*arp++)) if (!_gyro_reports->get(*arp++))
@ -841,21 +851,12 @@ MPU6000::ioctl(struct file *filp, int cmd, unsigned long arg)
if ((arg < 1) || (arg > 100)) if ((arg < 1) || (arg > 100))
return -EINVAL; return -EINVAL;
/* allocate new buffer */ irqstate_t flags = irqsave();
AccelReportBuffer *buf = new AccelReportBuffer(arg); if (!_accel_reports->resize(arg)) {
irqrestore(flags);
if (nullptr == buf)
return -ENOMEM;
if (buf->size() == 0) {
delete buf;
return -ENOMEM; return -ENOMEM;
} }
irqrestore(flags);
/* reset the measurement state machine with the new buffer, free the old */
stop();
delete _accel_reports;
_accel_reports = buf;
start();
return OK; return OK;
} }
@ -935,21 +936,12 @@ MPU6000::gyro_ioctl(struct file *filp, int cmd, unsigned long arg)
if ((arg < 1) || (arg > 100)) if ((arg < 1) || (arg > 100))
return -EINVAL; return -EINVAL;
/* allocate new buffer */ irqstate_t flags = irqsave();
GyroReportBuffer *buf = new GyroReportBuffer(arg); if (!_gyro_reports->resize(arg)) {
irqrestore(flags);
if (nullptr == buf)
return -ENOMEM;
if (buf->size() == 0) {
delete buf;
return -ENOMEM; return -ENOMEM;
} }
irqrestore(flags);
/* reset the measurement state machine with the new buffer, free the old */
stop();
delete _gyro_reports;
_gyro_reports = buf;
start();
return OK; return OK;
} }
@ -1197,13 +1189,13 @@ MPU6000::measure()
/* /*
* Report buffers. * Report buffers.
*/ */
accel_report arb; _accel_report arb;
gyro_report grb; _gyro_report grb;
/* /*
* Adjust and scale results to m/s^2. * Adjust and scale results to m/s^2.
*/ */
grb.timestamp = arb.timestamp = hrt_absolute_time(); grb.r.timestamp = arb.r.timestamp = hrt_absolute_time();
/* /*
@ -1224,53 +1216,53 @@ MPU6000::measure()
/* NOTE: Axes have been swapped to match the board a few lines above. */ /* NOTE: Axes have been swapped to match the board a few lines above. */
arb.x_raw = report.accel_x; arb.r.x_raw = report.accel_x;
arb.y_raw = report.accel_y; arb.r.y_raw = report.accel_y;
arb.z_raw = report.accel_z; arb.r.z_raw = report.accel_z;
float x_in_new = ((report.accel_x * _accel_range_scale) - _accel_scale.x_offset) * _accel_scale.x_scale; 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 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; 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.r.x = _accel_filter_x.apply(x_in_new);
arb.y = _accel_filter_y.apply(y_in_new); arb.r.y = _accel_filter_y.apply(y_in_new);
arb.z = _accel_filter_z.apply(z_in_new); arb.r.z = _accel_filter_z.apply(z_in_new);
arb.scaling = _accel_range_scale; arb.r.scaling = _accel_range_scale;
arb.range_m_s2 = _accel_range_m_s2; arb.r.range_m_s2 = _accel_range_m_s2;
arb.temperature_raw = report.temp; arb.r.temperature_raw = report.temp;
arb.temperature = (report.temp) / 361.0f + 35.0f; arb.r.temperature = (report.temp) / 361.0f + 35.0f;
grb.x_raw = report.gyro_x; grb.r.x_raw = report.gyro_x;
grb.y_raw = report.gyro_y; grb.r.y_raw = report.gyro_y;
grb.z_raw = report.gyro_z; grb.r.z_raw = report.gyro_z;
float x_gyro_in_new = ((report.gyro_x * _gyro_range_scale) - _gyro_scale.x_offset) * _gyro_scale.x_scale; 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 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; 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.r.x = _gyro_filter_x.apply(x_gyro_in_new);
grb.y = _gyro_filter_y.apply(y_gyro_in_new); grb.r.y = _gyro_filter_y.apply(y_gyro_in_new);
grb.z = _gyro_filter_z.apply(z_gyro_in_new); grb.r.z = _gyro_filter_z.apply(z_gyro_in_new);
grb.scaling = _gyro_range_scale; grb.r.scaling = _gyro_range_scale;
grb.range_rad_s = _gyro_range_rad_s; grb.r.range_rad_s = _gyro_range_rad_s;
grb.temperature_raw = report.temp; grb.r.temperature_raw = report.temp;
grb.temperature = (report.temp) / 361.0f + 35.0f; grb.r.temperature = (report.temp) / 361.0f + 35.0f;
_accel_reports->put(arb); _accel_reports->force(arb);
_gyro_reports->put(grb); _gyro_reports->force(grb);
/* notify anyone waiting for data */ /* notify anyone waiting for data */
poll_notify(POLLIN); poll_notify(POLLIN);
_gyro->parent_poll_notify(); _gyro->parent_poll_notify();
/* and publish for subscribers */ /* and publish for subscribers */
orb_publish(ORB_ID(sensor_accel), _accel_topic, &arb); orb_publish(ORB_ID(sensor_accel), _accel_topic, &arb.r);
if (_gyro_topic != -1) { if (_gyro_topic != -1) {
orb_publish(ORB_ID(sensor_gyro), _gyro_topic, &grb); orb_publish(ORB_ID(sensor_gyro), _gyro_topic, &grb.r);
} }
/* stop measuring */ /* stop measuring */
@ -1280,7 +1272,10 @@ MPU6000::measure()
void void
MPU6000::print_info() MPU6000::print_info()
{ {
perf_print_counter(_sample_perf);
printf("reads: %u\n", _reads); printf("reads: %u\n", _reads);
_accel_reports->print_info("accel queue");
_gyro_reports->print_info("gyro queue");
} }
MPU6000_gyro::MPU6000_gyro(MPU6000 *parent) : MPU6000_gyro::MPU6000_gyro(MPU6000 *parent) :