zrzk
/
px4_autopilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

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
sbg
Andrew Tridgell 12 years ago committed by Lorenz Meier
parent
dcee02148e
commit
c62710f80b
1 changed files with 64 additions and 69 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 133
      src/drivers/mpu6000/mpu6000.cpp

133
src/drivers/mpu6000/mpu6000.cpp
Unescape 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++))
@ -837,28 +847,19 @@ MPU6000::ioctl(struct file *filp, int cmd, unsigned long arg)
return 1000000 / _call_interval; return 1000000 / _call_interval;
case SENSORIOCSQUEUEDEPTH: { case SENSORIOCSQUEUEDEPTH: {
/* lower bound is mandatory, upper bound is a sanity check */ /* lower bound is mandatory, upper bound is a sanity check */
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;
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;
} }
irqrestore(flags);
return OK;
}
case SENSORIOCGQUEUEDEPTH: case SENSORIOCGQUEUEDEPTH:
return _accel_reports->size(); return _accel_reports->size();
@ -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) :

Write Preview
Loading…
Cancel
Save