diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt index 5d0401a96919..ab3d0dbff2d5 100644 --- a/msg/CMakeLists.txt +++ b/msg/CMakeLists.txt @@ -40,6 +40,8 @@ set(msg_files airspeed.msg att_pos_mocap.msg battery_status.msg + calibration_accel.msg + calibration_gyro.msg camera_capture.msg camera_trigger.msg collision_report.msg diff --git a/msg/calibration_accel.msg b/msg/calibration_accel.msg new file mode 100644 index 000000000000..78ca2df21d79 --- /dev/null +++ b/msg/calibration_accel.msg @@ -0,0 +1,10 @@ + +# scaling factors; Vout = (Vin * Vscale) + Voffset + +float32 x_offset +float32 y_offset +float32 z_offset + +float32 x_scale +float32 y_scale +float32 z_scale diff --git a/msg/calibration_gyro.msg b/msg/calibration_gyro.msg new file mode 100644 index 000000000000..78ca2df21d79 --- /dev/null +++ b/msg/calibration_gyro.msg @@ -0,0 +1,10 @@ + +# scaling factors; Vout = (Vin * Vscale) + Voffset + +float32 x_offset +float32 y_offset +float32 z_offset + +float32 x_scale +float32 y_scale +float32 z_scale diff --git a/src/drivers/drv_accel.h b/src/drivers/drv_accel.h index 43e566093a68..4b208f69c2d1 100644 --- a/src/drivers/drv_accel.h +++ b/src/drivers/drv_accel.h @@ -51,18 +51,6 @@ #define ACCEL1_DEVICE_PATH "/dev/accel1" #define ACCEL2_DEVICE_PATH "/dev/accel2" -#include -#define accel_report sensor_accel_s - -/** accel scaling factors; Vout = Vscale * (Vin + Voffset) */ -struct accel_calibration_s { - float x_offset; - float x_scale; - float y_offset; - float y_scale; - float z_offset; - float z_scale; -}; /* * ioctl() definitions * diff --git a/src/drivers/drv_gyro.h b/src/drivers/drv_gyro.h index 92331baab573..e5fb33095a70 100644 --- a/src/drivers/drv_gyro.h +++ b/src/drivers/drv_gyro.h @@ -51,19 +51,6 @@ #define GYRO1_DEVICE_PATH "/dev/gyro1" #define GYRO2_DEVICE_PATH "/dev/gyro2" -#include -#define gyro_report sensor_gyro_s - -/** gyro scaling factors; Vout = (Vin * Vscale) + Voffset */ -struct gyro_calibration_s { - float x_offset; - float x_scale; - float y_offset; - float y_scale; - float z_offset; - float z_scale; -}; - /* * ioctl() definitions */ diff --git a/src/drivers/imu/adis16448/adis16448.cpp b/src/drivers/imu/adis16448/adis16448.cpp index 2f0f5962dfff..e43e53b93753 100644 --- a/src/drivers/imu/adis16448/adis16448.cpp +++ b/src/drivers/imu/adis16448/adis16448.cpp @@ -81,6 +81,11 @@ #include #include +#include +#include +#include +#include + #define DIR_READ 0x00 #define DIR_WRITE 0x80 @@ -227,15 +232,15 @@ class ADIS16448 : public device::SPI struct hrt_call _call; unsigned _call_interval; - ringbuffer::RingBuffer *_gyro_reports; + ringbuffer::RingBuffer *_sensor_gyro_ss; - struct gyro_calibration_s _gyro_scale; + calibration_gyro_s _gyro_scale; float _gyro_range_scale; float _gyro_range_rad_s; - ringbuffer::RingBuffer *_accel_reports; + ringbuffer::RingBuffer *_sensor_accel_ss; - struct accel_calibration_s _accel_scale; + calibration_accel_s _accel_scale; float _accel_range_scale; float _accel_range_m_s2; orb_advert_t _accel_topic; @@ -490,11 +495,11 @@ ADIS16448::ADIS16448(int bus, const char *path_accel, const char *path_gyro, con _product(0), _call{}, _call_interval(0), - _gyro_reports(nullptr), + _sensor_gyro_ss(nullptr), _gyro_scale{}, _gyro_range_scale(0.0f), _gyro_range_rad_s(0.0f), - _accel_reports(nullptr), + _sensor_accel_ss(nullptr), _accel_scale{}, _accel_range_scale(0.0f), _accel_range_m_s2(0.0f), @@ -572,12 +577,12 @@ ADIS16448::~ADIS16448() delete _mag; /* free any existing reports */ - if (_gyro_reports != nullptr) { - delete _gyro_reports; + if (_sensor_gyro_ss != nullptr) { + delete _sensor_gyro_ss; } - if (_accel_reports != nullptr) { - delete _accel_reports; + if (_sensor_accel_ss != nullptr) { + delete _sensor_accel_ss; } if (_mag_reports != nullptr) { @@ -611,15 +616,15 @@ ADIS16448::init() } /* allocate basic report buffers */ - _gyro_reports = new ringbuffer::RingBuffer(2, sizeof(gyro_report)); + _sensor_gyro_ss = new ringbuffer::RingBuffer(2, sizeof(sensor_gyro_s)); - if (_gyro_reports == nullptr) { + if (_sensor_gyro_ss == nullptr) { goto out; } - _accel_reports = new ringbuffer::RingBuffer(2, sizeof(accel_report)); + _sensor_accel_ss = new ringbuffer::RingBuffer(2, sizeof(sensor_accel_s)); - if (_accel_reports == nullptr) { + if (_sensor_accel_ss == nullptr) { goto out; } @@ -679,8 +684,8 @@ ADIS16448::init() measure(); /* advertise sensor topic, measure manually to initialize valid report */ - struct accel_report arp; - _accel_reports->get(&arp); + sensor_accel_s arp; + _sensor_accel_ss->get(&arp); /* measurement will have generated a report, publish */ _accel_topic = orb_advertise_multi(ORB_ID(sensor_accel), &arp, @@ -690,9 +695,9 @@ ADIS16448::init() warnx("ADVERT FAIL"); } - struct gyro_report grp; + sensor_gyro_s grp; - _gyro_reports->get(&grp); + _sensor_gyro_ss->get(&grp); _gyro->_gyro_topic = orb_advertise_multi(ORB_ID(sensor_gyro), &grp, &_gyro->_gyro_orb_class_instance, ORB_PRIO_MAX); @@ -855,7 +860,7 @@ ADIS16448::_set_gyro_dyn_range(uint16_t desired_gyro_dyn_range) ssize_t ADIS16448::read(struct file *filp, char *buffer, size_t buflen) { - unsigned count = buflen / sizeof(accel_report); + unsigned count = buflen / sizeof(sensor_accel_s); /* buffer must be large enough */ if (count < 1) { @@ -864,23 +869,23 @@ ADIS16448::read(struct file *filp, char *buffer, size_t buflen) /* if automatic measurement is not enabled, get a fresh measurement into the buffer */ if (_call_interval == 0) { - _accel_reports->flush(); + _sensor_accel_ss->flush(); measure(); } /* if no data, error (we could block here) */ - if (_accel_reports->empty()) { + if (_sensor_accel_ss->empty()) { return -EAGAIN; } perf_count(_accel_reads); /* copy reports out of our buffer to the caller */ - accel_report *arp = reinterpret_cast(buffer); + sensor_accel_s *arp = reinterpret_cast(buffer); int transferred = 0; while (count--) { - if (!_accel_reports->get(arp)) { + if (!_sensor_accel_ss->get(arp)) { break; } @@ -889,7 +894,7 @@ ADIS16448::read(struct file *filp, char *buffer, size_t buflen) } /* return the number of bytes transferred */ - return (transferred * sizeof(accel_report)); + return (transferred * sizeof(sensor_accel_s)); } int @@ -926,7 +931,7 @@ ADIS16448::gyro_self_test() ssize_t ADIS16448::gyro_read(struct file *filp, char *buffer, size_t buflen) { - unsigned count = buflen / sizeof(gyro_report); + unsigned count = buflen / sizeof(sensor_gyro_s); /* buffer must be large enough */ if (count < 1) { @@ -935,23 +940,23 @@ ADIS16448::gyro_read(struct file *filp, char *buffer, size_t buflen) /* if automatic measurement is not enabled, get a fresh measurement into the buffer */ if (_call_interval == 0) { - _gyro_reports->flush(); + _sensor_gyro_ss->flush(); measure(); } /* if no data, error (we could block here) */ - if (_gyro_reports->empty()) { + if (_sensor_gyro_ss->empty()) { return -EAGAIN; } perf_count(_gyro_reads); /* copy reports out of our buffer to the caller */ - gyro_report *grp = reinterpret_cast(buffer); + sensor_gyro_s *grp = reinterpret_cast(buffer); int transferred = 0; while (count--) { - if (!_gyro_reports->get(grp)) { + if (!_sensor_gyro_ss->get(grp)) { break; } @@ -960,7 +965,7 @@ ADIS16448::gyro_read(struct file *filp, char *buffer, size_t buflen) } /* return the number of bytes transferred */ - return (transferred * sizeof(gyro_report)); + return (transferred * sizeof(sensor_gyro_s)); } ssize_t @@ -1095,7 +1100,7 @@ ADIS16448::ioctl(struct file *filp, int cmd, unsigned long arg) irqstate_t flags = px4_enter_critical_section(); - if (!_accel_reports->resize(arg)) { + if (!_sensor_accel_ss->resize(arg)) { px4_leave_critical_section(flags); return -ENOMEM; } @@ -1114,7 +1119,7 @@ ADIS16448::ioctl(struct file *filp, int cmd, unsigned long arg) case ACCELIOCSSCALE: { /* copy scale, but only if off by a few percent */ - struct accel_calibration_s *s = (struct accel_calibration_s *) arg; + calibration_accel_s *s = (calibration_accel_s *) arg; float sum = s->x_scale + s->y_scale + s->z_scale; if (sum > 2.0f && sum < 4.0f) { @@ -1128,7 +1133,7 @@ ADIS16448::ioctl(struct file *filp, int cmd, unsigned long arg) case ACCELIOCGSCALE: /* copy scale out */ - memcpy((struct accel_calibration_s *) arg, &_accel_scale, sizeof(_accel_scale)); + memcpy((calibration_accel_s *) arg, &_accel_scale, sizeof(_accel_scale)); return OK; case ACCELIOCSRANGE: @@ -1168,7 +1173,7 @@ ADIS16448::gyro_ioctl(struct file *filp, int cmd, unsigned long arg) irqstate_t flags = px4_enter_critical_section(); - if (!_gyro_reports->resize(arg)) { + if (!_sensor_gyro_ss->resize(arg)) { px4_leave_critical_section(flags); return -ENOMEM; } @@ -1187,12 +1192,12 @@ ADIS16448::gyro_ioctl(struct file *filp, int cmd, unsigned long arg) case GYROIOCSSCALE: /* copy scale in */ - memcpy(&_gyro_scale, (struct gyro_calibration_s *) arg, sizeof(_gyro_scale)); + memcpy(&_gyro_scale, (calibration_gyro_s *) arg, sizeof(_gyro_scale)); return OK; case GYROIOCGSCALE: /* copy scale out */ - memcpy((struct gyro_calibration_s *) arg, &_gyro_scale, sizeof(_gyro_scale)); + memcpy((calibration_gyro_s *) arg, &_gyro_scale, sizeof(_gyro_scale)); return OK; case GYROIOCSRANGE: @@ -1343,8 +1348,8 @@ ADIS16448::start() _call_interval = last_call_interval; /* discard any stale data in the buffers */ - _gyro_reports->flush(); - _accel_reports->flush(); + _sensor_gyro_ss->flush(); + _sensor_accel_ss->flush(); _mag_reports->flush(); /* start polling at the specified rate */ @@ -1423,8 +1428,8 @@ ADIS16448::measure() /* * Report buffers. */ - accel_report arb; - gyro_report grb; + sensor_accel_s arb; + sensor_gyro_s grb; mag_report mrb; grb.timestamp = arb.timestamp = mrb.timestamp = hrt_absolute_time(); @@ -1548,8 +1553,8 @@ ADIS16448::measure() grb.device_id = _gyro->_device_id.devid; mrb.device_id = _mag->_device_id.devid; - _gyro_reports ->force(&grb); - _accel_reports->force(&arb); + _sensor_gyro_ss ->force(&grb); + _sensor_accel_ss->force(&arb); _mag_reports ->force(&mrb); /* notify anyone waiting for data */ @@ -1622,8 +1627,8 @@ ADIS16448::print_info() perf_print_counter(_gyro_reads); perf_print_counter(_mag_reads); perf_print_counter(_bad_transfers); - _accel_reports->print_info("accel queue"); - _gyro_reports->print_info("gyro queue"); + _sensor_accel_ss->print_info("accel queue"); + _sensor_gyro_ss->print_info("gyro queue"); _mag_reports->print_info("mag queue"); printf("DEVICE ID:\nACCEL:\t%d\nGYRO:\t%d\nMAG:\t%d\n", _device_id.devid, _gyro->_device_id.devid, @@ -1826,8 +1831,8 @@ start(enum Rotation rotation) void test() { - accel_report a_report; - gyro_report g_report; + sensor_accel_s a_report; + sensor_gyro_s g_report; mag_report m_report; ssize_t sz; diff --git a/src/drivers/imu/adis16477/ADIS16477.cpp b/src/drivers/imu/adis16477/ADIS16477.cpp index d5c83b33c01e..968e8f3de957 100644 --- a/src/drivers/imu/adis16477/ADIS16477.cpp +++ b/src/drivers/imu/adis16477/ADIS16477.cpp @@ -165,7 +165,7 @@ ADIS16477::init() measure(); /* advertise sensor topic, measure manually to initialize valid report */ - accel_report arp = {}; + sensor_accel_s arp = {}; /* measurement will have generated a report, publish */ _accel_topic = orb_advertise_multi(ORB_ID(sensor_accel), &arp, &_accel_orb_class_instance, ORB_PRIO_MAX); @@ -174,7 +174,7 @@ ADIS16477::init() PX4_ERR("ADVERT FAIL"); } - gyro_report grp = {}; + sensor_gyro_s grp = {}; _gyro->_gyro_topic = orb_advertise_multi(ORB_ID(sensor_gyro), &grp, &_gyro->_gyro_orb_class_instance, ORB_PRIO_MAX); if (_gyro->_gyro_topic == nullptr) { @@ -369,7 +369,7 @@ ADIS16477::ioctl(struct file *filp, int cmd, unsigned long arg) case ACCELIOCSSCALE: { /* copy scale, but only if off by a few percent */ - struct accel_calibration_s *s = (struct accel_calibration_s *) arg; + calibration_accel_s *s = (calibration_accel_s *) arg; float sum = s->x_scale + s->y_scale + s->z_scale; if (sum > 2.0f && sum < 4.0f) { @@ -383,7 +383,7 @@ ADIS16477::ioctl(struct file *filp, int cmd, unsigned long arg) case ACCELIOCGSCALE: /* copy scale out */ - memcpy((struct accel_calibration_s *) arg, &_accel_scale, sizeof(_accel_scale)); + memcpy((calibration_accel_s *) arg, &_accel_scale, sizeof(_accel_scale)); return OK; case ACCELIOCSRANGE: @@ -418,12 +418,12 @@ ADIS16477::gyro_ioctl(struct file *filp, int cmd, unsigned long arg) case GYROIOCSSCALE: /* copy scale in */ - memcpy(&_gyro_scale, (struct gyro_calibration_s *) arg, sizeof(_gyro_scale)); + memcpy(&_gyro_scale, (calibration_gyro_s *) arg, sizeof(_gyro_scale)); return OK; case GYROIOCGSCALE: /* copy scale out */ - memcpy((struct gyro_calibration_s *) arg, &_gyro_scale, sizeof(_gyro_scale)); + memcpy((calibration_gyro_s *) arg, &_gyro_scale, sizeof(_gyro_scale)); return OK; case GYROIOCSRANGE: @@ -527,7 +527,7 @@ ADIS16477::publish_accel(const ADISReport &report) return false; } - accel_report arb = {}; + sensor_accel_s arb = {}; arb.timestamp = hrt_absolute_time(); arb.error_count = perf_event_count(_bad_transfers); @@ -586,7 +586,7 @@ ADIS16477::publish_gyro(const ADISReport &report) return false; } - gyro_report grb = {}; + sensor_gyro_s grb = {}; grb.timestamp = hrt_absolute_time(); grb.error_count = perf_event_count(_bad_transfers); diff --git a/src/drivers/imu/adis16477/ADIS16477.hpp b/src/drivers/imu/adis16477/ADIS16477.hpp index 3c3aeb5a5ef0..c53df3317303 100644 --- a/src/drivers/imu/adis16477/ADIS16477.hpp +++ b/src/drivers/imu/adis16477/ADIS16477.hpp @@ -50,6 +50,11 @@ #include #include +#include +#include +#include +#include + #define ADIS16477_GYRO_DEFAULT_RATE 250 #define ADIS16477_GYRO_DEFAULT_DRIVER_FILTER_FREQ 30 @@ -88,13 +93,13 @@ class ADIS16477 : public device::SPI struct hrt_call _call {}; unsigned _call_interval{0}; - struct gyro_calibration_s _gyro_scale {}; + calibration_gyro_s _gyro_scale {}; // gyro 0.025 °/sec/LSB float _gyro_range_scale{0.025f}; float _gyro_range_rad_s{math::radians(500.0f)}; - struct accel_calibration_s _accel_scale {}; + calibration_accel_s _accel_scale {}; // accel 1.25 mg/LSB float _accel_range_scale{1.25f * CONSTANTS_ONE_G / 1000.0f}; diff --git a/src/drivers/imu/adis16477/ADIS16477_gyro.hpp b/src/drivers/imu/adis16477/ADIS16477_gyro.hpp index d1fd071ab654..f7100d5fb1a2 100644 --- a/src/drivers/imu/adis16477/ADIS16477_gyro.hpp +++ b/src/drivers/imu/adis16477/ADIS16477_gyro.hpp @@ -39,6 +39,9 @@ #include #include +#include +#include + /** * Helper class implementing the gyro driver node. */ diff --git a/src/drivers/imu/adis16477/ADIS16477_main.cpp b/src/drivers/imu/adis16477/ADIS16477_main.cpp index fbcbcf159b93..2b3504810f17 100644 --- a/src/drivers/imu/adis16477/ADIS16477_main.cpp +++ b/src/drivers/imu/adis16477/ADIS16477_main.cpp @@ -113,8 +113,8 @@ start(enum Rotation rotation) void test() { - accel_report a_report; - gyro_report g_report; + sensor_accel_s a_report; + sensor_gyro_s g_report; ssize_t sz; diff --git a/src/drivers/imu/bma180/bma180.cpp b/src/drivers/imu/bma180/bma180.cpp index 58431f46cded..6273ea12a598 100644 --- a/src/drivers/imu/bma180/bma180.cpp +++ b/src/drivers/imu/bma180/bma180.cpp @@ -66,6 +66,10 @@ #include #include #include +#include +#include +#include +#include #define ACCEL_DEVICE_PATH "/dev/bma180" @@ -147,7 +151,7 @@ class BMA180 : public device::SPI ringbuffer::RingBuffer *_reports; - struct accel_calibration_s _accel_scale; + calibration_accel_s _accel_scale; float _accel_range_scale; float _accel_range_m_s2; orb_advert_t _accel_topic; @@ -278,7 +282,7 @@ BMA180::init() } /* allocate basic report buffers */ - _reports = new ringbuffer::RingBuffer(2, sizeof(accel_report)); + _reports = new ringbuffer::RingBuffer(2, sizeof(sensor_accel_s)); if (_reports == nullptr) { goto out; @@ -325,7 +329,7 @@ BMA180::init() measure(); if (_class_instance == CLASS_DEVICE_PRIMARY) { - struct accel_report arp; + sensor_accel_s arp; _reports->get(&arp); /* measurement will have generated a report, publish */ @@ -352,8 +356,8 @@ BMA180::probe() 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(buffer); + unsigned count = buflen / sizeof(sensor_accel_s); + sensor_accel_s *arp = reinterpret_cast(buffer); int ret = 0; /* buffer must be large enough */ @@ -484,12 +488,12 @@ BMA180::ioctl(struct file *filp, int cmd, unsigned long arg) case ACCELIOCSSCALE: /* copy scale in */ - memcpy(&_accel_scale, (struct accel_calibration_s *) arg, sizeof(_accel_scale)); + memcpy(&_accel_scale, (calibration_accel_s *) arg, sizeof(_accel_scale)); return OK; case ACCELIOCGSCALE: /* copy scale out */ - memcpy((struct accel_calibration_s *) arg, &_accel_scale, sizeof(_accel_scale)); + memcpy((calibration_accel_s *) arg, &_accel_scale, sizeof(_accel_scale)); return OK; case ACCELIOCSRANGE: @@ -681,7 +685,7 @@ BMA180::measure() // } raw_report; // #pragma pack(pop) - struct accel_report report; + sensor_accel_s report; /* start the performance counter */ perf_begin(_sample_perf); @@ -817,7 +821,7 @@ void test() { int fd = -1; - struct accel_report a_report; + sensor_accel_s a_report; ssize_t sz; /* get the driver */ diff --git a/src/drivers/imu/bmi055/bmi055.hpp b/src/drivers/imu/bmi055/bmi055.hpp index 884cf03efe2d..4bd9eccbe6c2 100644 --- a/src/drivers/imu/bmi055/bmi055.hpp +++ b/src/drivers/imu/bmi055/bmi055.hpp @@ -36,6 +36,10 @@ #include #include #include +#include +#include +#include +#include #define DIR_READ 0x80 #define DIR_WRITE 0x00 @@ -336,9 +340,9 @@ class BMI055_accel : public BMI055 private: - ringbuffer::RingBuffer *_accel_reports; + ringbuffer::RingBuffer *_sensor_accel_ss; - struct accel_calibration_s _accel_scale; + calibration_accel_s _accel_scale; float _accel_range_scale; float _accel_range_m_s2; orb_advert_t _accel_topic; @@ -489,9 +493,9 @@ class BMI055_gyro : public BMI055 virtual int probe(); private: - ringbuffer::RingBuffer *_gyro_reports; + ringbuffer::RingBuffer *_sensor_gyro_ss; - struct gyro_calibration_s _gyro_scale; + calibration_gyro_s _gyro_scale; float _gyro_range_scale; float _gyro_range_rad_s; diff --git a/src/drivers/imu/bmi055/bmi055_accel.cpp b/src/drivers/imu/bmi055/bmi055_accel.cpp index 690c6957077d..d36ebc67cc0b 100644 --- a/src/drivers/imu/bmi055/bmi055_accel.cpp +++ b/src/drivers/imu/bmi055/bmi055_accel.cpp @@ -16,7 +16,7 @@ const uint8_t BMI055_accel::_checked_registers[BMI055_ACCEL_NUM_CHECKED_REGISTER BMI055_accel::BMI055_accel(int bus, const char *path_accel, uint32_t device, enum Rotation rotation) : BMI055("BMI055_ACCEL", path_accel, bus, device, SPIDEV_MODE3, BMI055_BUS_SPEED, rotation), - _accel_reports(nullptr), + _sensor_accel_ss(nullptr), _accel_scale{}, _accel_range_scale(0.0f), _accel_range_m_s2(0.0f), @@ -55,8 +55,8 @@ BMI055_accel::~BMI055_accel() stop(); /* free any existing reports */ - if (_accel_reports != nullptr) { - delete _accel_reports; + if (_sensor_accel_ss != nullptr) { + delete _sensor_accel_ss; } @@ -84,9 +84,9 @@ BMI055_accel::init() } /* allocate basic report buffers */ - _accel_reports = new ringbuffer::RingBuffer(2, sizeof(accel_report)); + _sensor_accel_ss = new ringbuffer::RingBuffer(2, sizeof(sensor_accel_s)); - if (_accel_reports == nullptr) { + if (_sensor_accel_ss == nullptr) { goto out; } @@ -108,8 +108,8 @@ BMI055_accel::init() measure(); /* advertise sensor topic, measure manually to initialize valid report */ - struct accel_report arp; - _accel_reports->get(&arp); + sensor_accel_s arp; + _sensor_accel_ss->get(&arp); /* measurement will have generated a report, publish */ _accel_topic = orb_advertise_multi(ORB_ID(sensor_accel), &arp, @@ -239,7 +239,7 @@ BMI055_accel::accel_set_sample_rate(float frequency) ssize_t BMI055_accel::read(struct file *filp, char *buffer, size_t buflen) { - unsigned count = buflen / sizeof(accel_report); + unsigned count = buflen / sizeof(sensor_accel_s); /* buffer must be large enough */ if (count < 1) { @@ -248,23 +248,23 @@ BMI055_accel::read(struct file *filp, char *buffer, size_t buflen) /* if automatic measurement is not enabled, get a fresh measurement into the buffer */ if (_call_interval == 0) { - _accel_reports->flush(); + _sensor_accel_ss->flush(); measure(); } /* if no data, error (we could block here) */ - if (_accel_reports->empty()) { + if (_sensor_accel_ss->empty()) { return -EAGAIN; } perf_count(_accel_reads); /* copy reports out of our buffer to the caller */ - accel_report *arp = reinterpret_cast(buffer); + sensor_accel_s *arp = reinterpret_cast(buffer); int transferred = 0; while (count--) { - if (!_accel_reports->get(arp)) { + if (!_sensor_accel_ss->get(arp)) { break; } @@ -273,7 +273,7 @@ BMI055_accel::read(struct file *filp, char *buffer, size_t buflen) } /* return the number of bytes transferred */ - return (transferred * sizeof(accel_report)); + return (transferred * sizeof(sensor_accel_s)); } int @@ -424,7 +424,7 @@ BMI055_accel::ioctl(struct file *filp, int cmd, unsigned long arg) irqstate_t flags = px4_enter_critical_section(); - if (!_accel_reports->resize(arg)) { + if (!_sensor_accel_ss->resize(arg)) { px4_leave_critical_section(flags); return -ENOMEM; } @@ -442,7 +442,7 @@ BMI055_accel::ioctl(struct file *filp, int cmd, unsigned long arg) case ACCELIOCSSCALE: { /* copy scale, but only if off by a few percent */ - struct accel_calibration_s *s = (struct accel_calibration_s *) arg; + calibration_accel_s *s = (calibration_accel_s *) arg; float sum = s->x_scale + s->y_scale + s->z_scale; if (sum > 2.0f && sum < 4.0f) { @@ -456,7 +456,7 @@ BMI055_accel::ioctl(struct file *filp, int cmd, unsigned long arg) case ACCELIOCGSCALE: /* copy scale out */ - memcpy((struct accel_calibration_s *) arg, &_accel_scale, sizeof(_accel_scale)); + memcpy((calibration_accel_s *) arg, &_accel_scale, sizeof(_accel_scale)); return OK; case ACCELIOCSRANGE: @@ -551,7 +551,7 @@ BMI055_accel::start() stop(); /* discard any stale data in the buffers */ - _accel_reports->flush(); + _sensor_accel_ss->flush(); /* start polling at the specified rate */ hrt_call_every(&_call, @@ -715,7 +715,7 @@ BMI055_accel::measure() /* * Report buffers. */ - accel_report arb; + sensor_accel_s arb; arb.timestamp = hrt_absolute_time(); @@ -775,7 +775,7 @@ BMI055_accel::measure() arb.temperature = _last_temperature; arb.device_id = _device_id.devid; - _accel_reports->force(&arb); + _sensor_accel_ss->force(&arb); /* notify anyone waiting for data */ if (accel_notify) { @@ -803,7 +803,7 @@ BMI055_accel::print_info() perf_print_counter(_good_transfers); perf_print_counter(_reset_retries); perf_print_counter(_duplicates); - _accel_reports->print_info("accel queue"); + _sensor_accel_ss->print_info("accel queue"); ::printf("checked_next: %u\n", _checked_next); for (uint8_t i = 0; i < BMI055_ACCEL_NUM_CHECKED_REGISTERS; i++) { diff --git a/src/drivers/imu/bmi055/bmi055_gyro.cpp b/src/drivers/imu/bmi055/bmi055_gyro.cpp index 177494331799..606a7bb52061 100644 --- a/src/drivers/imu/bmi055/bmi055_gyro.cpp +++ b/src/drivers/imu/bmi055/bmi055_gyro.cpp @@ -19,7 +19,7 @@ const uint8_t BMI055_gyro::_checked_registers[BMI055_GYRO_NUM_CHECKED_REGISTERS] BMI055_gyro::BMI055_gyro(int bus, const char *path_gyro, uint32_t device, enum Rotation rotation) : BMI055("BMI055_GYRO", path_gyro, bus, device, SPIDEV_MODE3, BMI055_BUS_SPEED, rotation), - _gyro_reports(nullptr), + _sensor_gyro_ss(nullptr), _gyro_scale{}, _gyro_range_scale(0.0f), _gyro_range_rad_s(0.0f), @@ -57,8 +57,8 @@ BMI055_gyro::~BMI055_gyro() stop(); /* free any existing reports */ - if (_gyro_reports != nullptr) { - delete _gyro_reports; + if (_sensor_gyro_ss != nullptr) { + delete _sensor_gyro_ss; } if (_gyro_class_instance != -1) { @@ -84,9 +84,9 @@ BMI055_gyro::init() } /* allocate basic report buffers */ - _gyro_reports = new ringbuffer::RingBuffer(2, sizeof(accel_report)); + _sensor_gyro_ss = new ringbuffer::RingBuffer(2, sizeof(sensor_accel_s)); - if (_gyro_reports == nullptr) { + if (_sensor_gyro_ss == nullptr) { goto out; } @@ -114,8 +114,8 @@ BMI055_gyro::init() measure(); /* advertise sensor topic, measure manually to initialize valid report */ - struct gyro_report grp; - _gyro_reports->get(&grp); + sensor_gyro_s grp; + _sensor_gyro_ss->get(&grp); _gyro_topic = orb_advertise_multi(ORB_ID(sensor_gyro), &grp, &_gyro_orb_class_instance, (external()) ? ORB_PRIO_MAX - 1 : ORB_PRIO_HIGH - 1); @@ -301,7 +301,7 @@ BMI055_gyro::test_error() ssize_t BMI055_gyro::read(struct file *filp, char *buffer, size_t buflen) { - unsigned count = buflen / sizeof(gyro_report); + unsigned count = buflen / sizeof(sensor_gyro_s); /* buffer must be large enough */ if (count < 1) { @@ -310,23 +310,23 @@ BMI055_gyro::read(struct file *filp, char *buffer, size_t buflen) /* if automatic measurement is not enabled, get a fresh measurement into the buffer */ if (_call_interval == 0) { - _gyro_reports->flush(); + _sensor_gyro_ss->flush(); measure(); } /* if no data, error (we could block here) */ - if (_gyro_reports->empty()) { + if (_sensor_gyro_ss->empty()) { return -EAGAIN; } perf_count(_gyro_reads); /* copy reports out of our buffer to the caller */ - gyro_report *grp = reinterpret_cast(buffer); + sensor_gyro_s *grp = reinterpret_cast(buffer); int transferred = 0; while (count--) { - if (!_gyro_reports->get(grp)) { + if (!_sensor_gyro_ss->get(grp)) { break; } @@ -335,7 +335,7 @@ BMI055_gyro::read(struct file *filp, char *buffer, size_t buflen) } /* return the number of bytes transferred */ - return (transferred * sizeof(gyro_report)); + return (transferred * sizeof(sensor_gyro_s)); } @@ -425,7 +425,7 @@ BMI055_gyro::ioctl(struct file *filp, int cmd, unsigned long arg) irqstate_t flags = px4_enter_critical_section(); - if (!_gyro_reports->resize(arg)) { + if (!_sensor_gyro_ss->resize(arg)) { px4_leave_critical_section(flags); return -ENOMEM; } @@ -443,12 +443,12 @@ BMI055_gyro::ioctl(struct file *filp, int cmd, unsigned long arg) case GYROIOCSSCALE: /* copy scale in */ - memcpy(&_gyro_scale, (struct gyro_calibration_s *) arg, sizeof(_gyro_scale)); + memcpy(&_gyro_scale, (calibration_gyro_s *) arg, sizeof(_gyro_scale)); return OK; case GYROIOCGSCALE: /* copy scale out */ - memcpy((struct gyro_calibration_s *) arg, &_gyro_scale, sizeof(_gyro_scale)); + memcpy((calibration_gyro_s *) arg, &_gyro_scale, sizeof(_gyro_scale)); return OK; case GYROIOCSRANGE: @@ -549,7 +549,7 @@ BMI055_gyro::start() stop(); /* discard any stale data in the buffers */ - _gyro_reports->flush(); + _sensor_gyro_ss->flush(); /* start polling at the specified rate */ hrt_call_every(&_call, @@ -685,7 +685,7 @@ BMI055_gyro::measure() /* * Report buffers. */ - gyro_report grb; + sensor_gyro_s grb; grb.timestamp = hrt_absolute_time(); @@ -745,7 +745,7 @@ BMI055_gyro::measure() grb.temperature = _last_temperature; grb.device_id = _device_id.devid; - _gyro_reports->force(&grb); + _sensor_gyro_ss->force(&grb); /* notify anyone waiting for data */ if (gyro_notify) { @@ -772,7 +772,7 @@ BMI055_gyro::print_info() perf_print_counter(_good_transfers); perf_print_counter(_reset_retries); perf_print_counter(_duplicates); - _gyro_reports->print_info("gyro queue"); + _sensor_gyro_ss->print_info("gyro queue"); ::printf("checked_next: %u\n", _checked_next); for (uint8_t i = 0; i < BMI055_GYRO_NUM_CHECKED_REGISTERS; i++) { diff --git a/src/drivers/imu/bmi055/bmi055_main.cpp b/src/drivers/imu/bmi055/bmi055_main.cpp index fbbf2a2a4fbe..7b7aade7f8b5 100644 --- a/src/drivers/imu/bmi055/bmi055_main.cpp +++ b/src/drivers/imu/bmi055/bmi055_main.cpp @@ -199,8 +199,8 @@ test(bool external_bus, enum sensor_type sensor) { const char *path_accel = external_bus ? BMI055_DEVICE_PATH_ACCEL_EXT : BMI055_DEVICE_PATH_ACCEL; const char *path_gyro = external_bus ? BMI055_DEVICE_PATH_GYRO_EXT : BMI055_DEVICE_PATH_GYRO; - accel_report a_report; - gyro_report g_report; + sensor_accel_s a_report; + sensor_gyro_s g_report; ssize_t sz; if (sensor == BMI055_ACCEL) { diff --git a/src/drivers/imu/bmi160/bmi160.cpp b/src/drivers/imu/bmi160/bmi160.cpp index 30d5a46837b7..b42813f06ec3 100644 --- a/src/drivers/imu/bmi160/bmi160.cpp +++ b/src/drivers/imu/bmi160/bmi160.cpp @@ -24,14 +24,14 @@ BMI160::BMI160(int bus, const char *path_accel, const char *path_gyro, uint32_t _whoami(0), _call{}, _call_interval(0), - _accel_reports(nullptr), + _sensor_accel_ss(nullptr), _accel_scale{}, _accel_range_scale(0.0f), _accel_range_m_s2(0.0f), _accel_topic(nullptr), _accel_orb_class_instance(-1), _accel_class_instance(-1), - _gyro_reports(nullptr), + _sensor_gyro_ss(nullptr), _gyro_scale{}, _gyro_range_scale(0.0f), _gyro_range_rad_s(0.0f), @@ -100,12 +100,12 @@ BMI160::~BMI160() delete _gyro; /* free any existing reports */ - if (_accel_reports != nullptr) { - delete _accel_reports; + if (_sensor_accel_ss != nullptr) { + delete _sensor_accel_ss; } - if (_gyro_reports != nullptr) { - delete _gyro_reports; + if (_sensor_gyro_ss != nullptr) { + delete _sensor_gyro_ss; } if (_accel_class_instance != -1) { @@ -138,15 +138,15 @@ BMI160::init() } /* allocate basic report buffers */ - _accel_reports = new ringbuffer::RingBuffer(2, sizeof(accel_report)); + _sensor_accel_ss = new ringbuffer::RingBuffer(2, sizeof(sensor_accel_s)); - if (_accel_reports == nullptr) { + if (_sensor_accel_ss == nullptr) { goto out; } - _gyro_reports = new ringbuffer::RingBuffer(2, sizeof(gyro_report)); + _sensor_gyro_ss = new ringbuffer::RingBuffer(2, sizeof(sensor_gyro_s)); - if (_gyro_reports == nullptr) { + if (_sensor_gyro_ss == nullptr) { goto out; } @@ -184,8 +184,8 @@ BMI160::init() measure(); /* advertise sensor topic, measure manually to initialize valid report */ - struct accel_report arp; - _accel_reports->get(&arp); + sensor_accel_s arp; + _sensor_accel_ss->get(&arp); /* measurement will have generated a report, publish */ _accel_topic = orb_advertise_multi(ORB_ID(sensor_accel), &arp, @@ -197,8 +197,8 @@ BMI160::init() /* advertise sensor topic, measure manually to initialize valid report */ - struct gyro_report grp; - _gyro_reports->get(&grp); + sensor_gyro_s grp; + _sensor_gyro_ss->get(&grp); _gyro->_gyro_topic = orb_advertise_multi(ORB_ID(sensor_gyro), &grp, &_gyro->_gyro_orb_class_instance, (external()) ? ORB_PRIO_MAX - 1 : ORB_PRIO_HIGH - 1); @@ -444,7 +444,7 @@ BMI160::_set_dlpf_filter(uint16_t bandwidth) ssize_t BMI160::read(struct file *filp, char *buffer, size_t buflen) { - unsigned count = buflen / sizeof(accel_report); + unsigned count = buflen / sizeof(sensor_accel_s); /* buffer must be large enough */ if (count < 1) { @@ -453,23 +453,23 @@ BMI160::read(struct file *filp, char *buffer, size_t buflen) /* if automatic measurement is not enabled, get a fresh measurement into the buffer */ if (_call_interval == 0) { - _accel_reports->flush(); + _sensor_accel_ss->flush(); measure(); } /* if no data, error (we could block here) */ - if (_accel_reports->empty()) { + if (_sensor_accel_ss->empty()) { return -EAGAIN; } perf_count(_accel_reads); /* copy reports out of our buffer to the caller */ - accel_report *arp = reinterpret_cast(buffer); + sensor_accel_s *arp = reinterpret_cast(buffer); int transferred = 0; while (count--) { - if (!_accel_reports->get(arp)) { + if (!_sensor_accel_ss->get(arp)) { break; } @@ -478,7 +478,7 @@ BMI160::read(struct file *filp, char *buffer, size_t buflen) } /* return the number of bytes transferred */ - return (transferred * sizeof(accel_report)); + return (transferred * sizeof(sensor_accel_s)); } int @@ -563,7 +563,7 @@ BMI160::test_error() ssize_t BMI160::gyro_read(struct file *filp, char *buffer, size_t buflen) { - unsigned count = buflen / sizeof(gyro_report); + unsigned count = buflen / sizeof(sensor_gyro_s); /* buffer must be large enough */ if (count < 1) { @@ -572,23 +572,23 @@ BMI160::gyro_read(struct file *filp, char *buffer, size_t buflen) /* if automatic measurement is not enabled, get a fresh measurement into the buffer */ if (_call_interval == 0) { - _gyro_reports->flush(); + _sensor_gyro_ss->flush(); measure(); } /* if no data, error (we could block here) */ - if (_gyro_reports->empty()) { + if (_sensor_gyro_ss->empty()) { return -EAGAIN; } perf_count(_gyro_reads); /* copy reports out of our buffer to the caller */ - gyro_report *grp = reinterpret_cast(buffer); + sensor_gyro_s *grp = reinterpret_cast(buffer); int transferred = 0; while (count--) { - if (!_gyro_reports->get(grp)) { + if (!_sensor_gyro_ss->get(grp)) { break; } @@ -597,7 +597,7 @@ BMI160::gyro_read(struct file *filp, char *buffer, size_t buflen) } /* return the number of bytes transferred */ - return (transferred * sizeof(gyro_report)); + return (transferred * sizeof(sensor_gyro_s)); } @@ -701,7 +701,7 @@ BMI160::ioctl(struct file *filp, int cmd, unsigned long arg) irqstate_t flags = px4_enter_critical_section(); - if (!_accel_reports->resize(arg)) { + if (!_sensor_accel_ss->resize(arg)) { px4_leave_critical_section(flags); return -ENOMEM; } @@ -719,7 +719,7 @@ BMI160::ioctl(struct file *filp, int cmd, unsigned long arg) case ACCELIOCSSCALE: { /* copy scale, but only if off by a few percent */ - struct accel_calibration_s *s = (struct accel_calibration_s *) arg; + calibration_accel_s *s = (calibration_accel_s *) arg; float sum = s->x_scale + s->y_scale + s->z_scale; if (sum > 2.0f && sum < 4.0f) { @@ -733,7 +733,7 @@ BMI160::ioctl(struct file *filp, int cmd, unsigned long arg) case ACCELIOCGSCALE: /* copy scale out */ - memcpy((struct accel_calibration_s *) arg, &_accel_scale, sizeof(_accel_scale)); + memcpy((calibration_accel_s *) arg, &_accel_scale, sizeof(_accel_scale)); return OK; case ACCELIOCSRANGE: @@ -770,7 +770,7 @@ BMI160::gyro_ioctl(struct file *filp, int cmd, unsigned long arg) irqstate_t flags = px4_enter_critical_section(); - if (!_gyro_reports->resize(arg)) { + if (!_sensor_gyro_ss->resize(arg)) { px4_leave_critical_section(flags); return -ENOMEM; } @@ -788,12 +788,12 @@ BMI160::gyro_ioctl(struct file *filp, int cmd, unsigned long arg) case GYROIOCSSCALE: /* copy scale in */ - memcpy(&_gyro_scale, (struct gyro_calibration_s *) arg, sizeof(_gyro_scale)); + memcpy(&_gyro_scale, (calibration_gyro_s *) arg, sizeof(_gyro_scale)); return OK; case GYROIOCGSCALE: /* copy scale out */ - memcpy((struct gyro_calibration_s *) arg, &_gyro_scale, sizeof(_gyro_scale)); + memcpy((calibration_gyro_s *) arg, &_gyro_scale, sizeof(_gyro_scale)); return OK; case GYROIOCSRANGE: @@ -966,8 +966,8 @@ BMI160::start() stop(); /* discard any stale data in the buffers */ - _accel_reports->flush(); - _gyro_reports->flush(); + _sensor_accel_ss->flush(); + _sensor_gyro_ss->flush(); /* start polling at the specified rate */ hrt_call_every(&_call, @@ -1126,8 +1126,8 @@ BMI160::measure() /* * Report buffers. */ - accel_report arb; - gyro_report grb; + sensor_accel_s arb; + sensor_gyro_s grb; /* * Adjust and scale results to m/s^2. @@ -1232,8 +1232,8 @@ BMI160::measure() /* return device ID */ grb.device_id = _gyro->_device_id.devid; - _accel_reports->force(&arb); - _gyro_reports->force(&grb); + _sensor_accel_ss->force(&arb); + _sensor_gyro_ss->force(&grb); /* notify anyone waiting for data */ if (accel_notify) { @@ -1269,8 +1269,8 @@ BMI160::print_info() perf_print_counter(_good_transfers); perf_print_counter(_reset_retries); perf_print_counter(_duplicates); - _accel_reports->print_info("accel queue"); - _gyro_reports->print_info("gyro queue"); + _sensor_accel_ss->print_info("accel queue"); + _sensor_gyro_ss->print_info("gyro queue"); ::printf("checked_next: %u\n", _checked_next); for (uint8_t i = 0; i < BMI160_NUM_CHECKED_REGISTERS; i++) { diff --git a/src/drivers/imu/bmi160/bmi160.hpp b/src/drivers/imu/bmi160/bmi160.hpp index a08dd4de5051..87bde7088a4f 100644 --- a/src/drivers/imu/bmi160/bmi160.hpp +++ b/src/drivers/imu/bmi160/bmi160.hpp @@ -36,6 +36,10 @@ #include #include #include +#include +#include +#include +#include #define DIR_READ 0x80 #define DIR_WRITE 0x00 @@ -280,18 +284,18 @@ class BMI160 : public device::SPI struct hrt_call _call; unsigned _call_interval; - ringbuffer::RingBuffer *_accel_reports; + ringbuffer::RingBuffer *_sensor_accel_ss; - struct accel_calibration_s _accel_scale; + calibration_accel_s _accel_scale; float _accel_range_scale; float _accel_range_m_s2; orb_advert_t _accel_topic; int _accel_orb_class_instance; int _accel_class_instance; - ringbuffer::RingBuffer *_gyro_reports; + ringbuffer::RingBuffer *_sensor_gyro_ss; - struct gyro_calibration_s _gyro_scale; + calibration_gyro_s _gyro_scale; float _gyro_range_scale; float _gyro_range_rad_s; diff --git a/src/drivers/imu/bmi160/bmi160_main.cpp b/src/drivers/imu/bmi160/bmi160_main.cpp index a8c4f1062b09..dde6713e5d13 100644 --- a/src/drivers/imu/bmi160/bmi160_main.cpp +++ b/src/drivers/imu/bmi160/bmi160_main.cpp @@ -120,8 +120,8 @@ test(bool external_bus) { const char *path_accel = external_bus ? BMI160_DEVICE_PATH_ACCEL_EXT : BMI160_DEVICE_PATH_ACCEL; const char *path_gyro = external_bus ? BMI160_DEVICE_PATH_GYRO_EXT : BMI160_DEVICE_PATH_GYRO; - accel_report a_report; - gyro_report g_report; + sensor_accel_s a_report; + sensor_gyro_s g_report; ssize_t sz; /* get the driver */ diff --git a/src/drivers/imu/fxas21002c/fxas21002c.cpp b/src/drivers/imu/fxas21002c/fxas21002c.cpp index 51808a2602de..e806fadab5e2 100644 --- a/src/drivers/imu/fxas21002c/fxas21002c.cpp +++ b/src/drivers/imu/fxas21002c/fxas21002c.cpp @@ -73,6 +73,8 @@ #include #include #include +#include +#include /* SPI protocol address bits */ #define DIR_READ(a) ((a) | (1 << 7)) @@ -252,7 +254,7 @@ class FXAS21002C : public device::SPI ringbuffer::RingBuffer *_reports; - struct gyro_calibration_s _gyro_scale; + calibration_gyro_s _gyro_scale; float _gyro_range_scale; float _gyro_range_rad_s; orb_advert_t _gyro_topic; @@ -500,7 +502,7 @@ FXAS21002C::init() } /* allocate basic report buffers */ - _reports = new ringbuffer::RingBuffer(2, sizeof(gyro_report)); + _reports = new ringbuffer::RingBuffer(2, sizeof(sensor_gyro_s)); if (_reports == nullptr) { goto out; @@ -514,7 +516,7 @@ FXAS21002C::init() _class_instance = register_class_devname(GYRO_BASE_DEVICE_PATH); /* advertise sensor topic, measure manually to initialize valid report */ - struct gyro_report grp; + sensor_gyro_s grp; _reports->get(&grp); /* measurement will have generated a report, publish */ @@ -583,8 +585,8 @@ FXAS21002C::probe() ssize_t FXAS21002C::read(struct file *filp, char *buffer, size_t buflen) { - unsigned count = buflen / sizeof(struct gyro_report); - struct gyro_report *gbuf = reinterpret_cast(buffer); + unsigned count = buflen / sizeof(sensor_gyro_s); + sensor_gyro_s *gbuf = reinterpret_cast(buffer); int ret = 0; /* buffer must be large enough */ @@ -720,12 +722,12 @@ FXAS21002C::ioctl(struct file *filp, int cmd, unsigned long arg) case GYROIOCSSCALE: /* copy scale in */ - memcpy(&_gyro_scale, (struct gyro_calibration_s *) arg, sizeof(_gyro_scale)); + memcpy(&_gyro_scale, (calibration_gyro_s *) arg, sizeof(_gyro_scale)); return OK; case GYROIOCGSCALE: /* copy scale out */ - memcpy((struct gyro_calibration_s *) arg, &_gyro_scale, sizeof(_gyro_scale)); + memcpy((calibration_gyro_s *) arg, &_gyro_scale, sizeof(_gyro_scale)); return OK; case GYROIOCSRANGE: @@ -979,10 +981,10 @@ FXAS21002C::measure() int16_t x; int16_t y; int16_t z; - } raw_gyro_report; + } raw_sensor_gyro_s; #pragma pack(pop) - struct gyro_report gyro_report; + sensor_gyro_s sensor_gyro_s; /* start the performance counter */ perf_begin(_sample_perf); @@ -998,11 +1000,11 @@ FXAS21002C::measure() } /* fetch data from the sensor */ - memset(&raw_gyro_report, 0, sizeof(raw_gyro_report)); - raw_gyro_report.cmd = DIR_READ(FXAS21002C_STATUS); - transfer((uint8_t *)&raw_gyro_report, (uint8_t *)&raw_gyro_report, sizeof(raw_gyro_report)); + memset(&raw_sensor_gyro_s, 0, sizeof(raw_sensor_gyro_s)); + raw_sensor_gyro_s.cmd = DIR_READ(FXAS21002C_STATUS); + transfer((uint8_t *)&raw_sensor_gyro_s, (uint8_t *)&raw_sensor_gyro_s, sizeof(raw_sensor_gyro_s)); - if (!(raw_gyro_report.status & DR_STATUS_ZYXDR)) { + if (!(raw_sensor_gyro_s.status & DR_STATUS_ZYXDR)) { perf_end(_sample_perf); perf_count(_duplicates); return; @@ -1017,7 +1019,7 @@ FXAS21002C::measure() if ((_read % _current_rate) == 0) { _last_temperature = read_reg(FXAS21002C_TEMP) * 1.0f; - gyro_report.temperature = _last_temperature; + sensor_gyro_s.temperature = _last_temperature; } /* @@ -1035,21 +1037,21 @@ FXAS21002C::measure() * 74 from all measurements centers them around zero. */ - gyro_report.timestamp = hrt_absolute_time(); + sensor_gyro_s.timestamp = hrt_absolute_time(); // report the error count as the number of bad // register reads. This allows the higher level // code to decide if it should use this sensor based on // whether it has had failures - gyro_report.error_count = perf_event_count(_bad_registers); + sensor_gyro_s.error_count = perf_event_count(_bad_registers); - gyro_report.x_raw = swap16(raw_gyro_report.x); - gyro_report.y_raw = swap16(raw_gyro_report.y); - gyro_report.z_raw = swap16(raw_gyro_report.z); + sensor_gyro_s.x_raw = swap16(raw_sensor_gyro_s.x); + sensor_gyro_s.y_raw = swap16(raw_sensor_gyro_s.y); + sensor_gyro_s.z_raw = swap16(raw_sensor_gyro_s.z); - float xraw_f = gyro_report.x_raw; - float yraw_f = gyro_report.y_raw; - float zraw_f = gyro_report.z_raw; + float xraw_f = sensor_gyro_s.x_raw; + float yraw_f = sensor_gyro_s.y_raw; + float zraw_f = sensor_gyro_s.z_raw; // apply user specified rotation rotate_3f(_rotation, xraw_f, yraw_f, zraw_f); @@ -1058,26 +1060,26 @@ FXAS21002C::measure() float y_in_new = ((yraw_f * _gyro_range_scale) - _gyro_scale.y_offset) * _gyro_scale.y_scale; float z_in_new = ((zraw_f * _gyro_range_scale) - _gyro_scale.z_offset) * _gyro_scale.z_scale; - gyro_report.x = _gyro_filter_x.apply(x_in_new); - gyro_report.y = _gyro_filter_y.apply(y_in_new); - gyro_report.z = _gyro_filter_z.apply(z_in_new); + sensor_gyro_s.x = _gyro_filter_x.apply(x_in_new); + sensor_gyro_s.y = _gyro_filter_y.apply(y_in_new); + sensor_gyro_s.z = _gyro_filter_z.apply(z_in_new); matrix::Vector3f gval(x_in_new, y_in_new, z_in_new); matrix::Vector3f gval_integrated; - bool gyro_notify = _gyro_int.put(gyro_report.timestamp, gval, gval_integrated, gyro_report.integral_dt); - gyro_report.x_integral = gval_integrated(0); - gyro_report.y_integral = gval_integrated(1); - gyro_report.z_integral = gval_integrated(2); + bool gyro_notify = _gyro_int.put(sensor_gyro_s.timestamp, gval, gval_integrated, sensor_gyro_s.integral_dt); + sensor_gyro_s.x_integral = gval_integrated(0); + sensor_gyro_s.y_integral = gval_integrated(1); + sensor_gyro_s.z_integral = gval_integrated(2); - gyro_report.scaling = _gyro_range_scale; - gyro_report.range_rad_s = _gyro_range_rad_s; + sensor_gyro_s.scaling = _gyro_range_scale; + sensor_gyro_s.range_rad_s = _gyro_range_rad_s; /* return device ID */ - gyro_report.device_id = _device_id.devid; + sensor_gyro_s.device_id = _device_id.devid; - _reports->force(&gyro_report); + _reports->force(&sensor_gyro_s); /* notify anyone waiting for data */ if (gyro_notify) { @@ -1085,7 +1087,7 @@ FXAS21002C::measure() if (!(_pub_blocked)) { /* publish it */ - orb_publish(ORB_ID(sensor_gyro), _gyro_topic, &gyro_report); + orb_publish(ORB_ID(sensor_gyro), _gyro_topic, &sensor_gyro_s); } } @@ -1251,7 +1253,7 @@ void test() { int fd_gyro = -1; - struct gyro_report g_report; + sensor_gyro_s g_report; ssize_t sz; /* get the driver */ diff --git a/src/drivers/imu/fxos8701cq/fxos8701cq.cpp b/src/drivers/imu/fxos8701cq/fxos8701cq.cpp index ef4c87e044b2..eb33e9c18f78 100644 --- a/src/drivers/imu/fxos8701cq/fxos8701cq.cpp +++ b/src/drivers/imu/fxos8701cq/fxos8701cq.cpp @@ -75,6 +75,8 @@ #include #include #include +#include +#include /* SPI protocol address bits */ #define DIR_READ(a) ((a) & 0x7f) @@ -196,10 +198,10 @@ class FXOS8701CQ : public device::SPI unsigned _call_accel_interval; unsigned _call_mag_interval; - ringbuffer::RingBuffer *_accel_reports; + ringbuffer::RingBuffer *_sensor_accel_ss; ringbuffer::RingBuffer *_mag_reports; - struct accel_calibration_s _accel_scale; + calibration_accel_s _accel_scale; unsigned _accel_range_m_s2; float _accel_range_scale; unsigned _accel_samplerate; @@ -473,7 +475,7 @@ FXOS8701CQ::FXOS8701CQ(int bus, const char *path, uint32_t device, enum Rotation _mag_call{}, _call_accel_interval(0), _call_mag_interval(0), - _accel_reports(nullptr), + _sensor_accel_ss(nullptr), _mag_reports(nullptr), _accel_scale{}, _accel_range_m_s2(0.0f), @@ -539,8 +541,8 @@ FXOS8701CQ::~FXOS8701CQ() stop(); /* free any existing reports */ - if (_accel_reports != nullptr) { - delete _accel_reports; + if (_sensor_accel_ss != nullptr) { + delete _sensor_accel_ss; } if (_mag_reports != nullptr) { @@ -573,9 +575,9 @@ FXOS8701CQ::init() } /* allocate basic report buffers */ - _accel_reports = new ringbuffer::RingBuffer(2, sizeof(accel_report)); + _sensor_accel_ss = new ringbuffer::RingBuffer(2, sizeof(sensor_accel_s)); - if (_accel_reports == nullptr) { + if (_sensor_accel_ss == nullptr) { goto out; } @@ -614,8 +616,8 @@ FXOS8701CQ::init() _accel_class_instance = register_class_devname(ACCEL_BASE_DEVICE_PATH); /* advertise sensor topic, measure manually to initialize valid report */ - struct accel_report arp; - _accel_reports->get(&arp); + sensor_accel_s arp; + _sensor_accel_ss->get(&arp); /* measurement will have generated a report, publish */ _accel_topic = orb_advertise_multi(ORB_ID(sensor_accel), &arp, @@ -685,8 +687,8 @@ FXOS8701CQ::probe() ssize_t FXOS8701CQ::read(struct file *filp, char *buffer, size_t buflen) { - unsigned count = buflen / sizeof(struct accel_report); - accel_report *arb = reinterpret_cast(buffer); + unsigned count = buflen / sizeof(sensor_accel_s); + sensor_accel_s *arb = reinterpret_cast(buffer); int ret = 0; /* buffer must be large enough */ @@ -700,7 +702,7 @@ FXOS8701CQ::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 (_sensor_accel_ss->get(arb)) { ret += sizeof(*arb); arb++; } @@ -714,7 +716,7 @@ FXOS8701CQ::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 (_sensor_accel_ss->get(arb)) { ret = sizeof(*arb); } @@ -837,7 +839,7 @@ FXOS8701CQ::ioctl(struct file *filp, int cmd, unsigned long arg) irqstate_t flags = px4_enter_critical_section(); - if (!_accel_reports->resize(arg)) { + if (!_sensor_accel_ss->resize(arg)) { px4_leave_critical_section(flags); return -ENOMEM; } @@ -859,7 +861,7 @@ FXOS8701CQ::ioctl(struct file *filp, int cmd, unsigned long arg) case ACCELIOCSSCALE: { /* copy scale, but only if off by a few percent */ - struct accel_calibration_s *s = (struct accel_calibration_s *) arg; + calibration_accel_s *s = (calibration_accel_s *) arg; float sum = s->x_scale + s->y_scale + s->z_scale; if (sum > 2.0f && sum < 4.0f) { @@ -881,7 +883,7 @@ FXOS8701CQ::ioctl(struct file *filp, int cmd, unsigned long arg) case ACCELIOCGSCALE: /* copy scale out */ - memcpy((struct accel_calibration_s *) arg, &_accel_scale, sizeof(_accel_scale)); + memcpy((calibration_accel_s *) arg, &_accel_scale, sizeof(_accel_scale)); return OK; case ACCELIOCSELFTEST: @@ -1249,7 +1251,7 @@ FXOS8701CQ::start() stop(); /* reset the report ring */ - _accel_reports->flush(); + _sensor_accel_ss->flush(); _mag_reports->flush(); /* start polling at the specified rate */ @@ -1270,7 +1272,7 @@ FXOS8701CQ::stop() memset(_last_accel, 0, sizeof(_last_accel)); /* discard unread data in the buffers */ - _accel_reports->flush(); + _sensor_accel_ss->flush(); _mag_reports->flush(); } @@ -1341,7 +1343,7 @@ FXOS8701CQ::measure() } raw_accel_mag_report; #pragma pack(pop) - accel_report accel_report; + sensor_accel_s sensor_accel_s; /* start the performance counter */ perf_begin(_accel_sample_perf); @@ -1383,7 +1385,7 @@ FXOS8701CQ::measure() * 74 from all measurements centers them around zero. */ - accel_report.timestamp = hrt_absolute_time(); + sensor_accel_s.timestamp = hrt_absolute_time(); /* * Eight-bit 2’s complement sensor temperature value with 0.96 °C/LSB sensitivity. @@ -1395,17 +1397,17 @@ FXOS8701CQ::measure() _last_temperature = (read_reg(FXOS8701CQ_TEMP)) * 0.96f; - accel_report.temperature = _last_temperature; + sensor_accel_s.temperature = _last_temperature; // report the error count as the sum of the number of bad // register reads and bad values. This allows the higher level // code to decide if it should use this sensor based on // whether it has had failures - accel_report.error_count = perf_event_count(_bad_registers) + perf_event_count(_bad_values); + sensor_accel_s.error_count = perf_event_count(_bad_registers) + perf_event_count(_bad_values); - accel_report.x_raw = swap16RightJustify14(raw_accel_mag_report.x); - accel_report.y_raw = swap16RightJustify14(raw_accel_mag_report.y); - accel_report.z_raw = swap16RightJustify14(raw_accel_mag_report.z); + sensor_accel_s.x_raw = swap16RightJustify14(raw_accel_mag_report.x); + sensor_accel_s.y_raw = swap16RightJustify14(raw_accel_mag_report.y); + sensor_accel_s.z_raw = swap16RightJustify14(raw_accel_mag_report.z); /* Save off the Mag readings todo: revist integrating theses */ @@ -1413,9 +1415,9 @@ FXOS8701CQ::measure() _last_raw_mag_y = swap16(raw_accel_mag_report.my); _last_raw_mag_z = swap16(raw_accel_mag_report.mz); - float xraw_f = accel_report.x_raw; - float yraw_f = accel_report.y_raw; - float zraw_f = accel_report.z_raw; + float xraw_f = sensor_accel_s.x_raw; + float yraw_f = sensor_accel_s.y_raw; + float zraw_f = sensor_accel_s.z_raw; // apply user specified rotation rotate_3f(_rotation, xraw_f, yraw_f, zraw_f); @@ -1456,25 +1458,25 @@ FXOS8701CQ::measure() _last_accel[1] = y_in_new; _last_accel[2] = 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); + sensor_accel_s.x = _accel_filter_x.apply(x_in_new); + sensor_accel_s.y = _accel_filter_y.apply(y_in_new); + sensor_accel_s.z = _accel_filter_z.apply(z_in_new); matrix::Vector3f aval(x_in_new, y_in_new, z_in_new); matrix::Vector3f aval_integrated; - bool accel_notify = _accel_int.put(accel_report.timestamp, aval, aval_integrated, accel_report.integral_dt); - accel_report.x_integral = aval_integrated(0); - accel_report.y_integral = aval_integrated(1); - accel_report.z_integral = aval_integrated(2); + bool accel_notify = _accel_int.put(sensor_accel_s.timestamp, aval, aval_integrated, sensor_accel_s.integral_dt); + sensor_accel_s.x_integral = aval_integrated(0); + sensor_accel_s.y_integral = aval_integrated(1); + sensor_accel_s.z_integral = aval_integrated(2); - accel_report.scaling = _accel_range_scale; - accel_report.range_m_s2 = _accel_range_m_s2; + sensor_accel_s.scaling = _accel_range_scale; + sensor_accel_s.range_m_s2 = _accel_range_m_s2; /* return device ID */ - accel_report.device_id = _device_id.devid; + sensor_accel_s.device_id = _device_id.devid; - _accel_reports->force(&accel_report); + _sensor_accel_ss->force(&sensor_accel_s); /* notify anyone waiting for data */ if (accel_notify) { @@ -1482,7 +1484,7 @@ FXOS8701CQ::measure() if (!(_pub_blocked)) { /* publish it */ - orb_publish(ORB_ID(sensor_accel), _accel_topic, &accel_report); + orb_publish(ORB_ID(sensor_accel), _accel_topic, &sensor_accel_s); } } @@ -1567,7 +1569,7 @@ FXOS8701CQ::print_info() perf_print_counter(_bad_registers); perf_print_counter(_bad_values); perf_print_counter(_accel_duplicates); - _accel_reports->print_info("accel reports"); + _sensor_accel_ss->print_info("accel reports"); _mag_reports->print_info("mag reports"); ::printf("checked_next: %u\n", _checked_next); @@ -1787,7 +1789,7 @@ test() { int rv = 1; int fd_accel = -1; - struct accel_report accel_report; + sensor_accel_s sensor_accel_s; ssize_t sz; int ret; int fd_mag = -1; @@ -1803,22 +1805,22 @@ test() } /* do a simple demand read */ - sz = read(fd_accel, &accel_report, sizeof(accel_report)); + sz = read(fd_accel, &sensor_accel_s, sizeof(sensor_accel_s)); - if (sz != sizeof(accel_report)) { + if (sz != sizeof(sensor_accel_s)) { PX4_ERR("immediate read failed"); goto exit_with_accel; } - PX4_INFO("accel x: \t% 9.5f\tm/s^2", (double)accel_report.x); - PX4_INFO("accel y: \t% 9.5f\tm/s^2", (double)accel_report.y); - PX4_INFO("accel z: \t% 9.5f\tm/s^2", (double)accel_report.z); - PX4_INFO("accel x: \t%d\traw", (int)accel_report.x_raw); - PX4_INFO("accel y: \t%d\traw", (int)accel_report.y_raw); - PX4_INFO("accel z: \t%d\traw", (int)accel_report.z_raw); + PX4_INFO("accel x: \t% 9.5f\tm/s^2", (double)sensor_accel_s.x); + PX4_INFO("accel y: \t% 9.5f\tm/s^2", (double)sensor_accel_s.y); + PX4_INFO("accel z: \t% 9.5f\tm/s^2", (double)sensor_accel_s.z); + PX4_INFO("accel x: \t%d\traw", (int)sensor_accel_s.x_raw); + PX4_INFO("accel y: \t%d\traw", (int)sensor_accel_s.y_raw); + PX4_INFO("accel z: \t%d\traw", (int)sensor_accel_s.z_raw); - PX4_INFO("accel range: %8.4f m/s^2", (double)accel_report.range_m_s2); + PX4_INFO("accel range: %8.4f m/s^2", (double)sensor_accel_s.range_m_s2); /* get the driver */ fd_mag = open(FXOS8701C_DEVICE_PATH_MAG, O_RDONLY); diff --git a/src/drivers/imu/l3gd20/l3gd20.cpp b/src/drivers/imu/l3gd20/l3gd20.cpp index 99b80b912dd5..77f96def5f56 100644 --- a/src/drivers/imu/l3gd20/l3gd20.cpp +++ b/src/drivers/imu/l3gd20/l3gd20.cpp @@ -73,6 +73,9 @@ #include #include +#include +#include + #define L3GD20_DEVICE_PATH "/dev/l3gd20" /* Orientation on board */ @@ -224,7 +227,7 @@ class L3GD20 : public device::SPI ringbuffer::RingBuffer *_reports; - struct gyro_calibration_s _gyro_scale; + calibration_gyro_s _gyro_scale; float _gyro_range_scale; float _gyro_range_rad_s; orb_advert_t _gyro_topic; @@ -463,7 +466,7 @@ L3GD20::init() } /* allocate basic report buffers */ - _reports = new ringbuffer::RingBuffer(2, sizeof(gyro_report)); + _reports = new ringbuffer::RingBuffer(2, sizeof(sensor_gyro_s)); if (_reports == nullptr) { goto out; @@ -476,7 +479,7 @@ L3GD20::init() measure(); /* advertise sensor topic, measure manually to initialize valid report */ - struct gyro_report grp; + sensor_gyro_s grp; _reports->get(&grp); _gyro_topic = orb_advertise_multi(ORB_ID(sensor_gyro), &grp, @@ -528,8 +531,8 @@ L3GD20::probe() ssize_t L3GD20::read(struct file *filp, char *buffer, size_t buflen) { - unsigned count = buflen / sizeof(struct gyro_report); - struct gyro_report *gbuf = reinterpret_cast(buffer); + unsigned count = buflen / sizeof(sensor_gyro_s); + sensor_gyro_s *gbuf = reinterpret_cast(buffer); int ret = 0; /* buffer must be large enough */ @@ -669,12 +672,12 @@ L3GD20::ioctl(struct file *filp, int cmd, unsigned long arg) case GYROIOCSSCALE: /* copy scale in */ - memcpy(&_gyro_scale, (struct gyro_calibration_s *) arg, sizeof(_gyro_scale)); + memcpy(&_gyro_scale, (calibration_gyro_s *) arg, sizeof(_gyro_scale)); return OK; case GYROIOCGSCALE: /* copy scale out */ - memcpy((struct gyro_calibration_s *) arg, &_gyro_scale, sizeof(_gyro_scale)); + memcpy((calibration_gyro_s *) arg, &_gyro_scale, sizeof(_gyro_scale)); return OK; case GYROIOCSRANGE: @@ -951,7 +954,7 @@ L3GD20::measure() } raw_report; #pragma pack(pop) - gyro_report report; + sensor_gyro_s report; /* start the performance counter */ perf_begin(_sample_perf); @@ -1229,7 +1232,7 @@ void test() { int fd_gyro = -1; - struct gyro_report g_report; + sensor_gyro_s g_report; ssize_t sz; /* get the driver */ diff --git a/src/drivers/imu/lsm303d/lsm303d.cpp b/src/drivers/imu/lsm303d/lsm303d.cpp index 44eafac98911..d3b0ad04efbf 100644 --- a/src/drivers/imu/lsm303d/lsm303d.cpp +++ b/src/drivers/imu/lsm303d/lsm303d.cpp @@ -72,6 +72,8 @@ #include #include #include +#include +#include /* SPI protocol address bits */ #define DIR_READ (1<<7) @@ -264,10 +266,10 @@ class LSM303D : public device::SPI unsigned _call_accel_interval; unsigned _call_mag_interval; - ringbuffer::RingBuffer *_accel_reports; + ringbuffer::RingBuffer *_sensor_accel_ss; ringbuffer::RingBuffer *_mag_reports; - struct accel_calibration_s _accel_scale; + calibration_accel_s _accel_scale; unsigned _accel_range_m_s2; float _accel_range_scale; unsigned _accel_samplerate; @@ -538,7 +540,7 @@ LSM303D::LSM303D(int bus, const char *path, uint32_t device, enum Rotation rotat _mag_call{}, _call_accel_interval(0), _call_mag_interval(0), - _accel_reports(nullptr), + _sensor_accel_ss(nullptr), _mag_reports(nullptr), _accel_scale{}, _accel_range_m_s2(0.0f), @@ -597,8 +599,8 @@ LSM303D::~LSM303D() stop(); /* free any existing reports */ - if (_accel_reports != nullptr) { - delete _accel_reports; + if (_sensor_accel_ss != nullptr) { + delete _sensor_accel_ss; } if (_mag_reports != nullptr) { @@ -631,9 +633,9 @@ LSM303D::init() } /* allocate basic report buffers */ - _accel_reports = new ringbuffer::RingBuffer(2, sizeof(accel_report)); + _sensor_accel_ss = new ringbuffer::RingBuffer(2, sizeof(sensor_accel_s)); - if (_accel_reports == nullptr) { + if (_sensor_accel_ss == nullptr) { goto out; } @@ -672,8 +674,8 @@ LSM303D::init() _accel_class_instance = register_class_devname(ACCEL_BASE_DEVICE_PATH); /* advertise sensor topic, measure manually to initialize valid report */ - struct accel_report arp; - _accel_reports->get(&arp); + sensor_accel_s arp; + _sensor_accel_ss->get(&arp); /* measurement will have generated a report, publish */ _accel_topic = orb_advertise_multi(ORB_ID(sensor_accel), &arp, @@ -753,8 +755,8 @@ LSM303D::probe() ssize_t LSM303D::read(struct file *filp, char *buffer, size_t buflen) { - unsigned count = buflen / sizeof(struct accel_report); - accel_report *arb = reinterpret_cast(buffer); + unsigned count = buflen / sizeof(sensor_accel_s); + sensor_accel_s *arb = reinterpret_cast(buffer); int ret = 0; /* buffer must be large enough */ @@ -768,7 +770,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 (_sensor_accel_ss->get(arb)) { ret += sizeof(*arb); arb++; } @@ -782,7 +784,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 (_sensor_accel_ss->get(arb)) { ret = sizeof(*arb); } @@ -905,7 +907,7 @@ LSM303D::ioctl(struct file *filp, int cmd, unsigned long arg) irqstate_t flags = px4_enter_critical_section(); - if (!_accel_reports->resize(arg)) { + if (!_sensor_accel_ss->resize(arg)) { px4_leave_critical_section(flags); return -ENOMEM; } @@ -927,7 +929,7 @@ LSM303D::ioctl(struct file *filp, int cmd, unsigned long arg) case ACCELIOCSSCALE: { /* copy scale, but only if off by a few percent */ - struct accel_calibration_s *s = (struct accel_calibration_s *) arg; + calibration_accel_s *s = (calibration_accel_s *) arg; float sum = s->x_scale + s->y_scale + s->z_scale; if (sum > 2.0f && sum < 4.0f) { @@ -949,7 +951,7 @@ LSM303D::ioctl(struct file *filp, int cmd, unsigned long arg) case ACCELIOCGSCALE: /* copy scale out */ - memcpy((struct accel_calibration_s *) arg, &_accel_scale, sizeof(_accel_scale)); + memcpy((calibration_accel_s *) arg, &_accel_scale, sizeof(_accel_scale)); return OK; case ACCELIOCSELFTEST: @@ -1377,7 +1379,7 @@ LSM303D::start() stop(); /* reset the report ring */ - _accel_reports->flush(); + _sensor_accel_ss->flush(); _mag_reports->flush(); /* start polling at the specified rate */ @@ -1398,7 +1400,7 @@ LSM303D::stop() memset(_last_accel, 0, sizeof(_last_accel)); /* discard unread data in the buffers */ - _accel_reports->flush(); + _sensor_accel_ss->flush(); _mag_reports->flush(); } @@ -1462,10 +1464,10 @@ LSM303D::measure() int16_t x; int16_t y; int16_t z; - } raw_accel_report; + } raw_sensor_accel_s; #pragma pack(pop) - accel_report accel_report; + sensor_accel_s sensor_accel_s; /* start the performance counter */ perf_begin(_accel_sample_perf); @@ -1481,11 +1483,11 @@ LSM303D::measure() } /* fetch data from the sensor */ - memset(&raw_accel_report, 0, sizeof(raw_accel_report)); - raw_accel_report.cmd = ADDR_STATUS_A | DIR_READ | ADDR_INCREMENT; - transfer((uint8_t *)&raw_accel_report, (uint8_t *)&raw_accel_report, sizeof(raw_accel_report)); + memset(&raw_sensor_accel_s, 0, sizeof(raw_sensor_accel_s)); + raw_sensor_accel_s.cmd = ADDR_STATUS_A | DIR_READ | ADDR_INCREMENT; + transfer((uint8_t *)&raw_sensor_accel_s, (uint8_t *)&raw_sensor_accel_s, sizeof(raw_sensor_accel_s)); - if (!(raw_accel_report.status & REG_STATUS_A_NEW_ZYXADA)) { + if (!(raw_sensor_accel_s.status & REG_STATUS_A_NEW_ZYXADA)) { perf_end(_accel_sample_perf); perf_count(_accel_duplicates); return; @@ -1506,24 +1508,24 @@ LSM303D::measure() * 74 from all measurements centers them around zero. */ - accel_report.timestamp = hrt_absolute_time(); + sensor_accel_s.timestamp = hrt_absolute_time(); // use the temperature from the last mag reading - accel_report.temperature = _last_temperature; + sensor_accel_s.temperature = _last_temperature; // report the error count as the sum of the number of bad // register reads and bad values. This allows the higher level // code to decide if it should use this sensor based on // whether it has had failures - accel_report.error_count = perf_event_count(_bad_registers) + perf_event_count(_bad_values); + sensor_accel_s.error_count = perf_event_count(_bad_registers) + perf_event_count(_bad_values); - accel_report.x_raw = raw_accel_report.x; - accel_report.y_raw = raw_accel_report.y; - accel_report.z_raw = raw_accel_report.z; + sensor_accel_s.x_raw = raw_sensor_accel_s.x; + sensor_accel_s.y_raw = raw_sensor_accel_s.y; + sensor_accel_s.z_raw = raw_sensor_accel_s.z; - float xraw_f = raw_accel_report.x; - float yraw_f = raw_accel_report.y; - float zraw_f = raw_accel_report.z; + float xraw_f = raw_sensor_accel_s.x; + float yraw_f = raw_sensor_accel_s.y; + float zraw_f = raw_sensor_accel_s.z; // apply user specified rotation rotate_3f(_rotation, xraw_f, yraw_f, zraw_f); @@ -1564,25 +1566,25 @@ LSM303D::measure() _last_accel[1] = y_in_new; _last_accel[2] = 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); + sensor_accel_s.x = _accel_filter_x.apply(x_in_new); + sensor_accel_s.y = _accel_filter_y.apply(y_in_new); + sensor_accel_s.z = _accel_filter_z.apply(z_in_new); matrix::Vector3f aval(x_in_new, y_in_new, z_in_new); matrix::Vector3f aval_integrated; - bool accel_notify = _accel_int.put(accel_report.timestamp, aval, aval_integrated, accel_report.integral_dt); - accel_report.x_integral = aval_integrated(0); - accel_report.y_integral = aval_integrated(1); - accel_report.z_integral = aval_integrated(2); + bool accel_notify = _accel_int.put(sensor_accel_s.timestamp, aval, aval_integrated, sensor_accel_s.integral_dt); + sensor_accel_s.x_integral = aval_integrated(0); + sensor_accel_s.y_integral = aval_integrated(1); + sensor_accel_s.z_integral = aval_integrated(2); - accel_report.scaling = _accel_range_scale; - accel_report.range_m_s2 = _accel_range_m_s2; + sensor_accel_s.scaling = _accel_range_scale; + sensor_accel_s.range_m_s2 = _accel_range_m_s2; /* return device ID */ - accel_report.device_id = _device_id.devid; + sensor_accel_s.device_id = _device_id.devid; - _accel_reports->force(&accel_report); + _sensor_accel_ss->force(&sensor_accel_s); /* notify anyone waiting for data */ if (accel_notify) { @@ -1590,7 +1592,7 @@ LSM303D::measure() if (!(_pub_blocked)) { /* publish it */ - orb_publish(ORB_ID(sensor_accel), _accel_topic, &accel_report); + orb_publish(ORB_ID(sensor_accel), _accel_topic, &sensor_accel_s); } } @@ -1694,7 +1696,7 @@ LSM303D::print_info() perf_print_counter(_bad_registers); perf_print_counter(_bad_values); perf_print_counter(_accel_duplicates); - _accel_reports->print_info("accel reports"); + _sensor_accel_ss->print_info("accel reports"); _mag_reports->print_info("mag reports"); ::printf("checked_next: %u\n", _checked_next); @@ -1946,7 +1948,7 @@ void test() { int fd_accel = -1; - struct accel_report accel_report; + sensor_accel_s sensor_accel_s; ssize_t sz; int ret; @@ -1958,13 +1960,13 @@ test() } /* do a simple demand read */ - sz = read(fd_accel, &accel_report, sizeof(accel_report)); + sz = read(fd_accel, &sensor_accel_s, sizeof(sensor_accel_s)); - if (sz != sizeof(accel_report)) { + if (sz != sizeof(sensor_accel_s)) { err(1, "immediate read failed"); } - print_message(accel_report); + print_message(sensor_accel_s); int fd_mag = -1; struct mag_report m_report; diff --git a/src/drivers/imu/mpu6000/mpu6000.cpp b/src/drivers/imu/mpu6000/mpu6000.cpp index e408c6a0374c..4d6bf3542615 100644 --- a/src/drivers/imu/mpu6000/mpu6000.cpp +++ b/src/drivers/imu/mpu6000/mpu6000.cpp @@ -93,6 +93,10 @@ #include #include #include +#include +#include +#include +#include #include "mpu6000.h" @@ -184,18 +188,18 @@ class MPU6000 : public device::CDev struct hrt_call _call; unsigned _call_interval; - ringbuffer::RingBuffer *_accel_reports; + ringbuffer::RingBuffer *_sensor_accel_ss; - struct accel_calibration_s _accel_scale; + calibration_accel_s _accel_scale; float _accel_range_scale; float _accel_range_m_s2; orb_advert_t _accel_topic; int _accel_orb_class_instance; int _accel_class_instance; - ringbuffer::RingBuffer *_gyro_reports; + ringbuffer::RingBuffer *_sensor_gyro_ss; - struct gyro_calibration_s _gyro_scale; + calibration_gyro_s _gyro_scale; float _gyro_range_scale; float _gyro_range_rad_s; @@ -489,14 +493,14 @@ MPU6000::MPU6000(device::Device *interface, const char *path_accel, const char * #endif _call {}, _call_interval(0), - _accel_reports(nullptr), + _sensor_accel_ss(nullptr), _accel_scale{}, _accel_range_scale(0.0f), _accel_range_m_s2(0.0f), _accel_topic(nullptr), _accel_orb_class_instance(-1), _accel_class_instance(-1), - _gyro_reports(nullptr), + _sensor_gyro_ss(nullptr), _gyro_scale{}, _gyro_range_scale(0.0f), _gyro_range_rad_s(0.0f), @@ -600,12 +604,12 @@ MPU6000::~MPU6000() delete _gyro; /* free any existing reports */ - if (_accel_reports != nullptr) { - delete _accel_reports; + if (_sensor_accel_ss != nullptr) { + delete _sensor_accel_ss; } - if (_gyro_reports != nullptr) { - delete _gyro_reports; + if (_sensor_gyro_ss != nullptr) { + delete _sensor_gyro_ss; } if (_accel_class_instance != -1) { @@ -657,15 +661,15 @@ MPU6000::init() ret = -ENOMEM; /* allocate basic report buffers */ - _accel_reports = new ringbuffer::RingBuffer(2, sizeof(accel_report)); + _sensor_accel_ss = new ringbuffer::RingBuffer(2, sizeof(sensor_accel_s)); - if (_accel_reports == nullptr) { + if (_sensor_accel_ss == nullptr) { return ret; } - _gyro_reports = new ringbuffer::RingBuffer(2, sizeof(gyro_report)); + _sensor_gyro_ss = new ringbuffer::RingBuffer(2, sizeof(sensor_gyro_s)); - if (_gyro_reports == nullptr) { + if (_sensor_gyro_ss == nullptr) { return ret; } @@ -733,8 +737,8 @@ MPU6000::init() measure(); /* advertise sensor topic, measure manually to initialize valid report */ - struct accel_report arp; - _accel_reports->get(&arp); + sensor_accel_s arp; + _sensor_accel_ss->get(&arp); /* measurement will have generated a report, publish */ _accel_topic = orb_advertise_multi(ORB_ID(sensor_accel), &arp, @@ -745,8 +749,8 @@ MPU6000::init() } /* advertise sensor topic, measure manually to initialize valid report */ - struct gyro_report grp; - _gyro_reports->get(&grp); + sensor_gyro_s grp; + _sensor_gyro_ss->get(&grp); _gyro->_gyro_topic = orb_advertise_multi(ORB_ID(sensor_gyro), &grp, &_gyro->_gyro_orb_class_instance, (is_external()) ? ORB_PRIO_MAX : ORB_PRIO_HIGH); @@ -1033,7 +1037,7 @@ MPU6000::_set_icm_acc_dlpf_filter(uint16_t frequency_hz) ssize_t MPU6000::read(struct file *filp, char *buffer, size_t buflen) { - unsigned count = buflen / sizeof(accel_report); + unsigned count = buflen / sizeof(sensor_accel_s); /* buffer must be large enough */ if (count < 1) { @@ -1042,23 +1046,23 @@ MPU6000::read(struct file *filp, char *buffer, size_t buflen) /* if automatic measurement is not enabled, get a fresh measurement into the buffer */ if (_call_interval == 0) { - _accel_reports->flush(); + _sensor_accel_ss->flush(); measure(); } /* if no data, error (we could block here) */ - if (_accel_reports->empty()) { + if (_sensor_accel_ss->empty()) { return -EAGAIN; } perf_count(_accel_reads); /* copy reports out of our buffer to the caller */ - accel_report *arp = reinterpret_cast(buffer); + sensor_accel_s *arp = reinterpret_cast(buffer); int transferred = 0; while (count--) { - if (!_accel_reports->get(arp)) { + if (!_sensor_accel_ss->get(arp)) { break; } @@ -1067,7 +1071,7 @@ MPU6000::read(struct file *filp, char *buffer, size_t buflen) } /* return the number of bytes transferred */ - return (transferred * sizeof(accel_report)); + return (transferred * sizeof(sensor_accel_s)); } int @@ -1318,7 +1322,7 @@ MPU6000::test_error() ssize_t MPU6000::gyro_read(struct file *filp, char *buffer, size_t buflen) { - unsigned count = buflen / sizeof(gyro_report); + unsigned count = buflen / sizeof(sensor_gyro_s); /* buffer must be large enough */ if (count < 1) { @@ -1327,23 +1331,23 @@ MPU6000::gyro_read(struct file *filp, char *buffer, size_t buflen) /* if automatic measurement is not enabled, get a fresh measurement into the buffer */ if (_call_interval == 0) { - _gyro_reports->flush(); + _sensor_gyro_ss->flush(); measure(); } /* if no data, error (we could block here) */ - if (_gyro_reports->empty()) { + if (_sensor_gyro_ss->empty()) { return -EAGAIN; } perf_count(_gyro_reads); /* copy reports out of our buffer to the caller */ - gyro_report *grp = reinterpret_cast(buffer); + sensor_gyro_s *grp = reinterpret_cast(buffer); int transferred = 0; while (count--) { - if (!_gyro_reports->get(grp)) { + if (!_sensor_gyro_ss->get(grp)) { break; } @@ -1352,7 +1356,7 @@ MPU6000::gyro_read(struct file *filp, char *buffer, size_t buflen) } /* return the number of bytes transferred */ - return (transferred * sizeof(gyro_report)); + return (transferred * sizeof(sensor_gyro_s)); } int @@ -1454,7 +1458,7 @@ MPU6000::ioctl(struct file *filp, int cmd, unsigned long arg) irqstate_t flags = px4_enter_critical_section(); - if (!_accel_reports->resize(arg)) { + if (!_sensor_accel_ss->resize(arg)) { px4_leave_critical_section(flags); return -ENOMEM; } @@ -1473,7 +1477,7 @@ MPU6000::ioctl(struct file *filp, int cmd, unsigned long arg) case ACCELIOCSSCALE: { /* copy scale, but only if off by a few percent */ - struct accel_calibration_s *s = (struct accel_calibration_s *) arg; + calibration_accel_s *s = (calibration_accel_s *) arg; float sum = s->x_scale + s->y_scale + s->z_scale; if (sum > 2.0f && sum < 4.0f) { @@ -1487,7 +1491,7 @@ MPU6000::ioctl(struct file *filp, int cmd, unsigned long arg) case ACCELIOCGSCALE: /* copy scale out */ - memcpy((struct accel_calibration_s *) arg, &_accel_scale, sizeof(_accel_scale)); + memcpy((calibration_accel_s *) arg, &_accel_scale, sizeof(_accel_scale)); return OK; case ACCELIOCSRANGE: @@ -1527,7 +1531,7 @@ MPU6000::gyro_ioctl(struct file *filp, int cmd, unsigned long arg) irqstate_t flags = px4_enter_critical_section(); - if (!_gyro_reports->resize(arg)) { + if (!_sensor_gyro_ss->resize(arg)) { px4_leave_critical_section(flags); return -ENOMEM; } @@ -1546,12 +1550,12 @@ MPU6000::gyro_ioctl(struct file *filp, int cmd, unsigned long arg) case GYROIOCSSCALE: /* copy scale in */ - memcpy(&_gyro_scale, (struct gyro_calibration_s *) arg, sizeof(_gyro_scale)); + memcpy(&_gyro_scale, (calibration_gyro_s *) arg, sizeof(_gyro_scale)); return OK; case GYROIOCGSCALE: /* copy scale out */ - memcpy((struct gyro_calibration_s *) arg, &_gyro_scale, sizeof(_gyro_scale)); + memcpy((calibration_gyro_s *) arg, &_gyro_scale, sizeof(_gyro_scale)); return OK; case GYROIOCSRANGE: @@ -1681,8 +1685,8 @@ MPU6000::start() _call_interval = last_call_interval; /* discard any stale data in the buffers */ - _accel_reports->flush(); - _gyro_reports->flush(); + _sensor_accel_ss->flush(); + _sensor_gyro_ss->flush(); if (!is_i2c()) { /* start polling at the specified rate */ @@ -1717,12 +1721,12 @@ MPU6000::stop() memset(_last_accel, 0, sizeof(_last_accel)); /* discard unread data in the buffers */ - if (_accel_reports != nullptr) { - _accel_reports->flush(); + if (_sensor_accel_ss != nullptr) { + _sensor_accel_ss->flush(); } - if (_gyro_reports != nullptr) { - _gyro_reports->flush(); + if (_sensor_gyro_ss != nullptr) { + _sensor_gyro_ss->flush(); } } @@ -1950,8 +1954,8 @@ MPU6000::measure() /* * Report buffers. */ - accel_report arb; - gyro_report grb; + sensor_accel_s arb; + sensor_gyro_s grb; /* * Adjust and scale results to m/s^2. @@ -2061,8 +2065,8 @@ MPU6000::measure() /* return device ID */ grb.device_id = _gyro->_device_id.devid; - _accel_reports->force(&arb); - _gyro_reports->force(&grb); + _sensor_accel_ss->force(&arb); + _sensor_gyro_ss->force(&grb); /* notify anyone waiting for data */ if (accel_notify) { @@ -2099,8 +2103,8 @@ MPU6000::print_info() perf_print_counter(_good_transfers); perf_print_counter(_reset_retries); perf_print_counter(_duplicates); - _accel_reports->print_info("accel queue"); - _gyro_reports->print_info("gyro queue"); + _sensor_accel_ss->print_info("accel queue"); + _sensor_gyro_ss->print_info("gyro queue"); ::printf("checked_next: %u\n", _checked_next); for (uint8_t i = 0; i < MPU6000_NUM_CHECKED_REGISTERS; i++) { @@ -2119,6 +2123,8 @@ MPU6000::print_info() ::printf("accel cutoff set to %10.2f Hz\n", double(accel_cut)); float gyro_cut = _gyro_filter_x.get_cutoff_freq(); ::printf("gyro cutoff set to %10.2f Hz\n", double(gyro_cut)); + + print_message(_gyro_scale); } void @@ -2412,8 +2418,8 @@ void test(enum MPU6000_BUS busid) { struct mpu6000_bus_option &bus = find_bus(busid); - accel_report a_report; - gyro_report g_report; + sensor_accel_s a_report; + sensor_gyro_s g_report; ssize_t sz; /* get the driver */ diff --git a/src/drivers/imu/mpu9250/main.cpp b/src/drivers/imu/mpu9250/main.cpp index 73c047d42563..a2597fea2bcd 100644 --- a/src/drivers/imu/mpu9250/main.cpp +++ b/src/drivers/imu/mpu9250/main.cpp @@ -302,8 +302,8 @@ void test(enum MPU9250_BUS busid) { struct mpu9250_bus_option &bus = find_bus(busid); - accel_report a_report; - gyro_report g_report; + sensor_accel_s a_report; + sensor_gyro_s g_report; mag_report m_report; ssize_t sz; diff --git a/src/drivers/imu/mpu9250/mpu9250.cpp b/src/drivers/imu/mpu9250/mpu9250.cpp index c66b3db433a4..30e19c3de608 100644 --- a/src/drivers/imu/mpu9250/mpu9250.cpp +++ b/src/drivers/imu/mpu9250/mpu9250.cpp @@ -127,14 +127,14 @@ MPU9250::MPU9250(device::Device *interface, device::Device *mag_interface, const #endif _call {}, _call_interval(0), - _accel_reports(nullptr), + _sensor_accel_ss(nullptr), _accel_scale{}, _accel_range_scale(0.0f), _accel_range_m_s2(0.0f), _accel_topic(nullptr), _accel_orb_class_instance(-1), _accel_class_instance(-1), - _gyro_reports(nullptr), + _sensor_gyro_ss(nullptr), _gyro_scale{}, _gyro_range_scale(0.0f), _gyro_range_rad_s(0.0f), @@ -228,12 +228,12 @@ MPU9250::~MPU9250() delete _mag; /* free any existing reports */ - if (_accel_reports != nullptr) { - delete _accel_reports; + if (_sensor_accel_ss != nullptr) { + delete _sensor_accel_ss; } - if (_gyro_reports != nullptr) { - delete _gyro_reports; + if (_sensor_gyro_ss != nullptr) { + delete _sensor_gyro_ss; } if (_accel_class_instance != -1) { @@ -289,16 +289,16 @@ MPU9250::init() } /* allocate basic report buffers */ - _accel_reports = new ringbuffer::RingBuffer(2, sizeof(accel_report)); + _sensor_accel_ss = new ringbuffer::RingBuffer(2, sizeof(sensor_accel_s)); ret = -ENOMEM; - if (_accel_reports == nullptr) { + if (_sensor_accel_ss == nullptr) { return ret; } - _gyro_reports = new ringbuffer::RingBuffer(2, sizeof(gyro_report)); + _sensor_gyro_ss = new ringbuffer::RingBuffer(2, sizeof(sensor_gyro_s)); - if (_gyro_reports == nullptr) { + if (_sensor_gyro_ss == nullptr) { return ret; } @@ -395,8 +395,8 @@ MPU9250::init() measure(); /* advertise sensor topic, measure manually to initialize valid report */ - struct accel_report arp; - _accel_reports->get(&arp); + sensor_accel_s arp; + _sensor_accel_ss->get(&arp); /* measurement will have generated a report, publish */ _accel_topic = orb_advertise_multi(ORB_ID(sensor_accel), &arp, @@ -408,8 +408,8 @@ MPU9250::init() } /* advertise sensor topic, measure manually to initialize valid report */ - struct gyro_report grp; - _gyro_reports->get(&grp); + sensor_gyro_s grp; + _sensor_gyro_ss->get(&grp); _gyro->_gyro_topic = orb_advertise_multi(ORB_ID(sensor_gyro), &grp, &_gyro->_gyro_orb_class_instance, (is_external()) ? ORB_PRIO_MAX - 1 : ORB_PRIO_HIGH - 1); @@ -627,7 +627,7 @@ MPU9250::_set_dlpf_filter(uint16_t frequency_hz) ssize_t MPU9250::read(struct file *filp, char *buffer, size_t buflen) { - unsigned count = buflen / sizeof(accel_report); + unsigned count = buflen / sizeof(sensor_accel_s); /* buffer must be large enough */ if (count < 1) { @@ -636,23 +636,23 @@ MPU9250::read(struct file *filp, char *buffer, size_t buflen) /* if automatic measurement is not enabled, get a fresh measurement into the buffer */ if (_call_interval == 0) { - _accel_reports->flush(); + _sensor_accel_ss->flush(); measure(); } /* if no data, error (we could block here) */ - if (_accel_reports->empty()) { + if (_sensor_accel_ss->empty()) { return -EAGAIN; } perf_count(_accel_reads); /* copy reports out of our buffer to the caller */ - accel_report *arp = reinterpret_cast(buffer); + sensor_accel_s *arp = reinterpret_cast(buffer); int transferred = 0; while (count--) { - if (!_accel_reports->get(arp)) { + if (!_sensor_accel_ss->get(arp)) { break; } @@ -661,7 +661,7 @@ MPU9250::read(struct file *filp, char *buffer, size_t buflen) } /* return the number of bytes transferred */ - return (transferred * sizeof(accel_report)); + return (transferred * sizeof(sensor_accel_s)); } int @@ -752,7 +752,7 @@ MPU9250::test_error() ssize_t MPU9250::gyro_read(struct file *filp, char *buffer, size_t buflen) { - unsigned count = buflen / sizeof(gyro_report); + unsigned count = buflen / sizeof(sensor_gyro_s); /* buffer must be large enough */ if (count < 1) { @@ -761,23 +761,23 @@ MPU9250::gyro_read(struct file *filp, char *buffer, size_t buflen) /* if automatic measurement is not enabled, get a fresh measurement into the buffer */ if (_call_interval == 0) { - _gyro_reports->flush(); + _sensor_gyro_ss->flush(); measure(); } /* if no data, error (we could block here) */ - if (_gyro_reports->empty()) { + if (_sensor_gyro_ss->empty()) { return -EAGAIN; } perf_count(_gyro_reads); /* copy reports out of our buffer to the caller */ - gyro_report *grp = reinterpret_cast(buffer); + sensor_gyro_s *grp = reinterpret_cast(buffer); int transferred = 0; while (count--) { - if (!_gyro_reports->get(grp)) { + if (!_sensor_gyro_ss->get(grp)) { break; } @@ -786,7 +786,7 @@ MPU9250::gyro_read(struct file *filp, char *buffer, size_t buflen) } /* return the number of bytes transferred */ - return (transferred * sizeof(gyro_report)); + return (transferred * sizeof(sensor_gyro_s)); } int @@ -884,7 +884,7 @@ MPU9250::ioctl(struct file *filp, int cmd, unsigned long arg) irqstate_t flags = px4_enter_critical_section(); - if (!_accel_reports->resize(arg)) { + if (!_sensor_accel_ss->resize(arg)) { px4_leave_critical_section(flags); return -ENOMEM; } @@ -903,7 +903,7 @@ MPU9250::ioctl(struct file *filp, int cmd, unsigned long arg) case ACCELIOCSSCALE: { /* copy scale, but only if off by a few percent */ - struct accel_calibration_s *s = (struct accel_calibration_s *) arg; + calibration_accel_s *s = (calibration_accel_s *) arg; float sum = s->x_scale + s->y_scale + s->z_scale; if (sum > 2.0f && sum < 4.0f) { @@ -917,7 +917,7 @@ MPU9250::ioctl(struct file *filp, int cmd, unsigned long arg) case ACCELIOCGSCALE: /* copy scale out */ - memcpy((struct accel_calibration_s *) arg, &_accel_scale, sizeof(_accel_scale)); + memcpy((calibration_accel_s *) arg, &_accel_scale, sizeof(_accel_scale)); return OK; case ACCELIOCSRANGE: @@ -954,7 +954,7 @@ MPU9250::gyro_ioctl(struct file *filp, int cmd, unsigned long arg) irqstate_t flags = px4_enter_critical_section(); - if (!_gyro_reports->resize(arg)) { + if (!_sensor_gyro_ss->resize(arg)) { px4_leave_critical_section(flags); return -ENOMEM; } @@ -973,12 +973,12 @@ MPU9250::gyro_ioctl(struct file *filp, int cmd, unsigned long arg) case GYROIOCSSCALE: /* copy scale in */ - memcpy(&_gyro_scale, (struct gyro_calibration_s *) arg, sizeof(_gyro_scale)); + memcpy(&_gyro_scale, (calibration_gyro_s *) arg, sizeof(_gyro_scale)); return OK; case GYROIOCGSCALE: /* copy scale out */ - memcpy((struct gyro_calibration_s *) arg, &_gyro_scale, sizeof(_gyro_scale)); + memcpy((calibration_gyro_s *) arg, &_gyro_scale, sizeof(_gyro_scale)); return OK; case GYROIOCSRANGE: @@ -1105,8 +1105,8 @@ MPU9250::start() stop(); /* discard any stale data in the buffers */ - _accel_reports->flush(); - _gyro_reports->flush(); + _sensor_accel_ss->flush(); + _sensor_gyro_ss->flush(); _mag->_mag_reports->flush(); if (_use_hrt) { @@ -1379,8 +1379,8 @@ MPU9250::measure() /* * Report buffers. */ - accel_report arb; - gyro_report grb; + sensor_accel_s arb; + sensor_gyro_s grb; /* * Adjust and scale results to m/s^2. @@ -1484,8 +1484,8 @@ MPU9250::measure() /* return device ID */ grb.device_id = _gyro->_device_id.devid; - _accel_reports->force(&arb); - _gyro_reports->force(&grb); + _sensor_accel_ss->force(&arb); + _sensor_gyro_ss->force(&grb); /* notify anyone waiting for data */ if (accel_notify) { @@ -1521,8 +1521,8 @@ MPU9250::print_info() perf_print_counter(_good_transfers); perf_print_counter(_reset_retries); perf_print_counter(_duplicates); - _accel_reports->print_info("accel queue"); - _gyro_reports->print_info("gyro queue"); + _sensor_accel_ss->print_info("accel queue"); + _sensor_gyro_ss->print_info("gyro queue"); _mag->_mag_reports->print_info("mag queue"); ::printf("checked_next: %u\n", _checked_next); @@ -1549,6 +1549,9 @@ MPU9250::print_info() ::printf("accel cutoff set to %10.2f Hz\n", double(accel_cut)); float gyro_cut = _gyro_filter_x.get_cutoff_freq(); ::printf("gyro cutoff set to %10.2f Hz\n", double(gyro_cut)); + + print_message(_gyro_scale); + print_message(_accel_scale); } void diff --git a/src/drivers/imu/mpu9250/mpu9250.h b/src/drivers/imu/mpu9250/mpu9250.h index 15f11927aedc..d7259b336698 100644 --- a/src/drivers/imu/mpu9250/mpu9250.h +++ b/src/drivers/imu/mpu9250/mpu9250.h @@ -48,12 +48,14 @@ #include #include #include +#include +#include +#include +#include #include "mag.h" #include "gyro.h" - - #if defined(PX4_I2C_OBDEV_MPU9250) # define USE_I2C #endif @@ -291,18 +293,18 @@ class MPU9250 : public device::CDev struct hrt_call _call; unsigned _call_interval; - ringbuffer::RingBuffer *_accel_reports; + ringbuffer::RingBuffer *_sensor_accel_ss; - struct accel_calibration_s _accel_scale; + calibration_accel_s _accel_scale; float _accel_range_scale; float _accel_range_m_s2; orb_advert_t _accel_topic; int _accel_orb_class_instance; int _accel_class_instance; - ringbuffer::RingBuffer *_gyro_reports; + ringbuffer::RingBuffer *_sensor_gyro_ss; - struct gyro_calibration_s _gyro_scale; + calibration_gyro_s _gyro_scale; float _gyro_range_scale; float _gyro_range_rad_s; diff --git a/src/examples/matlab_csv_serial/matlab_csv_serial.c b/src/examples/matlab_csv_serial/matlab_csv_serial.c index 8e6cd45a0029..948a1da7a825 100644 --- a/src/examples/matlab_csv_serial/matlab_csv_serial.c +++ b/src/examples/matlab_csv_serial/matlab_csv_serial.c @@ -184,10 +184,10 @@ int matlab_csv_serial_thread_main(int argc, char *argv[]) } /* subscribe to vehicle status, attitude, sensors and flow*/ - struct accel_report accel0; - struct accel_report accel1; - struct gyro_report gyro0; - struct gyro_report gyro1; + sensor_accel_s accel0; + sensor_accel_s accel1; + sensor_gyro_s gyro0; + sensor_gyro_s gyro1; /* subscribe to parameter changes */ int accel0_sub = orb_subscribe_multi(ORB_ID(sensor_accel), 0); diff --git a/src/modules/commander/PreflightCheck.cpp b/src/modules/commander/PreflightCheck.cpp index fecc9176c97c..d7b6de5a702c 100644 --- a/src/modules/commander/PreflightCheck.cpp +++ b/src/modules/commander/PreflightCheck.cpp @@ -69,6 +69,10 @@ #include #include #include +#include +#include +#include +#include #include "PreflightCheck.h" @@ -290,7 +294,7 @@ static bool accelerometerCheck(orb_advert_t *mavlink_log_pub, unsigned instance, if (dynamic) { /* check measurement result range */ - struct accel_report acc; + sensor_accel_s acc; ret = h.read(&acc, sizeof(acc)); if (ret == sizeof(acc)) { diff --git a/src/modules/commander/accelerometer_calibration.cpp b/src/modules/commander/accelerometer_calibration.cpp index 455f1e5f949e..d163f6d85911 100644 --- a/src/modules/commander/accelerometer_calibration.cpp +++ b/src/modules/commander/accelerometer_calibration.cpp @@ -148,6 +148,8 @@ #include #include #include +#include +#include static const char *sensor_name = "accel"; @@ -171,13 +173,11 @@ typedef struct { int do_accel_calibration(orb_advert_t *mavlink_log_pub) { -#ifdef __PX4_NUTTX - int fd; -#endif + int fd = -1; calibration_log_info(mavlink_log_pub, CAL_QGC_STARTED_MSG, sensor_name); - struct accel_calibration_s accel_scale; + calibration_accel_s accel_scale; accel_scale.x_offset = 0.0f; accel_scale.x_scale = 1.0f; accel_scale.y_offset = 0.0f; @@ -191,56 +191,47 @@ int do_accel_calibration(orb_advert_t *mavlink_log_pub) /* reset all sensors */ for (unsigned s = 0; s < max_accel_sens; s++) { -#ifdef __PX4_NUTTX - sprintf(str, "%s%u", ACCEL_BASE_DEVICE_PATH, s); - /* reset all offsets to zero and all scales to one */ - fd = px4_open(str, 0); - - if (fd < 0) { - continue; - } - - device_id[s] = px4_ioctl(fd, DEVIOCGDEVICEID, 0); - res = px4_ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&accel_scale); - px4_close(fd); - - if (res != PX4_OK) { - calibration_log_critical(mavlink_log_pub, CAL_ERROR_RESET_CAL_MSG, s); - } -#else + // offsets (void)sprintf(str, "CAL_ACC%u_XOFF", s); res = param_set_no_notification(param_find(str), &accel_scale.x_offset); if (res != PX4_OK) { PX4_ERR("unable to reset %s", str); } + (void)sprintf(str, "CAL_ACC%u_YOFF", s); res = param_set_no_notification(param_find(str), &accel_scale.y_offset); if (res != PX4_OK) { PX4_ERR("unable to reset %s", str); } + (void)sprintf(str, "CAL_ACC%u_ZOFF", s); res = param_set_no_notification(param_find(str), &accel_scale.z_offset); if (res != PX4_OK) { PX4_ERR("unable to reset %s", str); } + + // scale factors (void)sprintf(str, "CAL_ACC%u_XSCALE", s); res = param_set_no_notification(param_find(str), &accel_scale.x_scale); if (res != PX4_OK) { PX4_ERR("unable to reset %s", str); } + (void)sprintf(str, "CAL_ACC%u_YSCALE", s); res = param_set_no_notification(param_find(str), &accel_scale.y_scale); if (res != PX4_OK) { PX4_ERR("unable to reset %s", str); } + (void)sprintf(str, "CAL_ACC%u_ZSCALE", s); res = param_set_no_notification(param_find(str), &accel_scale.z_scale); if (res != PX4_OK) { PX4_ERR("unable to reset %s", str); } + + param_notify_changes(); -#endif } float accel_offs[max_accel_sens][3]; @@ -387,7 +378,6 @@ int do_accel_calibration(orb_advert_t *mavlink_log_pub) return PX4_ERROR; } -#ifdef __PX4_NUTTX sprintf(str, "%s%u", ACCEL_BASE_DEVICE_PATH, uorb_index); fd = px4_open(str, 0); @@ -402,7 +392,6 @@ int do_accel_calibration(orb_advert_t *mavlink_log_pub) if (res != PX4_OK) { calibration_log_critical(mavlink_log_pub, CAL_ERROR_APPLY_CAL_MSG, uorb_index); } -#endif } if (res == PX4_OK) { @@ -479,16 +468,14 @@ calibrate_return do_accel_calibration_measurements(orb_advert_t *mavlink_log_pub for(unsigned i = 0; i < orb_accel_count && !found_cur_accel; i++) { worker_data.subs[cur_accel] = orb_subscribe_multi(ORB_ID(sensor_accel), i); - struct accel_report report = {}; + sensor_accel_s report = {}; orb_copy(ORB_ID(sensor_accel), worker_data.subs[cur_accel], &report); #ifdef __PX4_NUTTX - // For NuttX, we get the UNIQUE device ID from the sensor driver via an IOCTL // and match it up with the one from the uORB subscription, because the // instance ordering of uORB and the order of the FDs may not be the same. - - if(report.device_id == device_id[cur_accel]) { + if (report.device_id == device_id[cur_accel]) { // Device IDs match, correct ORB instance for this accel found_cur_accel = true; // store initial timestamp - used to infer which sensors are active @@ -496,17 +483,14 @@ calibrate_return do_accel_calibration_measurements(orb_advert_t *mavlink_log_pub } else { orb_unsubscribe(worker_data.subs[cur_accel]); } - #else - // For the DriverFramework drivers, we fill device ID (this is the first time) by copying one report. device_id[cur_accel] = report.device_id; found_cur_accel = true; - #endif } - if(!found_cur_accel) { + if (!found_cur_accel) { calibration_log_critical(mavlink_log_pub, "Accel #%u (ID %u) no matching uORB devid", cur_accel, device_id[cur_accel]); result = calibrate_return_error; break; @@ -538,7 +522,7 @@ calibrate_return do_accel_calibration_measurements(orb_advert_t *mavlink_log_pub for (unsigned i = 0; i < max_accel_sens; i++) { if (worker_data.subs[i] >= 0) { /* figure out which sensors were active */ - struct accel_report arp = {}; + sensor_accel_s arp = {}; (void)orb_copy(ORB_ID(sensor_accel), worker_data.subs[i], &arp); if (arp.timestamp != 0 && timestamps[i] != arp.timestamp) { (*active_sensors)++; @@ -626,7 +610,7 @@ calibrate_return read_accelerometer_avg(int sensor_correction_sub, int (&subs)[m if (changed) { - struct accel_report arp; + sensor_accel_s arp; orb_copy(ORB_ID(sensor_accel), subs[s], &arp); // Apply thermal offset corrections in sensor/board frame diff --git a/src/modules/commander/gyro_calibration.cpp b/src/modules/commander/gyro_calibration.cpp index 2e5e33a04e13..83aa7ef1751a 100644 --- a/src/modules/commander/gyro_calibration.cpp +++ b/src/modules/commander/gyro_calibration.cpp @@ -58,7 +58,8 @@ #include #include #include -#include +#include +#include static const char *sensor_name = "gyro"; @@ -70,8 +71,8 @@ typedef struct { int32_t device_id[max_gyros]; int gyro_sensor_sub[max_gyros]; int sensor_correction_sub; - struct gyro_calibration_s gyro_scale[max_gyros]; - struct gyro_report gyro_report_0; + calibration_gyro_s gyro_scale[max_gyros]; + sensor_gyro_s sensor_gyro_s_0; } gyro_worker_data_t; static calibrate_return gyro_calibration_worker(int cancel_sub, void* data) @@ -79,7 +80,7 @@ static calibrate_return gyro_calibration_worker(int cancel_sub, void* data) gyro_worker_data_t* worker_data = (gyro_worker_data_t*)(data); unsigned calibration_counter[max_gyros] = { 0 }, slow_count = 0; const unsigned calibration_count = 5000; - struct gyro_report gyro_report; + sensor_gyro_s sensor_gyro_s; unsigned poll_errcount = 0; struct sensor_correction_s sensor_correction; /**< sensor thermal corrections */ @@ -99,7 +100,7 @@ static calibrate_return gyro_calibration_worker(int cancel_sub, void* data) fds[s].events = POLLIN; } - memset(&worker_data->gyro_report_0, 0, sizeof(worker_data->gyro_report_0)); + memset(&worker_data->sensor_gyro_s_0, 0, sizeof(worker_data->sensor_gyro_s_0)); /* use slowest gyro to pace, but count correctly per-gyro for statistics */ @@ -130,34 +131,34 @@ static calibrate_return gyro_calibration_worker(int cancel_sub, void* data) orb_check(worker_data->gyro_sensor_sub[s], &changed); if (changed) { - orb_copy(ORB_ID(sensor_gyro), worker_data->gyro_sensor_sub[s], &gyro_report); + orb_copy(ORB_ID(sensor_gyro), worker_data->gyro_sensor_sub[s], &sensor_gyro_s); if (s == 0) { // take a working copy - worker_data->gyro_scale[s].x_offset += (gyro_report.x - sensor_correction.gyro_offset_0[0]) * sensor_correction.gyro_scale_0[0]; - worker_data->gyro_scale[s].y_offset += (gyro_report.y - sensor_correction.gyro_offset_0[1]) * sensor_correction.gyro_scale_0[1]; - worker_data->gyro_scale[s].z_offset += (gyro_report.z - sensor_correction.gyro_offset_0[2]) * sensor_correction.gyro_scale_0[2]; + worker_data->gyro_scale[s].x_offset += (sensor_gyro_s.x - sensor_correction.gyro_offset_0[0]) * sensor_correction.gyro_scale_0[0]; + worker_data->gyro_scale[s].y_offset += (sensor_gyro_s.y - sensor_correction.gyro_offset_0[1]) * sensor_correction.gyro_scale_0[1]; + worker_data->gyro_scale[s].z_offset += (sensor_gyro_s.z - sensor_correction.gyro_offset_0[2]) * sensor_correction.gyro_scale_0[2]; // take a reference copy of the primary sensor including correction for thermal drift - orb_copy(ORB_ID(sensor_gyro), worker_data->gyro_sensor_sub[s], &worker_data->gyro_report_0); - worker_data->gyro_report_0.x = (gyro_report.x - sensor_correction.gyro_offset_0[0]) * sensor_correction.gyro_scale_0[0]; - worker_data->gyro_report_0.y = (gyro_report.y - sensor_correction.gyro_offset_0[1]) * sensor_correction.gyro_scale_0[1]; - worker_data->gyro_report_0.z = (gyro_report.z - sensor_correction.gyro_offset_0[2]) * sensor_correction.gyro_scale_0[2]; + orb_copy(ORB_ID(sensor_gyro), worker_data->gyro_sensor_sub[s], &worker_data->sensor_gyro_s_0); + worker_data->sensor_gyro_s_0.x = (sensor_gyro_s.x - sensor_correction.gyro_offset_0[0]) * sensor_correction.gyro_scale_0[0]; + worker_data->sensor_gyro_s_0.y = (sensor_gyro_s.y - sensor_correction.gyro_offset_0[1]) * sensor_correction.gyro_scale_0[1]; + worker_data->sensor_gyro_s_0.z = (sensor_gyro_s.z - sensor_correction.gyro_offset_0[2]) * sensor_correction.gyro_scale_0[2]; } else if (s == 1) { - worker_data->gyro_scale[s].x_offset += (gyro_report.x - sensor_correction.gyro_offset_1[0]) * sensor_correction.gyro_scale_1[0]; - worker_data->gyro_scale[s].y_offset += (gyro_report.y - sensor_correction.gyro_offset_1[1]) * sensor_correction.gyro_scale_1[1]; - worker_data->gyro_scale[s].z_offset += (gyro_report.z - sensor_correction.gyro_offset_1[2]) * sensor_correction.gyro_scale_1[2]; + worker_data->gyro_scale[s].x_offset += (sensor_gyro_s.x - sensor_correction.gyro_offset_1[0]) * sensor_correction.gyro_scale_1[0]; + worker_data->gyro_scale[s].y_offset += (sensor_gyro_s.y - sensor_correction.gyro_offset_1[1]) * sensor_correction.gyro_scale_1[1]; + worker_data->gyro_scale[s].z_offset += (sensor_gyro_s.z - sensor_correction.gyro_offset_1[2]) * sensor_correction.gyro_scale_1[2]; } else if (s == 2) { - worker_data->gyro_scale[s].x_offset += (gyro_report.x - sensor_correction.gyro_offset_2[0]) * sensor_correction.gyro_scale_2[0]; - worker_data->gyro_scale[s].y_offset += (gyro_report.y - sensor_correction.gyro_offset_2[1]) * sensor_correction.gyro_scale_2[1]; - worker_data->gyro_scale[s].z_offset += (gyro_report.z - sensor_correction.gyro_offset_2[2]) * sensor_correction.gyro_scale_2[2]; + worker_data->gyro_scale[s].x_offset += (sensor_gyro_s.x - sensor_correction.gyro_offset_2[0]) * sensor_correction.gyro_scale_2[0]; + worker_data->gyro_scale[s].y_offset += (sensor_gyro_s.y - sensor_correction.gyro_offset_2[1]) * sensor_correction.gyro_scale_2[1]; + worker_data->gyro_scale[s].z_offset += (sensor_gyro_s.z - sensor_correction.gyro_offset_2[2]) * sensor_correction.gyro_scale_2[2]; } else { - worker_data->gyro_scale[s].x_offset += gyro_report.x; - worker_data->gyro_scale[s].y_offset += gyro_report.y; - worker_data->gyro_scale[s].z_offset += gyro_report.z; + worker_data->gyro_scale[s].x_offset += sensor_gyro_s.x; + worker_data->gyro_scale[s].y_offset += sensor_gyro_s.y; + worker_data->gyro_scale[s].z_offset += sensor_gyro_s.z; } @@ -214,7 +215,7 @@ int do_gyro_calibration(orb_advert_t *mavlink_log_pub) worker_data.mavlink_log_pub = mavlink_log_pub; - struct gyro_calibration_s gyro_scale_zero; + calibration_gyro_s gyro_scale_zero; gyro_scale_zero.x_offset = 0.0f; gyro_scale_zero.x_scale = 1.0f; gyro_scale_zero.y_offset = 0.0f; @@ -243,20 +244,8 @@ int do_gyro_calibration(orb_advert_t *mavlink_log_pub) // Reset all offsets to 0 and scales to 1 (void)memcpy(&worker_data.gyro_scale[s], &gyro_scale_zero, sizeof(gyro_scale_zero)); -#ifdef __PX4_NUTTX - sprintf(str, "%s%u", GYRO_BASE_DEVICE_PATH, s); - int fd = px4_open(str, 0); - if (fd >= 0) { - worker_data.device_id[s] = px4_ioctl(fd, DEVIOCGDEVICEID, 0); - res = px4_ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gyro_scale_zero); - px4_close(fd); - - if (res != PX4_OK) { - calibration_log_critical(mavlink_log_pub, CAL_ERROR_RESET_CAL_MSG, s); - return PX4_ERROR; - } - } -#else + + // set offsets (void)sprintf(str, "CAL_GYRO%u_XOFF", s); res = param_set_no_notification(param_find(str), &gyro_scale_zero.x_offset); if (res != PX4_OK) { @@ -272,6 +261,8 @@ int do_gyro_calibration(orb_advert_t *mavlink_log_pub) if (res != PX4_OK) { PX4_ERR("unable to reset %s", str); } + + // set scale factors (void)sprintf(str, "CAL_GYRO%u_XSCALE", s); res = param_set_no_notification(param_find(str), &gyro_scale_zero.x_scale); if (res != PX4_OK) { @@ -287,9 +278,9 @@ int do_gyro_calibration(orb_advert_t *mavlink_log_pub) if (res != PX4_OK) { PX4_ERR("unable to reset %s", str); } - param_notify_changes(); -#endif + + param_notify_changes(); } // We should not try to subscribe if the topic doesn't actually exist and can be counted. @@ -307,7 +298,7 @@ int do_gyro_calibration(orb_advert_t *mavlink_log_pub) for(unsigned i = 0; i < orb_gyro_count && !found_cur_gyro; i++) { worker_data.gyro_sensor_sub[cur_gyro] = orb_subscribe_multi(ORB_ID(sensor_gyro), i); - struct gyro_report report; + sensor_gyro_s report; orb_copy(ORB_ID(sensor_gyro), worker_data.gyro_sensor_sub[cur_gyro], &report); #ifdef __PX4_NUTTX @@ -322,13 +313,10 @@ int do_gyro_calibration(orb_advert_t *mavlink_log_pub) } else { orb_unsubscribe(worker_data.gyro_sensor_sub[cur_gyro]); } - #else - // For the DriverFramework drivers, we fill device ID (this is the first time) by copying one report. worker_data.device_id[cur_gyro] = report.device_id; found_cur_gyro = true; - #endif } @@ -372,9 +360,9 @@ int do_gyro_calibration(orb_advert_t *mavlink_log_pub) } else { /* check offsets */ - float xdiff = worker_data.gyro_report_0.x - worker_data.gyro_scale[0].x_offset; - float ydiff = worker_data.gyro_report_0.y - worker_data.gyro_scale[0].y_offset; - float zdiff = worker_data.gyro_report_0.z - worker_data.gyro_scale[0].z_offset; + float xdiff = worker_data.sensor_gyro_s_0.x - worker_data.gyro_scale[0].x_offset; + float ydiff = worker_data.sensor_gyro_s_0.y - worker_data.gyro_scale[0].y_offset; + float zdiff = worker_data.sensor_gyro_s_0.z - worker_data.gyro_scale[0].z_offset; /* maximum allowable calibration error in radians */ const float maxoff = 0.01f; @@ -472,24 +460,6 @@ int do_gyro_calibration(orb_advert_t *mavlink_log_pub) (void)sprintf(str, "CAL_GYRO%u_ID", uorb_index); failed |= (PX4_OK != param_set_no_notification(param_find(str), &(worker_data.device_id[uorb_index]))); - -#ifdef __PX4_NUTTX - /* apply new scaling and offsets */ - (void)sprintf(str, "%s%u", GYRO_BASE_DEVICE_PATH, uorb_index); - int fd = px4_open(str, 0); - - if (fd < 0) { - failed = true; - continue; - } - - res = px4_ioctl(fd, GYROIOCSSCALE, (long unsigned int)&worker_data.gyro_scale[uorb_index]); - px4_close(fd); - - if (res != PX4_OK) { - calibration_log_critical(mavlink_log_pub, CAL_ERROR_APPLY_CAL_MSG, 1); - } -#endif } } diff --git a/src/modules/commander/mag_calibration.cpp b/src/modules/commander/mag_calibration.cpp index e05ee2d227da..4b252e341e1b 100644 --- a/src/modules/commander/mag_calibration.cpp +++ b/src/modules/commander/mag_calibration.cpp @@ -61,6 +61,7 @@ #include #include #include +#include static const char *sensor_name = "mag"; static constexpr unsigned max_mags = 4; @@ -139,7 +140,7 @@ int do_mag_calibration(orb_advert_t *mavlink_log_pub) _last_mag_progress = 0; for (unsigned cur_mag = 0; cur_mag < max_mags; cur_mag++) { -#ifdef __PX4_NUTTX + // Reset mag id to mag not available (void)sprintf(str, "CAL_MAG%u_ID", cur_mag); result = param_set_no_notification(param_find(str), &(device_ids[cur_mag])); @@ -149,7 +150,6 @@ int do_mag_calibration(orb_advert_t *mavlink_log_pub) break; } -#else (void)sprintf(str, "CAL_MAG%u_XOFF", cur_mag); result = param_set_no_notification(param_find(str), &mscale_null.x_offset); @@ -192,8 +192,6 @@ int do_mag_calibration(orb_advert_t *mavlink_log_pub) PX4_ERR("unable to reset %s", str); } -#endif - param_notify_changes(); #ifdef __PX4_NUTTX @@ -262,8 +260,7 @@ int do_mag_calibration(orb_advert_t *mavlink_log_pub) return result; } -static bool reject_sample(float sx, float sy, float sz, float x[], float y[], float z[], unsigned count, - unsigned max_count) +static bool reject_sample(float sx, float sy, float sz, float x[], float y[], float z[], unsigned count, unsigned max_count) { float min_sample_dist = fabsf(5.4f * mag_sphere_radius / sqrtf(max_count)) / 3.0f; @@ -396,7 +393,7 @@ static calibrate_return mag_calibration_worker(detect_orientation_return orienta int poll_ret = px4_poll(fds, fd_count, 1000); if (poll_ret > 0) { - struct gyro_report gyro; + sensor_gyro_s gyro; orb_copy(ORB_ID(sensor_gyro), sub_gyro, &gyro); /* ensure we have a valid first timestamp */ @@ -604,25 +601,22 @@ calibrate_return mag_calibrate_all(orb_advert_t *mavlink_log_pub) orb_copy(ORB_ID(sensor_mag), worker_data.sub_mag[cur_mag], &report); #ifdef __PX4_NUTTX - // For NuttX, we get the UNIQUE device ID from the sensor driver via an IOCTL // and match it up with the one from the uORB subscription, because the // instance ordering of uORB and the order of the FDs may not be the same. - if(report.device_id == device_ids[cur_mag]) { + if (report.device_id == device_ids[cur_mag]) { // Device IDs match, correct ORB instance for this mag found_cur_mag = true; } else { orb_unsubscribe(worker_data.sub_mag[cur_mag]); } - #else - // For the DriverFramework drivers, we fill device ID (this is the first time) by copying one report. device_ids[cur_mag] = report.device_id; found_cur_mag = true; - #endif + } if(!found_cur_mag) { @@ -792,17 +786,15 @@ calibrate_return mag_calibrate_all(orb_advert_t *mavlink_log_pub) for (unsigned cur_mag = 0; cur_mag < max_mags; cur_mag++) { if (device_ids[cur_mag] != 0) { - struct mag_calibration_s mscale; + mag_calibration_s mscale = {}; mscale.x_scale = 1.0; mscale.y_scale = 1.0; mscale.z_scale = 1.0; #ifdef __PX4_NUTTX - int fd_mag = -1; - // Set new scale (void)sprintf(str, "%s%u", MAG_BASE_DEVICE_PATH, cur_mag); - fd_mag = px4_open(str, 0); + int fd_mag = px4_open(str, 0); if (fd_mag < 0) { calibration_log_critical(mavlink_log_pub, "ERROR: unable to open mag device #%u", cur_mag); @@ -815,7 +807,6 @@ calibrate_return mag_calibrate_all(orb_advert_t *mavlink_log_pub) result = calibrate_return_error; } } - #endif if (result == calibrate_return_ok) { @@ -827,22 +818,18 @@ calibrate_return mag_calibrate_all(orb_advert_t *mavlink_log_pub) mscale.z_scale = diag_z[cur_mag]; #ifdef __PX4_NUTTX - if (px4_ioctl(fd_mag, MAGIOCSSCALE, (long unsigned int)&mscale) != PX4_OK) { calibration_log_critical(mavlink_log_pub, CAL_ERROR_APPLY_CAL_MSG, cur_mag); result = calibrate_return_error; } - #endif } #ifdef __PX4_NUTTX - // Mag device no longer needed if (fd_mag >= 0) { px4_close(fd_mag); } - #endif if (result == calibrate_return_ok) { @@ -852,6 +839,7 @@ calibrate_return mag_calibrate_all(orb_advert_t *mavlink_log_pub) (void)sprintf(str, "CAL_MAG%u_ID", cur_mag); failed |= (PX4_OK != param_set_no_notification(param_find(str), &(device_ids[cur_mag]))); + (void)sprintf(str, "CAL_MAG%u_XOFF", cur_mag); failed |= (PX4_OK != param_set_no_notification(param_find(str), &(mscale.x_offset))); (void)sprintf(str, "CAL_MAG%u_YOFF", cur_mag); @@ -859,15 +847,12 @@ calibrate_return mag_calibrate_all(orb_advert_t *mavlink_log_pub) (void)sprintf(str, "CAL_MAG%u_ZOFF", cur_mag); failed |= (PX4_OK != param_set_no_notification(param_find(str), &(mscale.z_offset))); - // FIXME: scaling is not used right now on QURT -#ifndef __PX4_QURT (void)sprintf(str, "CAL_MAG%u_XSCALE", cur_mag); failed |= (PX4_OK != param_set_no_notification(param_find(str), &(mscale.x_scale))); (void)sprintf(str, "CAL_MAG%u_YSCALE", cur_mag); failed |= (PX4_OK != param_set_no_notification(param_find(str), &(mscale.y_scale))); (void)sprintf(str, "CAL_MAG%u_ZSCALE", cur_mag); failed |= (PX4_OK != param_set_no_notification(param_find(str), &(mscale.z_scale))); -#endif if (failed) { calibration_log_critical(mavlink_log_pub, CAL_ERROR_SET_PARAMS_MSG, cur_mag); @@ -876,10 +861,10 @@ calibrate_return mag_calibrate_all(orb_advert_t *mavlink_log_pub) } else { calibration_log_info(mavlink_log_pub, "[cal] mag #%u off: x:%.2f y:%.2f z:%.2f Ga", cur_mag, (double)mscale.x_offset, (double)mscale.y_offset, (double)mscale.z_offset); -#ifndef __PX4_QURT + calibration_log_info(mavlink_log_pub, "[cal] mag #%u scale: x:%.2f y:%.2f z:%.2f", cur_mag, (double)mscale.x_scale, (double)mscale.y_scale, (double)mscale.z_scale); -#endif + usleep(200000); } } diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index dc6eb1c04e66..b7a526875b85 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -54,8 +54,6 @@ #include #include #include -#include -#include #include #include #include @@ -83,6 +81,8 @@ #include #include +#include +#include #include #include "mavlink_bridge_header.h" @@ -1872,7 +1872,7 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg) /* gyro */ { - struct gyro_report gyro = {}; + sensor_gyro_s gyro = {}; gyro.timestamp = timestamp; gyro.x_raw = imu.xgyro * 1000.0f; @@ -1893,7 +1893,7 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg) /* accelerometer */ { - struct accel_report accel = {}; + sensor_accel_s accel = {}; accel.timestamp = timestamp; accel.x_raw = imu.xacc / (CONSTANTS_ONE_G / 1000.0f); @@ -2289,7 +2289,7 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg) /* accelerometer */ { - struct accel_report accel = {}; + sensor_accel_s accel = {}; accel.timestamp = timestamp; accel.x_raw = hil_state.xacc / CONSTANTS_ONE_G * 1e3f; diff --git a/src/modules/sensors/voted_sensors_update.cpp b/src/modules/sensors/voted_sensors_update.cpp index d47a828fb59a..f73b6d83bc7c 100644 --- a/src/modules/sensors/voted_sensors_update.cpp +++ b/src/modules/sensors/voted_sensors_update.cpp @@ -152,19 +152,19 @@ void VotedSensorsUpdate::parameters_update() if (topic_instance < _gyro.subscription_count) { // valid subscription, so get the driver id by getting the published sensor data - struct gyro_report report; + sensor_gyro_s report; - if (orb_copy(ORB_ID(sensor_gyro), _gyro.subscription[topic_instance], &report) == 0) { + if (orb_copy(ORB_ID(sensor_gyro), _gyro.subscription[topic_instance], &report) == PX4_OK) { int temp = _temperature_compensation.set_sensor_id_gyro(report.device_id, topic_instance); if (temp < 0) { PX4_ERR("%s temp compensation init: failed to find device ID %u for instance %i", "gyro", report.device_id, topic_instance); + _corrections.gyro_mapping[topic_instance] = 0; } else { _corrections.gyro_mapping[topic_instance] = temp; - } } } @@ -176,7 +176,7 @@ void VotedSensorsUpdate::parameters_update() if (topic_instance < _accel.subscription_count) { // valid subscription, so get the driver id by getting the published sensor data - struct accel_report report; + sensor_accel_s report; if (orb_copy(ORB_ID(sensor_accel), _accel.subscription[topic_instance], &report) == 0) { int temp = _temperature_compensation.set_sensor_id_accel(report.device_id, topic_instance); @@ -184,11 +184,11 @@ void VotedSensorsUpdate::parameters_update() if (temp < 0) { PX4_ERR("%s temp compensation init: failed to find device ID %u for instance %i", "accel", report.device_id, topic_instance); + _corrections.accel_mapping[topic_instance] = 0; } else { _corrections.accel_mapping[topic_instance] = temp; - } } } @@ -201,17 +201,17 @@ void VotedSensorsUpdate::parameters_update() // valid subscription, so get the driver id by getting the published sensor data struct baro_report report; - if (orb_copy(ORB_ID(sensor_baro), _baro.subscription[topic_instance], &report) == 0) { + if (orb_copy(ORB_ID(sensor_baro), _baro.subscription[topic_instance], &report) == PX4_OK) { int temp = _temperature_compensation.set_sensor_id_baro(report.device_id, topic_instance); if (temp < 0) { PX4_ERR("%s temp compensation init: failed to find device ID %u for instance %i", "baro", report.device_id, topic_instance); + _corrections.baro_mapping[topic_instance] = 0; } else { _corrections.baro_mapping[topic_instance] = temp; - } } } @@ -219,12 +219,11 @@ void VotedSensorsUpdate::parameters_update() /* set offset parameters to new values */ - bool failed; + bool failed = false; char str[30]; + unsigned gyro_count = 0; - unsigned accel_count = 0; unsigned gyro_cal_found_count = 0; - unsigned accel_cal_found_count = 0; /* run through all gyro sensors */ for (unsigned driver_index = 0; driver_index < GYRO_COUNT_MAX; driver_index++) { @@ -238,7 +237,7 @@ void VotedSensorsUpdate::parameters_update() continue; } - uint32_t driver_device_id = h.ioctl(DEVIOCGDEVICEID, 0); + const uint32_t driver_device_id = h.ioctl(DEVIOCGDEVICEID, 0); bool config_ok = false; /* run through all stored calibrations that are applied at the driver level*/ @@ -247,12 +246,12 @@ void VotedSensorsUpdate::parameters_update() failed = false; (void)sprintf(str, "CAL_GYRO%u_ID", i); - int32_t device_id; + int32_t device_id = 0; failed = failed || (OK != param_get(param_find(str), &device_id)); (void)sprintf(str, "CAL_GYRO%u_EN", i); int32_t device_enabled = 1; - failed = failed || (OK != param_get(param_find(str), &device_enabled)); + failed = failed || (PX4_OK != param_get(param_find(str), &device_enabled)); _gyro.enabled[i] = (device_enabled == 1); @@ -266,13 +265,15 @@ void VotedSensorsUpdate::parameters_update() /* if the calibration is for this device, apply it */ if (device_id == driver_device_id) { - struct gyro_calibration_s gscale = {}; + calibration_gyro_s gscale = {}; + (void)sprintf(str, "CAL_GYRO%u_XOFF", i); failed = failed || (OK != param_get(param_find(str), &gscale.x_offset)); (void)sprintf(str, "CAL_GYRO%u_YOFF", i); failed = failed || (OK != param_get(param_find(str), &gscale.y_offset)); (void)sprintf(str, "CAL_GYRO%u_ZOFF", i); failed = failed || (OK != param_get(param_find(str), &gscale.z_offset)); + (void)sprintf(str, "CAL_GYRO%u_XSCALE", i); failed = failed || (OK != param_get(param_find(str), &gscale.x_scale)); (void)sprintf(str, "CAL_GYRO%u_YSCALE", i); @@ -285,6 +286,8 @@ void VotedSensorsUpdate::parameters_update() } else { /* apply new scaling and offsets */ + gscale.timestamp = hrt_absolute_time(); + print_message(gscale); config_ok = apply_gyro_calibration(h, &gscale, device_id); if (!config_ok) { @@ -304,16 +307,17 @@ void VotedSensorsUpdate::parameters_update() // There are less gyros than calibrations // reset the board calibration and fail the initial load if (gyro_count < gyro_cal_found_count) { - // run through all stored calibrations and reset them for (unsigned i = 0; i < GYRO_COUNT_MAX; i++) { - int32_t device_id = 0; (void)sprintf(str, "CAL_GYRO%u_ID", i); (void)param_set(param_find(str), &device_id); } } + unsigned accel_count = 0; + unsigned accel_cal_found_count = 0; + /* run through all accel sensors */ for (unsigned driver_index = 0; driver_index < ACCEL_COUNT_MAX; driver_index++) { @@ -335,7 +339,7 @@ void VotedSensorsUpdate::parameters_update() failed = false; (void)sprintf(str, "CAL_ACC%u_ID", i); - int32_t device_id; + int32_t device_id = 0; failed = failed || (OK != param_get(param_find(str), &device_id)); (void)sprintf(str, "CAL_ACC%u_EN", i); @@ -354,13 +358,15 @@ void VotedSensorsUpdate::parameters_update() /* if the calibration is for this device, apply it */ if (device_id == driver_device_id) { - struct accel_calibration_s ascale = {}; + calibration_accel_s ascale = {}; + (void)sprintf(str, "CAL_ACC%u_XOFF", i); failed = failed || (OK != param_get(param_find(str), &ascale.x_offset)); (void)sprintf(str, "CAL_ACC%u_YOFF", i); failed = failed || (OK != param_get(param_find(str), &ascale.y_offset)); (void)sprintf(str, "CAL_ACC%u_ZOFF", i); failed = failed || (OK != param_get(param_find(str), &ascale.z_offset)); + (void)sprintf(str, "CAL_ACC%u_XSCALE", i); failed = failed || (OK != param_get(param_find(str), &ascale.x_scale)); (void)sprintf(str, "CAL_ACC%u_YSCALE", i); @@ -373,6 +379,8 @@ void VotedSensorsUpdate::parameters_update() } else { /* apply new scaling and offsets */ + ascale.timestamp = hrt_absolute_time(); + print_message(ascale); config_ok = apply_accel_calibration(h, &ascale, device_id); if (!config_ok) { @@ -392,10 +400,8 @@ void VotedSensorsUpdate::parameters_update() // There are less accels than calibrations // reset the board calibration and fail the initial load if (accel_count < accel_cal_found_count) { - // run through all stored calibrations and reset them for (unsigned i = 0; i < ACCEL_COUNT_MAX; i++) { - int32_t device_id = 0; (void)sprintf(str, "CAL_ACC%u_ID", i); (void)param_set(param_find(str), &device_id); @@ -409,13 +415,13 @@ void VotedSensorsUpdate::parameters_update() for (unsigned topic_instance = 0; topic_instance < MAG_COUNT_MAX && topic_instance < _mag.subscription_count; ++topic_instance) { - struct mag_report report; + mag_report report; - if (orb_copy(ORB_ID(sensor_mag), _mag.subscription[topic_instance], &report) != 0) { + if (orb_copy(ORB_ID(sensor_mag), _mag.subscription[topic_instance], &report) != PX4_OK) { continue; } - int topic_device_id = report.device_id; + const int topic_device_id = report.device_id; bool is_external = report.is_external; _mag_device_id[topic_instance] = topic_device_id; @@ -466,13 +472,15 @@ void VotedSensorsUpdate::parameters_update() /* if the calibration is for this device, apply it */ if (device_id == _mag_device_id[topic_instance]) { - struct mag_calibration_s mscale = {}; + mag_calibration_s mscale = {}; + (void)sprintf(str, "CAL_MAG%u_XOFF", i); failed = failed || (OK != param_get(param_find(str), &mscale.x_offset)); (void)sprintf(str, "CAL_MAG%u_YOFF", i); failed = failed || (OK != param_get(param_find(str), &mscale.y_offset)); (void)sprintf(str, "CAL_MAG%u_ZOFF", i); failed = failed || (OK != param_get(param_find(str), &mscale.z_offset)); + (void)sprintf(str, "CAL_MAG%u_XSCALE", i); failed = failed || (OK != param_get(param_find(str), &mscale.x_scale)); (void)sprintf(str, "CAL_MAG%u_YSCALE", i); @@ -481,12 +489,10 @@ void VotedSensorsUpdate::parameters_update() failed = failed || (OK != param_get(param_find(str), &mscale.z_scale)); (void)sprintf(str, "CAL_MAG%u_ROT", i); - int32_t mag_rot; param_get(param_find(str), &mag_rot); if (is_external) { - /* check if this mag is still set as internal, otherwise leave untouched */ if (mag_rot < 0) { /* it was marked as internal, change to external with no rotation */ @@ -516,8 +522,8 @@ void VotedSensorsUpdate::parameters_update() PX4_ERR(CAL_ERROR_APPLY_CAL_MSG, "mag", i); } else { - /* apply new scaling and offsets */ + //mscale.timestamp = hrt_absolute_time(); config_ok = apply_mag_calibration(h, &mscale, device_id); if (!config_ok) { @@ -542,11 +548,11 @@ void VotedSensorsUpdate::accel_poll(struct sensor_combined_s &raw) orb_check(_accel.subscription[uorb_index], &accel_updated); if (accel_updated && _accel.enabled[uorb_index]) { - struct accel_report accel_report; + sensor_accel_s sensor_accel_s; - int ret = orb_copy(ORB_ID(sensor_accel), _accel.subscription[uorb_index], &accel_report); + int ret = orb_copy(ORB_ID(sensor_accel), _accel.subscription[uorb_index], &sensor_accel_s); - if (ret != PX4_OK || accel_report.timestamp == 0) { + if (ret != PX4_OK || sensor_accel_s.timestamp == 0) { continue; //ignore invalid data } @@ -557,11 +563,11 @@ void VotedSensorsUpdate::accel_poll(struct sensor_combined_s &raw) _accel.priority[uorb_index] = (uint8_t)priority; } - _accel_device_id[uorb_index] = accel_report.device_id; + _accel_device_id[uorb_index] = sensor_accel_s.device_id; matrix::Vector3f accel_data; - if (accel_report.integral_dt != 0) { + if (sensor_accel_s.integral_dt != 0) { /* * Using data that has been integrated in the driver before downsampling is preferred * becasue it reduces aliasing errors. Correct the raw sensor data for scale factor errors @@ -570,33 +576,33 @@ void VotedSensorsUpdate::accel_poll(struct sensor_combined_s &raw) */ // convert the delta velocities to an equivalent acceleration before application of corrections - float dt_inv = 1.e6f / accel_report.integral_dt; - accel_data = matrix::Vector3f(accel_report.x_integral * dt_inv, - accel_report.y_integral * dt_inv, - accel_report.z_integral * dt_inv); + float dt_inv = 1.e6f / sensor_accel_s.integral_dt; + accel_data = matrix::Vector3f(sensor_accel_s.x_integral * dt_inv, + sensor_accel_s.y_integral * dt_inv, + sensor_accel_s.z_integral * dt_inv); - _last_sensor_data[uorb_index].accelerometer_integral_dt = accel_report.integral_dt; + _last_sensor_data[uorb_index].accelerometer_integral_dt = sensor_accel_s.integral_dt; } else { // using the value instead of the integral (the integral is the prefered choice) // Correct each sensor for temperature effects // Filtering and/or downsampling of temperature should be performed in the driver layer - accel_data = matrix::Vector3f(accel_report.x, accel_report.y, accel_report.z); + accel_data = matrix::Vector3f(sensor_accel_s.x, sensor_accel_s.y, sensor_accel_s.z); // handle the cse where this is our first output if (_last_accel_timestamp[uorb_index] == 0) { - _last_accel_timestamp[uorb_index] = accel_report.timestamp - 1000; + _last_accel_timestamp[uorb_index] = sensor_accel_s.timestamp - 1000; } // approximate the delta time using the difference in accel data time stamps _last_sensor_data[uorb_index].accelerometer_integral_dt = - (accel_report.timestamp - _last_accel_timestamp[uorb_index]); + (sensor_accel_s.timestamp - _last_accel_timestamp[uorb_index]); } // handle temperature compensation if (!_hil_enabled) { - if (_temperature_compensation.apply_corrections_accel(uorb_index, accel_data, accel_report.temperature, + if (_temperature_compensation.apply_corrections_accel(uorb_index, accel_data, sensor_accel_s.temperature, offsets[uorb_index], scales[uorb_index]) == 2) { _corrections_changed = true; } @@ -609,9 +615,9 @@ void VotedSensorsUpdate::accel_poll(struct sensor_combined_s &raw) _last_sensor_data[uorb_index].accelerometer_m_s2[1] = accel_data(1); _last_sensor_data[uorb_index].accelerometer_m_s2[2] = accel_data(2); - _last_accel_timestamp[uorb_index] = accel_report.timestamp; - _accel.voter.put(uorb_index, accel_report.timestamp, _last_sensor_data[uorb_index].accelerometer_m_s2, - accel_report.error_count, _accel.priority[uorb_index]); + _last_accel_timestamp[uorb_index] = sensor_accel_s.timestamp; + _accel.voter.put(uorb_index, sensor_accel_s.timestamp, _last_sensor_data[uorb_index].accelerometer_m_s2, + sensor_accel_s.error_count, _accel.priority[uorb_index]); } } @@ -647,11 +653,11 @@ void VotedSensorsUpdate::gyro_poll(struct sensor_combined_s &raw) orb_check(_gyro.subscription[uorb_index], &gyro_updated); if (gyro_updated && _gyro.enabled[uorb_index]) { - struct gyro_report gyro_report; + sensor_gyro_s sensor_gyro_s; - int ret = orb_copy(ORB_ID(sensor_gyro), _gyro.subscription[uorb_index], &gyro_report); + int ret = orb_copy(ORB_ID(sensor_gyro), _gyro.subscription[uorb_index], &sensor_gyro_s); - if (ret != PX4_OK || gyro_report.timestamp == 0) { + if (ret != PX4_OK || sensor_gyro_s.timestamp == 0) { continue; //ignore invalid data } @@ -662,11 +668,11 @@ void VotedSensorsUpdate::gyro_poll(struct sensor_combined_s &raw) _gyro.priority[uorb_index] = (uint8_t)priority; } - _gyro_device_id[uorb_index] = gyro_report.device_id; + _gyro_device_id[uorb_index] = sensor_gyro_s.device_id; matrix::Vector3f gyro_rate; - if (gyro_report.integral_dt != 0) { + if (sensor_gyro_s.integral_dt != 0) { /* * Using data that has been integrated in the driver before downsampling is preferred * becasue it reduces aliasing errors. Correct the raw sensor data for scale factor errors @@ -675,33 +681,33 @@ void VotedSensorsUpdate::gyro_poll(struct sensor_combined_s &raw) */ // convert the delta angles to an equivalent angular rate before application of corrections - float dt_inv = 1.e6f / gyro_report.integral_dt; - gyro_rate = matrix::Vector3f(gyro_report.x_integral * dt_inv, - gyro_report.y_integral * dt_inv, - gyro_report.z_integral * dt_inv); + float dt_inv = 1.e6f / sensor_gyro_s.integral_dt; + gyro_rate = matrix::Vector3f(sensor_gyro_s.x_integral * dt_inv, + sensor_gyro_s.y_integral * dt_inv, + sensor_gyro_s.z_integral * dt_inv); - _last_sensor_data[uorb_index].gyro_integral_dt = gyro_report.integral_dt; + _last_sensor_data[uorb_index].gyro_integral_dt = sensor_gyro_s.integral_dt; } else { //using the value instead of the integral (the integral is the prefered choice) // Correct each sensor for temperature effects // Filtering and/or downsampling of temperature should be performed in the driver layer - gyro_rate = matrix::Vector3f(gyro_report.x, gyro_report.y, gyro_report.z); + gyro_rate = matrix::Vector3f(sensor_gyro_s.x, sensor_gyro_s.y, sensor_gyro_s.z); // handle the case where this is our first output if (_last_sensor_data[uorb_index].timestamp == 0) { - _last_sensor_data[uorb_index].timestamp = gyro_report.timestamp - 1000; + _last_sensor_data[uorb_index].timestamp = sensor_gyro_s.timestamp - 1000; } // approximate the delta time using the difference in gyro data time stamps _last_sensor_data[uorb_index].gyro_integral_dt = - (gyro_report.timestamp - _last_sensor_data[uorb_index].timestamp); + (sensor_gyro_s.timestamp - _last_sensor_data[uorb_index].timestamp); } // handle temperature compensation if (!_hil_enabled) { - if (_temperature_compensation.apply_corrections_gyro(uorb_index, gyro_rate, gyro_report.temperature, + if (_temperature_compensation.apply_corrections_gyro(uorb_index, gyro_rate, sensor_gyro_s.temperature, offsets[uorb_index], scales[uorb_index]) == 2) { _corrections_changed = true; } @@ -714,9 +720,9 @@ void VotedSensorsUpdate::gyro_poll(struct sensor_combined_s &raw) _last_sensor_data[uorb_index].gyro_rad[1] = gyro_rate(1); _last_sensor_data[uorb_index].gyro_rad[2] = gyro_rate(2); - _last_sensor_data[uorb_index].timestamp = gyro_report.timestamp; - _gyro.voter.put(uorb_index, gyro_report.timestamp, _last_sensor_data[uorb_index].gyro_rad, - gyro_report.error_count, _gyro.priority[uorb_index]); + _last_sensor_data[uorb_index].timestamp = sensor_gyro_s.timestamp; + _gyro.voter.put(uorb_index, sensor_gyro_s.timestamp, _last_sensor_data[uorb_index].gyro_rad, + sensor_gyro_s.error_count, _gyro.priority[uorb_index]); } } @@ -972,7 +978,7 @@ void VotedSensorsUpdate::print_status() } bool -VotedSensorsUpdate::apply_gyro_calibration(DevHandle &h, const struct gyro_calibration_s *gcal, const int device_id) +VotedSensorsUpdate::apply_gyro_calibration(DevHandle &h, const calibration_gyro_s *gcal, const int device_id) { #if !defined(__PX4_QURT) && !defined(__PX4_POSIX_RPI) && !defined(__PX4_POSIX_BEBOP) @@ -986,7 +992,7 @@ VotedSensorsUpdate::apply_gyro_calibration(DevHandle &h, const struct gyro_calib } bool -VotedSensorsUpdate::apply_accel_calibration(DevHandle &h, const struct accel_calibration_s *acal, const int device_id) +VotedSensorsUpdate::apply_accel_calibration(DevHandle &h, const calibration_accel_s *acal, const int device_id) { #if !defined(__PX4_QURT) && !defined(__PX4_POSIX_RPI) && !defined(__PX4_POSIX_BEBOP) diff --git a/src/modules/sensors/voted_sensors_update.h b/src/modules/sensors/voted_sensors_update.h index 590118decaeb..8c457bb75f29 100644 --- a/src/modules/sensors/voted_sensors_update.h +++ b/src/modules/sensors/voted_sensors_update.h @@ -52,7 +52,11 @@ #include #include +#include +#include +#include #include +#include #include #include #include @@ -215,7 +219,7 @@ class VotedSensorsUpdate * @param device: the device id of the sensor. * @return: true if config is ok */ - bool apply_gyro_calibration(DriverFramework::DevHandle &h, const struct gyro_calibration_s *gcal, const int device_id); + bool apply_gyro_calibration(DriverFramework::DevHandle &h, const calibration_gyro_s *gcal, const int device_id); /** * Apply a accel calibration. @@ -225,7 +229,7 @@ class VotedSensorsUpdate * @param device: the device id of the sensor. * @return: true if config is ok */ - bool apply_accel_calibration(DriverFramework::DevHandle &h, const struct accel_calibration_s *acal, + bool apply_accel_calibration(DriverFramework::DevHandle &h, const calibration_accel_s *acal, const int device_id); /** @@ -236,7 +240,7 @@ class VotedSensorsUpdate * @param device: the device id of the sensor. * @return: true if config is ok */ - bool apply_mag_calibration(DriverFramework::DevHandle &h, const struct mag_calibration_s *mcal, const int device_id); + bool apply_mag_calibration(DriverFramework::DevHandle &h, const mag_calibration_s *mcal, const int device_id); SensorData _gyro; SensorData _accel; diff --git a/src/modules/simulator/accelsim/accelsim.cpp b/src/modules/simulator/accelsim/accelsim.cpp index c2e8cf4fade9..be8bfeda5042 100644 --- a/src/modules/simulator/accelsim/accelsim.cpp +++ b/src/modules/simulator/accelsim/accelsim.cpp @@ -63,6 +63,10 @@ #include #include #include +#include +#include +#include +#include #define ACCELSIM_DEVICE_PATH_ACCEL "/dev/sim_accel" #define ACCELSIM_DEVICE_PATH_MAG "/dev/sim_mag" @@ -110,16 +114,16 @@ class ACCELSIM : public VirtDevObj ACCELSIM_mag *_mag; - ringbuffer::RingBuffer *_accel_reports; + ringbuffer::RingBuffer *_sensor_accel_ss; ringbuffer::RingBuffer *_mag_reports; - struct accel_calibration_s _accel_scale; + calibration_accel_s _accel_scale; unsigned _accel_range_m_s2; float _accel_range_scale; unsigned _accel_samplerate; unsigned _accel_onchip_filter_bandwith; - struct mag_calibration_s _mag_scale; + mag_calibration_s _mag_scale; unsigned _mag_range_ga; float _mag_range_scale; unsigned _mag_samplerate; @@ -238,7 +242,7 @@ class ACCELSIM_mag : public VirtDevObj ACCELSIM::ACCELSIM(const char *path, enum Rotation rotation) : VirtDevObj("ACCELSIM", path, ACCEL_BASE_DEVICE_PATH, 1e6 / 400), _mag(new ACCELSIM_mag(this)), - _accel_reports(nullptr), + _sensor_accel_ss(nullptr), _mag_reports(nullptr), _accel_scale{}, _accel_range_m_s2(0.0f), @@ -296,8 +300,8 @@ ACCELSIM::~ACCELSIM() stop(); /* free any existing reports */ - if (_accel_reports != nullptr) { - delete _accel_reports; + if (_sensor_accel_ss != nullptr) { + delete _sensor_accel_ss; } if (_mag_reports != nullptr) { @@ -320,7 +324,7 @@ ACCELSIM::init() int ret = -1; struct mag_report mrp = {}; - struct accel_report arp = {}; + sensor_accel_s arp = {}; /* do SIM init first */ if (VirtDevObj::init() != 0) { @@ -329,9 +333,9 @@ ACCELSIM::init() } /* allocate basic report buffers */ - _accel_reports = new ringbuffer::RingBuffer(2, sizeof(accel_report)); + _sensor_accel_ss = new ringbuffer::RingBuffer(2, sizeof(sensor_accel_s)); - if (_accel_reports == nullptr) { + if (_sensor_accel_ss == nullptr) { goto out; } @@ -364,7 +368,7 @@ ACCELSIM::init() } /* advertise sensor topic, measure manually to initialize valid report */ - _accel_reports->get(&arp); + _sensor_accel_ss->get(&arp); /* measurement will have generated a report, publish */ _accel_topic = orb_advertise_multi(ORB_ID(sensor_accel), &arp, @@ -409,8 +413,8 @@ ACCELSIM::transfer(uint8_t *send, uint8_t *recv, unsigned len) ssize_t ACCELSIM::devRead(void *buffer, size_t buflen) { - unsigned count = buflen / sizeof(struct accel_report); - accel_report *arb = reinterpret_cast(buffer); + unsigned count = buflen / sizeof(sensor_accel_s); + sensor_accel_s *arb = reinterpret_cast(buffer); int ret = 0; /* buffer must be large enough */ @@ -424,7 +428,7 @@ ACCELSIM::devRead(void *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 (_sensor_accel_ss->get(arb)) { ret += sizeof(*arb); arb++; } @@ -438,7 +442,7 @@ ACCELSIM::devRead(void *buffer, size_t buflen) _measure(); /* measurement will have generated a report, copy it out */ - if (_accel_reports->get(arb)) { + if (_sensor_accel_ss->get(arb)) { ret = sizeof(*arb); } @@ -556,7 +560,7 @@ ACCELSIM::devIOCTL(unsigned long cmd, unsigned long arg) return -EINVAL; } - if (!_accel_reports->resize(ul_arg)) { + if (!_sensor_accel_ss->resize(ul_arg)) { return -ENOMEM; } @@ -576,7 +580,7 @@ ACCELSIM::devIOCTL(unsigned long cmd, unsigned long arg) case ACCELIOCSSCALE: { /* copy scale, but only if off by a few percent */ - struct accel_calibration_s *s = (struct accel_calibration_s *) arg; + calibration_accel_s *s = (calibration_accel_s *) arg; float sum = s->x_scale + s->y_scale + s->z_scale; if (sum > 2.0f && sum < 4.0f) { @@ -598,7 +602,7 @@ ACCELSIM::devIOCTL(unsigned long cmd, unsigned long arg) case ACCELIOCGSCALE: /* copy scale out */ - memcpy((struct accel_calibration_s *) arg, &(_accel_scale), sizeof(_accel_scale)); + memcpy((calibration_accel_s *) arg, &(_accel_scale), sizeof(_accel_scale)); return OK; case ACCELIOCSELFTEST: @@ -764,7 +768,7 @@ ACCELSIM::start() stop(); /* reset the report ring */ - _accel_reports->flush(); + _sensor_accel_ss->flush(); _mag_reports->flush(); int ret2 = VirtDevObj::start(); @@ -809,19 +813,19 @@ ACCELSIM::_measure() float x; float y; float z; - } raw_accel_report; + } raw_sensor_accel_s; #pragma pack(pop) - accel_report accel_report = {}; + sensor_accel_s sensor_accel_s = {}; /* start the performance counter */ perf_begin(_accel_sample_perf); /* fetch data from the sensor */ - memset(&raw_accel_report, 0, sizeof(raw_accel_report)); - raw_accel_report.cmd = DIR_READ | ACC_READ; + memset(&raw_sensor_accel_s, 0, sizeof(raw_sensor_accel_s)); + raw_sensor_accel_s.cmd = DIR_READ | ACC_READ; - if (OK != transfer((uint8_t *)&raw_accel_report, (uint8_t *)&raw_accel_report, sizeof(raw_accel_report))) { + if (OK != transfer((uint8_t *)&raw_sensor_accel_s, (uint8_t *)&raw_sensor_accel_s, sizeof(raw_sensor_accel_s))) { return; } @@ -841,29 +845,29 @@ ACCELSIM::_measure() */ - accel_report.timestamp = hrt_absolute_time(); + sensor_accel_s.timestamp = hrt_absolute_time(); // use the temperature from the last mag reading - accel_report.temperature = _last_temperature; + sensor_accel_s.temperature = _last_temperature; // report the error count as the sum of the number of bad // register reads and bad values. This allows the higher level // code to decide if it should use this sensor based on // whether it has had failures - accel_report.error_count = perf_event_count(_bad_registers) + perf_event_count(_bad_values); + sensor_accel_s.error_count = perf_event_count(_bad_registers) + perf_event_count(_bad_values); - accel_report.x_raw = (int16_t)(raw_accel_report.x / _accel_range_scale); - accel_report.y_raw = (int16_t)(raw_accel_report.y / _accel_range_scale); - accel_report.z_raw = (int16_t)(raw_accel_report.z / _accel_range_scale); + sensor_accel_s.x_raw = (int16_t)(raw_sensor_accel_s.x / _accel_range_scale); + sensor_accel_s.y_raw = (int16_t)(raw_sensor_accel_s.y / _accel_range_scale); + sensor_accel_s.z_raw = (int16_t)(raw_sensor_accel_s.z / _accel_range_scale); - accel_report.x = raw_accel_report.x; - accel_report.y = raw_accel_report.y; - accel_report.z = raw_accel_report.z; + sensor_accel_s.x = raw_sensor_accel_s.x; + sensor_accel_s.y = raw_sensor_accel_s.y; + sensor_accel_s.z = raw_sensor_accel_s.z; - accel_report.scaling = _accel_range_scale; - accel_report.range_m_s2 = _accel_range_m_s2; + sensor_accel_s.scaling = _accel_range_scale; + sensor_accel_s.range_m_s2 = _accel_range_m_s2; - _accel_reports->force(&accel_report); + _sensor_accel_ss->force(&sensor_accel_s); if (!(m_pub_blocked)) { /* publish it */ @@ -871,7 +875,7 @@ ACCELSIM::_measure() // The first call to measure() is from init() and _accel_topic is not // yet initialized if (_accel_topic != nullptr) { - orb_publish(ORB_ID(sensor_accel), _accel_topic, &accel_report); + orb_publish(ORB_ID(sensor_accel), _accel_topic, &sensor_accel_s); } } diff --git a/src/modules/simulator/gyrosim/gyrosim.cpp b/src/modules/simulator/gyrosim/gyrosim.cpp index a8ba20092879..4b92140983d2 100644 --- a/src/modules/simulator/gyrosim/gyrosim.cpp +++ b/src/modules/simulator/gyrosim/gyrosim.cpp @@ -75,6 +75,10 @@ #include #include #include +#include +#include +#include +#include #include "VirtDevObj.hpp" @@ -148,17 +152,17 @@ class GYROSIM : public VirtDevObj unsigned _call_interval; - ringbuffer::RingBuffer *_accel_reports; + ringbuffer::RingBuffer *_sensor_accel_ss; - struct accel_calibration_s _accel_scale; + calibration_accel_s _accel_scale; float _accel_range_scale; float _accel_range_m_s2; orb_advert_t _accel_topic; int _accel_orb_class_instance; - ringbuffer::RingBuffer *_gyro_reports; + ringbuffer::RingBuffer *_sensor_gyro_ss; - struct gyro_calibration_s _gyro_scale; + calibration_gyro_s _gyro_scale; float _gyro_range_scale; float _gyro_range_rad_s; @@ -303,13 +307,13 @@ GYROSIM::GYROSIM(const char *path_accel, const char *path_gyro, enum Rotation ro VirtDevObj("GYROSIM", path_accel, ACCEL_BASE_DEVICE_PATH, 1e6 / 400), _gyro(new GYROSIM_gyro(this, path_gyro)), _product(GYROSIMES_REV_C4), - _accel_reports(nullptr), + _sensor_accel_ss(nullptr), _accel_scale{}, _accel_range_scale(0.0f), _accel_range_m_s2(0.0f), _accel_topic(nullptr), _accel_orb_class_instance(-1), - _gyro_reports(nullptr), + _sensor_gyro_ss(nullptr), _gyro_scale{}, _gyro_range_scale(0.0f), _gyro_range_rad_s(0.0f), @@ -356,12 +360,12 @@ GYROSIM::~GYROSIM() delete _gyro; /* free any existing reports */ - if (_accel_reports != nullptr) { - delete _accel_reports; + if (_sensor_accel_ss != nullptr) { + delete _sensor_accel_ss; } - if (_gyro_reports != nullptr) { - delete _gyro_reports; + if (_sensor_gyro_ss != nullptr) { + delete _sensor_gyro_ss; } /* delete the perf counter */ @@ -377,9 +381,9 @@ GYROSIM::init() PX4_DEBUG("init"); int ret = 1; - struct accel_report arp = {}; + sensor_accel_s arp = {}; - struct gyro_report grp = {}; + sensor_gyro_s grp = {}; ret = VirtDevObj::init(); @@ -390,17 +394,17 @@ GYROSIM::init() } /* allocate basic report buffers */ - _accel_reports = new ringbuffer::RingBuffer(2, sizeof(accel_report)); + _sensor_accel_ss = new ringbuffer::RingBuffer(2, sizeof(sensor_accel_s)); - if (_accel_reports == nullptr) { - PX4_WARN("_accel_reports creation failed"); + if (_sensor_accel_ss == nullptr) { + PX4_WARN("_sensor_accel_ss creation failed"); goto out; } - _gyro_reports = new ringbuffer::RingBuffer(2, sizeof(gyro_report)); + _sensor_gyro_ss = new ringbuffer::RingBuffer(2, sizeof(sensor_gyro_s)); - if (_gyro_reports == nullptr) { - PX4_WARN("_gyro_reports creation failed"); + if (_sensor_gyro_ss == nullptr) { + PX4_WARN("_sensor_gyro_ss creation failed"); goto out; } @@ -446,7 +450,7 @@ GYROSIM::init() _measure(); /* advertise sensor topic, measure manually to initialize valid report */ - _accel_reports->get(&arp); + _sensor_accel_ss->get(&arp); /* measurement will have generated a report, publish */ _accel_topic = orb_advertise_multi(ORB_ID(sensor_accel), &arp, @@ -461,7 +465,7 @@ GYROSIM::init() /* advertise sensor topic, measure manually to initialize valid report */ - _gyro_reports->get(&grp); + _sensor_gyro_ss->get(&grp); _gyro->_gyro_topic = orb_advertise_multi(ORB_ID(sensor_gyro), &grp, &_gyro->_gyro_orb_class_instance, ORB_PRIO_HIGH); @@ -551,7 +555,7 @@ GYROSIM::_set_sample_rate(unsigned desired_sample_rate_hz) ssize_t GYROSIM::devRead(void *buffer, size_t buflen) { - unsigned count = buflen / sizeof(accel_report); + unsigned count = buflen / sizeof(sensor_accel_s); /* buffer must be large enough */ if (count < 1) { @@ -560,23 +564,23 @@ GYROSIM::devRead(void *buffer, size_t buflen) /* if automatic measurement is not enabled, get a fresh measurement into the buffer */ if (_call_interval == 0) { - _accel_reports->flush(); + _sensor_accel_ss->flush(); _measure(); } /* if no data, error (we could block here) */ - if (_accel_reports->empty()) { + if (_sensor_accel_ss->empty()) { return -EAGAIN; } perf_count(_accel_reads); /* copy reports out of our buffer to the caller */ - accel_report *arp = reinterpret_cast(buffer); + sensor_accel_s *arp = reinterpret_cast(buffer); int transferred = 0; while (count--) { - if (!_accel_reports->get(arp)) { + if (!_sensor_accel_ss->get(arp)) { break; } @@ -585,7 +589,7 @@ GYROSIM::devRead(void *buffer, size_t buflen) } /* return the number of bytes transferred */ - return (transferred * sizeof(accel_report)); + return (transferred * sizeof(sensor_accel_s)); } int @@ -665,7 +669,7 @@ GYROSIM::gyro_self_test() ssize_t GYROSIM::gyro_read(void *buffer, size_t buflen) { - unsigned count = buflen / sizeof(gyro_report); + unsigned count = buflen / sizeof(sensor_gyro_s); /* buffer must be large enough */ if (count < 1) { @@ -674,23 +678,23 @@ GYROSIM::gyro_read(void *buffer, size_t buflen) /* if automatic measurement is not enabled, get a fresh measurement into the buffer */ if (_call_interval == 0) { - _gyro_reports->flush(); + _sensor_gyro_ss->flush(); _measure(); } /* if no data, error (we could block here) */ - if (_gyro_reports->empty()) { + if (_sensor_gyro_ss->empty()) { return -EAGAIN; } perf_count(_gyro_reads); /* copy reports out of our buffer to the caller */ - gyro_report *grp = reinterpret_cast(buffer); + sensor_gyro_s *grp = reinterpret_cast(buffer); int transferred = 0; while (count--) { - if (!_gyro_reports->get(grp)) { + if (!_sensor_gyro_ss->get(grp)) { break; } @@ -699,7 +703,7 @@ GYROSIM::gyro_read(void *buffer, size_t buflen) } /* return the number of bytes transferred */ - return (transferred * sizeof(gyro_report)); + return (transferred * sizeof(sensor_gyro_s)); } int @@ -773,7 +777,7 @@ GYROSIM::devIOCTL(unsigned long cmd, unsigned long arg) return -EINVAL; } - if (!_accel_reports->resize(arg)) { + if (!_sensor_accel_ss->resize(arg)) { return -ENOMEM; } @@ -789,7 +793,7 @@ GYROSIM::devIOCTL(unsigned long cmd, unsigned long arg) case ACCELIOCSSCALE: { /* copy scale, but only if off by a few percent */ - struct accel_calibration_s *s = (struct accel_calibration_s *) arg; + calibration_accel_s *s = (calibration_accel_s *) arg; float sum = s->x_scale + s->y_scale + s->z_scale; if (sum > 2.0f && sum < 4.0f) { @@ -803,7 +807,7 @@ GYROSIM::devIOCTL(unsigned long cmd, unsigned long arg) case ACCELIOCGSCALE: /* copy scale out */ - memcpy((struct accel_calibration_s *) arg, &_accel_scale, sizeof(_accel_scale)); + memcpy((calibration_accel_s *) arg, &_accel_scale, sizeof(_accel_scale)); return OK; case ACCELIOCSRANGE: @@ -838,7 +842,7 @@ GYROSIM::gyro_ioctl(unsigned long cmd, unsigned long arg) return -EINVAL; } - if (!_gyro_reports->resize(arg)) { + if (!_sensor_gyro_ss->resize(arg)) { return -ENOMEM; } @@ -854,12 +858,12 @@ GYROSIM::gyro_ioctl(unsigned long cmd, unsigned long arg) case GYROIOCSSCALE: /* copy scale in */ - memcpy(&_gyro_scale, (struct gyro_calibration_s *) arg, sizeof(_gyro_scale)); + memcpy(&_gyro_scale, (calibration_gyro_s *) arg, sizeof(_gyro_scale)); return OK; case GYROIOCGSCALE: /* copy scale out */ - memcpy((struct gyro_calibration_s *) arg, &_gyro_scale, sizeof(_gyro_scale)); + memcpy((calibration_gyro_s *) arg, &_gyro_scale, sizeof(_gyro_scale)); return OK; case GYROIOCSRANGE: @@ -955,8 +959,8 @@ GYROSIM::start() stop(); /* discard any stale data in the buffers */ - _accel_reports->flush(); - _gyro_reports->flush(); + _sensor_accel_ss->flush(); + _sensor_gyro_ss->flush(); /* start polling at the specified rate */ return DevObj::start(); @@ -998,8 +1002,8 @@ GYROSIM::_measure() /* * Report buffers. */ - accel_report arb = {}; - gyro_report grb = {}; + sensor_accel_s arb = {}; + sensor_gyro_s grb = {}; // for now use local time but this should be the timestamp of the simulator grb.timestamp = hrt_absolute_time(); @@ -1080,8 +1084,8 @@ GYROSIM::_measure() /* fake device ID */ grb.device_id = 3467548; - _accel_reports->force(&arb); - _gyro_reports->force(&grb); + _sensor_accel_ss->force(&arb); + _sensor_gyro_ss->force(&grb); if (accel_notify) { @@ -1110,8 +1114,8 @@ GYROSIM::print_info() perf_print_counter(_gyro_reads); perf_print_counter(_good_transfers); perf_print_counter(_reset_retries); - _accel_reports->print_info("accel queue"); - _gyro_reports->print_info("gyro queue"); + _sensor_accel_ss->print_info("accel queue"); + _sensor_gyro_ss->print_info("gyro queue"); PX4_INFO("temperature: %.1f", (double)_last_temperature); } @@ -1277,8 +1281,8 @@ test() { const char *path_accel = MPU_DEVICE_PATH_ACCEL; const char *path_gyro = MPU_DEVICE_PATH_GYRO; - accel_report a_report; - gyro_report g_report; + sensor_accel_s a_report; + sensor_gyro_s g_report; ssize_t sz; /* get the driver */ diff --git a/src/modules/simulator/simulator_mavlink.cpp b/src/modules/simulator/simulator_mavlink.cpp index 4bbb291251ab..6fdfd95ada27 100644 --- a/src/modules/simulator/simulator_mavlink.cpp +++ b/src/modules/simulator/simulator_mavlink.cpp @@ -45,6 +45,8 @@ #include #include #include +#include +#include #include extern "C" __EXPORT hrt_abstime hrt_reset(void); @@ -1010,7 +1012,7 @@ int Simulator::publish_sensor_topics(mavlink_hil_sensor_t *imu) */ /* gyro */ { - struct gyro_report gyro = {}; + sensor_gyro_s gyro = {}; gyro.timestamp = timestamp; gyro.x_raw = imu->xgyro * 1000.0f; @@ -1028,7 +1030,7 @@ int Simulator::publish_sensor_topics(mavlink_hil_sensor_t *imu) /* accelerometer */ { - struct accel_report accel = {}; + sensor_accel_s accel = {}; accel.timestamp = timestamp; accel.x_raw = imu->xacc / (CONSTANTS_ONE_G / 1000.0f); diff --git a/src/platforms/posix/drivers/df_lsm9ds1_wrapper/df_lsm9ds1_wrapper.cpp b/src/platforms/posix/drivers/df_lsm9ds1_wrapper/df_lsm9ds1_wrapper.cpp index 2fdff30580f5..63981b58b3f9 100644 --- a/src/platforms/posix/drivers/df_lsm9ds1_wrapper/df_lsm9ds1_wrapper.cpp +++ b/src/platforms/posix/drivers/df_lsm9ds1_wrapper/df_lsm9ds1_wrapper.cpp @@ -65,6 +65,8 @@ #include #include +#include +#include #include #include @@ -118,7 +120,7 @@ class DfLsm9ds1Wrapper : public LSM9DS1 int _param_update_sub; - struct accel_calibration_s { + struct calibration_accel_s { float x_offset; float x_scale; float y_offset; @@ -127,7 +129,7 @@ class DfLsm9ds1Wrapper : public LSM9DS1 float z_scale; } _accel_calibration; - struct gyro_calibration_s { + struct calibration_gyro_s { float x_offset; float x_scale; float y_offset; @@ -239,8 +241,8 @@ DfLsm9ds1Wrapper::~DfLsm9ds1Wrapper() int DfLsm9ds1Wrapper::start() { // TODO: don't publish garbage here - accel_report accel_report = {}; - _accel_topic = orb_advertise_multi(ORB_ID(sensor_accel), &accel_report, + sensor_accel_s sensor_accel_s = {}; + _accel_topic = orb_advertise_multi(ORB_ID(sensor_accel), &sensor_accel_s, &_accel_orb_class_instance, ORB_PRIO_DEFAULT); if (_accel_topic == nullptr) { @@ -249,8 +251,8 @@ int DfLsm9ds1Wrapper::start() } // TODO: don't publish garbage here - gyro_report gyro_report = {}; - _gyro_topic = orb_advertise_multi(ORB_ID(sensor_gyro), &gyro_report, + sensor_gyro_s sensor_gyro_s = {}; + _gyro_topic = orb_advertise_multi(ORB_ID(sensor_gyro), &sensor_gyro_s, &_gyro_orb_class_instance, ORB_PRIO_DEFAULT); if (_gyro_topic == nullptr) { @@ -618,22 +620,22 @@ int DfLsm9ds1Wrapper::_publish(struct imu_sensor_data &data) perf_begin(_publish_perf); - accel_report accel_report = {}; - gyro_report gyro_report = {}; + sensor_accel_s sensor_accel_s = {}; + sensor_gyro_s sensor_gyro_s = {}; mag_report mag_report = {}; - accel_report.timestamp = gyro_report.timestamp = hrt_absolute_time(); - mag_report.timestamp = accel_report.timestamp; + sensor_accel_s.timestamp = sensor_gyro_s.timestamp = hrt_absolute_time(); + mag_report.timestamp = sensor_accel_s.timestamp; mag_report.is_external = false; // TODO: get these right - gyro_report.scaling = -1.0f; - gyro_report.range_rad_s = -1.0f; - gyro_report.device_id = m_id.dev_id; + sensor_gyro_s.scaling = -1.0f; + sensor_gyro_s.range_rad_s = -1.0f; + sensor_gyro_s.device_id = m_id.dev_id; - accel_report.scaling = -1.0f; - accel_report.range_m_s2 = -1.0f; - accel_report.device_id = m_id.dev_id; + sensor_accel_s.scaling = -1.0f; + sensor_accel_s.range_m_s2 = -1.0f; + sensor_accel_s.device_id = m_id.dev_id; if (_mag_enabled) { mag_report.scaling = -1.0f; @@ -642,13 +644,13 @@ int DfLsm9ds1Wrapper::_publish(struct imu_sensor_data &data) } // TODO: remove these (or get the values) - gyro_report.x_raw = 0; - gyro_report.y_raw = 0; - gyro_report.z_raw = 0; + sensor_gyro_s.x_raw = 0; + sensor_gyro_s.y_raw = 0; + sensor_gyro_s.z_raw = 0; - accel_report.x_raw = 0; - accel_report.y_raw = 0; - accel_report.z_raw = 0; + sensor_accel_s.x_raw = 0; + sensor_accel_s.y_raw = 0; + sensor_accel_s.z_raw = 0; if (_mag_enabled) { mag_report.x_raw = 0; @@ -660,17 +662,17 @@ int DfLsm9ds1Wrapper::_publish(struct imu_sensor_data &data) matrix::Vector3f accel_val_filt; // Read and reset. - matrix::Vector3f gyro_val_integ = _gyro_int.get_and_filtered(true, gyro_report.integral_dt, gyro_val_filt); - matrix::Vector3f accel_val_integ = _accel_int.get_and_filtered(true, accel_report.integral_dt, accel_val_filt); + matrix::Vector3f gyro_val_integ = _gyro_int.get_and_filtered(true, sensor_gyro_s.integral_dt, gyro_val_filt); + matrix::Vector3f accel_val_integ = _accel_int.get_and_filtered(true, sensor_accel_s.integral_dt, accel_val_filt); // Use the filtered (by integration) values to get smoother / less noisy data. - gyro_report.x = gyro_val_filt(0); - gyro_report.y = gyro_val_filt(1); - gyro_report.z = gyro_val_filt(2); + sensor_gyro_s.x = gyro_val_filt(0); + sensor_gyro_s.y = gyro_val_filt(1); + sensor_gyro_s.z = gyro_val_filt(2); - accel_report.x = accel_val_filt(0); - accel_report.y = accel_val_filt(1); - accel_report.z = accel_val_filt(2); + sensor_accel_s.x = accel_val_filt(0); + sensor_accel_s.y = accel_val_filt(1); + sensor_accel_s.z = accel_val_filt(2); if (_mag_enabled) { matrix::Vector3f mag_val(data.mag_ga_x, @@ -686,24 +688,24 @@ int DfLsm9ds1Wrapper::_publish(struct imu_sensor_data &data) mag_report.z = mag_val(2); } - gyro_report.x_integral = gyro_val_integ(0); - gyro_report.y_integral = gyro_val_integ(1); - gyro_report.z_integral = gyro_val_integ(2); + sensor_gyro_s.x_integral = gyro_val_integ(0); + sensor_gyro_s.y_integral = gyro_val_integ(1); + sensor_gyro_s.z_integral = gyro_val_integ(2); - accel_report.x_integral = accel_val_integ(0); - accel_report.y_integral = accel_val_integ(1); - accel_report.z_integral = accel_val_integ(2); + sensor_accel_s.x_integral = accel_val_integ(0); + sensor_accel_s.y_integral = accel_val_integ(1); + sensor_accel_s.z_integral = accel_val_integ(2); // TODO: when is this ever blocked? if (!(m_pub_blocked)) { if (_gyro_topic != nullptr) { - orb_publish(ORB_ID(sensor_gyro), _gyro_topic, &gyro_report); + orb_publish(ORB_ID(sensor_gyro), _gyro_topic, &sensor_gyro_s); } if (_accel_topic != nullptr) { - orb_publish(ORB_ID(sensor_accel), _accel_topic, &accel_report); + orb_publish(ORB_ID(sensor_accel), _accel_topic, &sensor_accel_s); } if (_mag_topic != nullptr) { diff --git a/src/platforms/posix/drivers/df_mpu6050_wrapper/df_mpu6050_wrapper.cpp b/src/platforms/posix/drivers/df_mpu6050_wrapper/df_mpu6050_wrapper.cpp index 8870fd851e82..0191105c21ed 100644 --- a/src/platforms/posix/drivers/df_mpu6050_wrapper/df_mpu6050_wrapper.cpp +++ b/src/platforms/posix/drivers/df_mpu6050_wrapper/df_mpu6050_wrapper.cpp @@ -61,6 +61,8 @@ #include #include +#include +#include #include #include @@ -113,7 +115,7 @@ class DfMPU6050Wrapper : public MPU6050 int _param_update_sub; - struct accel_calibration_s { + struct calibration_accel_s { float x_offset; float x_scale; float y_offset; @@ -122,7 +124,7 @@ class DfMPU6050Wrapper : public MPU6050 float z_scale; } _accel_calibration; - struct gyro_calibration_s { + struct calibration_gyro_s { float x_offset; float x_scale; float y_offset; @@ -488,70 +490,70 @@ int DfMPU6050Wrapper::_publish(struct imu_sensor_data &data) perf_begin(_publish_perf); - accel_report accel_report = {}; - gyro_report gyro_report = {}; + sensor_accel_s sensor_accel_s = {}; + sensor_gyro_s sensor_gyro_s = {}; - accel_report.timestamp = gyro_report.timestamp = hrt_absolute_time(); + sensor_accel_s.timestamp = sensor_gyro_s.timestamp = hrt_absolute_time(); // TODO: get these right - gyro_report.scaling = -1.0f; - gyro_report.range_rad_s = -1.0f; - gyro_report.device_id = m_id.dev_id; + sensor_gyro_s.scaling = -1.0f; + sensor_gyro_s.range_rad_s = -1.0f; + sensor_gyro_s.device_id = m_id.dev_id; - accel_report.scaling = -1.0f; - accel_report.range_m_s2 = -1.0f; - accel_report.device_id = m_id.dev_id; + sensor_accel_s.scaling = -1.0f; + sensor_accel_s.range_m_s2 = -1.0f; + sensor_accel_s.device_id = m_id.dev_id; // TODO: remove these (or get the values) - gyro_report.x_raw = 0; - gyro_report.y_raw = 0; - gyro_report.z_raw = 0; + sensor_gyro_s.x_raw = 0; + sensor_gyro_s.y_raw = 0; + sensor_gyro_s.z_raw = 0; - accel_report.x_raw = 0; - accel_report.y_raw = 0; - accel_report.z_raw = 0; + sensor_accel_s.x_raw = 0; + sensor_accel_s.y_raw = 0; + sensor_accel_s.z_raw = 0; matrix::Vector3f gyro_val_filt; matrix::Vector3f accel_val_filt; // Read and reset. - matrix::Vector3f gyro_val_integ = _gyro_int.get_and_filtered(true, gyro_report.integral_dt, gyro_val_filt); - matrix::Vector3f accel_val_integ = _accel_int.get_and_filtered(true, accel_report.integral_dt, accel_val_filt); + matrix::Vector3f gyro_val_integ = _gyro_int.get_and_filtered(true, sensor_gyro_s.integral_dt, gyro_val_filt); + matrix::Vector3f accel_val_integ = _accel_int.get_and_filtered(true, sensor_accel_s.integral_dt, accel_val_filt); // Use the filtered (by integration) values to get smoother / less noisy data. - gyro_report.x = gyro_val_filt(0); - gyro_report.y = gyro_val_filt(1); - gyro_report.z = gyro_val_filt(2); + sensor_gyro_s.x = gyro_val_filt(0); + sensor_gyro_s.y = gyro_val_filt(1); + sensor_gyro_s.z = gyro_val_filt(2); - accel_report.x = accel_val_filt(0); - accel_report.y = accel_val_filt(1); - accel_report.z = accel_val_filt(2); + sensor_accel_s.x = accel_val_filt(0); + sensor_accel_s.y = accel_val_filt(1); + sensor_accel_s.z = accel_val_filt(2); - gyro_report.x_integral = gyro_val_integ(0); - gyro_report.y_integral = gyro_val_integ(1); - gyro_report.z_integral = gyro_val_integ(2); + sensor_gyro_s.x_integral = gyro_val_integ(0); + sensor_gyro_s.y_integral = gyro_val_integ(1); + sensor_gyro_s.z_integral = gyro_val_integ(2); - accel_report.x_integral = accel_val_integ(0); - accel_report.y_integral = accel_val_integ(1); - accel_report.z_integral = accel_val_integ(2); + sensor_accel_s.x_integral = accel_val_integ(0); + sensor_accel_s.y_integral = accel_val_integ(1); + sensor_accel_s.z_integral = accel_val_integ(2); // TODO: when is this ever blocked? if (!(m_pub_blocked)) { if (_gyro_topic == nullptr) { - _gyro_topic = orb_advertise_multi(ORB_ID(sensor_gyro), &gyro_report, + _gyro_topic = orb_advertise_multi(ORB_ID(sensor_gyro), &sensor_gyro_s, &_gyro_orb_class_instance, ORB_PRIO_DEFAULT); } else { - orb_publish(ORB_ID(sensor_gyro), _gyro_topic, &gyro_report); + orb_publish(ORB_ID(sensor_gyro), _gyro_topic, &sensor_gyro_s); } if (_accel_topic == nullptr) { - _accel_topic = orb_advertise_multi(ORB_ID(sensor_accel), &accel_report, + _accel_topic = orb_advertise_multi(ORB_ID(sensor_accel), &sensor_accel_s, &_accel_orb_class_instance, ORB_PRIO_DEFAULT); } else { - orb_publish(ORB_ID(sensor_accel), _accel_topic, &accel_report); + orb_publish(ORB_ID(sensor_accel), _accel_topic, &sensor_accel_s); } // Report if there are high vibrations, every 10 times it happens. diff --git a/src/platforms/posix/drivers/df_mpu9250_wrapper/df_mpu9250_wrapper.cpp b/src/platforms/posix/drivers/df_mpu9250_wrapper/df_mpu9250_wrapper.cpp index 0adf2e1f4bff..83c5a45f9925 100644 --- a/src/platforms/posix/drivers/df_mpu9250_wrapper/df_mpu9250_wrapper.cpp +++ b/src/platforms/posix/drivers/df_mpu9250_wrapper/df_mpu9250_wrapper.cpp @@ -65,7 +65,8 @@ #include #include - +#include +#include #include #include @@ -124,7 +125,7 @@ class DfMpu9250Wrapper : public MPU9250 int _param_update_sub; - struct accel_calibration_s { + struct calibration_accel_s { float x_offset; float x_scale; float y_offset; @@ -133,7 +134,7 @@ class DfMpu9250Wrapper : public MPU9250 float z_scale; } _accel_calibration; - struct gyro_calibration_s { + struct calibration_gyro_s { float x_offset; float x_scale; float y_offset; @@ -285,8 +286,8 @@ DfMpu9250Wrapper::~DfMpu9250Wrapper() int DfMpu9250Wrapper::start() { // TODO: don't publish garbage here - accel_report accel_report = {}; - _accel_topic = orb_advertise_multi(ORB_ID(sensor_accel), &accel_report, + sensor_accel_s sensor_accel_s = {}; + _accel_topic = orb_advertise_multi(ORB_ID(sensor_accel), &sensor_accel_s, &_accel_orb_class_instance, ORB_PRIO_DEFAULT); if (_accel_topic == nullptr) { @@ -295,8 +296,8 @@ int DfMpu9250Wrapper::start() } // TODO: don't publish garbage here - gyro_report gyro_report = {}; - _gyro_topic = orb_advertise_multi(ORB_ID(sensor_gyro), &gyro_report, + sensor_gyro_s sensor_gyro_s = {}; + _gyro_topic = orb_advertise_multi(ORB_ID(sensor_gyro), &sensor_gyro_s, &_gyro_orb_class_instance, ORB_PRIO_DEFAULT); if (_gyro_topic == nullptr) { @@ -612,11 +613,11 @@ int DfMpu9250Wrapper::_publish(struct imu_sensor_data &data) _update_mag_calibration(); } - accel_report accel_report = {}; - gyro_report gyro_report = {}; + sensor_accel_s sensor_accel_s = {}; + sensor_gyro_s sensor_gyro_s = {}; mag_report mag_report = {}; - accel_report.timestamp = gyro_report.timestamp = hrt_absolute_time(); + sensor_accel_s.timestamp = sensor_gyro_s.timestamp = hrt_absolute_time(); // ACCEL @@ -629,26 +630,26 @@ int DfMpu9250Wrapper::_publish(struct imu_sensor_data &data) // MPU9250 driver from DriverFramework does not provide any raw values // TEMP We misuse the raw values on the Snapdragon to publish unfiltered data for VISLAM - accel_report.x_raw = (int16_t)(xraw_f * 1000); // (int16) [m / s^2 * 1000]; - accel_report.y_raw = (int16_t)(yraw_f * 1000); // (int16) [m / s^2 * 1000]; - accel_report.z_raw = (int16_t)(zraw_f * 1000); // (int16) [m / s^2 * 1000]; + sensor_accel_s.x_raw = (int16_t)(xraw_f * 1000); // (int16) [m / s^2 * 1000]; + sensor_accel_s.y_raw = (int16_t)(yraw_f * 1000); // (int16) [m / s^2 * 1000]; + sensor_accel_s.z_raw = (int16_t)(zraw_f * 1000); // (int16) [m / s^2 * 1000]; // adjust values according to the calibration float x_in_new = (xraw_f - _accel_calibration.x_offset) * _accel_calibration.x_scale; float y_in_new = (yraw_f - _accel_calibration.y_offset) * _accel_calibration.y_scale; float z_in_new = (zraw_f - _accel_calibration.z_offset) * _accel_calibration.z_scale; - 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); + sensor_accel_s.x = _accel_filter_x.apply(x_in_new); + sensor_accel_s.y = _accel_filter_y.apply(y_in_new); + sensor_accel_s.z = _accel_filter_z.apply(z_in_new); matrix::Vector3f aval(x_in_new, y_in_new, z_in_new); matrix::Vector3f aval_integrated; - _accel_int.put(accel_report.timestamp, aval, aval_integrated, accel_report.integral_dt); - accel_report.x_integral = aval_integrated(0); - accel_report.y_integral = aval_integrated(1); - accel_report.z_integral = aval_integrated(2); + _accel_int.put(sensor_accel_s.timestamp, aval, aval_integrated, sensor_accel_s.integral_dt); + sensor_accel_s.x_integral = aval_integrated(0); + sensor_accel_s.y_integral = aval_integrated(1); + sensor_accel_s.z_integral = aval_integrated(2); // GYRO @@ -661,26 +662,26 @@ int DfMpu9250Wrapper::_publish(struct imu_sensor_data &data) // MPU9250 driver from DriverFramework does not provide any raw values // TEMP We misuse the raw values on the Snapdragon to publish unfiltered data for VISLAM - gyro_report.x_raw = (int16_t)(xraw_f * 1000); // (int16) [rad / s * 1000]; - gyro_report.y_raw = (int16_t)(yraw_f * 1000); // (int16) [rad / s * 1000]; - gyro_report.z_raw = (int16_t)(zraw_f * 1000); // (int16) [rad / s * 1000]; + sensor_gyro_s.x_raw = (int16_t)(xraw_f * 1000); // (int16) [rad / s * 1000]; + sensor_gyro_s.y_raw = (int16_t)(yraw_f * 1000); // (int16) [rad / s * 1000]; + sensor_gyro_s.z_raw = (int16_t)(zraw_f * 1000); // (int16) [rad / s * 1000]; // adjust values according to the calibration float x_gyro_in_new = (xraw_f - _gyro_calibration.x_offset) * _gyro_calibration.x_scale; float y_gyro_in_new = (yraw_f - _gyro_calibration.y_offset) * _gyro_calibration.y_scale; float z_gyro_in_new = (zraw_f - _gyro_calibration.z_offset) * _gyro_calibration.z_scale; - gyro_report.x = _gyro_filter_x.apply(x_gyro_in_new); - gyro_report.y = _gyro_filter_y.apply(y_gyro_in_new); - gyro_report.z = _gyro_filter_z.apply(z_gyro_in_new); + sensor_gyro_s.x = _gyro_filter_x.apply(x_gyro_in_new); + sensor_gyro_s.y = _gyro_filter_y.apply(y_gyro_in_new); + sensor_gyro_s.z = _gyro_filter_z.apply(z_gyro_in_new); matrix::Vector3f gval(x_gyro_in_new, y_gyro_in_new, z_gyro_in_new); matrix::Vector3f gval_integrated; - bool sensor_notify = _gyro_int.put(gyro_report.timestamp, gval, gval_integrated, gyro_report.integral_dt); - gyro_report.x_integral = gval_integrated(0); - gyro_report.y_integral = gval_integrated(1); - gyro_report.z_integral = gval_integrated(2); + bool sensor_notify = _gyro_int.put(sensor_gyro_s.timestamp, gval, gval_integrated, sensor_gyro_s.integral_dt); + sensor_gyro_s.x_integral = gval_integrated(0); + sensor_gyro_s.y_integral = gval_integrated(1); + sensor_gyro_s.z_integral = gval_integrated(2); // if gyro integrator did not return a sample we can return here @@ -705,16 +706,16 @@ int DfMpu9250Wrapper::_publish(struct imu_sensor_data &data) perf_begin(_publish_perf); // TODO: get these right - gyro_report.scaling = -1.0f; - gyro_report.range_rad_s = -1.0f; - gyro_report.device_id = m_id.dev_id; + sensor_gyro_s.scaling = -1.0f; + sensor_gyro_s.range_rad_s = -1.0f; + sensor_gyro_s.device_id = m_id.dev_id; - accel_report.scaling = -1.0f; - accel_report.range_m_s2 = -1.0f; - accel_report.device_id = m_id.dev_id; + sensor_accel_s.scaling = -1.0f; + sensor_accel_s.range_m_s2 = -1.0f; + sensor_accel_s.device_id = m_id.dev_id; if (_mag_enabled) { - mag_report.timestamp = accel_report.timestamp; + mag_report.timestamp = sensor_accel_s.timestamp; mag_report.is_external = false; mag_report.scaling = -1.0f; @@ -742,11 +743,11 @@ int DfMpu9250Wrapper::_publish(struct imu_sensor_data &data) if (!(m_pub_blocked) && sensor_notify) { if (_gyro_topic != nullptr) { - orb_publish(ORB_ID(sensor_gyro), _gyro_topic, &gyro_report); + orb_publish(ORB_ID(sensor_gyro), _gyro_topic, &sensor_gyro_s); } if (_accel_topic != nullptr) { - orb_publish(ORB_ID(sensor_accel), _accel_topic, &accel_report); + orb_publish(ORB_ID(sensor_accel), _accel_topic, &sensor_accel_s); } if (_mag_enabled) { diff --git a/src/platforms/qurt/fc_addon/mpu_spi/mpu9x50_main.cpp b/src/platforms/qurt/fc_addon/mpu_spi/mpu9x50_main.cpp index 246e91d5489a..af9c6d7a899d 100644 --- a/src/platforms/qurt/fc_addon/mpu_spi/mpu9x50_main.cpp +++ b/src/platforms/qurt/fc_addon/mpu_spi/mpu9x50_main.cpp @@ -96,11 +96,11 @@ static int _accel_orb_class_instance; /**< instance handle for accel devi static orb_advert_t _mag_pub = nullptr; /**< compass data publication */ static int _mag_orb_class_instance; /**< instance handle for mag devices */ static int _params_sub; /**< parameter update subscription */ -static struct gyro_report _gyro; /**< gyro report */ -static struct accel_report _accel; /**< accel report */ +static sensor_gyro_s _gyro; /**< gyro report */ +static sensor_accel_s _accel; /**< accel report */ static struct mag_report _mag; /**< mag report */ -static struct gyro_calibration_s _gyro_sc; /**< gyro scale */ -static struct accel_calibration_s _accel_sc; /**< accel scale */ +static calibration_gyro_s _gyro_sc; /**< gyro scale */ +static calibration_accel_s _accel_sc; /**< accel scale */ static struct mag_calibration_s _mag_sc; /**< mag scale */ static enum gyro_lpf_e _gyro_lpf = MPU9X50_GYRO_LPF_20HZ; /**< gyro lpf enum value */ static enum acc_lpf_e _accel_lpf = MPU9X50_ACC_LPF_20HZ; /**< accel lpf enum value */ @@ -367,8 +367,8 @@ void parameters_init() bool create_pubs() { // initialize the reports - memset(&_gyro, 0, sizeof(struct gyro_report)); - memset(&_accel, 0, sizeof(struct accel_report)); + memset(&_gyro, 0, sizeof(sensor_gyro_s)); + memset(&_accel, 0, sizeof(sensor_accel_s)); memset(&_mag, 0, sizeof(struct mag_report)); _gyro_pub = orb_advertise_multi(ORB_ID(sensor_gyro), &_gyro, diff --git a/src/systemcmds/config/config.c b/src/systemcmds/config/config.c index c89fe28adf83..c1bab0233355 100644 --- a/src/systemcmds/config/config.c +++ b/src/systemcmds/config/config.c @@ -60,6 +60,10 @@ #include "systemlib/systemlib.h" #include +#include +#include +#include +#include __EXPORT int config_main(int argc, char *argv[]); @@ -214,7 +218,7 @@ do_gyro(int argc, char *argv[]) if (ret) { PX4_WARN("gyro self test FAILED! Check calibration:"); - struct gyro_calibration_s scale; + struct calibration_gyro_s scale; ret = ioctl(fd, GYROIOCGSCALE, (long unsigned int)&scale); if (ret) { @@ -390,7 +394,7 @@ do_accel(int argc, char *argv[]) if (ret) { PX4_WARN("accel self test FAILED! Check calibration:"); - struct accel_calibration_s scale; + struct calibration_accel_s scale; ret = ioctl(fd, ACCELIOCGSCALE, (long unsigned int)&scale); if (ret) { diff --git a/src/systemcmds/tests/test_sensors.c b/src/systemcmds/tests/test_sensors.c index 37faa3931843..31c6fe6d628b 100644 --- a/src/systemcmds/tests/test_sensors.c +++ b/src/systemcmds/tests/test_sensors.c @@ -59,6 +59,8 @@ #include #include #include +#include +#include static int accel(int argc, char *argv[], const char *path); static int gyro(int argc, char *argv[], const char *path); @@ -90,7 +92,7 @@ accel(int argc, char *argv[], const char *path) fflush(stdout); int fd; - struct accel_report buf; + struct sensor_accel_s buf; int ret; fd = px4_open(path, O_RDONLY); @@ -140,7 +142,7 @@ gyro(int argc, char *argv[], const char *path) fflush(stdout); int fd; - struct gyro_report buf; + struct sensor_gyro_s buf; int ret; fd = px4_open(path, O_RDONLY);