From a883d8eff983d8c9f374e64dff4e286b4b50f6a5 Mon Sep 17 00:00:00 2001
From: Daniel Agar <daniel@agar.ca>
Date: Sun, 19 May 2019 10:38:51 -0400
Subject: [PATCH] PX4Magnetometer now functional
---
src/lib/drivers/magnetometer/CMakeLists.txt | 2 +-
.../drivers/magnetometer/PX4Magnetometer.cpp | 136 ++++++++----------
.../drivers/magnetometer/PX4Magnetometer.hpp | 41 +++---
3 files changed, 76 insertions(+), 103 deletions(-)
diff --git a/src/lib/drivers/magnetometer/CMakeLists.txt b/src/lib/drivers/magnetometer/CMakeLists.txt
index 198fea075c..49b0cd0ad0 100644
--- a/src/lib/drivers/magnetometer/CMakeLists.txt
+++ b/src/lib/drivers/magnetometer/CMakeLists.txt
@@ -31,4 +31,4 @@
#
############################################################################
-px4_add_library(drivers__magnetometer PX4Magnetometer.cpp)
+px4_add_library(drivers_magnetometer PX4Magnetometer.cpp)
diff --git a/src/lib/drivers/magnetometer/PX4Magnetometer.cpp b/src/lib/drivers/magnetometer/PX4Magnetometer.cpp
index f75de60e98..d6d33e0575 100644
--- a/src/lib/drivers/magnetometer/PX4Magnetometer.cpp
+++ b/src/lib/drivers/magnetometer/PX4Magnetometer.cpp
@@ -34,118 +34,100 @@
#include "PX4Magnetometer.hpp"
-PX4Magnetometer::PX4Magnetometer(const char *path, device::Device *interface, uint8_t dev_type, enum Rotation rotation,
- float scale) :
- CDev(path),
- _interface(interface)
-{
- _device_id.devid = _interface->get_device_id();
- // _device_id.devid_s.bus_type = (device::Device::DeviceBusType)_interface->get_device_bus_type();
- // _device_id.devid_s.bus = _interface->get_device_bus();
- // _device_id.devid_s.address = _interface->get_device_address();
- _device_id.devid_s.devtype = dev_type;
-
- CDev::init();
+#include <lib/drivers/device/Device.hpp>
+PX4Magnetometer::PX4Magnetometer(uint32_t device_id, uint8_t priority, enum Rotation rotation) :
+ CDev(nullptr),
+ _sensor_mag_pub{ORB_ID(sensor_mag), priority},
+ _rotation{rotation}
+{
_class_device_instance = register_class_devname(MAG_BASE_DEVICE_PATH);
- _rotation = rotation;
- _scale = scale;
+ _sensor_mag_pub.get().device_id = device_id;
+ _sensor_mag_pub.get().scaling = 1.0f;
- _cal.x_offset = 0;
- _cal.x_scale = 1.0f;
- _cal.y_offset = 0;
- _cal.y_scale = 1.0f;
- _cal.z_offset = 0;
- _cal.z_scale = 1.0f;
+ // force initial publish to allocate uORB buffer
+ // TODO: can be removed once all drivers are in threads
+ _sensor_mag_pub.update();
}
PX4Magnetometer::~PX4Magnetometer()
{
- if (_topic != nullptr) {
- orb_unadvertise(_topic);
- }
-}
-
-int PX4Magnetometer::init()
-{
- mag_report report{};
- report.device_id = _device_id.devid;
-
- if (_topic == nullptr) {
- _topic = orb_advertise_multi(ORB_ID(sensor_mag), &report, &_orb_class_instance, ORB_PRIO_HIGH - 1);
-
- if (_topic == nullptr) {
- PX4_ERR("Advertise failed.");
- return PX4_ERROR;
- }
+ if (_class_device_instance != -1) {
+ unregister_class_devname(MAG_BASE_DEVICE_PATH, _class_device_instance);
}
-
- return PX4_OK;
}
int PX4Magnetometer::ioctl(cdev::file_t *filp, int cmd, unsigned long arg)
{
switch (cmd) {
- case MAGIOCSSCALE:
- // Copy scale in.
- memcpy(&_cal, (struct mag_calibration_s *) arg, sizeof(_cal));
- return OK;
+ case MAGIOCSSCALE: {
+ // Copy offsets and scale factors in
+ mag_calibration_s cal{};
+ memcpy(&cal, (mag_calibration_s *) arg, sizeof(cal));
+
+ _calibration_offset = matrix::Vector3f{cal.x_offset, cal.y_offset, cal.z_offset};
+ _calibration_scale = matrix::Vector3f{cal.x_scale, cal.y_scale, cal.z_scale};
+ }
- case MAGIOCGSCALE:
- // Copy scale out.
- memcpy((struct mag_calibration_s *) arg, &_cal, sizeof(_cal));
- return OK;
+ return PX4_OK;
case DEVIOCGDEVICEID:
- return _device_id.devid;
+ return _sensor_mag_pub.get().device_id;
default:
- // Give it to the superclass.
- return CDev::ioctl(filp, cmd, arg);
+ return -ENOTTY;
}
}
-void PX4Magnetometer::configure_filter(float sample_freq, float cutoff_freq)
+void PX4Magnetometer::set_device_type(uint8_t devtype)
{
- _filter_x.set_cutoff_frequency(sample_freq, cutoff_freq);
- _filter_y.set_cutoff_frequency(sample_freq, cutoff_freq);
- _filter_z.set_cutoff_frequency(sample_freq, cutoff_freq);
+ // current DeviceStructure
+ union device::Device::DeviceId device_id;
+ device_id.devid = _sensor_mag_pub.get().device_id;
+
+ // update to new device type
+ device_id.devid_s.devtype = devtype;
+
+ // copy back to report
+ _sensor_mag_pub.get().device_id = device_id.devid;
}
-// @TODO: use fixed point math to reclaim CPU usage
-int PX4Magnetometer::publish(float x, float y, float z, float temperature)
+void PX4Magnetometer::update(hrt_abstime timestamp, int16_t x, int16_t y, int16_t z)
{
- sensor_mag_s report{};
+ sensor_mag_s &report = _sensor_mag_pub.get();
+ report.timestamp = timestamp;
- report.device_id = _device_id.devid;
- report.error_count = 0;
- report.scaling = _scale;
- report.timestamp = hrt_absolute_time();
- report.temperature = temperature;
- report.is_external = false;
+ // Apply rotation (before scaling)
+ float xraw_f = x;
+ float yraw_f = y;
+ float zraw_f = z;
+ rotate_3f(_rotation, xraw_f, yraw_f, zraw_f);
+ // Apply range scale and the calibrating offset/scale
+ const matrix::Vector3f val_calibrated{(((matrix::Vector3f{xraw_f, yraw_f, zraw_f} * report.scaling) - _calibration_offset).emult(_calibration_scale))};
// Raw values (ADC units 0 - 65535)
report.x_raw = x;
report.y_raw = y;
report.z_raw = z;
- // Apply the rotation.
- rotate_3f(_rotation, x, y, z);
+ report.x = val_calibrated(0);
+ report.y = val_calibrated(1);
+ report.z = val_calibrated(2);
- // Apply FS range scale and the calibrating offset/scale
- x = ((x * _scale) - _cal.x_offset) * _cal.x_scale;
- y = ((y * _scale) - _cal.y_offset) * _cal.y_scale;
- z = ((z * _scale) - _cal.z_offset) * _cal.z_scale;
+ poll_notify(POLLIN);
+ _sensor_mag_pub.update();
+}
- // Filtered values
- report.x = _filter_x.apply(x);
- report.y = _filter_y.apply(y);
- report.z = _filter_z.apply(z);
+void PX4Magnetometer::print_status()
+{
+ PX4_INFO(MAG_BASE_DEVICE_PATH " device instance: %d", _class_device_instance);
- poll_notify(POLLIN);
- orb_publish(ORB_ID(sensor_mag), _topic, &report);
+ PX4_INFO("calibration scale: %.5f %.5f %.5f", (double)_calibration_scale(0), (double)_calibration_scale(1),
+ (double)_calibration_scale(2));
+ PX4_INFO("calibration offset: %.5f %.5f %.5f", (double)_calibration_offset(0), (double)_calibration_offset(1),
+ (double)_calibration_offset(2));
- return PX4_OK;
-}
\ No newline at end of file
+ print_message(_sensor_mag_pub.get());
+}
diff --git a/src/lib/drivers/magnetometer/PX4Magnetometer.hpp b/src/lib/drivers/magnetometer/PX4Magnetometer.hpp
index 2dff3c20c4..00f004965c 100644
--- a/src/lib/drivers/magnetometer/PX4Magnetometer.hpp
+++ b/src/lib/drivers/magnetometer/PX4Magnetometer.hpp
@@ -31,51 +31,42 @@
*
****************************************************************************/
-
-#include <mathlib/math/filter/LowPassFilter2p.hpp>
-
-#include <drivers/device/integrator.h>
#include <drivers/drv_mag.h>
#include <drivers/drv_hrt.h>
-#include <lib/drivers/device/Device.hpp>
#include <lib/cdev/CDev.hpp>
#include <lib/conversion/rotation.h>
-#include <uORB/topics/sensor_mag.h>
#include <uORB/uORB.h>
+#include <uORB/Publication.hpp>
+#include <uORB/topics/sensor_mag.h>
class PX4Magnetometer : public cdev::CDev
{
public:
- PX4Magnetometer(const char *name, device::Device *interface, uint8_t dev_type, enum Rotation rotation, float scale);
+ PX4Magnetometer(uint32_t device_id, uint8_t priority, enum Rotation rotation);
~PX4Magnetometer() override;
- int init() override;
int ioctl(cdev::file_t *filp, int cmd, unsigned long arg) override;
- int publish(float x, float y, float z, float temperature);
+ void set_device_type(uint8_t devtype);
+ void set_error_count(uint64_t error_count) { _sensor_mag_pub.get().error_count = error_count; }
+ void set_scale(float scale) { _sensor_mag_pub.get().scaling = scale; }
+ void set_temperature(float temperature) { _sensor_mag_pub.get().temperature = temperature; }
+ void set_external(bool external) { _sensor_mag_pub.get().is_external = external; }
- void configure_filter(float sample_freq, float cutoff_freq);
+ void update(hrt_abstime timestamp, int16_t x, int16_t y, int16_t z);
-private:
- // Pointer to the communication interface
- const device::Device *_interface{nullptr};
-
- mag_calibration_s _cal{};
+ void print_status();
- orb_advert_t _topic{nullptr};
-
- device::Device::DeviceId _device_id{};
+private:
- int _orb_class_instance{-1};
+ uORB::Publication<sensor_mag_s> _sensor_mag_pub;
- int _class_device_instance{-1};
+ const enum Rotation _rotation;
- enum Rotation _rotation {ROTATION_NONE};
+ matrix::Vector3f _calibration_scale{1.0f, 1.0f, 1.0f};
+ matrix::Vector3f _calibration_offset{0.0f, 0.0f, 0.0f};
- float _scale{1.0f};
+ int _class_device_instance{-1};
- math::LowPassFilter2p _filter_x{100, 20};
- math::LowPassFilter2p _filter_y{100, 20};
- math::LowPassFilter2p _filter_z{100, 20};
};
--
GitLab