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; };