fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-11-03 01:51:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

df_hmc5883_wrapper: rotation on driver level

Browse Source
This commit is contained in:
Julian Oes
2016-06-01 08:39:10 +02:00
committed by Lorenz Meier
parent 969940e98a
commit d5a7aaa09d
1 changed files with 33 additions and 21 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/platforms/posix/drivers/df_hmc5883_wrapper/df_hmc5883_wrapper.cpp
+33 -21
View File
@@ -59,8 +59,8 @@
#include <uORB/topics/parameter_update.h> #include <uORB/topics/parameter_update.h>
#include <board_config.h> #include <board_config.h>
//#include <mathlib/math/filter/LowPassFilter2p.hpp>
//#include <lib/conversion/rotation.h> #include <lib/conversion/rotation.h>
#include <hmc5883/HMC5883.hpp> #include <hmc5883/HMC5883.hpp>
#include <DevMgr.hpp> #include <DevMgr.hpp>
@@ -74,7 +74,7 @@ using namespace DriverFramework;
class DfHmc9250Wrapper : public HMC5883 class DfHmc9250Wrapper : public HMC5883
{ {
public: public:
DfHmc9250Wrapper(/*enum Rotation rotation*/); DfHmc9250Wrapper(enum Rotation rotation);
~DfHmc9250Wrapper(); ~DfHmc9250Wrapper();
@@ -97,8 +97,6 @@ private:
void _update_mag_calibration(); void _update_mag_calibration();
//enum Rotation _rotation;
orb_advert_t _mag_topic; orb_advert_t _mag_topic;
int _param_update_sub; int _param_update_sub;
@@ -112,20 +110,21 @@ private:
float z_scale; float z_scale;
} _mag_calibration; } _mag_calibration;
math::Matrix<3, 3> _rotation_matrix;
int _mag_orb_class_instance; int _mag_orb_class_instance;
perf_counter_t _mag_sample_perf; perf_counter_t _mag_sample_perf;
}; };
DfHmc9250Wrapper::DfHmc9250Wrapper(/*enum Rotation rotation*/) : DfHmc9250Wrapper::DfHmc9250Wrapper(enum Rotation rotation) :
HMC5883(MAG_DEVICE_PATH), HMC5883(MAG_DEVICE_PATH),
_mag_topic(nullptr), _mag_topic(nullptr),
_param_update_sub(-1), _param_update_sub(-1),
_mag_calibration{}, _mag_calibration{},
_mag_orb_class_instance(-1), _mag_orb_class_instance(-1),
_mag_sample_perf(perf_alloc(PC_ELAPSED, "df_mag_read")) _mag_sample_perf(perf_alloc(PC_ELAPSED, "df_mag_read"))
/*_rotation(rotation)*/
{ {
// Set sane default calibration values // Set sane default calibration values
_mag_calibration.x_scale = 1.0f; _mag_calibration.x_scale = 1.0f;
@@ -134,6 +133,9 @@ DfHmc9250Wrapper::DfHmc9250Wrapper(/*enum Rotation rotation*/) :
_mag_calibration.x_offset = 0.0f; _mag_calibration.x_offset = 0.0f;
_mag_calibration.y_offset = 0.0f; _mag_calibration.y_offset = 0.0f;
_mag_calibration.z_offset = 0.0f; _mag_calibration.z_offset = 0.0f;
// Get sensor rotation matrix
get_rot_matrix(rotation, &_rotation_matrix);
} }
DfHmc9250Wrapper::~DfHmc9250Wrapper() DfHmc9250Wrapper::~DfHmc9250Wrapper()
@@ -278,9 +280,19 @@ int DfHmc9250Wrapper::_publish(struct mag_sensor_data &data)
mag_report.x_raw = NAN; mag_report.x_raw = NAN;
mag_report.y_raw = NAN; mag_report.y_raw = NAN;
mag_report.z_raw = NAN; mag_report.z_raw = NAN;
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; math::Vector<3> mag_val((data.mag_ga_x - _mag_calibration.x_offset) * _mag_calibration.x_scale,
(data.mag_ga_y - _mag_calibration.y_offset) * _mag_calibration.y_scale,
(data.mag_ga_z - _mag_calibration.z_offset) * _mag_calibration.z_scale);
// apply sensor rotation on the accel measurement
mag_val = _rotation_matrix * mag_val;
mag_report.x = mag_val(0);
mag_report.y = mag_val(1);
mag_report.z = mag_val(2);
// TODO: get these right // TODO: get these right
//mag_report.scaling = -1.0f; //mag_report.scaling = -1.0f;
@@ -315,14 +327,14 @@ namespace df_hmc5883_wrapper
DfHmc9250Wrapper *g_dev = nullptr; DfHmc9250Wrapper *g_dev = nullptr;
int start(/* enum Rotation rotation */); int start(enum Rotation rotation);
int stop(); int stop();
int info(); int info();
void usage(); void usage();
int start(/*enum Rotation rotation*/) int start(enum Rotation rotation)
{ {
g_dev = new DfHmc9250Wrapper(/*rotation*/); g_dev = new DfHmc9250Wrapper(rotation);
if (g_dev == nullptr) { if (g_dev == nullptr) {
PX4_ERR("failed instantiating DfHmc9250Wrapper object"); PX4_ERR("failed instantiating DfHmc9250Wrapper object");
@@ -389,9 +401,9 @@ info()
void void
usage() usage()
{ {
PX4_WARN("Usage: df_hmc5883_wrapper 'start', 'info', 'stop'"); PX4_INFO("Usage: df_hmc5883_wrapper 'start', 'info', 'stop'");
PX4_WARN("options:"); PX4_INFO("options:");
//PX4_WARN(" -R rotation"); PX4_INFO(" -R rotation");
} }
} // namespace df_hmc5883_wrapper } // namespace df_hmc5883_wrapper
@@ -401,7 +413,7 @@ int
df_hmc5883_wrapper_main(int argc, char *argv[]) df_hmc5883_wrapper_main(int argc, char *argv[])
{ {
int ch; int ch;
// enum Rotation rotation = ROTATION_NONE; enum Rotation rotation = ROTATION_NONE;
int ret = 0; int ret = 0;
int myoptind = 1; int myoptind = 1;
const char *myoptarg = NULL; const char *myoptarg = NULL;
@@ -409,9 +421,9 @@ df_hmc5883_wrapper_main(int argc, char *argv[])
/* jump over start/off/etc and look at options first */ /* jump over start/off/etc and look at options first */
while ((ch = px4_getopt(argc, argv, "R:", &myoptind, &myoptarg)) != EOF) { while ((ch = px4_getopt(argc, argv, "R:", &myoptind, &myoptarg)) != EOF) {
switch (ch) { switch (ch) {
//case 'R': case 'R':
// rotation = (enum Rotation)atoi(myoptarg); rotation = (enum Rotation)atoi(myoptarg);
// break; break;
default: default:
df_hmc5883_wrapper::usage(); df_hmc5883_wrapper::usage();
@@ -428,7 +440,7 @@ df_hmc5883_wrapper_main(int argc, char *argv[])
if (!strcmp(verb, "start")) { if (!strcmp(verb, "start")) {
ret = df_hmc5883_wrapper::start(/*rotation*/); ret = df_hmc5883_wrapper::start(rotation);
} }
else if (!strcmp(verb, "stop")) { else if (!strcmp(verb, "stop")) {