From 6e6962ecac8a46b991b4427180729a04b9e1d344 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Wed, 6 Jun 2018 14:29:58 -0400 Subject: [PATCH] accel and gyro calibration move to msg/ --- msg/CMakeLists.txt | 2 + msg/calibration_accel.msg | 10 +++ msg/calibration_gyro.msg | 10 +++ msg/tools/px_generate_uorb_topic_helper.py | 4 +- src/drivers/drv_accel.h | 12 ---- src/drivers/drv_gyro.h | 13 ---- src/drivers/imu/adis16448/adis16448.cpp | 55 +++++++++------- src/drivers/imu/adis16477/ADIS16477.cpp | 16 ++--- src/drivers/imu/adis16477/ADIS16477.hpp | 9 ++- src/drivers/imu/adis16477/ADIS16477_gyro.hpp | 3 + src/drivers/imu/adis16477/ADIS16477_main.cpp | 4 +- src/drivers/imu/bma180/bma180.cpp | 22 ++++--- src/drivers/imu/bmi055/bmi055.hpp | 8 ++- src/drivers/imu/bmi055/bmi055_accel.cpp | 16 ++--- src/drivers/imu/bmi055/bmi055_gyro.cpp | 16 ++--- src/drivers/imu/bmi055/bmi055_main.cpp | 4 +- src/drivers/imu/bmi160/bmi160.cpp | 28 ++++---- src/drivers/imu/bmi160/bmi160.hpp | 8 ++- src/drivers/imu/bmi160/bmi160_main.cpp | 4 +- src/drivers/imu/fxas21002c/fxas21002c.cpp | 20 +++--- src/drivers/imu/fxos8701cq/fxos8701cq.cpp | 20 +++--- src/drivers/imu/l3gd20/l3gd20.cpp | 21 +++--- src/drivers/imu/lsm303d/lsm303d.cpp | 24 +++---- src/drivers/imu/mpu6000/mpu6000.cpp | 54 ++++++++------- src/drivers/imu/mpu9250/main.cpp | 4 +- src/drivers/imu/mpu9250/mpu9250.cpp | 35 +++++----- src/drivers/imu/mpu9250/mpu9250.h | 10 +-- .../matlab_csv_serial/matlab_csv_serial.c | 8 +-- src/modules/commander/PreflightCheck.cpp | 6 +- .../commander/accelerometer_calibration.cpp | 10 +-- src/modules/commander/gyro_calibration.cpp | 13 ++-- src/modules/commander/mag_calibration.cpp | 3 +- src/modules/mavlink/mavlink_receiver.cpp | 10 +-- src/modules/sensors/voted_sensors_update.cpp | 66 ++++++++++--------- src/modules/sensors/voted_sensors_update.h | 11 ++-- src/modules/simulator/accelsim/accelsim.cpp | 20 +++--- src/modules/simulator/gyrosim/gyrosim.cpp | 44 +++++++------ src/modules/simulator/simulator_mavlink.cpp | 6 +- .../df_lsm9ds1_wrapper/df_lsm9ds1_wrapper.cpp | 10 +-- .../df_mpu6050_wrapper/df_mpu6050_wrapper.cpp | 6 +- .../df_mpu9250_wrapper/df_mpu9250_wrapper.cpp | 11 ++-- .../qurt/fc_addon/mpu_spi/mpu9x50_main.cpp | 12 ++-- src/systemcmds/config/config.c | 8 ++- src/systemcmds/tests/test_sensors.c | 6 +- 44 files changed, 380 insertions(+), 302 deletions(-) create mode 100644 msg/calibration_accel.msg create mode 100644 msg/calibration_gyro.msg 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/msg/tools/px_generate_uorb_topic_helper.py b/msg/tools/px_generate_uorb_topic_helper.py index 2c0a4c79961f..9b534d521068 100644 --- a/msg/tools/px_generate_uorb_topic_helper.py +++ b/msg/tools/px_generate_uorb_topic_helper.py @@ -77,8 +77,8 @@ 'uint16': '%u', 'uint32': '%" PRIu32 "', 'uint64': '%" PRIu64 "', - 'float32': '%.3f', - 'float64': '%.3f', + 'float32': '%.5f', + 'float64': '%.6f', 'bool': '%u', 'char': '%c', } 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..15b271944094 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 @@ -224,21 +229,21 @@ class ADIS16448 : public device::SPI uint16_t _product; /** product code */ - struct hrt_call _call; + struct hrt_call _call; unsigned _call_interval; - ringbuffer::RingBuffer *_gyro_reports; + ringbuffer::RingBuffer *_gyro_reports; - 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 *_accel_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; + orb_advert_t _accel_topic; int _accel_orb_class_instance; int _accel_class_instance; @@ -611,13 +616,13 @@ ADIS16448::init() } /* allocate basic report buffers */ - _gyro_reports = new ringbuffer::RingBuffer(2, sizeof(gyro_report)); + _gyro_reports = new ringbuffer::RingBuffer(2, sizeof(sensor_gyro_s)); if (_gyro_reports == nullptr) { goto out; } - _accel_reports = new ringbuffer::RingBuffer(2, sizeof(accel_report)); + _accel_reports = new ringbuffer::RingBuffer(2, sizeof(sensor_accel_s)); if (_accel_reports == nullptr) { goto out; @@ -679,7 +684,7 @@ ADIS16448::init() measure(); /* advertise sensor topic, measure manually to initialize valid report */ - struct accel_report arp; + sensor_accel_s arp; _accel_reports->get(&arp); /* measurement will have generated a report, publish */ @@ -690,7 +695,7 @@ ADIS16448::init() warnx("ADVERT FAIL"); } - struct gyro_report grp; + sensor_gyro_s grp; _gyro_reports->get(&grp); @@ -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) { @@ -876,7 +881,7 @@ ADIS16448::read(struct file *filp, char *buffer, size_t buflen) 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--) { @@ -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) { @@ -947,7 +952,7 @@ ADIS16448::gyro_read(struct file *filp, char *buffer, size_t buflen) 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--) { @@ -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 @@ -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: @@ -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: @@ -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(); @@ -1826,9 +1831,9 @@ start(enum Rotation rotation) void test() { - accel_report a_report; - gyro_report g_report; - mag_report m_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 6fd2bdb4aadf..9436a0a92106 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..71f19b1ec12c 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 @@ -338,7 +342,7 @@ class BMI055_accel : public BMI055 ringbuffer::RingBuffer *_accel_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; @@ -491,7 +495,7 @@ class BMI055_gyro : public BMI055 ringbuffer::RingBuffer *_gyro_reports; - 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..853e996ea6d9 100644 --- a/src/drivers/imu/bmi055/bmi055_accel.cpp +++ b/src/drivers/imu/bmi055/bmi055_accel.cpp @@ -84,7 +84,7 @@ BMI055_accel::init() } /* allocate basic report buffers */ - _accel_reports = new ringbuffer::RingBuffer(2, sizeof(accel_report)); + _accel_reports = new ringbuffer::RingBuffer(2, sizeof(sensor_accel_s)); if (_accel_reports == nullptr) { goto out; @@ -108,7 +108,7 @@ BMI055_accel::init() measure(); /* advertise sensor topic, measure manually to initialize valid report */ - struct accel_report arp; + sensor_accel_s arp; _accel_reports->get(&arp); /* measurement will have generated a report, publish */ @@ -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) { @@ -260,7 +260,7 @@ BMI055_accel::read(struct file *filp, char *buffer, size_t buflen) 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--) { @@ -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 @@ -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: @@ -715,7 +715,7 @@ BMI055_accel::measure() /* * Report buffers. */ - accel_report arb; + sensor_accel_s arb; arb.timestamp = hrt_absolute_time(); diff --git a/src/drivers/imu/bmi055/bmi055_gyro.cpp b/src/drivers/imu/bmi055/bmi055_gyro.cpp index 177494331799..690a06838a38 100644 --- a/src/drivers/imu/bmi055/bmi055_gyro.cpp +++ b/src/drivers/imu/bmi055/bmi055_gyro.cpp @@ -84,7 +84,7 @@ BMI055_gyro::init() } /* allocate basic report buffers */ - _gyro_reports = new ringbuffer::RingBuffer(2, sizeof(accel_report)); + _gyro_reports = new ringbuffer::RingBuffer(2, sizeof(sensor_gyro_s)); if (_gyro_reports == nullptr) { goto out; @@ -114,7 +114,7 @@ BMI055_gyro::init() measure(); /* advertise sensor topic, measure manually to initialize valid report */ - struct gyro_report grp; + sensor_gyro_s grp; _gyro_reports->get(&grp); _gyro_topic = orb_advertise_multi(ORB_ID(sensor_gyro), &grp, @@ -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) { @@ -322,7 +322,7 @@ BMI055_gyro::read(struct file *filp, char *buffer, size_t buflen) 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--) { @@ -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)); } @@ -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: @@ -685,7 +685,7 @@ BMI055_gyro::measure() /* * Report buffers. */ - gyro_report grb; + sensor_gyro_s grb; grb.timestamp = hrt_absolute_time(); 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..7d718c75895e 100644 --- a/src/drivers/imu/bmi160/bmi160.cpp +++ b/src/drivers/imu/bmi160/bmi160.cpp @@ -138,13 +138,13 @@ BMI160::init() } /* allocate basic report buffers */ - _accel_reports = new ringbuffer::RingBuffer(2, sizeof(accel_report)); + _accel_reports = new ringbuffer::RingBuffer(2, sizeof(sensor_accel_s)); if (_accel_reports == nullptr) { goto out; } - _gyro_reports = new ringbuffer::RingBuffer(2, sizeof(gyro_report)); + _gyro_reports = new ringbuffer::RingBuffer(2, sizeof(sensor_gyro_s)); if (_gyro_reports == nullptr) { goto out; @@ -184,7 +184,7 @@ BMI160::init() measure(); /* advertise sensor topic, measure manually to initialize valid report */ - struct accel_report arp; + sensor_accel_s arp; _accel_reports->get(&arp); /* measurement will have generated a report, publish */ @@ -197,7 +197,7 @@ BMI160::init() /* advertise sensor topic, measure manually to initialize valid report */ - struct gyro_report grp; + sensor_gyro_s grp; _gyro_reports->get(&grp); _gyro->_gyro_topic = orb_advertise_multi(ORB_ID(sensor_gyro), &grp, @@ -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) { @@ -465,7 +465,7 @@ BMI160::read(struct file *filp, char *buffer, size_t buflen) 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--) { @@ -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) { @@ -584,7 +584,7 @@ BMI160::gyro_read(struct file *filp, char *buffer, size_t buflen) 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--) { @@ -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)); } @@ -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: @@ -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. diff --git a/src/drivers/imu/bmi160/bmi160.hpp b/src/drivers/imu/bmi160/bmi160.hpp index a08dd4de5051..b4bec910fc99 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 @@ -282,7 +286,7 @@ class BMI160 : public device::SPI ringbuffer::RingBuffer *_accel_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; @@ -291,7 +295,7 @@ class BMI160 : public device::SPI ringbuffer::RingBuffer *_gyro_reports; - 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 04ba7fdd454e..10837fad3aa4 100644 --- a/src/drivers/imu/bmi160/bmi160_main.cpp +++ b/src/drivers/imu/bmi160/bmi160_main.cpp @@ -121,8 +121,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..104dc285cf7c 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: @@ -982,7 +984,7 @@ FXAS21002C::measure() } raw_gyro_report; #pragma pack(pop) - struct gyro_report gyro_report; + sensor_gyro_s gyro_report; /* start the performance counter */ perf_begin(_sample_perf); @@ -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..5c102c0989e0 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) @@ -199,7 +201,7 @@ class FXOS8701CQ : public device::SPI ringbuffer::RingBuffer *_accel_reports; 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; @@ -573,7 +575,7 @@ FXOS8701CQ::init() } /* allocate basic report buffers */ - _accel_reports = new ringbuffer::RingBuffer(2, sizeof(accel_report)); + _accel_reports = new ringbuffer::RingBuffer(2, sizeof(sensor_accel_s)); if (_accel_reports == nullptr) { goto out; @@ -614,7 +616,7 @@ 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; + sensor_accel_s arp; _accel_reports->get(&arp); /* measurement will have generated a report, publish */ @@ -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 */ @@ -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: @@ -1341,7 +1343,7 @@ FXOS8701CQ::measure() } raw_accel_mag_report; #pragma pack(pop) - accel_report accel_report; + sensor_accel_s accel_report; /* start the performance counter */ perf_begin(_accel_sample_perf); @@ -1787,7 +1789,7 @@ test() { int rv = 1; int fd_accel = -1; - struct accel_report accel_report; + sensor_accel_s accel_report; ssize_t sz; int ret; int fd_mag = -1; 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..b6010f45006d 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) @@ -265,9 +267,9 @@ class LSM303D : public device::SPI unsigned _call_mag_interval; ringbuffer::RingBuffer *_accel_reports; - ringbuffer::RingBuffer *_mag_reports; + 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; @@ -631,7 +633,7 @@ LSM303D::init() } /* allocate basic report buffers */ - _accel_reports = new ringbuffer::RingBuffer(2, sizeof(accel_report)); + _accel_reports = new ringbuffer::RingBuffer(2, sizeof(sensor_accel_s)); if (_accel_reports == nullptr) { goto out; @@ -672,7 +674,7 @@ 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; + sensor_accel_s arp; _accel_reports->get(&arp); /* measurement will have generated a report, publish */ @@ -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 */ @@ -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: @@ -1465,7 +1467,7 @@ LSM303D::measure() } raw_accel_report; #pragma pack(pop) - accel_report accel_report; + sensor_accel_s accel_report; /* start the performance counter */ perf_begin(_accel_sample_perf); @@ -1946,7 +1948,7 @@ void test() { int fd_accel = -1; - struct accel_report accel_report; + sensor_accel_s accel_report; ssize_t sz; int ret; @@ -1958,7 +1960,7 @@ test() } /* do a simple demand read */ - sz = read(fd_accel, &accel_report, sizeof(accel_report)); + sz = read(fd_accel, &accel_report, sizeof(sensor_accel_s)); if (sz != sizeof(accel_report)) { err(1, "immediate read failed"); diff --git a/src/drivers/imu/mpu6000/mpu6000.cpp b/src/drivers/imu/mpu6000/mpu6000.cpp index 0f0d1aaeba4e..a9ccbf43cc00 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" @@ -186,7 +190,7 @@ class MPU6000 : public device::CDev ringbuffer::RingBuffer *_accel_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; @@ -195,7 +199,7 @@ class MPU6000 : public device::CDev ringbuffer::RingBuffer *_gyro_reports; - struct gyro_calibration_s _gyro_scale; + calibration_gyro_s _gyro_scale; float _gyro_range_scale; float _gyro_range_rad_s; @@ -657,13 +661,13 @@ MPU6000::init() ret = -ENOMEM; /* allocate basic report buffers */ - _accel_reports = new ringbuffer::RingBuffer(2, sizeof(accel_report)); + _accel_reports = new ringbuffer::RingBuffer(2, sizeof(sensor_accel_s)); if (_accel_reports == nullptr) { return ret; } - _gyro_reports = new ringbuffer::RingBuffer(2, sizeof(gyro_report)); + _gyro_reports = new ringbuffer::RingBuffer(2, sizeof(sensor_gyro_s)); if (_gyro_reports == nullptr) { return ret; @@ -733,7 +737,7 @@ MPU6000::init() measure(); /* advertise sensor topic, measure manually to initialize valid report */ - struct accel_report arp; + sensor_accel_s arp; _accel_reports->get(&arp); /* measurement will have generated a report, publish */ @@ -745,7 +749,7 @@ MPU6000::init() } /* advertise sensor topic, measure manually to initialize valid report */ - struct gyro_report grp; + sensor_gyro_s grp; _gyro_reports->get(&grp); _gyro->_gyro_topic = orb_advertise_multi(ORB_ID(sensor_gyro), &grp, @@ -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) { @@ -1054,7 +1058,7 @@ MPU6000::read(struct file *filp, char *buffer, size_t buflen) 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--) { @@ -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) { @@ -1339,7 +1343,7 @@ MPU6000::gyro_read(struct file *filp, char *buffer, size_t buflen) 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--) { @@ -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 @@ -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: @@ -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: @@ -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. @@ -2115,10 +2119,12 @@ MPU6000::print_info() } ::printf("temperature: %.1f\n", (double)_last_temperature); - float accel_cut = _accel_filter_x.get_cutoff_freq(); - ::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)); + ::printf("accel cutoff set to %10.2f Hz\n", double(_accel_filter_x.get_cutoff_freq())); + ::printf("gyro cutoff set to %10.2f Hz\n", double(_gyro_filter_x.get_cutoff_freq())); + + print_message(_accel_scale); + print_message(_gyro_scale); + } void @@ -2418,8 +2424,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 1f1772905f17..162a7d3f061e 100644 --- a/src/drivers/imu/mpu9250/main.cpp +++ b/src/drivers/imu/mpu9250/main.cpp @@ -316,8 +316,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 0d5b6b715a7d..9a7e72d5033f 100644 --- a/src/drivers/imu/mpu9250/mpu9250.cpp +++ b/src/drivers/imu/mpu9250/mpu9250.cpp @@ -288,14 +288,14 @@ MPU9250::init() } /* allocate basic report buffers */ - _accel_reports = new ringbuffer::RingBuffer(2, sizeof(accel_report)); + _accel_reports = new ringbuffer::RingBuffer(2, sizeof(sensor_accel_s)); ret = -ENOMEM; if (_accel_reports == nullptr) { return ret; } - _gyro_reports = new ringbuffer::RingBuffer(2, sizeof(gyro_report)); + _gyro_reports = new ringbuffer::RingBuffer(2, sizeof(sensor_gyro_s)); if (_gyro_reports == nullptr) { return ret; @@ -394,7 +394,7 @@ MPU9250::init() measure(); /* advertise sensor topic, measure manually to initialize valid report */ - struct accel_report arp; + sensor_accel_s arp; _accel_reports->get(&arp); /* measurement will have generated a report, publish */ @@ -407,7 +407,7 @@ MPU9250::init() } /* advertise sensor topic, measure manually to initialize valid report */ - struct gyro_report grp; + sensor_gyro_s grp; _gyro_reports->get(&grp); _gyro->_gyro_topic = orb_advertise_multi(ORB_ID(sensor_gyro), &grp, @@ -626,7 +626,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) { @@ -647,7 +647,7 @@ MPU9250::read(struct file *filp, char *buffer, size_t buflen) 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--) { @@ -660,7 +660,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 @@ -751,7 +751,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) { @@ -772,7 +772,7 @@ MPU9250::gyro_read(struct file *filp, char *buffer, size_t buflen) 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--) { @@ -785,7 +785,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 @@ -902,7 +902,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) { @@ -916,7 +916,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: @@ -972,12 +972,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: @@ -1378,8 +1378,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. @@ -1548,6 +1548,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..300eb536b296 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 @@ -293,7 +295,7 @@ class MPU9250 : public device::CDev ringbuffer::RingBuffer *_accel_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; @@ -302,7 +304,7 @@ class MPU9250 : public device::CDev ringbuffer::RingBuffer *_gyro_reports; - 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 9ad055b1c929..e920c49a252d 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 #include "PreflightCheck.h" @@ -305,7 +309,7 @@ static bool accelerometerCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s & 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..a64191384e43 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"; @@ -177,7 +179,7 @@ int do_accel_calibration(orb_advert_t *mavlink_log_pub) 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; @@ -479,7 +481,7 @@ 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 @@ -538,7 +540,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 +628,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..3e287ad7d464 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 gyro_report_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 gyro_report; unsigned poll_errcount = 0; struct sensor_correction_s sensor_correction; /**< sensor thermal corrections */ @@ -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; @@ -307,7 +308,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 diff --git a/src/modules/commander/mag_calibration.cpp b/src/modules/commander/mag_calibration.cpp index e05ee2d227da..b6de7f2998c6 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; @@ -396,7 +397,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 */ diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 793eee92866c..3d0f802de291 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 @@ -91,6 +89,8 @@ #include #include +#include +#include #include #include "mavlink_bridge_header.h" @@ -1925,7 +1925,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; @@ -1946,7 +1946,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); @@ -2342,7 +2342,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 eb8c3e7e87eb..5ecef5187e51 100644 --- a/src/modules/sensors/voted_sensors_update.cpp +++ b/src/modules/sensors/voted_sensors_update.cpp @@ -153,19 +153,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; - } } } @@ -177,7 +177,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); @@ -185,11 +185,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; - } } } @@ -202,17 +202,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; - } } } @@ -220,12 +220,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++) { @@ -239,7 +238,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*/ @@ -248,12 +247,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); @@ -267,13 +266,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); @@ -286,6 +287,7 @@ void VotedSensorsUpdate::parameters_update() } else { /* apply new scaling and offsets */ + gscale.timestamp = hrt_absolute_time(); config_ok = apply_gyro_calibration(h, &gscale, device_id); if (!config_ok) { @@ -305,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++) { @@ -336,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); @@ -355,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); @@ -374,6 +379,7 @@ void VotedSensorsUpdate::parameters_update() } else { /* apply new scaling and offsets */ + ascale.timestamp = hrt_absolute_time(); config_ok = apply_accel_calibration(h, &ascale, device_id); if (!config_ok) { @@ -393,10 +399,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); @@ -410,13 +414,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; @@ -467,13 +471,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); @@ -482,12 +488,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 */ @@ -517,8 +521,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) { @@ -543,7 +547,7 @@ 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 accel_report; int ret = orb_copy(ORB_ID(sensor_accel), _accel.subscription[uorb_index], &accel_report); @@ -648,7 +652,7 @@ 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 gyro_report; int ret = orb_copy(ORB_ID(sensor_gyro), _gyro.subscription[uorb_index], &gyro_report); @@ -1008,7 +1012,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) @@ -1022,7 +1026,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 f905690506eb..754843c6ce5a 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 @@ -216,7 +220,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. @@ -226,8 +230,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, - const int device_id); + bool apply_accel_calibration(DriverFramework::DevHandle &h, const calibration_accel_s *acal, const int device_id); /** * Apply a mag calibration. @@ -237,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..7e28aea09876 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" @@ -113,7 +117,7 @@ class ACCELSIM : public VirtDevObj ringbuffer::RingBuffer *_accel_reports; 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; @@ -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,7 +333,7 @@ ACCELSIM::init() } /* allocate basic report buffers */ - _accel_reports = new ringbuffer::RingBuffer(2, sizeof(accel_report)); + _accel_reports = new ringbuffer::RingBuffer(2, sizeof(sensor_accel_s)); if (_accel_reports == nullptr) { goto out; @@ -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 */ @@ -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: @@ -812,7 +816,7 @@ ACCELSIM::_measure() } raw_accel_report; #pragma pack(pop) - accel_report accel_report = {}; + sensor_accel_s accel_report = {}; /* start the performance counter */ perf_begin(_accel_sample_perf); diff --git a/src/modules/simulator/gyrosim/gyrosim.cpp b/src/modules/simulator/gyrosim/gyrosim.cpp index a8ba20092879..fb08c91bcfcf 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" @@ -150,7 +154,7 @@ class GYROSIM : public VirtDevObj ringbuffer::RingBuffer *_accel_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; @@ -158,7 +162,7 @@ class GYROSIM : public VirtDevObj ringbuffer::RingBuffer *_gyro_reports; - struct gyro_calibration_s _gyro_scale; + calibration_gyro_s _gyro_scale; float _gyro_range_scale; float _gyro_range_rad_s; @@ -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,14 +394,14 @@ GYROSIM::init() } /* allocate basic report buffers */ - _accel_reports = new ringbuffer::RingBuffer(2, sizeof(accel_report)); + _accel_reports = new ringbuffer::RingBuffer(2, sizeof(sensor_accel_s)); if (_accel_reports == nullptr) { PX4_WARN("_accel_reports creation failed"); goto out; } - _gyro_reports = new ringbuffer::RingBuffer(2, sizeof(gyro_report)); + _gyro_reports = new ringbuffer::RingBuffer(2, sizeof(sensor_gyro_s)); if (_gyro_reports == nullptr) { PX4_WARN("_gyro_reports creation failed"); @@ -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) { @@ -572,7 +576,7 @@ GYROSIM::devRead(void *buffer, size_t buflen) 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--) { @@ -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) { @@ -686,7 +690,7 @@ GYROSIM::gyro_read(void *buffer, size_t buflen) 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--) { @@ -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 @@ -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: @@ -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: @@ -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(); @@ -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..d34fc06854c6 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 @@ -239,7 +241,7 @@ DfLsm9ds1Wrapper::~DfLsm9ds1Wrapper() int DfLsm9ds1Wrapper::start() { // TODO: don't publish garbage here - accel_report accel_report = {}; + sensor_accel_s accel_report = {}; _accel_topic = orb_advertise_multi(ORB_ID(sensor_accel), &accel_report, &_accel_orb_class_instance, ORB_PRIO_DEFAULT); @@ -249,7 +251,7 @@ int DfLsm9ds1Wrapper::start() } // TODO: don't publish garbage here - gyro_report gyro_report = {}; + sensor_gyro_s gyro_report = {}; _gyro_topic = orb_advertise_multi(ORB_ID(sensor_gyro), &gyro_report, &_gyro_orb_class_instance, ORB_PRIO_DEFAULT); @@ -618,8 +620,8 @@ int DfLsm9ds1Wrapper::_publish(struct imu_sensor_data &data) perf_begin(_publish_perf); - accel_report accel_report = {}; - gyro_report gyro_report = {}; + sensor_accel_s accel_report = {}; + sensor_gyro_s gyro_report = {}; mag_report mag_report = {}; accel_report.timestamp = gyro_report.timestamp = hrt_absolute_time(); 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..ca685e8a57ec 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 @@ -488,8 +490,8 @@ int DfMPU6050Wrapper::_publish(struct imu_sensor_data &data) perf_begin(_publish_perf); - accel_report accel_report = {}; - gyro_report gyro_report = {}; + sensor_accel_s accel_report = {}; + sensor_gyro_s gyro_report = {}; accel_report.timestamp = gyro_report.timestamp = hrt_absolute_time(); 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..40884a1072c9 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 @@ -285,7 +286,7 @@ DfMpu9250Wrapper::~DfMpu9250Wrapper() int DfMpu9250Wrapper::start() { // TODO: don't publish garbage here - accel_report accel_report = {}; + sensor_accel_s accel_report = {}; _accel_topic = orb_advertise_multi(ORB_ID(sensor_accel), &accel_report, &_accel_orb_class_instance, ORB_PRIO_DEFAULT); @@ -295,7 +296,7 @@ int DfMpu9250Wrapper::start() } // TODO: don't publish garbage here - gyro_report gyro_report = {}; + sensor_gyro_s gyro_report = {}; _gyro_topic = orb_advertise_multi(ORB_ID(sensor_gyro), &gyro_report, &_gyro_orb_class_instance, ORB_PRIO_DEFAULT); @@ -612,8 +613,8 @@ int DfMpu9250Wrapper::_publish(struct imu_sensor_data &data) _update_mag_calibration(); } - accel_report accel_report = {}; - gyro_report gyro_report = {}; + sensor_accel_s accel_report = {}; + sensor_gyro_s gyro_report = {}; mag_report mag_report = {}; accel_report.timestamp = gyro_report.timestamp = hrt_absolute_time(); 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);