diff --git a/libraries/AP_Compass/AP_Compass.h b/libraries/AP_Compass/AP_Compass.h
index ebc458f0713f52ab4abe6e120cc6975d3b9d41c6..83112a359eb353593de69071138c00278c904f05 100644
--- a/libraries/AP_Compass/AP_Compass.h
+++ b/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"
diff --git a/libraries/AP_Compass/AP_Compass_VRBRAIN.cpp b/libraries/AP_Compass/AP_Compass_VRBRAIN.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..de3a809ac73fde2e8aba3cf8675bc272357f10c7
--- /dev/null
+++ b/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
diff --git a/libraries/AP_Compass/AP_Compass_VRBRAIN.h b/libraries/AP_Compass/AP_Compass_VRBRAIN.h
new file mode 100644
index 0000000000000000000000000000000000000000..0a560f926c7d929e06a3de489624635ced294876
--- /dev/null
+++ b/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
+
diff --git a/libraries/AP_Compass/Compass.h b/libraries/AP_Compass/Compass.h
index 418368456b7ba1e23bb2d9f53e448a65cdd41172..7efe596c4edd14fbc183e8c59020d42c82c438ad 100644
--- a/libraries/AP_Compass/Compass.h
+++ b/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