fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-06-29 08:40: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-05-31 18:04:42 +01: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 <board_config.h>
//#include <mathlib/math/filter/LowPassFilter2p.hpp>
//#include <lib/conversion/rotation.h>
#include <lib/conversion/rotation.h>
#include <hmc5883/HMC5883.hpp>
#include <DevMgr.hpp>
@@ -74,7 +74,7 @@ using namespace DriverFramework;
class DfHmc9250Wrapper : public HMC5883
{
public:
DfHmc9250Wrapper(/*enum Rotation rotation*/);
DfHmc9250Wrapper(enum Rotation rotation);
~DfHmc9250Wrapper();
@@ -97,8 +97,6 @@ private:
void _update_mag_calibration();
//enum Rotation _rotation;
orb_advert_t _mag_topic;
int _param_update_sub;
@@ -112,20 +110,21 @@ private:
float z_scale;
} _mag_calibration;
math::Matrix<3, 3> _rotation_matrix;
int _mag_orb_class_instance;
perf_counter_t _mag_sample_perf;
};
DfHmc9250Wrapper::DfHmc9250Wrapper(/*enum Rotation rotation*/) :
DfHmc9250Wrapper::DfHmc9250Wrapper(enum Rotation rotation) :
HMC5883(MAG_DEVICE_PATH),
_mag_topic(nullptr),
_param_update_sub(-1),
_mag_calibration{},
_mag_orb_class_instance(-1),
_mag_sample_perf(perf_alloc(PC_ELAPSED, "df_mag_read"))
/*_rotation(rotation)*/
{
// Set sane default calibration values
_mag_calibration.x_scale = 1.0f;
@@ -134,6 +133,9 @@ DfHmc9250Wrapper::DfHmc9250Wrapper(/*enum Rotation rotation*/) :
_mag_calibration.x_offset = 0.0f;
_mag_calibration.y_offset = 0.0f;
_mag_calibration.z_offset = 0.0f;
// Get sensor rotation matrix
get_rot_matrix(rotation, &_rotation_matrix);
}
DfHmc9250Wrapper::~DfHmc9250Wrapper()
@@ -278,9 +280,19 @@ int DfHmc9250Wrapper::_publish(struct mag_sensor_data &data)
mag_report.x_raw = NAN;
mag_report.y_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
//mag_report.scaling = -1.0f;
@@ -315,14 +327,14 @@ namespace df_hmc5883_wrapper
DfHmc9250Wrapper *g_dev = nullptr;
int start(/* enum Rotation rotation */);
int start(enum Rotation rotation);
int stop();
int info();
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) {
PX4_ERR("failed instantiating DfHmc9250Wrapper object");
@@ -389,9 +401,9 @@ info()
void
usage()
{
PX4_WARN("Usage: df_hmc5883_wrapper 'start', 'info', 'stop'");
PX4_WARN("options:");
//PX4_WARN(" -R rotation");
PX4_INFO("Usage: df_hmc5883_wrapper 'start', 'info', 'stop'");
PX4_INFO("options:");
PX4_INFO(" -R rotation");
}
} // namespace df_hmc5883_wrapper
@@ -401,7 +413,7 @@ int
df_hmc5883_wrapper_main(int argc, char *argv[])
{
int ch;
// enum Rotation rotation = ROTATION_NONE;
enum Rotation rotation = ROTATION_NONE;
int ret = 0;
int myoptind = 1;
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 */
while ((ch = px4_getopt(argc, argv, "R:", &myoptind, &myoptarg)) != EOF) {
switch (ch) {
//case 'R':
// rotation = (enum Rotation)atoi(myoptarg);
// break;
case 'R':
rotation = (enum Rotation)atoi(myoptarg);
break;
default:
df_hmc5883_wrapper::usage();
@@ -428,7 +440,7 @@ df_hmc5883_wrapper_main(int argc, char *argv[])
if (!strcmp(verb, "start")) {
ret = df_hmc5883_wrapper::start(/*rotation*/);
ret = df_hmc5883_wrapper::start(rotation);
}
else if (!strcmp(verb, "stop")) {