From f24b2a701f9f4bf411fd85f4f3f1e6457e19319a Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Mon, 7 Mar 2016 22:32:02 +0100 Subject: [PATCH] sensors: first part of a calibration refactor This adds uORB messages to publish calibration data by sensors which is then consumed by the sensors. Currently this is only used on Snapdragon and guarded by QURT ifdefs. --- msg/accel_calibration.msg | 10 + msg/gyro_calibration.msg | 10 + msg/mag_calibration.msg | 10 + src/modules/sensors/sensors.cpp | 190 ++++++++++++++---- src/modules/uORB/objects_common.cpp | 9 + .../df_mpu9250_wrapper/df_mpu9250_wrapper.cpp | 173 +++++++++++----- 6 files changed, 314 insertions(+), 88 deletions(-) create mode 100644 msg/accel_calibration.msg create mode 100644 msg/gyro_calibration.msg create mode 100644 msg/mag_calibration.msg diff --git a/msg/accel_calibration.msg b/msg/accel_calibration.msg new file mode 100644 index 0000000000..6e58d17c35 --- /dev/null +++ b/msg/accel_calibration.msg @@ -0,0 +1,10 @@ +uint64 timestamp + +int32 device_id + +float32 x_offset +float32 x_scale +float32 y_offset +float32 y_scale +float32 z_offset +float32 z_scale diff --git a/msg/gyro_calibration.msg b/msg/gyro_calibration.msg new file mode 100644 index 0000000000..6e58d17c35 --- /dev/null +++ b/msg/gyro_calibration.msg @@ -0,0 +1,10 @@ +uint64 timestamp + +int32 device_id + +float32 x_offset +float32 x_scale +float32 y_offset +float32 y_scale +float32 z_offset +float32 z_scale diff --git a/msg/mag_calibration.msg b/msg/mag_calibration.msg new file mode 100644 index 0000000000..6e58d17c35 --- /dev/null +++ b/msg/mag_calibration.msg @@ -0,0 +1,10 @@ +uint64 timestamp + +int32 device_id + +float32 x_offset +float32 x_scale +float32 y_offset +float32 y_scale +float32 z_offset +float32 z_scale diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 776a1b5d6b..bb5dcae0ff 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2012-2015 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-2016 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -41,7 +41,7 @@ * well instead of relying on the sensor_combined topic. * * @author Lorenz Meier - * @author Julian Oes + * @author Julian Oes * @author Thomas Gubler * @author Anton Babushkin */ @@ -100,6 +100,9 @@ #include #include #include +#include +#include +#include #include @@ -453,6 +456,36 @@ private: */ void parameter_update_poll(bool forced = false); + /** + * Apply a gyro calibration. + * + * @param h: reference to the DevHandle in use + * @param gscale: the calibration data. + * @param device: the device id of the sensor. + * @return: true if config is ok + */ + bool apply_gyro_calibration(DevHandle &h, const struct gyro_scale *gscale, const int device_id); + + /** + * Apply a accel calibration. + * + * @param h: reference to the DevHandle in use + * @param ascale: the calibration data. + * @param device: the device id of the sensor. + * @return: true if config is ok + */ + bool apply_accel_calibration(DevHandle &h, const struct accel_scale *ascale, const int device_id); + + /** + * Apply a mag calibration. + * + * @param h: reference to the DevHandle in use + * @param gscale: the calibration data. + * @param device: the device id of the sensor. + * @return: true if config is ok + */ + bool apply_mag_calibration(DevHandle &h, const struct mag_scale *mscale, const int device_id); + /** * Check for changes in rc_parameter_map */ @@ -1184,7 +1217,6 @@ Sensors::parameter_update_poll(bool forced) /* set offset parameters to new values */ bool failed; - int res; char str[30]; unsigned mag_count = 0; unsigned gyro_count = 0; @@ -1193,7 +1225,6 @@ Sensors::parameter_update_poll(bool forced) /* run through all gyro sensors */ for (unsigned s = 0; s < SENSOR_COUNT_MAX; s++) { - res = ERROR; (void)sprintf(str, "%s%u", GYRO_BASE_DEVICE_PATH, s); DevHandle h; @@ -1243,13 +1274,10 @@ Sensors::parameter_update_poll(bool forced) } else { /* apply new scaling and offsets */ - res = h.ioctl(GYROIOCSSCALE, (long unsigned int)&gscale); + config_ok = apply_gyro_calibration(h, &gscale, device_id); - if (res) { - warnx(CAL_ERROR_APPLY_CAL_MSG, "gyro", i); - - } else { - config_ok = true; + if (!config_ok) { + warnx(CAL_ERROR_APPLY_CAL_MSG, "gyro ", i); } } @@ -1265,7 +1293,6 @@ Sensors::parameter_update_poll(bool forced) /* run through all accel sensors */ for (unsigned s = 0; s < SENSOR_COUNT_MAX; s++) { - res = ERROR; (void)sprintf(str, "%s%u", ACCEL_BASE_DEVICE_PATH, s); DevHandle h; @@ -1296,32 +1323,29 @@ Sensors::parameter_update_poll(bool forced) /* if the calibration is for this device, apply it */ if (device_id == h.ioctl(DEVIOCGDEVICEID, 0)) { - struct accel_scale gscale = {}; + struct accel_scale ascale = {}; (void)sprintf(str, "CAL_ACC%u_XOFF", i); - failed = failed || (OK != param_get(param_find(str), &gscale.x_offset)); + 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), &gscale.y_offset)); + 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), &gscale.z_offset)); + 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), &gscale.x_scale)); + failed = failed || (OK != param_get(param_find(str), &ascale.x_scale)); (void)sprintf(str, "CAL_ACC%u_YSCALE", i); - failed = failed || (OK != param_get(param_find(str), &gscale.y_scale)); + failed = failed || (OK != param_get(param_find(str), &ascale.y_scale)); (void)sprintf(str, "CAL_ACC%u_ZSCALE", i); - failed = failed || (OK != param_get(param_find(str), &gscale.z_scale)); + failed = failed || (OK != param_get(param_find(str), &ascale.z_scale)); if (failed) { warnx(CAL_ERROR_APPLY_CAL_MSG, "accel", i); } else { /* apply new scaling and offsets */ - res = h.ioctl(ACCELIOCSSCALE, (long unsigned int)&gscale); + config_ok = apply_accel_calibration(h, &ascale, device_id); - if (res) { - warnx(CAL_ERROR_APPLY_CAL_MSG, "accel", i); - - } else { - config_ok = true; + if (!config_ok) { + warnx(CAL_ERROR_APPLY_CAL_MSG, "accel ", i); } } @@ -1343,7 +1367,6 @@ Sensors::parameter_update_poll(bool forced) */ _mag_rotation[s] = _board_rotation; - res = ERROR; (void)sprintf(str, "%s%u", MAG_BASE_DEVICE_PATH, s); DevHandle h; @@ -1377,19 +1400,19 @@ Sensors::parameter_update_poll(bool forced) /* if the calibration is for this device, apply it */ if (device_id == h.ioctl(DEVIOCGDEVICEID, 0)) { - struct mag_scale gscale = {}; + struct mag_scale mscale = {}; (void)sprintf(str, "CAL_MAG%u_XOFF", i); - failed = failed || (OK != param_get(param_find(str), &gscale.x_offset)); + 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), &gscale.y_offset)); + 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), &gscale.z_offset)); + 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), &gscale.x_scale)); + failed = failed || (OK != param_get(param_find(str), &mscale.x_scale)); (void)sprintf(str, "CAL_MAG%u_YSCALE", i); - failed = failed || (OK != param_get(param_find(str), &gscale.y_scale)); + failed = failed || (OK != param_get(param_find(str), &mscale.y_scale)); (void)sprintf(str, "CAL_MAG%u_ZSCALE", i); - failed = failed || (OK != param_get(param_find(str), &gscale.z_scale)); + failed = failed || (OK != param_get(param_find(str), &mscale.z_scale)); (void)sprintf(str, "CAL_MAG%u_ROT", i); @@ -1451,14 +1474,12 @@ Sensors::parameter_update_poll(bool forced) warnx(CAL_ERROR_APPLY_CAL_MSG, "mag", i); } else { + /* apply new scaling and offsets */ - res = h.ioctl(MAGIOCSSCALE, (long unsigned int)&gscale); + config_ok = apply_mag_calibration(h, &mscale, device_id); - if (res) { - warnx(CAL_ERROR_APPLY_CAL_MSG, "mag", i); - - } else { - config_ok = true; + if (!config_ok) { + warnx(CAL_ERROR_APPLY_CAL_MSG, "mag ", i); } } @@ -1493,6 +1514,99 @@ Sensors::parameter_update_poll(bool forced) } } +bool +Sensors::apply_gyro_calibration(DevHandle &h, const struct gyro_scale *gscale, const int device_id) +{ +#ifndef __PX4_QURT + + /* On most systems, we can just use the IOCTL call to set the calibration params. */ + const int res = h.ioctl(GYROIOCSSCALE, (long unsigned int)gscale); + + if (res) { + return false; + + } else { + return true; + } + +#else + /* On QURT, we can't use the ioctl calls, therefore we publish the calibration over uORB. */ + + static orb_advert_t gyro_calibration_pub = nullptr; + + if (gyro_calibration_pub != nullptr) { + orb_publish(ORB_ID(gyro_calibration), gyro_calibration_pub, gscale); + + } else { + gyro_calibration_pub = orb_advertise(ORB_ID(gyro_calibration), gscale); + } + + return true; +#endif +} + +bool +Sensors::apply_accel_calibration(DevHandle &h, const struct accel_scale *ascale, const int device_id) +{ +#ifndef __PX4_QURT + + /* On most systems, we can just use the IOCTL call to set the calibration params. */ + const int res = h.ioctl(ACCELIOCSSCALE, (long unsigned int)ascale); + + if (res) { + return false; + + } else { + return true; + } + +#else + /* On QURT, we can't use the ioctl calls, therefore we publish the calibration over uORB. */ + + static orb_advert_t accel_calibration_pub = nullptr; + + if (accel_calibration_pub != nullptr) { + orb_publish(ORB_ID(accel_calibration), accel_calibration_pub, ascale); + + } else { + accel_calibration_pub = orb_advertise(ORB_ID(accel_calibration), ascale); + } + + return true; +#endif +} + +bool +Sensors::apply_mag_calibration(DevHandle &h, const struct mag_scale *mscale, const int device_id) +{ +#ifndef __PX4_QURT + + /* On most systems, we can just use the IOCTL call to set the calibration params. */ + const int res = h.ioctl(MAGIOCSSCALE, (long unsigned int)mscale); + + if (res) { + return false; + + } else { + return true; + } + +#else + /* On QURT, we can't use the ioctl calls, therefore we publish the calibration over uORB. */ + + static orb_advert_t mag_calibration_pub = nullptr; + + if (mag_calibration_pub != nullptr) { + orb_publish(ORB_ID(mag_calibration), mag_calibration_pub, mscale); + + } else { + mag_calibration_pub = orb_advertise(ORB_ID(mag_calibration), mscale); + } + + return true; +#endif +} + void Sensors::rc_parameter_map_poll(bool forced) { diff --git a/src/modules/uORB/objects_common.cpp b/src/modules/uORB/objects_common.cpp index 5f05123f7e..7bdaedfa6b 100644 --- a/src/modules/uORB/objects_common.cpp +++ b/src/modules/uORB/objects_common.cpp @@ -282,3 +282,12 @@ ORB_DEFINE(ekf2_replay, struct ekf2_replay_s); #include "topics/qshell_req.h" ORB_DEFINE(qshell_req, struct qshell_req_s); + +#include "topics/gyro_calibration.h" +ORB_DEFINE(gyro_calibration, struct gyro_calibration_s); + +#include "topics/accel_calibration.h" +ORB_DEFINE(accel_calibration, struct accel_calibration_s); + +#include "topics/mag_calibration.h" +ORB_DEFINE(mag_calibration, struct mag_calibration_s); 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 50d67b60fe..443cb1125e 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 @@ -57,9 +57,8 @@ #include #include -#include -//#include -//#include +#include +#include #include #include @@ -94,16 +93,28 @@ public: private: int _publish(struct imu_sensor_data &data); + void _update_accel_calibration(); + void _update_gyro_calibration(); + //enum Rotation _rotation; - orb_advert_t _accel_topic; - orb_advert_t _gyro_topic; + orb_advert_t _accel_topic; + orb_advert_t _gyro_topic; - int _accel_orb_class_instance; - int _gyro_orb_class_instance; + int _accel_calibration_sub; + int _gyro_calibration_sub; - perf_counter_t _accel_sample_perf; - perf_counter_t _gyro_sample_perf; + struct accel_calibration_s _accel_calibration; + struct gyro_calibration_s _gyro_calibration; + + bool _accel_calibration_set; + bool _gyro_calibration_set; + + int _accel_orb_class_instance; + int _gyro_orb_class_instance; + + perf_counter_t _accel_sample_perf; + perf_counter_t _gyro_sample_perf; }; @@ -111,6 +122,12 @@ DfMpu9250Wrapper::DfMpu9250Wrapper(/*enum Rotation rotation*/) : MPU9250(IMU_DEVICE_PATH), _accel_topic(nullptr), _gyro_topic(nullptr), + _accel_calibration_sub(-1), + _gyro_calibration_sub(-1), + _accel_calibration{}, + _gyro_calibration{}, + _accel_calibration_set(false), + _gyro_calibration_set(false), _accel_orb_class_instance(-1), _gyro_orb_class_instance(-1), _accel_sample_perf(perf_alloc(PC_ELAPSED, "df_accel_read")), @@ -147,6 +164,14 @@ int DfMpu9250Wrapper::start() return -1; } + if (_accel_calibration_sub < 0) { + _accel_calibration_sub = orb_subscribe(ORB_ID(accel_calibration)); + } + + if (_gyro_calibration_sub < 0) { + _gyro_calibration_sub = orb_subscribe(ORB_ID(gyro_calibration)); + } + /* Init device and start sensor. */ int ret = init(); @@ -178,66 +203,114 @@ int DfMpu9250Wrapper::stop() return 0; } +void DfMpu9250Wrapper::_update_gyro_calibration() +{ + bool updated; + orb_check(_gyro_calibration_sub, &updated); + + if (updated) { + gyro_calibration_s new_calibration; + orb_copy(ORB_ID(gyro_calibration), _gyro_calibration_sub, &new_calibration); + + /* Only accept calibration for this device. */ + if (m_id.dev_id == new_calibration.device_id) { + _gyro_calibration = new_calibration; + _gyro_calibration_set = true; + } + } +} + +void DfMpu9250Wrapper::_update_accel_calibration() +{ + bool updated; + orb_check(_accel_calibration_sub, &updated); + + if (updated) { + accel_calibration_s new_calibration; + orb_copy(ORB_ID(accel_calibration), _accel_calibration_sub, &new_calibration); + + /* Only accept calibration for this device. */ + if (m_id.dev_id == new_calibration.device_id) { + _accel_calibration = new_calibration; + _accel_calibration_set = true; + } + } +} + int DfMpu9250Wrapper::_publish(struct imu_sensor_data &data) { - /* Publish accel first. */ - perf_begin(_accel_sample_perf); + /* Check if calibration values are still up-to-date */ + _update_accel_calibration(); + _update_gyro_calibration(); - accel_report accel_report = {}; - accel_report.timestamp = data.last_read_time_usec; + /* Don't publish if we have not received calibration data. */ + if (_accel_calibration_set) { - // TODO: remove these (or get the values) - accel_report.x_raw = NAN; - accel_report.y_raw = NAN; - accel_report.z_raw = NAN; - accel_report.x = data.accel_m_s2_x; - accel_report.y = data.accel_m_s2_y; - accel_report.z = data.accel_m_s2_z; + /* Publish accel first. */ + perf_begin(_accel_sample_perf); - // TODO: get these right - accel_report.scaling = -1.0f; - accel_report.range_m_s2 = -1.0f; + accel_report accel_report = {}; + accel_report.timestamp = data.last_read_time_usec; - // TODO: when is this ever blocked? - if (!(m_pub_blocked)) { + // TODO: remove these (or get the values) + accel_report.x_raw = NAN; + accel_report.y_raw = NAN; + accel_report.z_raw = NAN; + accel_report.x = (data.accel_m_s2_x - _gyro_calibration.x_offset) * _gyro_calibration.x_scale; + accel_report.y = (data.accel_m_s2_y - _gyro_calibration.y_offset) * _gyro_calibration.y_scale; + accel_report.z = (data.accel_m_s2_z - _gyro_calibration.z_offset) * _gyro_calibration.z_scale; - if (_accel_topic != nullptr) { - orb_publish(ORB_ID(sensor_accel), _accel_topic, &accel_report); + // TODO: get these right + accel_report.scaling = -1.0f; + accel_report.range_m_s2 = -1.0f; + + // TODO: when is this ever blocked? + if (!(m_pub_blocked)) { + + if (_accel_topic != nullptr) { + orb_publish(ORB_ID(sensor_accel), _accel_topic, &accel_report); + } } + + perf_end(_accel_sample_perf); } - perf_end(_accel_sample_perf); + /* Don't publish if we have not received calibration data. */ + if (_gyro_calibration_set) { - /* Then publish gyro. */ - perf_begin(_gyro_sample_perf); + /* Then publish gyro. */ + perf_begin(_gyro_sample_perf); - gyro_report gyro_report = {}; - gyro_report.timestamp = data.last_read_time_usec; + gyro_report gyro_report = {}; + gyro_report.timestamp = data.last_read_time_usec; - // TODO: remove these (or get the values) - gyro_report.x_raw = NAN; - gyro_report.y_raw = NAN; - gyro_report.z_raw = NAN; - gyro_report.x = data.gyro_rad_s_x; - gyro_report.y = data.gyro_rad_s_y; - gyro_report.z = data.gyro_rad_s_z; + // TODO: remove these (or get the values) + gyro_report.x_raw = NAN; + gyro_report.y_raw = NAN; + gyro_report.z_raw = NAN; + gyro_report.x = data.gyro_rad_s_x; + gyro_report.y = data.gyro_rad_s_y; + gyro_report.z = data.gyro_rad_s_z; - // TODO: get these right - gyro_report.scaling = -1.0f; - gyro_report.range_rad_s = -1.0f; + // TODO: get these right + gyro_report.scaling = -1.0f; + gyro_report.range_rad_s = -1.0f; - // TODO: when is this ever blocked? - if (!(m_pub_blocked)) { + // TODO: when is this ever blocked? + if (!(m_pub_blocked)) { - if (_gyro_topic != nullptr) { - orb_publish(ORB_ID(sensor_gyro), _gyro_topic, &gyro_report); + if (_gyro_topic != nullptr) { + orb_publish(ORB_ID(sensor_gyro), _gyro_topic, &gyro_report); + } } + + perf_end(_gyro_sample_perf); } - perf_end(_gyro_sample_perf); - - /* Notify anyone waiting for data. */ - DevMgr::updateNotify(*this); + if (_accel_calibration_set || _gyro_calibration_set) { + /* Notify anyone waiting for data. */ + DevMgr::updateNotify(*this); + } return 0; };