AP_Compass: new files and definition for VRBRAIN board

libraries/AP_Compass/AP_Compass.h

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

libraries/AP_Compass/AP_Compass_VRBRAIN.cpp

@@ -0,0 +1,155 @@
+/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+/*
+   This program is free software: you can redistribute it and/or modify
+   it under the terms of the GNU General Public License as published by
+   the Free Software Foundation, either version 3 of the License, or
+   (at your option) any later version.
+
+   This program is distributed in the hope that it will be useful,
+   but WITHOUT ANY WARRANTY; without even the implied warranty of
+   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+   GNU General Public License for more details.
+
+   You should have received a copy of the GNU General Public License
+   along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+/*
+ *       AP_Compass_VRBRAIN.cpp - Arduino Library for VRBRAIN magnetometer
+ *
+ */
+
+
+#include <AP_HAL.h>
+
+#if CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
+#include "AP_Compass_VRBRAIN.h"
+
+#include <sys/types.h>
+#include <sys/stat.h>
+#include <fcntl.h>
+#include <unistd.h>
+
+#include <drivers/drv_mag.h>
+#include <drivers/drv_hrt.h>
+#include <stdio.h>
+#include <errno.h>
+
+extern const AP_HAL::HAL& hal;
+
+
+// Public Methods //////////////////////////////////////////////////////////////
+
+bool AP_Compass_VRBRAIN::init(void)
+{
+	_mag_fd[0] = open(MAG_DEVICE_PATH, O_RDONLY);
+	if (_mag_fd[0] < 0) {
+        hal.console->printf("Unable to open " MAG_DEVICE_PATH "\n");
+        return false;
+	}
+
+	_mag_fd[1] = open(MAG_DEVICE_PATH "1", O_RDONLY);
+	if (_mag_fd[1] >= 0) {
+        _num_instances = 2;
+	} else {
+        _num_instances = 1;
+    }
+
+    for (uint8_t i=0; i<_num_instances; i++) {
+        // average over up to 20 samples
+        if (ioctl(_mag_fd[i], SENSORIOCSQUEUEDEPTH, 20) != 0) {
+            hal.console->printf("Failed to setup compass queue\n");
+            return false;                
+        }
+
+        // remember if the compass is external
+        _is_external[i] = (ioctl(_mag_fd[i], MAGIOCGEXTERNAL, 0) > 0);
+        if (_is_external[i]) {
+            hal.console->printf("Using external compass[%u]\n", (unsigned)i);
+        }
+        _count[0] = 0;
+        _sum[i].zero();
+        _healthy[i] = false;
+    }
+
+    // give the driver a chance to run, and gather one sample
+    hal.scheduler->delay(40);
+    accumulate();
+    if (_count[0] == 0) {
+        hal.console->printf("Failed initial compass accumulate\n");        
+    }
+    return true;
+}
+
+bool AP_Compass_VRBRAIN::read(void)
+{
+    // try to accumulate one more sample, so we have the latest data
+    accumulate();
+
+    // consider the compass healthy if we got a reading in the last 0.2s
+    for (uint8_t i=0; i<_num_instances; i++) {
+        _healthy[i] = (hrt_absolute_time() - _last_timestamp[i] < 200000);
+    }
+
+    for (uint8_t i=0; i<_num_instances; i++) {
+        _sum[i] /= _count[i];
+        _sum[i] *= 1000;
+
+        // apply default board orientation for this compass type. This is
+        // a noop on most boards
+        _sum[i].rotate(MAG_BOARD_ORIENTATION);
+
+        // override any user setting of COMPASS_EXTERNAL 
+        _external.set(_is_external[0]);
+
+        if (_is_external[i]) {
+            // add user selectable orientation
+            _sum[i].rotate((enum Rotation)_orientation.get());
+        } else {
+            // add in board orientation from AHRS
+            _sum[i].rotate(_board_orientation);
+        }
+
+        _sum[i] += _offset[i].get();
+
+        // apply motor compensation
+        if (_motor_comp_type != AP_COMPASS_MOT_COMP_DISABLED && _thr_or_curr != 0.0f) {
+            _motor_offset[i] = _motor_compensation[i].get() * _thr_or_curr;
+            _sum[i] += _motor_offset[i];
+        } else {
+            _motor_offset[i].zero();
+        }
+    
+        _field[i] = _sum[i];
+    
+        _sum[i].zero();
+        _count[i] = 0;
+    }
+
+    last_update = _last_timestamp[0];
+    
+    return _healthy[0];
+}
+
+void AP_Compass_VRBRAIN::accumulate(void)
+{
+    struct mag_report mag_report;
+    for (uint8_t i=0; i<_num_instances; i++) {
+        while (::read(_mag_fd[i], &mag_report, sizeof(mag_report)) == sizeof(mag_report) &&
+               mag_report.timestamp != _last_timestamp[i]) {
+            _sum[i] += Vector3f(mag_report.x, mag_report.y, mag_report.z);
+            _count[i]++;
+            _last_timestamp[i] = mag_report.timestamp;
+        }
+    }
+}
+
+uint8_t AP_Compass_VRBRAIN::_get_primary(void) const
+{
+    for (uint8_t i=0; i<_num_instances; i++) {
+        if (_healthy[i]) return i;
+    }    
+    return 0;
+}
+
+#endif // CONFIG_HAL_BOARD

libraries/AP_Compass/AP_Compass_VRBRAIN.h

@@ -0,0 +1,33 @@
+/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+
+#ifndef AP_Compass_VRBRAIN_H
+#define AP_Compass_VRBRAIN_H
+
+#include "Compass.h"
+
+class AP_Compass_VRBRAIN : public Compass
+{
+public:
+    AP_Compass_VRBRAIN() : Compass() {
+        product_id = AP_COMPASS_TYPE_VRBRAIN;
+        _num_instances = 0;
+    }
+    bool        init(void);
+    bool        read(void);
+    void        accumulate(void);
+
+    // return the number of compass instances
+    uint8_t get_count(void) const { return _num_instances; }
+
+private:
+    uint8_t _get_primary(void) const;
+    uint8_t _num_instances;
+    int _mag_fd[COMPASS_MAX_INSTANCES];
+    Vector3f _sum[COMPASS_MAX_INSTANCES];
+    uint32_t _count[COMPASS_MAX_INSTANCES];
+    uint64_t _last_timestamp[COMPASS_MAX_INSTANCES];
+    bool _is_external[COMPASS_MAX_INSTANCES];
+};
+
+#endif // AP_Compass_VRBRAIN_H
+

libraries/AP_Compass/Compass.h

@@ -14,6 +14,7 @@
 #define AP_COMPASS_TYPE_HMC5843  0x02
 #define AP_COMPASS_TYPE_HMC5883L 0x03
 #define AP_COMPASS_TYPE_PX4      0x04
+#define AP_COMPASS_TYPE_VRBRAIN  0x05
 
 // motor compensation types (for use with motor_comp_enabled)
 #define AP_COMPASS_MOT_COMP_DISABLED    0x00
@@ -33,6 +34,8 @@
 # define MAG_BOARD_ORIENTATION ROTATION_NONE
 #elif CONFIG_HAL_BOARD == HAL_BOARD_LINUX
 # define MAG_BOARD_ORIENTATION ROTATION_YAW_90
+#elif CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
+# define MAG_BOARD_ORIENTATION ROTATION_NONE
 #else
 # error "You must define a default compass orientation for this board"
 #endif