fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-30 07:53:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

sensors: move mag aggregation to new VehicleMagnetometer WorkItem

Browse Source 
- purge all reminaing magnetometer IOCTL usage
 - mag calibration add off diagonal (soft iron) scale factors
This commit is contained in:
Daniel Agar
2020-08-21 10:12:13 -04:00
parent ad148af2fd
commit 971b1e4b4d
72 changed files with 870 additions and 1134 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/drivers/magnetometer/PX4Magnetometer.cpp
+13 -66
View File
@@ -38,13 +38,14 @@
PX4Magnetometer::PX4Magnetometer(uint32_t device_id, ORB_PRIO priority, enum Rotation rotation) :
CDev(nullptr),
_sensor_mag_pub{ORB_ID(sensor_mag), priority},
_rotation{rotation},
_device_id{device_id}
_sensor_pub{ORB_ID(sensor_mag), priority},
_device_id{device_id},
_rotation{rotation}
{
_class_device_instance = register_class_devname(MAG_BASE_DEVICE_PATH);
// advertise immediately to keep instance numbering in sync
_sensor_pub.advertise();
_sensor_mag_pub.advertise();
_class_device_instance = register_class_devname(MAG_BASE_DEVICE_PATH);
}
PX4Magnetometer::~PX4Magnetometer()
@@ -53,46 +54,7 @@ PX4Magnetometer::~PX4Magnetometer()
unregister_class_devname(MAG_BASE_DEVICE_PATH, _class_device_instance);
}
_sensor_mag_pub.unadvertise();
}
int PX4Magnetometer::ioctl(cdev::file_t *filp, int cmd, unsigned long arg)
{
switch (cmd) {
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};
}
return PX4_OK;
case MAGIOCGSCALE: {
// copy out scale factors
mag_calibration_s cal{};
cal.x_offset = _calibration_offset(0);
cal.y_offset = _calibration_offset(1);
cal.z_offset = _calibration_offset(2);
cal.x_scale = _calibration_scale(0);
cal.y_scale = _calibration_scale(1);
cal.z_scale = _calibration_scale(2);
memcpy((mag_calibration_s *)arg, &cal, sizeof(cal));
}
return 0;
case MAGIOCGEXTERNAL:
return _external;
case DEVIOCGDEVICEID:
return _device_id;
default:
return -ENOTTY;
}
_sensor_pub.unadvertise();
}
void PX4Magnetometer::set_device_type(uint8_t devtype)
@@ -104,11 +66,11 @@ void PX4Magnetometer::set_device_type(uint8_t devtype)
// update to new device type
device_id.devid_s.devtype = devtype;
// copy back to report
// copy back
_device_id = device_id.devid;
}
void PX4Magnetometer::update(hrt_abstime timestamp_sample, float x, float y, float z)
void PX4Magnetometer::update(const hrt_abstime &timestamp_sample, float x, float y, float z)
{
sensor_mag_s report;
report.timestamp_sample = timestamp_sample;
@@ -119,27 +81,12 @@ void PX4Magnetometer::update(hrt_abstime timestamp_sample, float x, float y, flo
// Apply rotation (before scaling)
rotate_3f(_rotation, x, y, z);
const matrix::Vector3f raw_f{x, y, z};
// Apply range scale and the calibrating offset/scale
const matrix::Vector3f val_calibrated{(((raw_f * _scale) - _calibration_offset).emult(_calibration_scale))};
report.x = val_calibrated(0);
report.y = val_calibrated(1);
report.z = val_calibrated(2);
report.x = x * _scale;
report.y = y * _scale;
report.z = z * _scale;
report.is_external = _external;
report.timestamp = hrt_absolute_time();
_sensor_mag_pub.publish(report);
}
void PX4Magnetometer::print_status()
{
PX4_INFO(MAG_BASE_DEVICE_PATH " device instance: %d", _class_device_instance);
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));
_sensor_pub.publish(report);
}
src/lib/drivers/magnetometer/PX4Magnetometer.hpp
+8 -15
View File
@@ -37,7 +37,6 @@
#include <drivers/drv_hrt.h>
#include <lib/cdev/CDev.hpp>
#include <lib/conversion/rotation.h>
#include <uORB/uORB.h>
#include <uORB/PublicationMulti.hpp>
#include <uORB/topics/sensor_mag.h>
@@ -47,10 +46,9 @@ public:
PX4Magnetometer(uint32_t device_id, ORB_PRIO priority = ORB_PRIO_DEFAULT, enum Rotation rotation = ROTATION_NONE);
~PX4Magnetometer() override;
int ioctl(cdev::file_t *filp, int cmd, unsigned long arg) override;
bool external() { return _external; }
void set_device_id(uint32_t device_id) { _device_id = device_id; }
void set_device_type(uint8_t devtype);
void set_error_count(uint32_t error_count) { _error_count = error_count; }
void increase_error_count() { _error_count++; }
@@ -58,24 +56,19 @@ public:
void set_temperature(float temperature) { _temperature = temperature; }
void set_external(bool external) { _external = external; }
void update(hrt_abstime timestamp_sample, float x, float y, float z);
void update(const hrt_abstime &timestamp_sample, float x, float y, float z);
int get_class_instance() { return _class_device_instance; };
void print_status();
private:
uORB::PublicationMulti<sensor_mag_s> _sensor_mag_pub;
uORB::PublicationMulti<sensor_mag_s> _sensor_pub;
matrix::Vector3f _calibration_scale{1.f, 1.f, 1.f};
matrix::Vector3f _calibration_offset{0.f, 0.f, 0.f};
uint32_t _device_id{0};
const enum Rotation _rotation;
const enum Rotation _rotation;
uint32_t _device_id{0};
float _temperature{NAN};
float _scale{1.f};
uint32_t _error_count{0};
float _scale{1.f};
float _temperature{NAN};
uint32_t _error_count{0};
bool _external{false};