From 3b546bb2429b7b68b593e885a45db5f24c64f351 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 14 Dec 2015 16:00:15 +1100 Subject: [PATCH] AP_Compass: added qflight driver --- libraries/AP_Compass/AP_Compass_qflight.cpp | 112 ++++++++++++++++++++ libraries/AP_Compass/AP_Compass_qflight.h | 32 ++++++ libraries/AP_Compass/Compass.cpp | 9 +- libraries/AP_Compass/Compass.h | 1 + 4 files changed, 151 insertions(+), 3 deletions(-) create mode 100644 libraries/AP_Compass/AP_Compass_qflight.cpp create mode 100644 libraries/AP_Compass/AP_Compass_qflight.h diff --git a/libraries/AP_Compass/AP_Compass_qflight.cpp b/libraries/AP_Compass/AP_Compass_qflight.cpp new file mode 100644 index 0000000000..e583053765 --- /dev/null +++ b/libraries/AP_Compass/AP_Compass_qflight.cpp @@ -0,0 +1,112 @@ +/// -*- 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_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_QFLIGHT + +#include "AP_Compass_qflight.h" +#include +#include +#include "AP_Compass_qflight.h" +#include + +extern const AP_HAL::HAL& hal; + +// constructor +AP_Compass_QFLIGHT::AP_Compass_QFLIGHT(Compass &compass): + AP_Compass_Backend(compass) +{ +} + +// detect the sensor +AP_Compass_Backend *AP_Compass_QFLIGHT::detect(Compass &compass) +{ + AP_Compass_QFLIGHT *sensor = new AP_Compass_QFLIGHT(compass); + if (sensor == NULL) { + return NULL; + } + if (!sensor->init()) { + delete sensor; + return NULL; + } + return sensor; +} + +bool AP_Compass_QFLIGHT::init(void) +{ + instance = register_compass(); + hal.scheduler->register_timer_process(FUNCTOR_BIND_MEMBER(&AP_Compass_QFLIGHT::timer_update, void)); + // give time for at least one sample + hal.scheduler->delay(100); + return true; +} + +void AP_Compass_QFLIGHT::read(void) +{ + if (count > 0) { + hal.scheduler->suspend_timer_procs(); + publish_filtered_field(sum/count, instance); + sum.zero(); + count = 0; + hal.scheduler->resume_timer_procs(); + } +} + +void AP_Compass_QFLIGHT::timer_update(void) +{ + uint32_t now = AP_HAL::millis(); + if (now - last_check_ms < 25) { + return; + } + last_check_ms = now; + + if (magbuf == nullptr) { + magbuf = QFLIGHT_RPC_ALLOCATE(DSPBuffer::MAG); + if (magbuf == nullptr) { + AP_HAL::panic("unable to allocate MAG buffer"); + } + } + int ret = qflight_get_mag_data((uint8_t *)magbuf, sizeof(*magbuf)); + if (ret != 0) { + return; + } + uint64_t time_us = AP_HAL::micros64(); + for (uint16_t i=0; inum_samples; i++) { + DSPBuffer::MAG::BUF &b = magbuf->buf[i]; + + // get raw_field - sensor frame, uncorrected + Vector3f raw_field(b.mag_raw[0], b.mag_raw[1], -b.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, time_us, 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, time_us, instance); + + // accumulate into averaging filter + sum += raw_field; + count++; + } +} + +#endif // CONFIG_HAL_BOARD_SUBTYPE + diff --git a/libraries/AP_Compass/AP_Compass_qflight.h b/libraries/AP_Compass/AP_Compass_qflight.h new file mode 100644 index 0000000000..be4cfff46f --- /dev/null +++ b/libraries/AP_Compass/AP_Compass_qflight.h @@ -0,0 +1,32 @@ +/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- + +#pragma once + +#include "Compass.h" +#include "AP_Compass_Backend.h" + +#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_QFLIGHT + +#include + +class AP_Compass_QFLIGHT : public AP_Compass_Backend +{ +public: + bool init(void); + void read(void); + + AP_Compass_QFLIGHT(Compass &compass); + + // detect the sensor + static AP_Compass_Backend *detect(Compass &compass); + +private: + void timer_update(); + DSPBuffer::MAG *magbuf; + uint8_t instance; + Vector3f sum; + uint32_t count; + uint32_t last_check_ms; +}; + +#endif // CONFIG_HAL_BOARD_SUBTYPE diff --git a/libraries/AP_Compass/Compass.cpp b/libraries/AP_Compass/Compass.cpp index b043ad60b6..c02092767e 100644 --- a/libraries/AP_Compass/Compass.cpp +++ b/libraries/AP_Compass/Compass.cpp @@ -436,9 +436,10 @@ void Compass::_detect_backends(void) } else { _add_backend(AP_Compass_AK8963::detect_mpu9250(*this, 0)); } -#elif CONFIG_HAL_BOARD == HAL_BOARD_LINUX && \ - CONFIG_HAL_BOARD_SUBTYPE != HAL_BOARD_SUBTYPE_LINUX_NONE && \ - CONFIG_HAL_BOARD_SUBTYPE != HAL_BOARD_SUBTYPE_LINUX_BEBOP && \ +#elif CONFIG_HAL_BOARD == HAL_BOARD_LINUX && \ + CONFIG_HAL_BOARD_SUBTYPE != HAL_BOARD_SUBTYPE_LINUX_NONE && \ + CONFIG_HAL_BOARD_SUBTYPE != HAL_BOARD_SUBTYPE_LINUX_BEBOP && \ + CONFIG_HAL_BOARD_SUBTYPE != HAL_BOARD_SUBTYPE_LINUX_QFLIGHT && \ CONFIG_HAL_BOARD_SUBTYPE != HAL_BOARD_SUBTYPE_LINUX_MINLURE _add_backend(AP_Compass_HMC5843::detect_i2c(*this, hal.i2c)); _add_backend(AP_Compass_AK8963::detect_mpu9250(*this, 0)); @@ -458,6 +459,8 @@ void Compass::_detect_backends(void) _add_backend(AP_Compass_PX4::detect(*this)); #elif HAL_COMPASS_DEFAULT == HAL_COMPASS_AK8963_MPU9250 _add_backend(AP_Compass_AK8963::detect_mpu9250(*this, 0)); +#elif HAL_COMPASS_DEFAULT == HAL_COMPASS_QFLIGHT + _add_backend(AP_Compass_QFLIGHT::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 449fcee56b..3e6137c7f5 100644 --- a/libraries/AP_Compass/Compass.h +++ b/libraries/AP_Compass/Compass.h @@ -412,4 +412,5 @@ private: #include "AP_Compass_AK8963.h" #include "AP_Compass_PX4.h" #include "AP_Compass_LSM303D.h" +#include "AP_Compass_qflight.h" #endif