From 493ab96b290572e47b06c17254e44d485eb2ce22 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 10 Dec 2015 09:18:19 +1100 Subject: [PATCH] AP_Compass: added QURT driver --- libraries/AP_Compass/AP_Compass_QURT.cpp | 99 ++++++++++++++++++++++++ libraries/AP_Compass/AP_Compass_QURT.h | 28 +++++++ libraries/AP_Compass/Compass.cpp | 2 + libraries/AP_Compass/Compass.h | 1 + 4 files changed, 130 insertions(+) create mode 100644 libraries/AP_Compass/AP_Compass_QURT.cpp create mode 100644 libraries/AP_Compass/AP_Compass_QURT.h diff --git a/libraries/AP_Compass/AP_Compass_QURT.cpp b/libraries/AP_Compass/AP_Compass_QURT.cpp new file mode 100644 index 0000000000..34a5d34608 --- /dev/null +++ b/libraries/AP_Compass/AP_Compass_QURT.cpp @@ -0,0 +1,99 @@ +/// -*- 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 . + */ + + +#include + +#if CONFIG_HAL_BOARD == HAL_BOARD_QURT +#include "AP_Compass_QURT.h" +#include + +extern const AP_HAL::HAL& hal; + +// Public Methods ////////////////////////////////////////////////////////////// + +// constructor +AP_Compass_QURT::AP_Compass_QURT(Compass &compass): + AP_Compass_Backend(compass) +{ +} + +// detect the sensor +AP_Compass_Backend *AP_Compass_QURT::detect(Compass &compass) +{ + AP_Compass_QURT *sensor = new AP_Compass_QURT(compass); + if (sensor == NULL) { + return NULL; + } + if (!sensor->init()) { + delete sensor; + return NULL; + } + return sensor; +} + +bool AP_Compass_QURT::init(void) +{ + instance = register_compass(); + // publish a zero as a hack + publish_filtered_field(Vector3f(), instance); + hal.scheduler->register_timer_process(FUNCTOR_BIND_MEMBER(&AP_Compass_QURT::timer_update, void)); + return true; +} + +void AP_Compass_QURT::read(void) +{ + // avoid division by zero if we haven't received any mag reports + if (count != 0) { + sum /= count; + publish_filtered_field(sum, instance); + sum.zero(); + count = 0; + } +} + +void AP_Compass_QURT::timer_update(void) +{ + // cope the data + struct mpu9x50_data data; + if (mpu9250_mag_buffer == nullptr || !mpu9250_mag_buffer->pop(data)) { + return; + } + + last_timestamp = data.timestamp; + + Vector3f raw_field(data.mag_raw[0], + data.mag_raw[1], + -data.mag_raw[2]); + + // rotate raw_field from sensor frame to body frame + rotate_field(raw_field, instance); + + // publish raw_field (uncorrected point sample) for calibration use + publish_raw_field(raw_field, data.timestamp, instance); + + // correct raw_field for known errors + correct_field(raw_field, instance); + + // publish raw_field (corrected point sample) for EKF use + publish_unfiltered_field(raw_field, data.timestamp, instance); + + // accumulate into averaging filter + sum += raw_field; + count++; +} + +#endif // CONFIG_HAL_BOARD diff --git a/libraries/AP_Compass/AP_Compass_QURT.h b/libraries/AP_Compass/AP_Compass_QURT.h new file mode 100644 index 0000000000..3d800db28a --- /dev/null +++ b/libraries/AP_Compass/AP_Compass_QURT.h @@ -0,0 +1,28 @@ +/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- + +#pragma once + +#include "Compass.h" +#include "AP_Compass_Backend.h" + +class AP_Compass_QURT : public AP_Compass_Backend +{ +public: + bool init(void) override; + void read(void) override; + + AP_Compass_QURT(Compass &compass); + + // detect the sensor + static AP_Compass_Backend *detect(Compass &compass); + +private: + void timer_update(void); + + uint8_t instance; + Vector3f sum; + uint32_t count; + uint64_t last_timestamp; +}; + + diff --git a/libraries/AP_Compass/Compass.cpp b/libraries/AP_Compass/Compass.cpp index c02092767e..244e68f128 100644 --- a/libraries/AP_Compass/Compass.cpp +++ b/libraries/AP_Compass/Compass.cpp @@ -461,6 +461,8 @@ void Compass::_detect_backends(void) _add_backend(AP_Compass_AK8963::detect_mpu9250(*this, 0)); #elif HAL_COMPASS_DEFAULT == HAL_COMPASS_QFLIGHT _add_backend(AP_Compass_QFLIGHT::detect(*this)); +#elif HAL_COMPASS_DEFAULT == HAL_COMPASS_QURT + _add_backend(AP_Compass_QURT::detect(*this)); #else #error Unrecognised HAL_COMPASS_TYPE setting #endif diff --git a/libraries/AP_Compass/Compass.h b/libraries/AP_Compass/Compass.h index 3e6137c7f5..b901410557 100644 --- a/libraries/AP_Compass/Compass.h +++ b/libraries/AP_Compass/Compass.h @@ -413,4 +413,5 @@ private: #include "AP_Compass_PX4.h" #include "AP_Compass_LSM303D.h" #include "AP_Compass_qflight.h" +#include "AP_Compass_QURT.h" #endif