PX4 Firmware
PX4 Autopilot Software http://px4.io
df_hmc5883_wrapper.cpp
Go to the documentation of this file.
1 /****************************************************************************
2  *
3  * Copyright (c) 2016 PX4 Development Team. All rights reserved.
4  *
5  * Redistribution and use in source and binary forms, with or without
6  * modification, are permitted provided that the following conditions
7  * are met:
8  *
9  * 1. Redistributions of source code must retain the above copyright
10  * notice, this list of conditions and the following disclaimer.
11  * 2. Redistributions in binary form must reproduce the above copyright
12  * notice, this list of conditions and the following disclaimer in
13  * the documentation and/or other materials provided with the
14  * distribution.
15  * 3. Neither the name PX4 nor the names of its contributors may be
16  * used to endorse or promote products derived from this software
17  * without specific prior written permission.
18  *
19  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
20  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
21  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
22  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
23  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
24  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
25  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
26  * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
27  * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
28  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
29  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
30  * POSSIBILITY OF SUCH DAMAGE.
31  *
32  ****************************************************************************/
33 
34 /**
35  * @file df_hmc5883_wrapper.cpp
36  * Lightweight driver to access the HMC5883 of the DriverFramework.
37  *
38  * @author Julian Oes <julian@oes.ch>
39  */
40 
41 #include <px4_platform_common/px4_config.h>
42 
43 #include <sys/types.h>
44 #include <sys/stat.h>
45 #include <stdint.h>
46 #include <stddef.h>
47 #include <stdlib.h>
48 #include <string.h>
49 #include <math.h>
50 #include <unistd.h>
51 #include <px4_platform_common/getopt.h>
52 #include <errno.h>
53 #include <lib/parameters/param.h>
54 #include <perf/perf_counter.h>
55 #include <systemlib/err.h>
56 
57 #include <drivers/drv_mag.h>
58 #include <drivers/drv_hrt.h>
59 
61 
62 #include <board_config.h>
63 
65 
66 #include <hmc5883/HMC5883.hpp>
67 #include <DevMgr.hpp>
68 
69 
70 extern "C" { __EXPORT int df_hmc5883_wrapper_main(int argc, char *argv[]); }
71 
72 using namespace DriverFramework;
73 
74 
75 class DfHmc5883Wrapper : public HMC5883
76 {
77 public:
78  DfHmc5883Wrapper(enum Rotation rotation, const char *path);
80 
81 
82  /**
83  * Start automatic measurement.
84  *
85  * @return 0 on success
86  */
87  int start();
88 
89  /**
90  * Stop automatic measurement.
91  *
92  * @return 0 on success
93  */
94  int stop();
95 
96 private:
97  int _publish(struct mag_sensor_data &data);
98 
99  void _update_mag_calibration();
100 
102 
104 
106  float x_offset;
107  float x_scale;
108  float y_offset;
109  float y_scale;
110  float z_offset;
111  float z_scale;
112  } _mag_calibration;
113 
115 
117 
119 
120 };
121 
122 DfHmc5883Wrapper::DfHmc5883Wrapper(enum Rotation rotation, const char *path) :
123  HMC5883(path),
124  _mag_topic(nullptr),
125  _param_update_sub(-1),
126  _mag_calibration{},
128  _mag_sample_perf(perf_alloc(PC_ELAPSED, "df_mag_read"))
129 {
130  // Set sane default calibration values
131  _mag_calibration.x_scale = 1.0f;
132  _mag_calibration.y_scale = 1.0f;
133  _mag_calibration.z_scale = 1.0f;
134  _mag_calibration.x_offset = 0.0f;
135  _mag_calibration.y_offset = 0.0f;
136  _mag_calibration.z_offset = 0.0f;
137 
138  // Get sensor rotation matrix
139  _rotation_matrix = get_rot_matrix(rotation);
140 }
141 
143 {
145 }
146 
148 {
149  /* Subscribe to param update topic. */
150  if (_param_update_sub < 0) {
151  _param_update_sub = orb_subscribe(ORB_ID(parameter_update));
152  }
153 
154  /* Init device and start sensor. */
155  int ret = init();
156 
157  if (ret != 0) {
158  PX4_ERR("HMC5883 init fail: %d", ret);
159  return ret;
160  }
161 
162  ret = HMC5883::start();
163 
164  if (ret != 0) {
165  PX4_ERR("HMC5883 start fail: %d", ret);
166  return ret;
167  }
168 
169  /* Force getting the calibration values. */
171 
172  return 0;
173 }
174 
176 {
177  /* Stop sensor. */
178  int ret = HMC5883::stop();
179 
180  if (ret != 0) {
181  PX4_ERR("HMC5883 stop fail: %d", ret);
182  return ret;
183  }
184 
185  return 0;
186 }
187 
189 {
190  // TODO: replace magic number
191  for (unsigned i = 0; i < 3; ++i) {
192 
193  // TODO: remove printfs and add error counter
194 
195  char str[30];
196  (void)sprintf(str, "CAL_MAG%u_ID", i);
197  int32_t device_id;
198  int res = param_get(param_find(str), &device_id);
199 
200  if (res != OK) {
201  PX4_ERR("Could not access param %s", str);
202  continue;
203  }
204 
205  if ((uint32_t)device_id != m_id.dev_id) {
206  continue;
207  }
208 
209  (void)sprintf(str, "CAL_MAG%u_XSCALE", i);
211 
212  if (res != OK) {
213  PX4_ERR("Could not access param %s", str);
214  }
215 
216  (void)sprintf(str, "CAL_MAG%u_YSCALE", i);
218 
219  if (res != OK) {
220  PX4_ERR("Could not access param %s", str);
221  }
222 
223  (void)sprintf(str, "CAL_MAG%u_ZSCALE", i);
225 
226  if (res != OK) {
227  PX4_ERR("Could not access param %s", str);
228  }
229 
230  (void)sprintf(str, "CAL_MAG%u_XOFF", i);
232 
233  if (res != OK) {
234  PX4_ERR("Could not access param %s", str);
235  }
236 
237  (void)sprintf(str, "CAL_MAG%u_YOFF", i);
239 
240  if (res != OK) {
241  PX4_ERR("Could not access param %s", str);
242  }
243 
244  (void)sprintf(str, "CAL_MAG%u_ZOFF", i);
246 
247  if (res != OK) {
248  PX4_ERR("Could not access param %s", str);
249  }
250  }
251 }
252 
253 
254 int DfHmc5883Wrapper::_publish(struct mag_sensor_data &data)
255 {
256  /* Check if calibration values are still up-to-date. */
257  bool updated;
258  orb_check(_param_update_sub, &updated);
259 
260  if (updated) {
261  parameter_update_s parameter_update;
262  orb_copy(ORB_ID(parameter_update), _param_update_sub, &parameter_update);
263 
265  }
266 
267  /* Publish mag first. */
269 
270  mag_report mag_report = {};
271  mag_report.timestamp = hrt_absolute_time();
272  mag_report.is_external = true;
273 
274  /* The standard external mag by 3DR has x pointing to the
275  * right, y pointing backwards, and z down, therefore switch x
276  * and y and invert y. */
277  const float tmp = data.field_x_ga;
278  data.field_x_ga = -data.field_y_ga;
279  data.field_y_ga = tmp;
280 
281  // TODO: remove these (or get the values)
282  mag_report.x_raw = 0;
283  mag_report.y_raw = 0;
284  mag_report.z_raw = 0;
285 
286  matrix::Vector3f mag_val(data.field_x_ga,
287  data.field_y_ga,
288  data.field_z_ga);
289 
290  // apply sensor rotation on the accel measurement
291  mag_val = _rotation_matrix * mag_val;
292 
293  // Apply calibration after rotation.
294  mag_report.x = (mag_val(0) - _mag_calibration.x_offset) * _mag_calibration.x_scale;
295  mag_report.y = (mag_val(1) - _mag_calibration.y_offset) * _mag_calibration.y_scale;
296  mag_report.z = (mag_val(2) - _mag_calibration.z_offset) * _mag_calibration.z_scale;
297 
298  // TODO: get these right
299  //mag_report.scaling = -1.0f;
300 
301  mag_report.device_id = m_id.dev_id;
302 
303  // TODO: when is this ever blocked?
304  if (!(m_pub_blocked)) {
305 
306  if (_mag_topic == nullptr) {
307  _mag_topic = orb_advertise_multi(ORB_ID(sensor_mag), &mag_report,
309 
310  } else {
311  orb_publish(ORB_ID(sensor_mag), _mag_topic, &mag_report);
312  }
313 
314  }
315 
317 
318  return 0;
319 };
320 
321 
323 {
324 
326 
327 int start(enum Rotation rotation, const char *path);
328 int stop();
329 int info();
330 void usage();
331 
332 int start(enum Rotation rotation, const char *path)
333 {
334  g_dev = new DfHmc5883Wrapper(rotation, path);
335 
336  if (g_dev == nullptr) {
337  PX4_ERR("failed instantiating DfHmc5883Wrapper object");
338  return -1;
339  }
340 
341  int ret = g_dev->start();
342 
343  if (ret != 0) {
344  PX4_ERR("DfHmc5883Wrapper start failed");
345  return ret;
346  }
347 
348  // Open the MAG sensor
349  DevHandle h;
350  DevMgr::getHandle(path, h);
351 
352  if (!h.isValid()) {
353  DF_LOG_INFO("Error: unable to obtain a valid handle for the receiver at: %s (%d)",
354  path, h.getError());
355  return -1;
356  }
357 
358  DevMgr::releaseHandle(h);
359 
360  return 0;
361 }
362 
363 int stop()
364 {
365  if (g_dev == nullptr) {
366  PX4_ERR("driver not running");
367  return 1;
368  }
369 
370  int ret = g_dev->stop();
371 
372  if (ret != 0) {
373  PX4_ERR("driver could not be stopped");
374  return ret;
375  }
376 
377  delete g_dev;
378  g_dev = nullptr;
379  return 0;
380 }
381 
382 /**
383  * Print a little info about the driver.
384  */
385 int
387 {
388  if (g_dev == nullptr) {
389  PX4_ERR("driver not running");
390  return 1;
391  }
392 
393  PX4_DEBUG("state @ %p", g_dev);
394 
395  return 0;
396 }
397 
398 void
400 {
401  PX4_INFO("Usage: df_hmc5883_wrapper 'start', 'info', 'stop'");
402  PX4_INFO("options:");
403  PX4_INFO(" -R rotation");
404 }
405 
406 } // namespace df_hmc5883_wrapper
407 
408 
409 int
410 df_hmc5883_wrapper_main(int argc, char *argv[])
411 {
412  int ch;
413  enum Rotation rotation = ROTATION_NONE;
414  int ret = 0;
415  int myoptind = 1;
416  const char *myoptarg = NULL;
417  const char *device_path = MAG_DEVICE_PATH;
418 
419  /* jump over start/off/etc and look at options first */
420  while ((ch = px4_getopt(argc, argv, "R:D:", &myoptind, &myoptarg)) != EOF) {
421  switch (ch) {
422  case 'R':
423  rotation = (enum Rotation)atoi(myoptarg);
424  break;
425 
426  case 'D':
427  device_path = myoptarg;
428  break;
429 
430  default:
432  return 0;
433  }
434  }
435 
436  if (argc <= 1) {
438  return 1;
439  }
440 
441  const char *verb = argv[myoptind];
442 
443 
444  if (!strcmp(verb, "start")) {
445  ret = df_hmc5883_wrapper::start(rotation, device_path);
446  }
447 
448  else if (!strcmp(verb, "stop")) {
449  ret = df_hmc5883_wrapper::stop();
450  }
451 
452  else if (!strcmp(verb, "info")) {
453  ret = df_hmc5883_wrapper::info();
454  }
455 
456  else {
458  return 1;
459  }
460 
461  return ret;
462 }
int orb_copy(const struct orb_metadata *meta, int handle, void *buffer)
Definition: uORB.cpp:90
int start(enum Rotation rotation, const char *path)
DfHmc5883Wrapper * g_dev
measure the time elapsed performing an event
Definition: perf_counter.h:56
__EXPORT int param_get(param_t param, void *val)
Copy the value of a parameter.
Definition: parameters.cpp:589
DfHmc5883Wrapper(enum Rotation rotation, const char *path)
matrix::Dcmf _rotation_matrix
void usage(const char *reason)
Print the correct usage.
Definition: Commander.cpp:4238
int info()
Print a little info about the driver.
Definition: I2C.hpp:51
static void stop()
Definition: dataman.cpp:1491
int stop()
Stop automatic measurement.
static int32_t device_id[max_accel_sens]
Vector rotation library.
High-resolution timer with callouts and timekeeping.
Global flash based parameter store.
int orb_subscribe(const struct orb_metadata *meta)
Definition: uORB.cpp:75
#define ORB_ID(_name)
Generates a pointer to the uORB metadata structure for a given topic.
Definition: uORB.h:87
Header common to all counters.
void perf_free(perf_counter_t handle)
Free a counter.
int stop()
Stop the driver.
int start()
Start automatic measurement.
#define perf_alloc(a, b)
Definition: px4io.h:59
Rotation
Enum for board and external compass rotations.
Definition: rotation.h:51
int _publish(struct mag_sensor_data &data)
uint8_t * data
Definition: dataman.cpp:149
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.
Definition: uORB.h:134
int orb_publish(const struct orb_metadata *meta, orb_advert_t handle, const void *data)
Definition: uORB.cpp:70
__EXPORT param_t param_find(const char *name)
Look up a parameter by name.
Definition: parameters.cpp:370
static int start()
Definition: dataman.cpp:1452
virtual int init()
Definition: HMC5883.cpp:95
int orb_check(int handle, bool *updated)
Definition: uORB.cpp:95
__EXPORT matrix::Dcmf get_rot_matrix(enum Rotation rot)
Get the rotation matrix.
Definition: rotation.cpp:45
void start()
Initialise the automatic measurement state machine and start it.
Definition: HMC5883.cpp:392
#define OK
Definition: uavcan_main.cpp:71
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)
Definition: uORB.cpp:53
struct DfHmc5883Wrapper::mag_calibration_s _mag_calibration
void stop()
Stop the automatic measurement state machine.
Definition: HMC5883.cpp:403
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).
Performance measuring tools.
#define mag_report
Definition: drv_mag.h:53