AP_Compass: first cut at a PX4 compass driver

libraries/AP_Compass/AP_Compass.h

@@ -5,3 +5,4 @@
 
 #include "AP_Compass_HMC5843.h"
 #include "AP_Compass_HIL.h"
+#include "AP_Compass_PX4.h"

libraries/AP_Compass/AP_Compass_PX4.cpp

@@ -0,0 +1,102 @@
+/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+/*
+ *       AP_Compass_PX4.cpp - Arduino Library for PX4 magnetometer
+ *
+ *       This library is free software; you can redistribute it and / or
+ *       modify it under the terms of the GNU Lesser General Public
+ *       License as published by the Free Software Foundation; either
+ *       version 2.1 of the License, or (at your option) any later version.
+ */
+
+
+#include <AP_HAL.h>
+
+#if CONFIG_HAL_BOARD == HAL_BOARD_PX4
+#include "AP_Compass_PX4.h"
+
+#include <sys/types.h>
+#include <sys/stat.h>
+#include <fcntl.h>
+#include <unistd.h>
+
+#include <drivers/drv_mag.h>
+#include <poll.h>
+#include <stdio.h>
+#include <errno.h>
+
+extern const AP_HAL::HAL& hal;
+
+// Public Methods //////////////////////////////////////////////////////////////
+
+bool AP_Compass_PX4::init(void)
+{
+    hal.console->printf("PX4 compass init started\n");
+	_mag_fd = open(MAG_DEVICE_PATH, O_RDONLY);
+	if (_mag_fd < 0) {
+        hal.console->printf("Unable to open " MAG_DEVICE_PATH);
+        return false;
+	}
+
+	/* set the mag internal poll rate to at least 150Hz */
+	ioctl(_mag_fd, MAGIOCSSAMPLERATE, 150);
+
+	/* set the driver to poll at 150Hz */
+	ioctl(_mag_fd, SENSORIOCSPOLLRATE, 150);
+
+    healthy = false;
+    _count = 0;
+    _sum.zero();
+
+    hal.console->printf("PX4 compass init done\n");
+
+    return true;
+}
+
+bool AP_Compass_PX4::read(void)
+{
+    while (_count == 0) {
+        accumulate();
+        if (_count == 0) {
+            hal.scheduler->delay(1);
+        }
+    }
+
+    _sum /= _count;
+    _sum *= 1000;
+    _sum.rotate(_orientation);
+
+    _sum += _offset.get();
+
+    mag_x = _sum.x;
+    mag_y = _sum.y;
+    mag_z = _sum.z;
+
+    _sum.zero();
+    _count = 0;
+
+    last_update = hal.scheduler->micros();
+
+    return true;
+}
+
+void AP_Compass_PX4::accumulate(void)
+{
+    struct pollfd fds;
+    int ret;
+
+    fds.fd = _mag_fd;
+    fds.events = POLLIN;
+    ret = poll(&fds, 1, 0);
+
+	if (ret == 1) {
+        // data is available
+		struct mag_report mag_report;
+        if (::read(_mag_fd, &mag_report, sizeof(mag_report)) == sizeof(mag_report)) {
+            _sum += Vector3f(mag_report.x, mag_report.y, mag_report.z);
+            _count++;
+            healthy = true;
+        }
+	}
+}
+
+#endif // CONFIG_HAL_BOARD

libraries/AP_Compass/AP_Compass_PX4.h

@@ -0,0 +1,25 @@
+/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+
+#ifndef AP_Compass_PX4_H
+#define AP_Compass_PX4_H
+
+#include "Compass.h"
+
+class AP_Compass_PX4 : public Compass
+{
+public:
+    AP_Compass_PX4() : Compass() {
+        product_id = AP_COMPASS_TYPE_PX4;
+    }
+    bool        init(void);
+    bool        read(void);
+    void        accumulate(void);
+
+private:
+    int _mag_fd;
+    Vector3f _sum;
+    uint32_t _count;
+};
+
+#endif // AP_Compass_PX4_H
+

libraries/AP_Compass/Compass.h

@@ -13,6 +13,7 @@
 #define AP_COMPASS_TYPE_HIL      0x01
 #define AP_COMPASS_TYPE_HMC5843  0x02
 #define AP_COMPASS_TYPE_HMC5883L 0x03
+#define AP_COMPASS_TYPE_PX4      0x04
 
 class Compass
 {

libraries/AP_Compass/examples/AP_Compass_test/AP_Compass_test.pde

@@ -8,6 +8,8 @@
 #include <AP_Param.h>
 #include <AP_HAL.h>
 #include <AP_HAL_AVR.h>
+#include <AP_HAL_PX4.h>
+#include <AP_HAL_Empty.h>
 
 #include <AP_Math.h>    // ArduPilot Mega Vector/Matrix math Library
 #include <AP_Declination.h>
@@ -15,16 +17,21 @@
 
 const AP_HAL::HAL& hal = AP_HAL_BOARD_DRIVER;
 
+#if CONFIG_HAL_BOARD == HAL_BOARD_PX4
+AP_Compass_PX4 compass;
+#else
 AP_Compass_HMC5843 compass;
+#endif
 uint32_t timer;
 
 void setup() {
-    hal.console->println("Compass library test (HMC5843 and HMC5883L)");
+    hal.console->println("Compass library test");
 
     if (!compass.init()) {
         hal.console->println("compass initialisation failed!");
         while (1) ;
     }
+    hal.console->println("init done");
 
     compass.set_orientation(AP_COMPASS_COMPONENTS_DOWN_PINS_FORWARD); // set compass's orientation on aircraft.
     compass.set_offsets(0,0,0); // set offsets to account for surrounding interference
@@ -41,6 +48,9 @@ void setup() {
     case AP_COMPASS_TYPE_HMC5883L:
         hal.console->println("HMC5883L");
         break;
+    case AP_COMPASS_TYPE_PX4:
+        hal.console->println("PX4");
+        break;
     default:
         hal.console->println("unknown");
         break;
@@ -92,12 +102,12 @@ void loop()
         offset[2] = -(max[2]+min[2])/2;
 
         // display all to user
-        hal.console->printf("Heading: %.2f (%3u,%3u,%3u) i2c error: %u",
-                      ToDeg(heading),
-                      compass.mag_x,
-                      compass.mag_y,
-                      compass.mag_z, 
-		              hal.i2c->lockup_count());
+        hal.console->printf("Heading: %.2f (%3d,%3d,%3d) i2c error: %u",
+			    ToDeg(heading),
+			    (int)compass.mag_x,
+			    (int)compass.mag_y,
+			    (int)compass.mag_z, 
+			    (unsigned)hal.i2c->lockup_count());
 
         // display offsets
         hal.console->printf(" offsets(%.2f, %.2f, %.2f)",
@@ -106,6 +116,8 @@ void loop()
         hal.console->printf(" t=%u", (unsigned)read_time);
 
         hal.console->println();
+    } else {
+	    hal.scheduler->delay(1);
     }
 }