41 #include <px4_platform_common/px4_config.h> 43 #include <sys/types.h> 51 #include <px4_platform_common/getopt.h> 62 #include <board_config.h> 97 int _publish(
struct mag_sensor_data &
data);
99 void _update_mag_calibration();
125 _param_update_sub(-1),
158 PX4_ERR(
"HMC5883 init fail: %d", ret);
165 PX4_ERR(
"HMC5883 start fail: %d", ret);
181 PX4_ERR(
"HMC5883 stop fail: %d", ret);
191 for (
unsigned i = 0; i < 3; ++i) {
196 (void)sprintf(str,
"CAL_MAG%u_ID", i);
201 PX4_ERR(
"Could not access param %s", str);
205 if ((uint32_t)device_id != m_id.dev_id) {
209 (void)sprintf(str,
"CAL_MAG%u_XSCALE", i);
213 PX4_ERR(
"Could not access param %s", str);
216 (void)sprintf(str,
"CAL_MAG%u_YSCALE", i);
220 PX4_ERR(
"Could not access param %s", str);
223 (void)sprintf(str,
"CAL_MAG%u_ZSCALE", i);
227 PX4_ERR(
"Could not access param %s", str);
230 (void)sprintf(str,
"CAL_MAG%u_XOFF", i);
234 PX4_ERR(
"Could not access param %s", str);
237 (void)sprintf(str,
"CAL_MAG%u_YOFF", i);
241 PX4_ERR(
"Could not access param %s", str);
244 (void)sprintf(str,
"CAL_MAG%u_ZOFF", i);
248 PX4_ERR(
"Could not access param %s", str);
272 mag_report.is_external =
true;
277 const float tmp = data.field_x_ga;
278 data.field_x_ga = -data.field_y_ga;
279 data.field_y_ga = tmp;
282 mag_report.x_raw = 0;
283 mag_report.y_raw = 0;
284 mag_report.z_raw = 0;
301 mag_report.device_id = m_id.dev_id;
304 if (!(m_pub_blocked)) {
336 if (g_dev ==
nullptr) {
337 PX4_ERR(
"failed instantiating DfHmc5883Wrapper object");
341 int ret = g_dev->
start();
344 PX4_ERR(
"DfHmc5883Wrapper start failed");
350 DevMgr::getHandle(path, h);
353 DF_LOG_INFO(
"Error: unable to obtain a valid handle for the receiver at: %s (%d)",
358 DevMgr::releaseHandle(h);
365 if (g_dev ==
nullptr) {
366 PX4_ERR(
"driver not running");
370 int ret = g_dev->
stop();
373 PX4_ERR(
"driver could not be stopped");
388 if (g_dev ==
nullptr) {
389 PX4_ERR(
"driver not running");
393 PX4_DEBUG(
"state @ %p", g_dev);
401 PX4_INFO(
"Usage: df_hmc5883_wrapper 'start', 'info', 'stop'");
402 PX4_INFO(
"options:");
403 PX4_INFO(
" -R rotation");
416 const char *myoptarg = NULL;
417 const char *device_path = MAG_DEVICE_PATH;
420 while ((ch = px4_getopt(argc, argv,
"R:D:", &myoptind, &myoptarg)) != EOF) {
423 rotation = (
enum Rotation)atoi(myoptarg);
427 device_path = myoptarg;
441 const char *verb = argv[myoptind];
444 if (!strcmp(verb,
"start")) {
448 else if (!strcmp(verb,
"stop")) {
452 else if (!strcmp(verb,
"info")) {
int orb_copy(const struct orb_metadata *meta, int handle, void *buffer)
int start(enum Rotation rotation, const char *path)
measure the time elapsed performing an event
__EXPORT int param_get(param_t param, void *val)
Copy the value of a parameter.
DfHmc5883Wrapper(enum Rotation rotation, const char *path)
matrix::Dcmf _rotation_matrix
void usage(const char *reason)
Print the correct usage.
int info()
Print a little info about the driver.
int stop()
Stop automatic measurement.
static int32_t device_id[max_accel_sens]
High-resolution timer with callouts and timekeeping.
Global flash based parameter store.
int orb_subscribe(const struct orb_metadata *meta)
#define ORB_ID(_name)
Generates a pointer to the uORB metadata structure for a given topic.
void perf_free(perf_counter_t handle)
Free a counter.
int stop()
Stop the driver.
int start()
Start automatic measurement.
Rotation
Enum for board and external compass rotations.
int _publish(struct mag_sensor_data &data)
Simple error/warning functions, heavily inspired by the BSD functions of the same names...
__EXPORT int df_hmc5883_wrapper_main(int argc, char *argv[])
void perf_end(perf_counter_t handle)
End a performance event.
__BEGIN_DECLS typedef void * orb_advert_t
ORB topic advertiser handle.
int orb_publish(const struct orb_metadata *meta, orb_advert_t handle, const void *data)
__EXPORT param_t param_find(const char *name)
Look up a parameter by name.
void _update_mag_calibration()
int orb_check(int handle, bool *updated)
__EXPORT matrix::Dcmf get_rot_matrix(enum Rotation rot)
Get the rotation matrix.
void start()
Initialise the automatic measurement state machine and start it.
void usage()
Prints info about the driver argument usage.
orb_advert_t orb_advertise_multi(const struct orb_metadata *meta, const void *data, int *instance, int priority)
struct DfHmc5883Wrapper::mag_calibration_s _mag_calibration
void stop()
Stop the automatic measurement state machine.
void perf_begin(perf_counter_t handle)
Begin a performance event.
perf_counter_t _mag_sample_perf
__EXPORT hrt_abstime hrt_absolute_time(void)
Get absolute time in [us] (does not wrap).
int _mag_orb_class_instance
Performance measuring tools.