|
|
|
@ -72,11 +72,11 @@ extern "C" { __EXPORT int df_hmc5883_wrapper_main(int argc, char *argv[]); }
@@ -72,11 +72,11 @@ extern "C" { __EXPORT int df_hmc5883_wrapper_main(int argc, char *argv[]); }
|
|
|
|
|
using namespace DriverFramework; |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
class DfHmc9250Wrapper : public HMC5883 |
|
|
|
|
class DfHmc5883Wrapper : public HMC5883 |
|
|
|
|
{ |
|
|
|
|
public: |
|
|
|
|
DfHmc9250Wrapper(enum Rotation rotation); |
|
|
|
|
~DfHmc9250Wrapper(); |
|
|
|
|
DfHmc5883Wrapper(enum Rotation rotation, const char *path); |
|
|
|
|
~DfHmc5883Wrapper(); |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
/**
|
|
|
|
@ -119,8 +119,8 @@ private:
@@ -119,8 +119,8 @@ private:
|
|
|
|
|
|
|
|
|
|
}; |
|
|
|
|
|
|
|
|
|
DfHmc9250Wrapper::DfHmc9250Wrapper(enum Rotation rotation) : |
|
|
|
|
HMC5883(MAG_DEVICE_PATH), |
|
|
|
|
DfHmc5883Wrapper::DfHmc5883Wrapper(enum Rotation rotation, const char *path) : |
|
|
|
|
HMC5883(path), |
|
|
|
|
_mag_topic(nullptr), |
|
|
|
|
_param_update_sub(-1), |
|
|
|
|
_mag_calibration{}, |
|
|
|
@ -139,12 +139,12 @@ DfHmc9250Wrapper::DfHmc9250Wrapper(enum Rotation rotation) :
@@ -139,12 +139,12 @@ DfHmc9250Wrapper::DfHmc9250Wrapper(enum Rotation rotation) :
|
|
|
|
|
get_rot_matrix(rotation, &_rotation_matrix); |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
DfHmc9250Wrapper::~DfHmc9250Wrapper() |
|
|
|
|
DfHmc5883Wrapper::~DfHmc5883Wrapper() |
|
|
|
|
{ |
|
|
|
|
perf_free(_mag_sample_perf); |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
int DfHmc9250Wrapper::start() |
|
|
|
|
int DfHmc5883Wrapper::start() |
|
|
|
|
{ |
|
|
|
|
/* Subscribe to param update topic. */ |
|
|
|
|
if (_param_update_sub < 0) { |
|
|
|
@ -172,7 +172,7 @@ int DfHmc9250Wrapper::start()
@@ -172,7 +172,7 @@ int DfHmc9250Wrapper::start()
|
|
|
|
|
return 0; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
int DfHmc9250Wrapper::stop() |
|
|
|
|
int DfHmc5883Wrapper::stop() |
|
|
|
|
{ |
|
|
|
|
/* Stop sensor. */ |
|
|
|
|
int ret = HMC5883::stop(); |
|
|
|
@ -185,7 +185,7 @@ int DfHmc9250Wrapper::stop()
@@ -185,7 +185,7 @@ int DfHmc9250Wrapper::stop()
|
|
|
|
|
return 0; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
void DfHmc9250Wrapper::_update_mag_calibration() |
|
|
|
|
void DfHmc5883Wrapper::_update_mag_calibration() |
|
|
|
|
{ |
|
|
|
|
// TODO: replace magic number
|
|
|
|
|
for (unsigned i = 0; i < 3; ++i) { |
|
|
|
@ -251,7 +251,7 @@ void DfHmc9250Wrapper::_update_mag_calibration()
@@ -251,7 +251,7 @@ void DfHmc9250Wrapper::_update_mag_calibration()
|
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
int DfHmc9250Wrapper::_publish(struct mag_sensor_data &data) |
|
|
|
|
int DfHmc5883Wrapper::_publish(struct mag_sensor_data &data) |
|
|
|
|
{ |
|
|
|
|
/* Check if calibration values are still up-to-date. */ |
|
|
|
|
bool updated; |
|
|
|
@ -323,36 +323,36 @@ int DfHmc9250Wrapper::_publish(struct mag_sensor_data &data)
@@ -323,36 +323,36 @@ int DfHmc9250Wrapper::_publish(struct mag_sensor_data &data)
|
|
|
|
|
namespace df_hmc5883_wrapper |
|
|
|
|
{ |
|
|
|
|
|
|
|
|
|
DfHmc9250Wrapper *g_dev = nullptr; |
|
|
|
|
DfHmc5883Wrapper *g_dev = nullptr; |
|
|
|
|
|
|
|
|
|
int start(enum Rotation rotation); |
|
|
|
|
int start(enum Rotation rotation, const char *path); |
|
|
|
|
int stop(); |
|
|
|
|
int info(); |
|
|
|
|
void usage(); |
|
|
|
|
|
|
|
|
|
int start(enum Rotation rotation) |
|
|
|
|
int start(enum Rotation rotation, const char *path) |
|
|
|
|
{ |
|
|
|
|
g_dev = new DfHmc9250Wrapper(rotation); |
|
|
|
|
g_dev = new DfHmc5883Wrapper(rotation, path); |
|
|
|
|
|
|
|
|
|
if (g_dev == nullptr) { |
|
|
|
|
PX4_ERR("failed instantiating DfHmc9250Wrapper object"); |
|
|
|
|
PX4_ERR("failed instantiating DfHmc5883Wrapper object"); |
|
|
|
|
return -1; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
int ret = g_dev->start(); |
|
|
|
|
|
|
|
|
|
if (ret != 0) { |
|
|
|
|
PX4_ERR("DfHmc9250Wrapper start failed"); |
|
|
|
|
PX4_ERR("DfHmc5883Wrapper start failed"); |
|
|
|
|
return ret; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
// Open the MAG sensor
|
|
|
|
|
DevHandle h; |
|
|
|
|
DevMgr::getHandle(MAG_DEVICE_PATH, h); |
|
|
|
|
DevMgr::getHandle(path, h); |
|
|
|
|
|
|
|
|
|
if (!h.isValid()) { |
|
|
|
|
DF_LOG_INFO("Error: unable to obtain a valid handle for the receiver at: %s (%d)", |
|
|
|
|
MAG_DEVICE_PATH, h.getError()); |
|
|
|
|
path, h.getError()); |
|
|
|
|
return -1; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
@ -415,14 +415,19 @@ df_hmc5883_wrapper_main(int argc, char *argv[])
@@ -415,14 +415,19 @@ df_hmc5883_wrapper_main(int argc, char *argv[])
|
|
|
|
|
int ret = 0; |
|
|
|
|
int myoptind = 1; |
|
|
|
|
const char *myoptarg = NULL; |
|
|
|
|
const char *device_path = MAG_DEVICE_PATH; |
|
|
|
|
|
|
|
|
|
/* 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:D:", &myoptind, &myoptarg)) != EOF) { |
|
|
|
|
switch (ch) { |
|
|
|
|
case 'R': |
|
|
|
|
rotation = (enum Rotation)atoi(myoptarg); |
|
|
|
|
break; |
|
|
|
|
|
|
|
|
|
case 'D': |
|
|
|
|
device_path = myoptarg; |
|
|
|
|
break; |
|
|
|
|
|
|
|
|
|
default: |
|
|
|
|
df_hmc5883_wrapper::usage(); |
|
|
|
|
return 0; |
|
|
|
@ -438,7 +443,7 @@ df_hmc5883_wrapper_main(int argc, char *argv[])
@@ -438,7 +443,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, device_path); |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
else if (!strcmp(verb, "stop")) { |
|
|
|
|