diff --git a/msg/accel_calibration.msg b/msg/accel_calibration.msg deleted file mode 100644 index 6e58d17c35..0000000000 --- a/msg/accel_calibration.msg +++ /dev/null @@ -1,10 +0,0 @@ -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 deleted file mode 100644 index 6e58d17c35..0000000000 --- a/msg/gyro_calibration.msg +++ /dev/null @@ -1,10 +0,0 @@ -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 deleted file mode 100644 index 6e58d17c35..0000000000 --- a/msg/mag_calibration.msg +++ /dev/null @@ -1,10 +0,0 @@ -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/sensor_accel.msg b/msg/sensor_accel.msg index 2eca557465..0ed71957ef 100644 --- a/msg/sensor_accel.msg +++ b/msg/sensor_accel.msg @@ -15,3 +15,5 @@ int16 x_raw int16 y_raw int16 z_raw int16 temperature_raw + +uint32 device_id diff --git a/msg/sensor_gyro.msg b/msg/sensor_gyro.msg index 65aa4a8706..badaec0eb5 100644 --- a/msg/sensor_gyro.msg +++ b/msg/sensor_gyro.msg @@ -15,3 +15,5 @@ int16 x_raw int16 y_raw int16 z_raw int16 temperature_raw + +uint32 device_id diff --git a/msg/sensor_mag.msg b/msg/sensor_mag.msg index 3ac97227ea..8ea03d1fc3 100644 --- a/msg/sensor_mag.msg +++ b/msg/sensor_mag.msg @@ -10,3 +10,5 @@ float32 temperature int16 x_raw int16 y_raw int16 z_raw + +uint32 device_id diff --git a/src/drivers/drv_accel.h b/src/drivers/drv_accel.h index 92b3a1b2ca..5d4ac5ea31 100644 --- a/src/drivers/drv_accel.h +++ b/src/drivers/drv_accel.h @@ -42,7 +42,6 @@ #include #include -#include #include "drv_sensor.h" #include "drv_orb_dev.h" @@ -55,6 +54,15 @@ #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 8c1f91f606..124c6d2b34 100644 --- a/src/drivers/drv_gyro.h +++ b/src/drivers/drv_gyro.h @@ -42,7 +42,6 @@ #include #include -#include #include "drv_sensor.h" #include "drv_orb_dev.h" @@ -55,6 +54,15 @@ #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/drv_mag.h b/src/drivers/drv_mag.h index 64a9c03680..f4766c1569 100644 --- a/src/drivers/drv_mag.h +++ b/src/drivers/drv_mag.h @@ -40,7 +40,6 @@ #include #include -#include #include "drv_sensor.h" #include "drv_orb_dev.h" @@ -53,6 +52,15 @@ #include #define mag_report sensor_mag_s +/** mag scaling factors; Vout = (Vin * Vscale) + Voffset */ +struct mag_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/modules/commander/accelerometer_calibration.cpp b/src/modules/commander/accelerometer_calibration.cpp index 5c97bdad92..d00f3a7604 100644 --- a/src/modules/commander/accelerometer_calibration.cpp +++ b/src/modules/commander/accelerometer_calibration.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2013-2015 PX4 Development Team. All rights reserved. + * Copyright (c) 2013-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 @@ -157,7 +157,6 @@ static const int ERROR = -1; static const char *sensor_name = "accel"; static int32_t device_id[max_accel_sens]; -static int device_prio_max = 0; static int32_t device_id_primary = 0; calibrate_return do_accel_calibration_measurements(int mavlink_fd, float (&accel_offs)[max_accel_sens][3], float (&accel_T)[max_accel_sens][3][3], unsigned *active_sensors); @@ -193,9 +192,9 @@ int do_accel_calibration(int mavlink_fd) char str[30]; -#ifndef __PX4_QURT /* reset all sensors */ for (unsigned s = 0; s < max_accel_sens; s++) { +#ifndef __PX4_QURT sprintf(str, "%s%u", ACCEL_BASE_DEVICE_PATH, s); /* reset all offsets to zero and all scales to one */ fd = px4_open(str, 0); @@ -212,8 +211,39 @@ int do_accel_calibration(int mavlink_fd) if (res != OK) { mavlink_and_console_log_critical(mavlink_fd, CAL_ERROR_RESET_CAL_MSG, s); } - } +#else + (void)sprintf(str, "CAL_ACC%u_XOFF", s); + res = param_set(param_find(str), &accel_scale.x_offset); + if (res != OK) { + PX4_ERR("unable to reset %s", str); + } + (void)sprintf(str, "CAL_ACC%u_YOFF", s); + res = param_set(param_find(str), &accel_scale.y_offset); + if (res != OK) { + PX4_ERR("unable to reset %s", str); + } + (void)sprintf(str, "CAL_ACC%u_ZOFF", s); + res = param_set(param_find(str), &accel_scale.z_offset); + if (res != OK) { + PX4_ERR("unable to reset %s", str); + } + (void)sprintf(str, "CAL_ACC%u_XSCALE", s); + res = param_set(param_find(str), &accel_scale.x_scale); + if (res != OK) { + PX4_ERR("unable to reset %s", str); + } + (void)sprintf(str, "CAL_ACC%u_YSCALE", s); + res = param_set(param_find(str), &accel_scale.y_scale); + if (res != OK) { + PX4_ERR("unable to reset %s", str); + } + (void)sprintf(str, "CAL_ACC%u_ZSCALE", s); + res = param_set(param_find(str), &accel_scale.z_scale); + if (res != OK) { + PX4_ERR("unable to reset %s", str); + } #endif + } float accel_offs[max_accel_sens][3]; float accel_T[max_accel_sens][3][3]; @@ -267,6 +297,16 @@ int do_accel_calibration(int mavlink_fd) failed = failed || (OK != param_set_no_notification(param_find("CAL_ACC_PRIME"), &(device_id_primary))); + + PX4_DEBUG("found offset %d: x: %.6f, y: %.6f, z: %.6f", i, + (double)accel_scale.x_offset, + (double)accel_scale.y_offset, + (double)accel_scale.z_offset); + PX4_DEBUG("found scale %d: x: %.6f, y: %.6f, z: %.6f", i, + (double)accel_scale.x_scale, + (double)accel_scale.y_scale, + (double)accel_scale.z_scale); + /* set parameters */ (void)sprintf(str, "CAL_ACC%u_XOFF", i); failed |= (OK != param_set_no_notification(param_find(str), &(accel_scale.x_offset))); @@ -280,10 +320,8 @@ int do_accel_calibration(int mavlink_fd) failed |= (OK != param_set_no_notification(param_find(str), &(accel_scale.y_scale))); (void)sprintf(str, "CAL_ACC%u_ZSCALE", i); failed |= (OK != param_set_no_notification(param_find(str), &(accel_scale.z_scale))); -#ifndef __PX4_QURT (void)sprintf(str, "CAL_ACC%u_ID", i); failed |= (OK != param_set_no_notification(param_find(str), &(device_id[i]))); -#endif if (failed) { mavlink_and_console_log_critical(mavlink_fd, CAL_ERROR_SET_PARAMS_MSG, i); @@ -387,10 +425,18 @@ calibrate_return do_accel_calibration_measurements(int mavlink_fd, float (&accel int32_t prio; orb_priority(worker_data.subs[i], &prio); +#ifndef __PX4_QURT + int device_prio_max = 0; if (prio > device_prio_max) { device_prio_max = prio; device_id_primary = device_id[i]; } +#else + PX4_INFO("found device id: %d", arp.device_id); + + // TODO FIXME: this is hacky but should get the device ID for now + device_id[i] = arp.device_id; +#endif } if (result == calibrate_return_ok) { diff --git a/src/modules/commander/gyro_calibration.cpp b/src/modules/commander/gyro_calibration.cpp index 2b09a5329d..775fc0ed8e 100644 --- a/src/modules/commander/gyro_calibration.cpp +++ b/src/modules/commander/gyro_calibration.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2013-2015 PX4 Development Team. All rights reserved. + * Copyright (c) 2013-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 @@ -167,12 +167,11 @@ int do_gyro_calibration(int mavlink_fd) gyro_scale_zero.z_offset = 0.0f; gyro_scale_zero.z_scale = 1.0f; - int device_prio_max = 0; int32_t device_id_primary = 0; for (unsigned s = 0; s < max_gyros; s++) { char str[30]; -#ifndef __PX4_QURT + // Reset gyro ids to unavailable worker_data.device_id[s] = 0; (void)sprintf(str, "CAL_GYRO%u_ID", s); @@ -181,14 +180,6 @@ int do_gyro_calibration(int mavlink_fd) mavlink_and_console_log_critical(mavlink_fd, "[cal] Unable to reset CAL_GYRO%u_ID", s); return ERROR; } -#else - (void)sprintf(str, "CAL_GYRO%u_ID", s); - res = param_get(param_find(str), &(worker_data.device_id[s])); - if (res != OK) { - mavlink_log_critical(mavlink_fd, "[cal] Unable to get CAL_GYRO%u_ID", s); - return ERROR; - } -#endif // Reset all offsets to 0 and scales to 1 (void)memcpy(&worker_data.gyro_scale[s], &gyro_scale_zero, sizeof(gyro_scale_zero)); @@ -205,6 +196,37 @@ int do_gyro_calibration(int mavlink_fd) return ERROR; } } +#else + (void)sprintf(str, "CAL_GYRO%u_XOFF", s); + res = param_set(param_find(str), &gyro_scale_zero.x_offset); + if (res != OK) { + PX4_ERR("unable to reset %s", str); + } + (void)sprintf(str, "CAL_GYRO%u_YOFF", s); + res = param_set(param_find(str), &gyro_scale_zero.y_offset); + if (res != OK) { + PX4_ERR("unable to reset %s", str); + } + (void)sprintf(str, "CAL_GYRO%u_ZOFF", s); + res = param_set(param_find(str), &gyro_scale_zero.z_offset); + if (res != OK) { + PX4_ERR("unable to reset %s", str); + } + (void)sprintf(str, "CAL_GYRO%u_XSCALE", s); + res = param_set(param_find(str), &gyro_scale_zero.x_scale); + if (res != OK) { + PX4_ERR("unable to reset %s", str); + } + (void)sprintf(str, "CAL_GYRO%u_YSCALE", s); + res = param_set(param_find(str), &gyro_scale_zero.y_scale); + if (res != OK) { + PX4_ERR("unable to reset %s", str); + } + (void)sprintf(str, "CAL_GYRO%u_ZSCALE", s); + res = param_set(param_find(str), &gyro_scale_zero.z_scale); + if (res != OK) { + PX4_ERR("unable to reset %s", str); + } #endif } @@ -216,10 +238,20 @@ int do_gyro_calibration(int mavlink_fd) int32_t prio; orb_priority(worker_data.gyro_sensor_sub[s], &prio); +#ifndef __PX4_QURT + int device_prio_max = 0; if (prio > device_prio_max) { device_prio_max = prio; device_id_primary = worker_data.device_id[s]; } +#else + gyro_report report = {}; + orb_copy(ORB_ID(sensor_gyro), worker_data.gyro_sensor_sub[s], &report); + //PX4_INFO("found device id: %d", report.device_id); + + // TODO FIXME: this is hacky but should get the device ID for now + worker_data.device_id[s] = report.device_id; +#endif } int cancel_sub = calibrate_cancel_subscribe(); @@ -296,10 +328,10 @@ int do_gyro_calibration(int mavlink_fd) (void)sprintf(str, "CAL_GYRO%u_ZOFF", s); failed |= (OK != param_set_no_notification(param_find(str), &(worker_data.gyro_scale[s].z_offset))); -#ifndef __PX4_QURT (void)sprintf(str, "CAL_GYRO%u_ID", s); failed |= (OK != param_set_no_notification(param_find(str), &(worker_data.device_id[s]))); +#ifndef __PX4_QURT /* apply new scaling and offsets */ (void)sprintf(str, "%s%u", GYRO_BASE_DEVICE_PATH, s); int fd = px4_open(str, 0); diff --git a/src/modules/commander/mag_calibration.cpp b/src/modules/commander/mag_calibration.cpp index b8a0e47a36..5846df029e 100644 --- a/src/modules/commander/mag_calibration.cpp +++ b/src/modules/commander/mag_calibration.cpp @@ -102,7 +102,6 @@ int do_mag_calibration(int mavlink_fd) { mavlink_and_console_log_info(mavlink_fd, CAL_QGC_STARTED_MSG, sensor_name); -#ifndef __PX4_QURT struct mag_calibration_s mscale_null; mscale_null.x_offset = 0.0f; mscale_null.x_scale = 1.0f; @@ -110,7 +109,6 @@ int do_mag_calibration(int mavlink_fd) mscale_null.y_scale = 1.0f; mscale_null.z_offset = 0.0f; mscale_null.z_scale = 1.0f; -#endif int result = OK; @@ -132,11 +130,35 @@ int do_mag_calibration(int mavlink_fd) break; } #else - (void)sprintf(str, "CAL_MAG%u_ID", cur_mag); - result = param_get(param_find(str), &(device_ids[cur_mag])); + (void)sprintf(str, "CAL_MAG%u_XOFF", cur_mag); + result = param_set(param_find(str), &mscale_null.x_offset); if (result != OK) { - mavlink_and_console_log_info(mavlink_fd, "[cal] Unable to reset CAL_MAG%u_ID", cur_mag); - break; + PX4_ERR("unable to reset %s", str); + } + (void)sprintf(str, "CAL_MAG%u_YOFF", cur_mag); + result = param_set(param_find(str), &mscale_null.y_offset); + if (result != OK) { + PX4_ERR("unable to reset %s", str); + } + (void)sprintf(str, "CAL_MAG%u_ZOFF", cur_mag); + result = param_set(param_find(str), &mscale_null.z_offset); + if (result != OK) { + PX4_ERR("unable to reset %s", str); + } + (void)sprintf(str, "CAL_MAG%u_XSCALE", cur_mag); + result = param_set(param_find(str), &mscale_null.x_scale); + if (result != OK) { + PX4_ERR("unable to reset %s", str); + } + (void)sprintf(str, "CAL_MAG%u_YSCALE", cur_mag); + result = param_set(param_find(str), &mscale_null.y_scale); + if (result != OK) { + PX4_ERR("unable to reset %s", str); + } + (void)sprintf(str, "CAL_MAG%u_ZSCALE", cur_mag); + result = param_set(param_find(str), &mscale_null.z_scale); + if (result != OK) { + PX4_ERR("unable to reset %s", str); } #endif @@ -326,12 +348,13 @@ static calibrate_return mag_calibration_worker(detect_orientation_return orienta px4_pollfd_struct_t fds[max_mags]; size_t fd_count = 0; for (size_t cur_mag=0; cur_magsub_mag[cur_mag] >= 0) { + if (worker_data->sub_mag[cur_mag] >= 0 && device_ids[cur_mag] > 0) { fds[fd_count].fd = worker_data->sub_mag[cur_mag]; fds[fd_count].events = POLLIN; fd_count++; } } + int poll_ret = px4_poll(fds, fd_count, 1000); if (poll_ret > 0) { @@ -446,24 +469,31 @@ calibrate_return mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mag // Setup subscriptions to mag sensors if (result == calibrate_return_ok) { for (unsigned cur_mag=0; cur_mag device_prio_max) { - device_prio_max = prio; - device_id_primary = device_ids[cur_mag]; - } + // Mag in this slot is available + worker_data.sub_mag[cur_mag] = orb_subscribe_multi(ORB_ID(sensor_mag), cur_mag); + if (worker_data.sub_mag[cur_mag] < 0) { + mavlink_and_console_log_critical(mavlink_fd, "[cal] Mag #%u not found, abort", cur_mag); + result = calibrate_return_error; + break; } + + // Get priority + int32_t prio; + orb_priority(worker_data.sub_mag[cur_mag], &prio); + +#ifndef __PX4_QURT + if (prio > device_prio_max) { + device_prio_max = prio; + device_id_primary = device_ids[cur_mag]; + } +#else + mag_report report = {}; + orb_copy(ORB_ID(sensor_mag), worker_data.sub_mag[cur_mag], &report); + + // TODO FIXME: this is hacky but should get the device ID for now + device_ids[cur_mag] = report.device_id; + PX4_INFO("found device id: %d", report.device_id); +#endif } } @@ -593,11 +623,11 @@ calibrate_return mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mag for (unsigned cur_mag=0; cur_mag= 0) { px4_close(fd_mag); } +#endif if (result == calibrate_return_ok) { bool failed = false; + PX4_DEBUG("found offset %d: x: %.6f, y: %.6f, z: %.6f", cur_mag, + (double)mscale.x_offset, + (double)mscale.y_offset, + (double)mscale.z_offset); + PX4_DEBUG("found scale %d: x: %.6f, y: %.6f, z: %.6f", cur_mag, + (double)mscale.x_scale, + (double)mscale.y_scale, + (double)mscale.z_scale); + /* set parameters */ -#ifndef __PX4_QURT (void)sprintf(str, "CAL_MAG%u_ID", cur_mag); failed |= (OK != param_set_no_notification(param_find(str), &(device_ids[cur_mag]))); -#endif (void)sprintf(str, "CAL_MAG%u_XOFF", cur_mag); failed |= (OK != param_set_no_notification(param_find(str), &(mscale.x_offset))); (void)sprintf(str, "CAL_MAG%u_YOFF", cur_mag); @@ -644,11 +686,12 @@ calibrate_return mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mag (void)sprintf(str, "CAL_MAG%u_ZOFF", cur_mag); failed |= (OK != param_set_no_notification(param_find(str), &(mscale.z_offset))); (void)sprintf(str, "CAL_MAG%u_XSCALE", cur_mag); - failed |= (OK != param_set_no_notification(param_find(str), &(mscale.x_scale))); - (void)sprintf(str, "CAL_MAG%u_YSCALE", cur_mag); - failed |= (OK != param_set_no_notification(param_find(str), &(mscale.y_scale))); - (void)sprintf(str, "CAL_MAG%u_ZSCALE", cur_mag); - failed |= (OK != param_set_no_notification(param_find(str), &(mscale.z_scale))); + // FIXME: scaling is not used right now + //failed |= (OK != param_set_no_notification(param_find(str), &(mscale.x_scale))); + //(void)sprintf(str, "CAL_MAG%u_YSCALE", cur_mag); + //failed |= (OK != param_set_no_notification(param_find(str), &(mscale.y_scale))); + //(void)sprintf(str, "CAL_MAG%u_ZSCALE", cur_mag); + //failed |= (OK != param_set_no_notification(param_find(str), &(mscale.z_scale))); if (failed) { mavlink_and_console_log_critical(mavlink_fd, CAL_ERROR_SET_PARAMS_MSG, cur_mag); diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 2ea05bdba2..ba29c4211d 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -100,9 +100,6 @@ #include #include #include -#include -#include -#include #include @@ -1530,17 +1527,7 @@ Sensors::apply_gyro_calibration(DevHandle &h, const struct gyro_calibration_s *g } #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, gcal); - - } else { - gyro_calibration_pub = orb_advertise(ORB_ID(gyro_calibration), gcal); - } - + /* On QURT, the params are read directly in the respective wrappers. */ return true; #endif } @@ -1561,17 +1548,7 @@ Sensors::apply_accel_calibration(DevHandle &h, const struct accel_calibration_s } #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, acal); - - } else { - accel_calibration_pub = orb_advertise(ORB_ID(accel_calibration), acal); - } - + /* On QURT, the params are read directly in the respective wrappers. */ return true; #endif } @@ -1592,17 +1569,7 @@ Sensors::apply_mag_calibration(DevHandle &h, const struct mag_calibration_s *mca } #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, mcal); - - } else { - mag_calibration_pub = orb_advertise(ORB_ID(mag_calibration), mcal); - } - + /* On QURT, the params are read directly in the respective wrappers. */ return true; #endif } diff --git a/src/modules/uORB/objects_common.cpp b/src/modules/uORB/objects_common.cpp index 7bdaedfa6b..5f05123f7e 100644 --- a/src/modules/uORB/objects_common.cpp +++ b/src/modules/uORB/objects_common.cpp @@ -282,12 +282,3 @@ 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_hmc5883_wrapper/df_hmc5883_wrapper.cpp b/src/platforms/posix/drivers/df_hmc5883_wrapper/df_hmc5883_wrapper.cpp index 45415146e1..740f6805fa 100644 --- a/src/platforms/posix/drivers/df_hmc5883_wrapper/df_hmc5883_wrapper.cpp +++ b/src/platforms/posix/drivers/df_hmc5883_wrapper/df_hmc5883_wrapper.cpp @@ -56,7 +56,7 @@ #include -#include +#include #include //#include @@ -101,11 +101,16 @@ private: orb_advert_t _mag_topic; - int _mag_calibration_sub; + int _param_update_sub; - struct mag_calibration_s _mag_calibration; - - bool _mag_calibration_set; + struct mag_calibration_s { + float x_offset; + float x_scale; + float y_offset; + float y_scale; + float z_offset; + float z_scale; + } _mag_calibration; int _mag_orb_class_instance; @@ -116,10 +121,19 @@ private: DfHmc9250Wrapper::DfHmc9250Wrapper(/*enum Rotation rotation*/) : HMC5883(MAG_DEVICE_PATH), _mag_topic(nullptr), + _param_update_sub(-1), + _mag_calibration{}, _mag_orb_class_instance(-1), _mag_sample_perf(perf_alloc(PC_ELAPSED, "df_mag_read")) /*_rotation(rotation)*/ { + // Set sane default calibration values + _mag_calibration.x_scale = 1.0f; + _mag_calibration.y_scale = 1.0f; + _mag_calibration.z_scale = 1.0f; + _mag_calibration.x_offset = 0.0f; + _mag_calibration.y_offset = 0.0f; + _mag_calibration.z_offset = 0.0f; } DfHmc9250Wrapper::~DfHmc9250Wrapper() @@ -139,9 +153,9 @@ int DfHmc9250Wrapper::start() return -1; } - /* Subscribe to calibration topic. */ - if (_mag_calibration_sub < 0) { - _mag_calibration_sub = orb_subscribe(ORB_ID(mag_calibration)); + /* Subscribe to param update topic. */ + if (_param_update_sub < 0) { + _param_update_sub = orb_subscribe(ORB_ID(parameter_update)); } /* Init device and start sensor. */ @@ -159,6 +173,9 @@ int DfHmc9250Wrapper::start() return ret; } + /* Force getting the calibration values. */ + _update_mag_calibration(); + return 0; } @@ -177,17 +194,65 @@ int DfHmc9250Wrapper::stop() void DfHmc9250Wrapper::_update_mag_calibration() { - bool updated; - orb_check(_mag_calibration_sub, &updated); + // TODO: replace magic number + for (unsigned i = 0; i < 3; ++i) { - if (updated) { - mag_calibration_s new_calibration; - orb_copy(ORB_ID(mag_calibration), _mag_calibration_sub, &new_calibration); + // TODO: remove printfs and add error counter - /* Only accept calibration for this device. */ - if (m_id.dev_id == new_calibration.device_id) { - _mag_calibration = new_calibration; - _mag_calibration_set = true; + char str[30]; + (void)sprintf(str, "CAL_MAG%u_ID", i); + int32_t device_id; + int res = param_get(param_find(str), &device_id); + + if (res != OK) { + PX4_ERR("Could not access param %s", str); + continue; + } + + if ((uint32_t)device_id != m_id.dev_id) { + continue; + } + + (void)sprintf(str, "CAL_MAG%u_XSCALE", i); + res = param_get(param_find(str), &_mag_calibration.x_scale); + + if (res != OK) { + PX4_ERR("Could not access param %s", str); + } + + (void)sprintf(str, "CAL_MAG%u_YSCALE", i); + res = param_get(param_find(str), &_mag_calibration.y_scale); + + if (res != OK) { + PX4_ERR("Could not access param %s", str); + } + + (void)sprintf(str, "CAL_MAG%u_ZSCALE", i); + res = param_get(param_find(str), &_mag_calibration.z_scale); + + if (res != OK) { + PX4_ERR("Could not access param %s", str); + } + + (void)sprintf(str, "CAL_MAG%u_XOFF", i); + res = param_get(param_find(str), &_mag_calibration.x_offset); + + if (res != OK) { + PX4_ERR("Could not access param %s", str); + } + + (void)sprintf(str, "CAL_MAG%u_YOFF", i); + res = param_get(param_find(str), &_mag_calibration.y_offset); + + if (res != OK) { + PX4_ERR("Could not access param %s", str); + } + + (void)sprintf(str, "CAL_MAG%u_ZOFF", i); + res = param_get(param_find(str), &_mag_calibration.z_offset); + + if (res != OK) { + PX4_ERR("Could not access param %s", str); } } } @@ -196,11 +261,14 @@ void DfHmc9250Wrapper::_update_mag_calibration() int DfHmc9250Wrapper::_publish(struct mag_sensor_data &data) { /* Check if calibration values are still up-to-date. */ - _update_mag_calibration(); + bool updated; + orb_check(_param_update_sub, &updated); - if (!_mag_calibration_set) { - // TODO: check the return codes of this function - return 0; + if (updated) { + parameter_update_s parameter_update; + orb_copy(ORB_ID(parameter_update), _param_update_sub, ¶meter_update); + + _update_mag_calibration(); } /* Publish mag first. */ @@ -220,14 +288,16 @@ int DfHmc9250Wrapper::_publish(struct mag_sensor_data &data) mag_report.x_raw = NAN; mag_report.y_raw = NAN; mag_report.z_raw = NAN; - mag_report.x = data.field_x_ga; - mag_report.y = data.field_y_ga; - mag_report.z = data.field_z_ga; + mag_report.x = (data.field_x_ga - _mag_calibration.x_offset) * _mag_calibration.x_scale; + mag_report.y = (data.field_y_ga - _mag_calibration.y_offset) * _mag_calibration.y_scale; + mag_report.z = (data.field_z_ga - _mag_calibration.z_offset) * _mag_calibration.z_scale; // TODO: get these right //mag_report.scaling = -1.0f; //mag_report.range_m_s2 = -1.0f; + mag_report.device_id = m_id.dev_id; + // TODO: when is this ever blocked? if (!(m_pub_blocked)) { 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 f0115152ac..384e1b48f8 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,8 +57,7 @@ #include #include -#include -#include +#include #include #include @@ -101,14 +100,25 @@ private: orb_advert_t _accel_topic; orb_advert_t _gyro_topic; - int _accel_calibration_sub; - int _gyro_calibration_sub; + int _param_update_sub; - struct accel_calibration_s _accel_calibration; - struct gyro_calibration_s _gyro_calibration; + struct accel_calibration_s { + float x_offset; + float x_scale; + float y_offset; + float y_scale; + float z_offset; + float z_scale; + } _accel_calibration; - bool _accel_calibration_set; - bool _gyro_calibration_set; + struct gyro_calibration_s { + float x_offset; + float x_scale; + float y_offset; + float y_scale; + float z_offset; + float z_scale; + } _gyro_calibration; int _accel_orb_class_instance; int _gyro_orb_class_instance; @@ -122,18 +132,29 @@ DfMpu9250Wrapper::DfMpu9250Wrapper(/*enum Rotation rotation*/) : MPU9250(IMU_DEVICE_PATH), _accel_topic(nullptr), _gyro_topic(nullptr), - _accel_calibration_sub(-1), - _gyro_calibration_sub(-1), + _param_update_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")), _gyro_sample_perf(perf_alloc(PC_ELAPSED, "df_gyro_read")) /*_rotation(rotation)*/ { + // Set sane default calibration values + _accel_calibration.x_scale = 1.0f; + _accel_calibration.y_scale = 1.0f; + _accel_calibration.z_scale = 1.0f; + _accel_calibration.x_offset = 0.0f; + _accel_calibration.y_offset = 0.0f; + _accel_calibration.z_offset = 0.0f; + + _gyro_calibration.x_scale = 1.0f; + _gyro_calibration.y_scale = 1.0f; + _gyro_calibration.z_scale = 1.0f; + _gyro_calibration.x_offset = 0.0f; + _gyro_calibration.y_offset = 0.0f; + _gyro_calibration.z_offset = 0.0f; } DfMpu9250Wrapper::~DfMpu9250Wrapper() @@ -164,13 +185,9 @@ int DfMpu9250Wrapper::start() return -1; } - /* Subscribe to calibration topics. */ - 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)); + /* Subscribe to param update topic. */ + if (_param_update_sub < 0) { + _param_update_sub = orb_subscribe(ORB_ID(parameter_update)); } /* Init device and start sensor. */ @@ -188,6 +205,12 @@ int DfMpu9250Wrapper::start() return ret; } + PX4_INFO("MPU9250 device id is: %d", m_id.dev_id); + + /* Force getting the calibration values. */ + _update_accel_calibration(); + _update_gyro_calibration(); + return 0; } @@ -206,34 +229,130 @@ int DfMpu9250Wrapper::stop() void DfMpu9250Wrapper::_update_gyro_calibration() { - bool updated; - orb_check(_gyro_calibration_sub, &updated); + // TODO: replace magic number + for (unsigned i = 0; i < 3; ++i) { - if (updated) { - gyro_calibration_s new_calibration; - orb_copy(ORB_ID(gyro_calibration), _gyro_calibration_sub, &new_calibration); + // TODO: remove printfs and add error counter - /* Only accept calibration for this device. */ - if (m_id.dev_id == new_calibration.device_id) { - _gyro_calibration = new_calibration; - _gyro_calibration_set = true; + char str[30]; + (void)sprintf(str, "CAL_GYRO%u_ID", i); + int32_t device_id; + int res = param_get(param_find(str), &device_id); + + if (res != OK) { + PX4_ERR("Could not access param %s", str); + continue; + } + + if ((uint32_t)device_id != m_id.dev_id) { + continue; + } + + (void)sprintf(str, "CAL_GYRO%u_XSCALE", i); + res = param_get(param_find(str), &_gyro_calibration.x_scale); + + if (res != OK) { + PX4_ERR("Could not access param %s", str); + } + + (void)sprintf(str, "CAL_GYRO%u_YSCALE", i); + res = param_get(param_find(str), &_gyro_calibration.y_scale); + + if (res != OK) { + PX4_ERR("Could not access param %s", str); + } + + (void)sprintf(str, "CAL_GYRO%u_ZSCALE", i); + res = param_get(param_find(str), &_gyro_calibration.z_scale); + + if (res != OK) { + PX4_ERR("Could not access param %s", str); + } + + (void)sprintf(str, "CAL_GYRO%u_XOFF", i); + res = param_get(param_find(str), &_gyro_calibration.x_offset); + + if (res != OK) { + PX4_ERR("Could not access param %s", str); + } + + (void)sprintf(str, "CAL_GYRO%u_YOFF", i); + res = param_get(param_find(str), &_gyro_calibration.y_offset); + + if (res != OK) { + PX4_ERR("Could not access param %s", str); + } + + (void)sprintf(str, "CAL_GYRO%u_ZOFF", i); + res = param_get(param_find(str), &_gyro_calibration.z_offset); + + if (res != OK) { + PX4_ERR("Could not access param %s", str); } } } void DfMpu9250Wrapper::_update_accel_calibration() { - bool updated; - orb_check(_accel_calibration_sub, &updated); + // TODO: replace magic number + for (unsigned i = 0; i < 3; ++i) { - if (updated) { - accel_calibration_s new_calibration; - orb_copy(ORB_ID(accel_calibration), _accel_calibration_sub, &new_calibration); + // TODO: remove printfs and add error counter - /* Only accept calibration for this device. */ - if (m_id.dev_id == new_calibration.device_id) { - _accel_calibration = new_calibration; - _accel_calibration_set = true; + char str[30]; + (void)sprintf(str, "CAL_ACC%u_ID", i); + int32_t device_id; + int res = param_get(param_find(str), &device_id); + + if (res != OK) { + PX4_ERR("Could not access param %s", str); + continue; + } + + if ((uint32_t)device_id != m_id.dev_id) { + continue; + } + + (void)sprintf(str, "CAL_ACC%u_XSCALE", i); + res = param_get(param_find(str), &_accel_calibration.x_scale); + + if (res != OK) { + PX4_ERR("Could not access param %s", str); + } + + (void)sprintf(str, "CAL_ACC%u_YSCALE", i); + res = param_get(param_find(str), &_accel_calibration.y_scale); + + if (res != OK) { + PX4_ERR("Could not access param %s", str); + } + + (void)sprintf(str, "CAL_ACC%u_ZSCALE", i); + res = param_get(param_find(str), &_accel_calibration.z_scale); + + if (res != OK) { + PX4_ERR("Could not access param %s", str); + } + + (void)sprintf(str, "CAL_ACC%u_XOFF", i); + res = param_get(param_find(str), &_accel_calibration.x_offset); + + if (res != OK) { + PX4_ERR("Could not access param %s", str); + } + + (void)sprintf(str, "CAL_ACC%u_YOFF", i); + res = param_get(param_find(str), &_accel_calibration.y_offset); + + if (res != OK) { + PX4_ERR("Could not access param %s", str); + } + + (void)sprintf(str, "CAL_ACC%u_ZOFF", i); + res = param_get(param_find(str), &_accel_calibration.z_offset); + + if (res != OK) { + PX4_ERR("Could not access param %s", str); } } } @@ -241,77 +360,80 @@ void DfMpu9250Wrapper::_update_accel_calibration() int DfMpu9250Wrapper::_publish(struct imu_sensor_data &data) { /* Check if calibration values are still up-to-date. */ - _update_accel_calibration(); - _update_gyro_calibration(); + bool updated; + orb_check(_param_update_sub, &updated); - /* Don't publish if we have not received calibration data. */ - if (_accel_calibration_set) { + if (updated) { + parameter_update_s parameter_update; + orb_copy(ORB_ID(parameter_update), _param_update_sub, ¶meter_update); - /* Publish accel first. */ - perf_begin(_accel_sample_perf); + _update_accel_calibration(); + _update_gyro_calibration(); + } - accel_report accel_report = {}; - accel_report.timestamp = data.last_read_time_usec; + /* Publish accel first. */ + perf_begin(_accel_sample_perf); - // 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; + accel_report accel_report = {}; + accel_report.timestamp = data.last_read_time_usec; - // TODO: get these right - accel_report.scaling = -1.0f; - accel_report.range_m_s2 = -1.0f; + // 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_calibration.x_offset) * _accel_calibration.x_scale; + accel_report.y = (data.accel_m_s2_y - _accel_calibration.y_offset) * _accel_calibration.y_scale; + accel_report.z = (data.accel_m_s2_z - _accel_calibration.z_offset) * _accel_calibration.z_scale; - // TODO: when is this ever blocked? - if (!(m_pub_blocked)) { + // TODO: get these right + accel_report.scaling = -1.0f; + accel_report.range_m_s2 = -1.0f; - if (_accel_topic != nullptr) { - orb_publish(ORB_ID(sensor_accel), _accel_topic, &accel_report); - } + accel_report.device_id = m_id.dev_id; + + // 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); } - /* Don't publish if we have not received calibration data. */ - if (_gyro_calibration_set) { + perf_end(_accel_sample_perf); - /* Then publish gyro. */ - perf_begin(_gyro_sample_perf); - gyro_report gyro_report = {}; - gyro_report.timestamp = data.last_read_time_usec; + /* Then publish gyro. */ + perf_begin(_gyro_sample_perf); - // 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; + gyro_report gyro_report = {}; + gyro_report.timestamp = data.last_read_time_usec; - // TODO: get these right - gyro_report.scaling = -1.0f; - gyro_report.range_rad_s = -1.0f; + // 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_calibration.x_offset) * _gyro_calibration.x_scale; + gyro_report.y = (data.gyro_rad_s_y - _gyro_calibration.y_offset) * _gyro_calibration.y_scale; + gyro_report.z = (data.gyro_rad_s_z - _gyro_calibration.z_offset) * _gyro_calibration.z_scale; - // TODO: when is this ever blocked? - if (!(m_pub_blocked)) { + // TODO: get these right + gyro_report.scaling = -1.0f; + gyro_report.range_rad_s = -1.0f; - if (_gyro_topic != nullptr) { - orb_publish(ORB_ID(sensor_gyro), _gyro_topic, &gyro_report); - } + gyro_report.device_id = m_id.dev_id; + + // TODO: when is this ever blocked? + if (!(m_pub_blocked)) { + + if (_gyro_topic != nullptr) { + orb_publish(ORB_ID(sensor_gyro), _gyro_topic, &gyro_report); } - - perf_end(_gyro_sample_perf); } - if (_accel_calibration_set || _gyro_calibration_set) { - /* Notify anyone waiting for data. */ - DevMgr::updateNotify(*this); - } + perf_end(_gyro_sample_perf); + + /* Notify anyone waiting for data. */ + DevMgr::updateNotify(*this); // TODO: check the return codes of this function return 0;