From ce7d8d2270ecd6f50a5bb468fd12e76a7544f6b2 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Beat=20K=C3=BCng?= <beat-kueng@gmx.net>
Date: Wed, 14 Jun 2017 11:19:29 +0200
Subject: [PATCH] sensor_mag.msg: add is_external flag & set it in the mag
drivers
With this we don't have to use the ioctl MAGIOCGEXTERNAL, which does not
work on POSIX (eg. RPi).
---
msg/sensor_mag.msg | 2 ++
src/drivers/bmm150/bmm150.cpp | 6 ++--
src/drivers/bmm150/bmm150.hpp | 1 -
src/drivers/hmc5883/hmc5883.cpp | 1 +
src/drivers/ist8310/ist8310.cpp | 1 +
src/drivers/lis3mdl/lis3mdl.cpp | 1 +
src/drivers/lsm303d/lsm303d.cpp | 4 +--
src/drivers/mpu9250/mag.cpp | 1 +
src/modules/sensors/voted_sensors_update.cpp | 36 +++++++++----------
src/modules/uavcan/sensors/mag.cpp | 1 +
.../posix/drivers/accelsim/accelsim.cpp | 4 +--
.../df_ak8963_wrapper/df_ak8963_wrapper.cpp | 1 +
.../df_hmc5883_wrapper/df_hmc5883_wrapper.cpp | 1 +
.../df_lsm9ds1_wrapper/df_lsm9ds1_wrapper.cpp | 1 +
.../df_mpu9250_wrapper/df_mpu9250_wrapper.cpp | 1 +
15 files changed, 35 insertions(+), 27 deletions(-)
diff --git a/msg/sensor_mag.msg b/msg/sensor_mag.msg
index c41cd47d42..bb8fb60290 100644
--- a/msg/sensor_mag.msg
+++ b/msg/sensor_mag.msg
@@ -11,3 +11,5 @@ int16 y_raw
int16 z_raw
uint32 device_id # unique device ID for the sensor that does not change between power cycles
+uint8 is_external # if 1, the mag is external (i.e. not built into the board)
+
diff --git a/src/drivers/bmm150/bmm150.cpp b/src/drivers/bmm150/bmm150.cpp
index 50f55c61e0..12cedc9718 100644
--- a/src/drivers/bmm150/bmm150.cpp
+++ b/src/drivers/bmm150/bmm150.cpp
@@ -251,7 +251,6 @@ BMM150 :: BMM150(int bus, const char *path, bool external, enum Rotation rotatio
_external(false),
_running(false),
_call_interval(0),
- _report{0},
_reports(nullptr),
_collect_phase(false),
_scale{},
@@ -344,7 +343,7 @@ int BMM150::init()
}
/* allocate basic report buffers */
- _reports = new ringbuffer::RingBuffer(2, sizeof(_report));
+ _reports = new ringbuffer::RingBuffer(2, sizeof(mag_report));
if (_reports == nullptr) {
goto out;
@@ -433,7 +432,7 @@ BMM150::stop()
ssize_t
BMM150::read(struct file *filp, char *buffer, size_t buflen)
{
- unsigned count = buflen / sizeof(_report);
+ unsigned count = buflen / sizeof(mag_report);
struct mag_report *mag_buf = reinterpret_cast<struct mag_report *>(buffer);
int ret = 0;
@@ -684,6 +683,7 @@ BMM150::collect()
mrb.timestamp = hrt_absolute_time();
+ mrb.is_external = is_external();
// report the error count as the number of bad transfers.
// This allows the higher level code to decide if it
diff --git a/src/drivers/bmm150/bmm150.hpp b/src/drivers/bmm150/bmm150.hpp
index c41be6299f..8598b9ad4d 100644
--- a/src/drivers/bmm150/bmm150.hpp
+++ b/src/drivers/bmm150/bmm150.hpp
@@ -228,7 +228,6 @@ private:
unsigned _call_interval;
- struct mag_report _report;
ringbuffer::RingBuffer *_reports;
bool _collect_phase;
diff --git a/src/drivers/hmc5883/hmc5883.cpp b/src/drivers/hmc5883/hmc5883.cpp
index c6d8dd6e91..52f69689b0 100644
--- a/src/drivers/hmc5883/hmc5883.cpp
+++ b/src/drivers/hmc5883/hmc5883.cpp
@@ -997,6 +997,7 @@ HMC5883::collect()
// XXX revisit for SPI part, might require a bus type IOCTL
unsigned dummy;
sensor_is_onboard = !_interface->ioctl(MAGIOCGEXTERNAL, dummy);
+ new_report.is_external = !sensor_is_onboard;
if (sensor_is_onboard) {
// convert onboard so it matches offboard for the
diff --git a/src/drivers/ist8310/ist8310.cpp b/src/drivers/ist8310/ist8310.cpp
index 26822b2437..c2693d1b16 100644
--- a/src/drivers/ist8310/ist8310.cpp
+++ b/src/drivers/ist8310/ist8310.cpp
@@ -913,6 +913,7 @@ IST8310::collect()
/* this should be fairly close to the end of the measurement, so the best approximation of the time */
new_report.timestamp = hrt_absolute_time();
+ new_report.is_external = !sensor_is_onboard;
new_report.error_count = perf_event_count(_comms_errors);
new_report.scaling = _range_scale;
new_report.device_id = _device_id.devid;
diff --git a/src/drivers/lis3mdl/lis3mdl.cpp b/src/drivers/lis3mdl/lis3mdl.cpp
index 32a3989282..b5735da35e 100644
--- a/src/drivers/lis3mdl/lis3mdl.cpp
+++ b/src/drivers/lis3mdl/lis3mdl.cpp
@@ -945,6 +945,7 @@ LIS3MDL::collect()
unsigned dummy;
sensor_is_onboard = !_interface->ioctl(MAGIOCGEXTERNAL, dummy);
+ new_report.is_external = !sensor_is_onboard;
/*
* RAW outputs
diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp
index 53fc9d20df..90bbdce997 100644
--- a/src/drivers/lsm303d/lsm303d.cpp
+++ b/src/drivers/lsm303d/lsm303d.cpp
@@ -1653,8 +1653,7 @@ LSM303D::mag_measure()
} raw_mag_report;
#pragma pack(pop)
- mag_report mag_report;
- memset(&mag_report, 0, sizeof(mag_report));
+ mag_report mag_report {};
/* start the performance counter */
perf_begin(_mag_sample_perf);
@@ -1680,6 +1679,7 @@ LSM303D::mag_measure()
*/
mag_report.timestamp = hrt_absolute_time();
+ mag_report.is_external = is_external();
mag_report.x_raw = raw_mag_report.x;
mag_report.y_raw = raw_mag_report.y;
diff --git a/src/drivers/mpu9250/mag.cpp b/src/drivers/mpu9250/mag.cpp
index a27c23aa66..ec35f5bde5 100644
--- a/src/drivers/mpu9250/mag.cpp
+++ b/src/drivers/mpu9250/mag.cpp
@@ -205,6 +205,7 @@ MPU9250_mag::_measure(struct ak8963_regs data)
mag_report mrb;
mrb.timestamp = hrt_absolute_time();
+ mrb.is_external = false;
/*
* Align axes - note the accel & gryo are also re-aligned so this
diff --git a/src/modules/sensors/voted_sensors_update.cpp b/src/modules/sensors/voted_sensors_update.cpp
index 1e372d7a2e..742a33e0e2 100644
--- a/src/modules/sensors/voted_sensors_update.cpp
+++ b/src/modules/sensors/voted_sensors_update.cpp
@@ -406,6 +406,7 @@ void VotedSensorsUpdate::parameters_update()
}
int topic_device_id = report.device_id;
+ bool is_external = (bool)report.is_external;
_mag_device_id[topic_instance] = topic_device_id;
// find the driver handle that matches the topic_device_id
@@ -465,28 +466,25 @@ void VotedSensorsUpdate::parameters_update()
(void)sprintf(str, "CAL_MAG%u_ROT", i);
- if (h.isValid()) { //FIXME: get the 'is external' information via uorb topic
- if (h.ioctl(MAGIOCGEXTERNAL, 0) <= 0) {
- /* mag is internal - reset param to -1 to indicate internal mag */
- int32_t minus_one;
- param_get(param_find(str), &minus_one);
+ if (is_external) {
+ int32_t mag_rot;
+ param_get(param_find(str), &mag_rot);
- if (minus_one != MAG_ROT_VAL_INTERNAL) {
- minus_one = MAG_ROT_VAL_INTERNAL;
- param_set_no_notification(param_find(str), &minus_one);
- }
+ /* 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 */
+ mag_rot = 0;
+ param_set_no_notification(param_find(str), &mag_rot);
+ }
- } else {
- /* mag is external */
- int32_t mag_rot;
- param_get(param_find(str), &mag_rot);
+ } else {
+ /* mag is internal - reset param to -1 to indicate internal mag */
+ int32_t minus_one;
+ param_get(param_find(str), &minus_one);
- /* 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 */
- mag_rot = 0;
- param_set_no_notification(param_find(str), &mag_rot);
- }
+ if (minus_one != MAG_ROT_VAL_INTERNAL) {
+ minus_one = MAG_ROT_VAL_INTERNAL;
+ param_set_no_notification(param_find(str), &minus_one);
}
}
diff --git a/src/modules/uavcan/sensors/mag.cpp b/src/modules/uavcan/sensors/mag.cpp
index 22e8de1625..9e7afb95da 100644
--- a/src/modules/uavcan/sensors/mag.cpp
+++ b/src/modules/uavcan/sensors/mag.cpp
@@ -156,6 +156,7 @@ void UavcanMagnetometerBridge::mag_sub_cb(const uavcan::ReceivedDataStructure<ua
*/
_report.timestamp = hrt_absolute_time();
_report.device_id = _device_id.devid;
+ _report.is_external = true;
_report.x = (msg.magnetic_field_ga[0] - _scale.x_offset) * _scale.x_scale;
_report.y = (msg.magnetic_field_ga[1] - _scale.y_offset) * _scale.y_scale;
diff --git a/src/platforms/posix/drivers/accelsim/accelsim.cpp b/src/platforms/posix/drivers/accelsim/accelsim.cpp
index dbb2bdab9c..a16531efe7 100644
--- a/src/platforms/posix/drivers/accelsim/accelsim.cpp
+++ b/src/platforms/posix/drivers/accelsim/accelsim.cpp
@@ -915,8 +915,7 @@ ACCELSIM::mag_measure()
} raw_mag_report;
#pragma pack(pop)
- mag_report mag_report;
- memset(&mag_report, 0, sizeof(mag_report));
+ mag_report mag_report = {};
/* start the performance counter */
perf_begin(_mag_sample_perf);
@@ -946,6 +945,7 @@ ACCELSIM::mag_measure()
mag_report.timestamp = hrt_absolute_time();
+ mag_report.is_external = false;
mag_report.x_raw = (int16_t)(raw_mag_report.x / _mag_range_scale);
mag_report.y_raw = (int16_t)(raw_mag_report.y / _mag_range_scale);
diff --git a/src/platforms/posix/drivers/df_ak8963_wrapper/df_ak8963_wrapper.cpp b/src/platforms/posix/drivers/df_ak8963_wrapper/df_ak8963_wrapper.cpp
index 34ae326b68..475c502f60 100644
--- a/src/platforms/posix/drivers/df_ak8963_wrapper/df_ak8963_wrapper.cpp
+++ b/src/platforms/posix/drivers/df_ak8963_wrapper/df_ak8963_wrapper.cpp
@@ -278,6 +278,7 @@ int DfAK8963Wrapper::_publish(struct mag_sensor_data &data)
mag_report mag_report = {};
mag_report.timestamp = hrt_absolute_time();
+ mag_report.is_external = true;
// TODO: remove these (or get the values)
mag_report.x_raw = 0;
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 044f8d7f10..461238a69f 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
@@ -269,6 +269,7 @@ int DfHmc9250Wrapper::_publish(struct mag_sensor_data &data)
mag_report mag_report = {};
mag_report.timestamp = hrt_absolute_time();
+ mag_report.is_external = true;
/* The standard external mag by 3DR has x pointing to the
* right, y pointing backwards, and z down, therefore switch x
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 aac936e1b8..86c1a6581a 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
@@ -624,6 +624,7 @@ int DfLsm9ds1Wrapper::_publish(struct imu_sensor_data &data)
accel_report.timestamp = gyro_report.timestamp = hrt_absolute_time();
mag_report.timestamp = accel_report.timestamp;
+ mag_report.is_external = false;
// TODO: get these right
gyro_report.scaling = -1.0f;
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 3f50762402..660e9604bd 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
@@ -700,6 +700,7 @@ int DfMpu9250Wrapper::_publish(struct imu_sensor_data &data)
if (_mag_enabled) {
mag_report.timestamp = accel_report.timestamp;
+ mag_report.is_external = false;
mag_report.scaling = -1.0f;
mag_report.range_ga = -1.0f;