mirror of
https://github.com/PX4/PX4-Autopilot.git
synced 2026-06-13 04:34:19 +08:00
Rework the ringbuffer class so that it's not templated, and refactor its clients so they aren't dancing around the linker anymore.
This commit is contained in:
px4dev
Lorenz Meier
@@ -79,6 +79,7 @@
|
||||
Airspeed::Airspeed(int bus, int address, unsigned conversion_interval) :
|
||||
I2C("Airspeed", AIRSPEED_DEVICE_PATH, bus, address, 100000),
|
||||
_reports(nullptr),
|
||||
_buffer_overflows(perf_alloc(PC_COUNT, "airspeed_buffer_overflows")),
|
||||
_max_differential_pressure_pa(0),
|
||||
_sensor_ok(false),
|
||||
_measure_ticks(0),
|
||||
@@ -87,8 +88,7 @@ Airspeed::Airspeed(int bus, int address, unsigned conversion_interval) :
|
||||
_airspeed_pub(-1),
|
||||
_conversion_interval(conversion_interval),
|
||||
_sample_perf(perf_alloc(PC_ELAPSED, "airspeed_read")),
|
||||
_comms_errors(perf_alloc(PC_COUNT, "airspeed_comms_errors")),
|
||||
_buffer_overflows(perf_alloc(PC_COUNT, "airspeed_buffer_overflows"))
|
||||
_comms_errors(perf_alloc(PC_COUNT, "airspeed_comms_errors"))
|
||||
{
|
||||
// enable debug() calls
|
||||
_debug_enabled = true;
|
||||
@@ -122,7 +122,7 @@ Airspeed::init()
|
||||
goto out;
|
||||
|
||||
/* allocate basic report buffers */
|
||||
_reports = new RingBuffer<differential_pressure_s>(2);
|
||||
_reports = new RingBuffer(2, sizeof(differential_pressure_s));
|
||||
if (_reports == nullptr)
|
||||
goto out;
|
||||
|
||||
@@ -282,7 +282,7 @@ Airspeed::read(struct file *filp, char *buffer, size_t buflen)
|
||||
* we are careful to avoid racing with them.
|
||||
*/
|
||||
while (count--) {
|
||||
if (_reports->get(*abuf)) {
|
||||
if (_reports->get(abuf)) {
|
||||
ret += sizeof(*abuf);
|
||||
abuf++;
|
||||
}
|
||||
@@ -312,7 +312,7 @@ Airspeed::read(struct file *filp, char *buffer, size_t buflen)
|
||||
}
|
||||
|
||||
/* state machine will have generated a report, copy it out */
|
||||
if (_reports->get(*abuf)) {
|
||||
if (_reports->get(abuf)) {
|
||||
ret = sizeof(*abuf);
|
||||
}
|
||||
|
||||
@@ -375,6 +375,6 @@ Airspeed::print_info()
|
||||
void
|
||||
Airspeed::new_report(const differential_pressure_s &report)
|
||||
{
|
||||
if (!_reports->force(report))
|
||||
if (!_reports->force(&report))
|
||||
perf_count(_buffer_overflows);
|
||||
}
|
||||
|
||||
@@ -104,7 +104,7 @@ public:
|
||||
virtual void print_info();
|
||||
|
||||
private:
|
||||
RingBuffer<differential_pressure_s> *_reports;
|
||||
RingBuffer *_reports;
|
||||
perf_counter_t _buffer_overflows;
|
||||
|
||||
protected:
|
||||
|
||||
@@ -147,14 +147,7 @@ private:
|
||||
struct hrt_call _call;
|
||||
unsigned _call_interval;
|
||||
|
||||
/*
|
||||
this wrapper type is needed to avoid a linker error for
|
||||
RingBuffer instances which appear in two drivers.
|
||||
*/
|
||||
struct _accel_report {
|
||||
accel_report r;
|
||||
};
|
||||
RingBuffer<struct _accel_report> *_reports;
|
||||
RingBuffer *_reports;
|
||||
|
||||
struct accel_scale _accel_scale;
|
||||
float _accel_range_scale;
|
||||
@@ -284,7 +277,7 @@ BMA180::init()
|
||||
goto out;
|
||||
|
||||
/* allocate basic report buffers */
|
||||
_reports = new RingBuffer<_accel_report>(2);
|
||||
_reports = new RingBuffer(2, sizeof(accel_report));
|
||||
|
||||
if (_reports == nullptr)
|
||||
goto out;
|
||||
@@ -349,7 +342,7 @@ ssize_t
|
||||
BMA180::read(struct file *filp, char *buffer, size_t buflen)
|
||||
{
|
||||
unsigned count = buflen / sizeof(struct accel_report);
|
||||
struct _accel_report *arp = reinterpret_cast<struct _accel_report *>(buffer);
|
||||
struct accel_report *arp = reinterpret_cast<struct accel_report *>(buffer);
|
||||
int ret = 0;
|
||||
|
||||
/* buffer must be large enough */
|
||||
@@ -365,7 +358,7 @@ BMA180::read(struct file *filp, char *buffer, size_t buflen)
|
||||
* we are careful to avoid racing with it.
|
||||
*/
|
||||
while (count--) {
|
||||
if (_reports->get(*arp)) {
|
||||
if (_reports->get(arp)) {
|
||||
ret += sizeof(*arp);
|
||||
arp++;
|
||||
}
|
||||
@@ -380,7 +373,7 @@ BMA180::read(struct file *filp, char *buffer, size_t buflen)
|
||||
measure();
|
||||
|
||||
/* measurement will have generated a report, copy it out */
|
||||
if (_reports->get(*arp))
|
||||
if (_reports->get(arp))
|
||||
ret = sizeof(*arp);
|
||||
|
||||
return ret;
|
||||
@@ -676,7 +669,7 @@ BMA180::measure()
|
||||
// } raw_report;
|
||||
// #pragma pack(pop)
|
||||
|
||||
struct _accel_report report;
|
||||
struct accel_report report;
|
||||
|
||||
/* start the performance counter */
|
||||
perf_begin(_sample_perf);
|
||||
@@ -696,40 +689,40 @@ BMA180::measure()
|
||||
* them before. There is no good way to synchronise with the internal
|
||||
* measurement flow without using the external interrupt.
|
||||
*/
|
||||
report.r.timestamp = hrt_absolute_time();
|
||||
report.timestamp = hrt_absolute_time();
|
||||
/*
|
||||
* y of board is x of sensor and x of board is -y of sensor
|
||||
* perform only the axis assignment here.
|
||||
* Two non-value bits are discarded directly
|
||||
*/
|
||||
report.r.y_raw = read_reg(ADDR_ACC_X_LSB + 0);
|
||||
report.r.y_raw |= read_reg(ADDR_ACC_X_LSB + 1) << 8;
|
||||
report.r.x_raw = read_reg(ADDR_ACC_X_LSB + 2);
|
||||
report.r.x_raw |= read_reg(ADDR_ACC_X_LSB + 3) << 8;
|
||||
report.r.z_raw = read_reg(ADDR_ACC_X_LSB + 4);
|
||||
report.r.z_raw |= read_reg(ADDR_ACC_X_LSB + 5) << 8;
|
||||
report.y_raw = read_reg(ADDR_ACC_X_LSB + 0);
|
||||
report.y_raw |= read_reg(ADDR_ACC_X_LSB + 1) << 8;
|
||||
report.x_raw = read_reg(ADDR_ACC_X_LSB + 2);
|
||||
report.x_raw |= read_reg(ADDR_ACC_X_LSB + 3) << 8;
|
||||
report.z_raw = read_reg(ADDR_ACC_X_LSB + 4);
|
||||
report.z_raw |= read_reg(ADDR_ACC_X_LSB + 5) << 8;
|
||||
|
||||
/* discard two non-value bits in the 16 bit measurement */
|
||||
report.r.x_raw = (report.r.x_raw / 4);
|
||||
report.r.y_raw = (report.r.y_raw / 4);
|
||||
report.r.z_raw = (report.r.z_raw / 4);
|
||||
report.x_raw = (report.x_raw / 4);
|
||||
report.y_raw = (report.y_raw / 4);
|
||||
report.z_raw = (report.z_raw / 4);
|
||||
|
||||
/* invert y axis, due to 14 bit data no overflow can occur in the negation */
|
||||
report.r.y_raw = -report.r.y_raw;
|
||||
report.y_raw = -report.y_raw;
|
||||
|
||||
report.r.x = ((report.r.x_raw * _accel_range_scale) - _accel_scale.x_offset) * _accel_scale.x_scale;
|
||||
report.r.y = ((report.r.y_raw * _accel_range_scale) - _accel_scale.y_offset) * _accel_scale.y_scale;
|
||||
report.r.z = ((report.r.z_raw * _accel_range_scale) - _accel_scale.z_offset) * _accel_scale.z_scale;
|
||||
report.r.scaling = _accel_range_scale;
|
||||
report.r.range_m_s2 = _accel_range_m_s2;
|
||||
report.x = ((report.x_raw * _accel_range_scale) - _accel_scale.x_offset) * _accel_scale.x_scale;
|
||||
report.y = ((report.y_raw * _accel_range_scale) - _accel_scale.y_offset) * _accel_scale.y_scale;
|
||||
report.z = ((report.z_raw * _accel_range_scale) - _accel_scale.z_offset) * _accel_scale.z_scale;
|
||||
report.scaling = _accel_range_scale;
|
||||
report.range_m_s2 = _accel_range_m_s2;
|
||||
|
||||
_reports->force(report);
|
||||
_reports->force(&report);
|
||||
|
||||
/* notify anyone waiting for data */
|
||||
poll_notify(POLLIN);
|
||||
|
||||
/* publish for subscribers */
|
||||
orb_publish(ORB_ID(sensor_accel), _accel_topic, &report.r);
|
||||
orb_publish(ORB_ID(sensor_accel), _accel_topic, &report);
|
||||
|
||||
/* stop the perf counter */
|
||||
perf_end(_sample_perf);
|
||||
|
||||
+267
-107
File diff suppressed because it is too large
Load Diff
@@ -149,7 +149,7 @@ private:
|
||||
work_s _work;
|
||||
unsigned _measure_ticks;
|
||||
|
||||
RingBuffer<struct mag_report> *_reports;
|
||||
RingBuffer *_reports;
|
||||
mag_scale _scale;
|
||||
float _range_scale;
|
||||
float _range_ga;
|
||||
@@ -367,7 +367,7 @@ HMC5883::init()
|
||||
goto out;
|
||||
|
||||
/* allocate basic report buffers */
|
||||
_reports = new RingBuffer<struct mag_report>(2);
|
||||
_reports = new RingBuffer(2, sizeof(mag_report));
|
||||
if (_reports == nullptr)
|
||||
goto out;
|
||||
|
||||
@@ -496,7 +496,7 @@ HMC5883::read(struct file *filp, char *buffer, size_t buflen)
|
||||
* we are careful to avoid racing with them.
|
||||
*/
|
||||
while (count--) {
|
||||
if (_reports->get(*mag_buf)) {
|
||||
if (_reports->get(mag_buf)) {
|
||||
ret += sizeof(struct mag_report);
|
||||
mag_buf++;
|
||||
}
|
||||
@@ -526,7 +526,7 @@ HMC5883::read(struct file *filp, char *buffer, size_t buflen)
|
||||
break;
|
||||
}
|
||||
|
||||
if (_reports->get(*mag_buf)) {
|
||||
if (_reports->get(mag_buf)) {
|
||||
ret = sizeof(struct mag_report);
|
||||
}
|
||||
} while (0);
|
||||
@@ -878,7 +878,7 @@ HMC5883::collect()
|
||||
orb_publish(ORB_ID(sensor_mag), _mag_topic, &new_report);
|
||||
|
||||
/* post a report to the ring */
|
||||
if (_reports->force(new_report)) {
|
||||
if (_reports->force(&new_report)) {
|
||||
perf_count(_buffer_overflows);
|
||||
}
|
||||
|
||||
|
||||
@@ -185,7 +185,7 @@ private:
|
||||
struct hrt_call _call;
|
||||
unsigned _call_interval;
|
||||
|
||||
RingBuffer<gyro_report> *_reports;
|
||||
RingBuffer *_reports;
|
||||
|
||||
struct gyro_scale _gyro_scale;
|
||||
float _gyro_range_scale;
|
||||
@@ -347,7 +347,7 @@ L3GD20::init()
|
||||
goto out;
|
||||
|
||||
/* allocate basic report buffers */
|
||||
_reports = new RingBuffer<struct gyro_report>(2);
|
||||
_reports = new RingBuffer(2, sizeof(gyro_report));
|
||||
|
||||
if (_reports == nullptr)
|
||||
goto out;
|
||||
@@ -421,7 +421,7 @@ L3GD20::read(struct file *filp, char *buffer, size_t buflen)
|
||||
* we are careful to avoid racing with it.
|
||||
*/
|
||||
while (count--) {
|
||||
if (_reports->get(*gbuf)) {
|
||||
if (_reports->get(gbuf)) {
|
||||
ret += sizeof(*gbuf);
|
||||
gbuf++;
|
||||
}
|
||||
@@ -436,7 +436,7 @@ L3GD20::read(struct file *filp, char *buffer, size_t buflen)
|
||||
measure();
|
||||
|
||||
/* measurement will have generated a report, copy it out */
|
||||
if (_reports->get(*gbuf)) {
|
||||
if (_reports->get(gbuf)) {
|
||||
ret = sizeof(*gbuf);
|
||||
}
|
||||
|
||||
@@ -815,7 +815,7 @@ L3GD20::measure()
|
||||
report.scaling = _gyro_range_scale;
|
||||
report.range_rad_s = _gyro_range_rad_s;
|
||||
|
||||
_reports->force(report);
|
||||
_reports->force(&report);
|
||||
|
||||
/* notify anyone waiting for data */
|
||||
poll_notify(POLLIN);
|
||||
|
||||
@@ -219,19 +219,8 @@ private:
|
||||
unsigned _call_accel_interval;
|
||||
unsigned _call_mag_interval;
|
||||
|
||||
/*
|
||||
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;
|
||||
};
|
||||
RingBuffer<struct _accel_report> *_accel_reports;
|
||||
|
||||
struct _mag_report {
|
||||
struct mag_report r;
|
||||
};
|
||||
RingBuffer<struct _mag_report> *_mag_reports;
|
||||
RingBuffer *_accel_reports;
|
||||
RingBuffer *_mag_reports;
|
||||
|
||||
struct accel_scale _accel_scale;
|
||||
unsigned _accel_range_m_s2;
|
||||
@@ -499,7 +488,7 @@ LSM303D::init()
|
||||
}
|
||||
|
||||
/* allocate basic report buffers */
|
||||
_accel_reports = new RingBuffer<struct _accel_report>(2);
|
||||
_accel_reports = new RingBuffer(2, sizeof(accel_report));
|
||||
|
||||
if (_accel_reports == nullptr)
|
||||
goto out;
|
||||
@@ -509,7 +498,7 @@ LSM303D::init()
|
||||
memset(&zero_report, 0, sizeof(zero_report));
|
||||
_accel_topic = orb_advertise(ORB_ID(sensor_accel), &zero_report);
|
||||
|
||||
_mag_reports = new RingBuffer<struct _mag_report>(2);
|
||||
_mag_reports = new RingBuffer(2, sizeof(mag_report));
|
||||
|
||||
if (_mag_reports == nullptr)
|
||||
goto out;
|
||||
@@ -579,7 +568,7 @@ ssize_t
|
||||
LSM303D::read(struct file *filp, char *buffer, size_t buflen)
|
||||
{
|
||||
unsigned count = buflen / sizeof(struct accel_report);
|
||||
struct _accel_report *arb = reinterpret_cast<struct _accel_report *>(buffer);
|
||||
accel_report *arb = reinterpret_cast<accel_report *>(buffer);
|
||||
int ret = 0;
|
||||
|
||||
/* buffer must be large enough */
|
||||
@@ -592,7 +581,7 @@ LSM303D::read(struct file *filp, char *buffer, size_t buflen)
|
||||
* While there is space in the caller's buffer, and reports, copy them.
|
||||
*/
|
||||
while (count--) {
|
||||
if (_accel_reports->get(*arb)) {
|
||||
if (_accel_reports->get(arb)) {
|
||||
ret += sizeof(*arb);
|
||||
arb++;
|
||||
}
|
||||
@@ -606,7 +595,7 @@ LSM303D::read(struct file *filp, char *buffer, size_t buflen)
|
||||
measure();
|
||||
|
||||
/* measurement will have generated a report, copy it out */
|
||||
if (_accel_reports->get(*arb))
|
||||
if (_accel_reports->get(arb))
|
||||
ret = sizeof(*arb);
|
||||
|
||||
return ret;
|
||||
@@ -616,7 +605,7 @@ ssize_t
|
||||
LSM303D::mag_read(struct file *filp, char *buffer, size_t buflen)
|
||||
{
|
||||
unsigned count = buflen / sizeof(struct mag_report);
|
||||
struct _mag_report *mrb = reinterpret_cast<struct _mag_report *>(buffer);
|
||||
mag_report *mrb = reinterpret_cast<mag_report *>(buffer);
|
||||
int ret = 0;
|
||||
|
||||
/* buffer must be large enough */
|
||||
@@ -630,7 +619,7 @@ LSM303D::mag_read(struct file *filp, char *buffer, size_t buflen)
|
||||
* While there is space in the caller's buffer, and reports, copy them.
|
||||
*/
|
||||
while (count--) {
|
||||
if (_mag_reports->get(*mrb)) {
|
||||
if (_mag_reports->get(mrb)) {
|
||||
ret += sizeof(*mrb);
|
||||
mrb++;
|
||||
}
|
||||
@@ -645,7 +634,7 @@ LSM303D::mag_read(struct file *filp, char *buffer, size_t buflen)
|
||||
measure();
|
||||
|
||||
/* measurement will have generated a report, copy it out */
|
||||
if (_mag_reports->get(*mrb))
|
||||
if (_mag_reports->get(mrb))
|
||||
ret = sizeof(*mrb);
|
||||
|
||||
return ret;
|
||||
@@ -1237,7 +1226,7 @@ LSM303D::measure()
|
||||
} raw_accel_report;
|
||||
#pragma pack(pop)
|
||||
|
||||
struct _accel_report accel_report;
|
||||
accel_report accel_report;
|
||||
|
||||
/* start the performance counter */
|
||||
perf_begin(_accel_sample_perf);
|
||||
@@ -1262,30 +1251,30 @@ LSM303D::measure()
|
||||
*/
|
||||
|
||||
|
||||
accel_report.r.timestamp = hrt_absolute_time();
|
||||
accel_report.timestamp = hrt_absolute_time();
|
||||
|
||||
accel_report.r.x_raw = raw_accel_report.x;
|
||||
accel_report.r.y_raw = raw_accel_report.y;
|
||||
accel_report.r.z_raw = raw_accel_report.z;
|
||||
accel_report.x_raw = raw_accel_report.x;
|
||||
accel_report.y_raw = raw_accel_report.y;
|
||||
accel_report.z_raw = raw_accel_report.z;
|
||||
|
||||
float x_in_new = ((accel_report.r.x_raw * _accel_range_scale) - _accel_scale.x_offset) * _accel_scale.x_scale;
|
||||
float y_in_new = ((accel_report.r.y_raw * _accel_range_scale) - _accel_scale.y_offset) * _accel_scale.y_scale;
|
||||
float z_in_new = ((accel_report.r.z_raw * _accel_range_scale) - _accel_scale.z_offset) * _accel_scale.z_scale;
|
||||
float x_in_new = ((accel_report.x_raw * _accel_range_scale) - _accel_scale.x_offset) * _accel_scale.x_scale;
|
||||
float y_in_new = ((accel_report.y_raw * _accel_range_scale) - _accel_scale.y_offset) * _accel_scale.y_scale;
|
||||
float z_in_new = ((accel_report.z_raw * _accel_range_scale) - _accel_scale.z_offset) * _accel_scale.z_scale;
|
||||
|
||||
accel_report.r.x = _accel_filter_x.apply(x_in_new);
|
||||
accel_report.r.y = _accel_filter_y.apply(y_in_new);
|
||||
accel_report.r.z = _accel_filter_z.apply(z_in_new);
|
||||
accel_report.x = _accel_filter_x.apply(x_in_new);
|
||||
accel_report.y = _accel_filter_y.apply(y_in_new);
|
||||
accel_report.z = _accel_filter_z.apply(z_in_new);
|
||||
|
||||
accel_report.r.scaling = _accel_range_scale;
|
||||
accel_report.r.range_m_s2 = _accel_range_m_s2;
|
||||
accel_report.scaling = _accel_range_scale;
|
||||
accel_report.range_m_s2 = _accel_range_m_s2;
|
||||
|
||||
_accel_reports->force(accel_report);
|
||||
_accel_reports->force(&accel_report);
|
||||
|
||||
/* notify anyone waiting for data */
|
||||
poll_notify(POLLIN);
|
||||
|
||||
/* publish for subscribers */
|
||||
orb_publish(ORB_ID(sensor_accel), _accel_topic, &accel_report.r);
|
||||
orb_publish(ORB_ID(sensor_accel), _accel_topic, &accel_report);
|
||||
|
||||
_accel_read++;
|
||||
|
||||
@@ -1307,7 +1296,7 @@ LSM303D::mag_measure()
|
||||
} raw_mag_report;
|
||||
#pragma pack(pop)
|
||||
|
||||
struct _mag_report mag_report;
|
||||
mag_report mag_report;
|
||||
|
||||
/* start the performance counter */
|
||||
perf_begin(_mag_sample_perf);
|
||||
@@ -1332,25 +1321,25 @@ LSM303D::mag_measure()
|
||||
*/
|
||||
|
||||
|
||||
mag_report.r.timestamp = hrt_absolute_time();
|
||||
mag_report.timestamp = hrt_absolute_time();
|
||||
|
||||
mag_report.r.x_raw = raw_mag_report.x;
|
||||
mag_report.r.y_raw = raw_mag_report.y;
|
||||
mag_report.r.z_raw = raw_mag_report.z;
|
||||
mag_report.r.x = ((mag_report.r.x_raw * _mag_range_scale) - _mag_scale.x_offset) * _mag_scale.x_scale;
|
||||
mag_report.r.y = ((mag_report.r.y_raw * _mag_range_scale) - _mag_scale.y_offset) * _mag_scale.y_scale;
|
||||
mag_report.r.z = ((mag_report.r.z_raw * _mag_range_scale) - _mag_scale.z_offset) * _mag_scale.z_scale;
|
||||
mag_report.r.scaling = _mag_range_scale;
|
||||
mag_report.r.range_ga = (float)_mag_range_ga;
|
||||
mag_report.x_raw = raw_mag_report.x;
|
||||
mag_report.y_raw = raw_mag_report.y;
|
||||
mag_report.z_raw = raw_mag_report.z;
|
||||
mag_report.x = ((mag_report.x_raw * _mag_range_scale) - _mag_scale.x_offset) * _mag_scale.x_scale;
|
||||
mag_report.y = ((mag_report.y_raw * _mag_range_scale) - _mag_scale.y_offset) * _mag_scale.y_scale;
|
||||
mag_report.z = ((mag_report.z_raw * _mag_range_scale) - _mag_scale.z_offset) * _mag_scale.z_scale;
|
||||
mag_report.scaling = _mag_range_scale;
|
||||
mag_report.range_ga = (float)_mag_range_ga;
|
||||
|
||||
_mag_reports->force(mag_report);
|
||||
_mag_reports->force(&mag_report);
|
||||
|
||||
/* XXX please check this poll_notify, is it the right one? */
|
||||
/* notify anyone waiting for data */
|
||||
poll_notify(POLLIN);
|
||||
|
||||
/* publish for subscribers */
|
||||
orb_publish(ORB_ID(sensor_mag), _mag_topic, &mag_report.r);
|
||||
orb_publish(ORB_ID(sensor_mag), _mag_topic, &mag_report);
|
||||
|
||||
_mag_read++;
|
||||
|
||||
|
||||
@@ -120,7 +120,7 @@ private:
|
||||
float _min_distance;
|
||||
float _max_distance;
|
||||
work_s _work;
|
||||
RingBuffer<struct range_finder_report> *_reports;
|
||||
RingBuffer *_reports;
|
||||
bool _sensor_ok;
|
||||
int _measure_ticks;
|
||||
bool _collect_phase;
|
||||
@@ -226,7 +226,7 @@ MB12XX::init()
|
||||
goto out;
|
||||
|
||||
/* allocate basic report buffers */
|
||||
_reports = new RingBuffer<struct range_finder_report>(2);
|
||||
_reports = new RingBuffer(2, sizeof(range_finder_report));
|
||||
|
||||
if (_reports == nullptr)
|
||||
goto out;
|
||||
@@ -403,7 +403,7 @@ MB12XX::read(struct file *filp, char *buffer, size_t buflen)
|
||||
* we are careful to avoid racing with them.
|
||||
*/
|
||||
while (count--) {
|
||||
if (_reports->get(*rbuf)) {
|
||||
if (_reports->get(rbuf)) {
|
||||
ret += sizeof(*rbuf);
|
||||
rbuf++;
|
||||
}
|
||||
@@ -433,7 +433,7 @@ MB12XX::read(struct file *filp, char *buffer, size_t buflen)
|
||||
}
|
||||
|
||||
/* state machine will have generated a report, copy it out */
|
||||
if (_reports->get(*rbuf)) {
|
||||
if (_reports->get(rbuf)) {
|
||||
ret = sizeof(*rbuf);
|
||||
}
|
||||
|
||||
@@ -496,7 +496,7 @@ MB12XX::collect()
|
||||
/* publish it */
|
||||
orb_publish(ORB_ID(sensor_range_finder), _range_finder_topic, &report);
|
||||
|
||||
if (_reports->force(report)) {
|
||||
if (_reports->force(&report)) {
|
||||
perf_count(_buffer_overflows);
|
||||
}
|
||||
|
||||
|
||||
@@ -194,26 +194,14 @@ private:
|
||||
struct hrt_call _call;
|
||||
unsigned _call_interval;
|
||||
|
||||
/*
|
||||
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;
|
||||
RingBuffer *_accel_reports;
|
||||
|
||||
struct accel_scale _accel_scale;
|
||||
float _accel_range_scale;
|
||||
float _accel_range_m_s2;
|
||||
orb_advert_t _accel_topic;
|
||||
|
||||
struct _gyro_report {
|
||||
struct gyro_report r;
|
||||
};
|
||||
typedef RingBuffer<_gyro_report> GyroReportBuffer;
|
||||
GyroReportBuffer *_gyro_reports;
|
||||
RingBuffer *_gyro_reports;
|
||||
|
||||
struct gyro_scale _gyro_scale;
|
||||
float _gyro_range_scale;
|
||||
@@ -441,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;
|
||||
|
||||
@@ -475,16 +463,16 @@ MPU6000::init()
|
||||
if (gyro_ret != OK) {
|
||||
_gyro_topic = -1;
|
||||
} else {
|
||||
_gyro_report gr;
|
||||
_gyro_reports->get(gr);
|
||||
gyro_report gr;
|
||||
_gyro_reports->get(&gr);
|
||||
|
||||
_gyro_topic = orb_advertise(ORB_ID(sensor_gyro), &gr.r);
|
||||
_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.r);
|
||||
accel_report ar;
|
||||
_accel_reports->get(&ar);
|
||||
_accel_topic = orb_advertise(ORB_ID(sensor_accel), &ar);
|
||||
|
||||
out:
|
||||
return ret;
|
||||
@@ -665,10 +653,10 @@ MPU6000::read(struct file *filp, char *buffer, size_t buflen)
|
||||
return -EAGAIN;
|
||||
|
||||
/* 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;
|
||||
while (count--) {
|
||||
if (!_accel_reports->get(*arp))
|
||||
if (!_accel_reports->get(arp))
|
||||
break;
|
||||
transferred++;
|
||||
arp++;
|
||||
@@ -759,10 +747,10 @@ MPU6000::gyro_read(struct file *filp, char *buffer, size_t buflen)
|
||||
return -EAGAIN;
|
||||
|
||||
/* copy reports out of our buffer to the caller */
|
||||
_gyro_report *grp = reinterpret_cast<_gyro_report *>(buffer);
|
||||
gyro_report *grp = reinterpret_cast<gyro_report *>(buffer);
|
||||
int transferred = 0;
|
||||
while (count--) {
|
||||
if (!_gyro_reports->get(*grp))
|
||||
if (!_gyro_reports->get(grp))
|
||||
break;
|
||||
transferred++;
|
||||
grp++;
|
||||
@@ -1191,13 +1179,13 @@ MPU6000::measure()
|
||||
/*
|
||||
* Report buffers.
|
||||
*/
|
||||
_accel_report arb;
|
||||
_gyro_report grb;
|
||||
accel_report arb;
|
||||
gyro_report grb;
|
||||
|
||||
/*
|
||||
* Adjust and scale results to m/s^2.
|
||||
*/
|
||||
grb.r.timestamp = arb.r.timestamp = hrt_absolute_time();
|
||||
grb.timestamp = arb.timestamp = hrt_absolute_time();
|
||||
|
||||
|
||||
/*
|
||||
@@ -1218,53 +1206,53 @@ MPU6000::measure()
|
||||
|
||||
/* NOTE: Axes have been swapped to match the board a few lines above. */
|
||||
|
||||
arb.r.x_raw = report.accel_x;
|
||||
arb.r.y_raw = report.accel_y;
|
||||
arb.r.z_raw = report.accel_z;
|
||||
arb.x_raw = report.accel_x;
|
||||
arb.y_raw = report.accel_y;
|
||||
arb.z_raw = report.accel_z;
|
||||
|
||||
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.r.x = _accel_filter_x.apply(x_in_new);
|
||||
arb.r.y = _accel_filter_y.apply(y_in_new);
|
||||
arb.r.z = _accel_filter_z.apply(z_in_new);
|
||||
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);
|
||||
|
||||
arb.r.scaling = _accel_range_scale;
|
||||
arb.r.range_m_s2 = _accel_range_m_s2;
|
||||
arb.scaling = _accel_range_scale;
|
||||
arb.range_m_s2 = _accel_range_m_s2;
|
||||
|
||||
arb.r.temperature_raw = report.temp;
|
||||
arb.r.temperature = (report.temp) / 361.0f + 35.0f;
|
||||
arb.temperature_raw = report.temp;
|
||||
arb.temperature = (report.temp) / 361.0f + 35.0f;
|
||||
|
||||
grb.r.x_raw = report.gyro_x;
|
||||
grb.r.y_raw = report.gyro_y;
|
||||
grb.r.z_raw = report.gyro_z;
|
||||
grb.x_raw = report.gyro_x;
|
||||
grb.y_raw = report.gyro_y;
|
||||
grb.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 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.r.x = _gyro_filter_x.apply(x_gyro_in_new);
|
||||
grb.r.y = _gyro_filter_y.apply(y_gyro_in_new);
|
||||
grb.r.z = _gyro_filter_z.apply(z_gyro_in_new);
|
||||
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.r.scaling = _gyro_range_scale;
|
||||
grb.r.range_rad_s = _gyro_range_rad_s;
|
||||
grb.scaling = _gyro_range_scale;
|
||||
grb.range_rad_s = _gyro_range_rad_s;
|
||||
|
||||
grb.r.temperature_raw = report.temp;
|
||||
grb.r.temperature = (report.temp) / 361.0f + 35.0f;
|
||||
grb.temperature_raw = report.temp;
|
||||
grb.temperature = (report.temp) / 361.0f + 35.0f;
|
||||
|
||||
_accel_reports->force(arb);
|
||||
_gyro_reports->force(grb);
|
||||
_accel_reports->force(&arb);
|
||||
_gyro_reports->force(&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, &arb.r);
|
||||
orb_publish(ORB_ID(sensor_accel), _accel_topic, &arb);
|
||||
if (_gyro_topic != -1) {
|
||||
orb_publish(ORB_ID(sensor_gyro), _gyro_topic, &grb.r);
|
||||
orb_publish(ORB_ID(sensor_gyro), _gyro_topic, &grb);
|
||||
}
|
||||
|
||||
/* stop measuring */
|
||||
|
||||
@@ -115,7 +115,7 @@ protected:
|
||||
struct work_s _work;
|
||||
unsigned _measure_ticks;
|
||||
|
||||
RingBuffer<struct baro_report> *_reports;
|
||||
RingBuffer *_reports;
|
||||
|
||||
bool _collect_phase;
|
||||
unsigned _measure_phase;
|
||||
@@ -241,7 +241,7 @@ MS5611::init()
|
||||
}
|
||||
|
||||
/* allocate basic report buffers */
|
||||
_reports = new RingBuffer<struct baro_report>(2);
|
||||
_reports = new RingBuffer(2, sizeof(baro_report));
|
||||
|
||||
if (_reports == nullptr) {
|
||||
debug("can't get memory for reports");
|
||||
@@ -285,7 +285,7 @@ MS5611::read(struct file *filp, char *buffer, size_t buflen)
|
||||
* we are careful to avoid racing with them.
|
||||
*/
|
||||
while (count--) {
|
||||
if (_reports->get(*brp)) {
|
||||
if (_reports->get(brp)) {
|
||||
ret += sizeof(*brp);
|
||||
brp++;
|
||||
}
|
||||
@@ -327,7 +327,7 @@ MS5611::read(struct file *filp, char *buffer, size_t buflen)
|
||||
}
|
||||
|
||||
/* state machine will have generated a report, copy it out */
|
||||
if (_reports->get(*brp))
|
||||
if (_reports->get(brp))
|
||||
ret = sizeof(*brp);
|
||||
|
||||
} while (0);
|
||||
@@ -664,7 +664,7 @@ MS5611::collect()
|
||||
/* publish it */
|
||||
orb_publish(ORB_ID(sensor_baro), _baro_topic, &report);
|
||||
|
||||
if (_reports->force(report)) {
|
||||
if (_reports->force(&report)) {
|
||||
perf_count(_buffer_overflows);
|
||||
}
|
||||
|
||||
|
||||
Reference in New Issue
Block a user