forked from Archive/PX4-Autopilot
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).
This commit is contained in:
parent
19cdbcfd4f
commit
ce7d8d2270
|
@ -11,3 +11,5 @@ int16 y_raw
|
||||||
int16 z_raw
|
int16 z_raw
|
||||||
|
|
||||||
uint32 device_id # unique device ID for the sensor that does not change between power cycles
|
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)
|
||||||
|
|
||||||
|
|
|
@ -251,7 +251,6 @@ BMM150 :: BMM150(int bus, const char *path, bool external, enum Rotation rotatio
|
||||||
_external(false),
|
_external(false),
|
||||||
_running(false),
|
_running(false),
|
||||||
_call_interval(0),
|
_call_interval(0),
|
||||||
_report{0},
|
|
||||||
_reports(nullptr),
|
_reports(nullptr),
|
||||||
_collect_phase(false),
|
_collect_phase(false),
|
||||||
_scale{},
|
_scale{},
|
||||||
|
@ -344,7 +343,7 @@ int BMM150::init()
|
||||||
}
|
}
|
||||||
|
|
||||||
/* allocate basic report buffers */
|
/* allocate basic report buffers */
|
||||||
_reports = new ringbuffer::RingBuffer(2, sizeof(_report));
|
_reports = new ringbuffer::RingBuffer(2, sizeof(mag_report));
|
||||||
|
|
||||||
if (_reports == nullptr) {
|
if (_reports == nullptr) {
|
||||||
goto out;
|
goto out;
|
||||||
|
@ -433,7 +432,7 @@ BMM150::stop()
|
||||||
ssize_t
|
ssize_t
|
||||||
BMM150::read(struct file *filp, char *buffer, size_t buflen)
|
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);
|
struct mag_report *mag_buf = reinterpret_cast<struct mag_report *>(buffer);
|
||||||
int ret = 0;
|
int ret = 0;
|
||||||
|
|
||||||
|
@ -684,6 +683,7 @@ BMM150::collect()
|
||||||
|
|
||||||
|
|
||||||
mrb.timestamp = hrt_absolute_time();
|
mrb.timestamp = hrt_absolute_time();
|
||||||
|
mrb.is_external = is_external();
|
||||||
|
|
||||||
// report the error count as the number of bad transfers.
|
// report the error count as the number of bad transfers.
|
||||||
// This allows the higher level code to decide if it
|
// This allows the higher level code to decide if it
|
||||||
|
|
|
@ -228,7 +228,6 @@ private:
|
||||||
unsigned _call_interval;
|
unsigned _call_interval;
|
||||||
|
|
||||||
|
|
||||||
struct mag_report _report;
|
|
||||||
ringbuffer::RingBuffer *_reports;
|
ringbuffer::RingBuffer *_reports;
|
||||||
|
|
||||||
bool _collect_phase;
|
bool _collect_phase;
|
||||||
|
|
|
@ -997,6 +997,7 @@ HMC5883::collect()
|
||||||
// XXX revisit for SPI part, might require a bus type IOCTL
|
// XXX revisit for SPI part, might require a bus type IOCTL
|
||||||
unsigned dummy;
|
unsigned dummy;
|
||||||
sensor_is_onboard = !_interface->ioctl(MAGIOCGEXTERNAL, dummy);
|
sensor_is_onboard = !_interface->ioctl(MAGIOCGEXTERNAL, dummy);
|
||||||
|
new_report.is_external = !sensor_is_onboard;
|
||||||
|
|
||||||
if (sensor_is_onboard) {
|
if (sensor_is_onboard) {
|
||||||
// convert onboard so it matches offboard for the
|
// convert onboard so it matches offboard for the
|
||||||
|
|
|
@ -913,6 +913,7 @@ IST8310::collect()
|
||||||
|
|
||||||
/* this should be fairly close to the end of the measurement, so the best approximation of the time */
|
/* 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.timestamp = hrt_absolute_time();
|
||||||
|
new_report.is_external = !sensor_is_onboard;
|
||||||
new_report.error_count = perf_event_count(_comms_errors);
|
new_report.error_count = perf_event_count(_comms_errors);
|
||||||
new_report.scaling = _range_scale;
|
new_report.scaling = _range_scale;
|
||||||
new_report.device_id = _device_id.devid;
|
new_report.device_id = _device_id.devid;
|
||||||
|
|
|
@ -945,6 +945,7 @@ LIS3MDL::collect()
|
||||||
|
|
||||||
unsigned dummy;
|
unsigned dummy;
|
||||||
sensor_is_onboard = !_interface->ioctl(MAGIOCGEXTERNAL, dummy);
|
sensor_is_onboard = !_interface->ioctl(MAGIOCGEXTERNAL, dummy);
|
||||||
|
new_report.is_external = !sensor_is_onboard;
|
||||||
|
|
||||||
/*
|
/*
|
||||||
* RAW outputs
|
* RAW outputs
|
||||||
|
|
|
@ -1653,8 +1653,7 @@ LSM303D::mag_measure()
|
||||||
} raw_mag_report;
|
} raw_mag_report;
|
||||||
#pragma pack(pop)
|
#pragma pack(pop)
|
||||||
|
|
||||||
mag_report mag_report;
|
mag_report mag_report {};
|
||||||
memset(&mag_report, 0, sizeof(mag_report));
|
|
||||||
|
|
||||||
/* start the performance counter */
|
/* start the performance counter */
|
||||||
perf_begin(_mag_sample_perf);
|
perf_begin(_mag_sample_perf);
|
||||||
|
@ -1680,6 +1679,7 @@ LSM303D::mag_measure()
|
||||||
*/
|
*/
|
||||||
|
|
||||||
mag_report.timestamp = hrt_absolute_time();
|
mag_report.timestamp = hrt_absolute_time();
|
||||||
|
mag_report.is_external = is_external();
|
||||||
|
|
||||||
mag_report.x_raw = raw_mag_report.x;
|
mag_report.x_raw = raw_mag_report.x;
|
||||||
mag_report.y_raw = raw_mag_report.y;
|
mag_report.y_raw = raw_mag_report.y;
|
||||||
|
|
|
@ -205,6 +205,7 @@ MPU9250_mag::_measure(struct ak8963_regs data)
|
||||||
|
|
||||||
mag_report mrb;
|
mag_report mrb;
|
||||||
mrb.timestamp = hrt_absolute_time();
|
mrb.timestamp = hrt_absolute_time();
|
||||||
|
mrb.is_external = false;
|
||||||
|
|
||||||
/*
|
/*
|
||||||
* Align axes - note the accel & gryo are also re-aligned so this
|
* Align axes - note the accel & gryo are also re-aligned so this
|
||||||
|
|
|
@ -406,6 +406,7 @@ void VotedSensorsUpdate::parameters_update()
|
||||||
}
|
}
|
||||||
|
|
||||||
int topic_device_id = report.device_id;
|
int topic_device_id = report.device_id;
|
||||||
|
bool is_external = (bool)report.is_external;
|
||||||
_mag_device_id[topic_instance] = topic_device_id;
|
_mag_device_id[topic_instance] = topic_device_id;
|
||||||
|
|
||||||
// find the driver handle that matches the 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);
|
(void)sprintf(str, "CAL_MAG%u_ROT", i);
|
||||||
|
|
||||||
if (h.isValid()) { //FIXME: get the 'is external' information via uorb topic
|
if (is_external) {
|
||||||
if (h.ioctl(MAGIOCGEXTERNAL, 0) <= 0) {
|
int32_t mag_rot;
|
||||||
/* mag is internal - reset param to -1 to indicate internal mag */
|
param_get(param_find(str), &mag_rot);
|
||||||
int32_t minus_one;
|
|
||||||
param_get(param_find(str), &minus_one);
|
|
||||||
|
|
||||||
if (minus_one != MAG_ROT_VAL_INTERNAL) {
|
/* check if this mag is still set as internal, otherwise leave untouched */
|
||||||
minus_one = MAG_ROT_VAL_INTERNAL;
|
if (mag_rot < 0) {
|
||||||
param_set_no_notification(param_find(str), &minus_one);
|
/* it was marked as internal, change to external with no rotation */
|
||||||
}
|
mag_rot = 0;
|
||||||
|
param_set_no_notification(param_find(str), &mag_rot);
|
||||||
|
}
|
||||||
|
|
||||||
} else {
|
} else {
|
||||||
/* mag is external */
|
/* mag is internal - reset param to -1 to indicate internal mag */
|
||||||
int32_t mag_rot;
|
int32_t minus_one;
|
||||||
param_get(param_find(str), &mag_rot);
|
param_get(param_find(str), &minus_one);
|
||||||
|
|
||||||
/* check if this mag is still set as internal, otherwise leave untouched */
|
if (minus_one != MAG_ROT_VAL_INTERNAL) {
|
||||||
if (mag_rot < 0) {
|
minus_one = MAG_ROT_VAL_INTERNAL;
|
||||||
/* it was marked as internal, change to external with no rotation */
|
param_set_no_notification(param_find(str), &minus_one);
|
||||||
mag_rot = 0;
|
|
||||||
param_set_no_notification(param_find(str), &mag_rot);
|
|
||||||
}
|
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
@ -156,6 +156,7 @@ void UavcanMagnetometerBridge::mag_sub_cb(const uavcan::ReceivedDataStructure<ua
|
||||||
*/
|
*/
|
||||||
_report.timestamp = hrt_absolute_time();
|
_report.timestamp = hrt_absolute_time();
|
||||||
_report.device_id = _device_id.devid;
|
_report.device_id = _device_id.devid;
|
||||||
|
_report.is_external = true;
|
||||||
|
|
||||||
_report.x = (msg.magnetic_field_ga[0] - _scale.x_offset) * _scale.x_scale;
|
_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;
|
_report.y = (msg.magnetic_field_ga[1] - _scale.y_offset) * _scale.y_scale;
|
||||||
|
|
|
@ -915,8 +915,7 @@ ACCELSIM::mag_measure()
|
||||||
} raw_mag_report;
|
} raw_mag_report;
|
||||||
#pragma pack(pop)
|
#pragma pack(pop)
|
||||||
|
|
||||||
mag_report mag_report;
|
mag_report mag_report = {};
|
||||||
memset(&mag_report, 0, sizeof(mag_report));
|
|
||||||
|
|
||||||
/* start the performance counter */
|
/* start the performance counter */
|
||||||
perf_begin(_mag_sample_perf);
|
perf_begin(_mag_sample_perf);
|
||||||
|
@ -946,6 +945,7 @@ ACCELSIM::mag_measure()
|
||||||
|
|
||||||
|
|
||||||
mag_report.timestamp = hrt_absolute_time();
|
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.x_raw = (int16_t)(raw_mag_report.x / _mag_range_scale);
|
||||||
mag_report.y_raw = (int16_t)(raw_mag_report.y / _mag_range_scale);
|
mag_report.y_raw = (int16_t)(raw_mag_report.y / _mag_range_scale);
|
||||||
|
|
|
@ -278,6 +278,7 @@ int DfAK8963Wrapper::_publish(struct mag_sensor_data &data)
|
||||||
|
|
||||||
mag_report mag_report = {};
|
mag_report mag_report = {};
|
||||||
mag_report.timestamp = hrt_absolute_time();
|
mag_report.timestamp = hrt_absolute_time();
|
||||||
|
mag_report.is_external = true;
|
||||||
|
|
||||||
// TODO: remove these (or get the values)
|
// TODO: remove these (or get the values)
|
||||||
mag_report.x_raw = 0;
|
mag_report.x_raw = 0;
|
||||||
|
|
|
@ -269,6 +269,7 @@ int DfHmc9250Wrapper::_publish(struct mag_sensor_data &data)
|
||||||
|
|
||||||
mag_report mag_report = {};
|
mag_report mag_report = {};
|
||||||
mag_report.timestamp = hrt_absolute_time();
|
mag_report.timestamp = hrt_absolute_time();
|
||||||
|
mag_report.is_external = true;
|
||||||
|
|
||||||
/* The standard external mag by 3DR has x pointing to the
|
/* The standard external mag by 3DR has x pointing to the
|
||||||
* right, y pointing backwards, and z down, therefore switch x
|
* right, y pointing backwards, and z down, therefore switch x
|
||||||
|
|
|
@ -624,6 +624,7 @@ int DfLsm9ds1Wrapper::_publish(struct imu_sensor_data &data)
|
||||||
|
|
||||||
accel_report.timestamp = gyro_report.timestamp = hrt_absolute_time();
|
accel_report.timestamp = gyro_report.timestamp = hrt_absolute_time();
|
||||||
mag_report.timestamp = accel_report.timestamp;
|
mag_report.timestamp = accel_report.timestamp;
|
||||||
|
mag_report.is_external = false;
|
||||||
|
|
||||||
// TODO: get these right
|
// TODO: get these right
|
||||||
gyro_report.scaling = -1.0f;
|
gyro_report.scaling = -1.0f;
|
||||||
|
|
|
@ -700,6 +700,7 @@ int DfMpu9250Wrapper::_publish(struct imu_sensor_data &data)
|
||||||
|
|
||||||
if (_mag_enabled) {
|
if (_mag_enabled) {
|
||||||
mag_report.timestamp = accel_report.timestamp;
|
mag_report.timestamp = accel_report.timestamp;
|
||||||
|
mag_report.is_external = false;
|
||||||
|
|
||||||
mag_report.scaling = -1.0f;
|
mag_report.scaling = -1.0f;
|
||||||
mag_report.range_ga = -1.0f;
|
mag_report.range_ga = -1.0f;
|
||||||
|
|
Loading…
Reference in New Issue