diff --git a/libraries/AP_Baro/AP_Baro.cpp b/libraries/AP_Baro/AP_Baro.cpp index 4dd7863396..974989a132 100644 --- a/libraries/AP_Baro/AP_Baro.cpp +++ b/libraries/AP_Baro/AP_Baro.cpp @@ -29,6 +29,7 @@ #include "AP_Baro_MS5611.h" #include "AP_Baro_PX4.h" #include "AP_Baro_qflight.h" +#include "AP_Baro_QURT.h" extern const AP_HAL::HAL& hal; @@ -333,6 +334,11 @@ void AP_Baro::init(void) drivers[0] = new AP_Baro_QFLIGHT(*this); _num_drivers = 1; } +#elif HAL_BARO_DEFAULT == HAL_BARO_QURT + { + drivers[0] = new AP_Baro_QURT(*this); + _num_drivers = 1; + } #endif if (_num_drivers == 0 || _num_sensors == 0 || drivers[0] == NULL) { AP_HAL::panic("Baro: unable to initialise driver"); diff --git a/libraries/AP_Baro/AP_Baro_QURT.cpp b/libraries/AP_Baro/AP_Baro_QURT.cpp new file mode 100644 index 0000000000..61228f6058 --- /dev/null +++ b/libraries/AP_Baro/AP_Baro_QURT.cpp @@ -0,0 +1,84 @@ +/// -*- 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 "AP_Baro_QURT.h" + +#include +#include + +#if CONFIG_HAL_BOARD == HAL_BOARD_QURT + +extern const AP_HAL::HAL &hal; + +AP_Baro_QURT::AP_Baro_QURT(AP_Baro &baro) + : AP_Baro_Backend(baro) +{ + instance = _frontend.register_sensor(); + hal.scheduler->register_timer_process(FUNCTOR_BIND_MEMBER(&AP_Baro_QURT::timer, void)); +} + +/* + transfer data to the frontend + */ +void AP_Baro_QURT::update(void) +{ + if (count == 0) { + return; + } + + if (lock.take(1)) { + float temperature = temp_sum / count; + float pressure = press_sum / count; + + count = 0; + temp_sum = 0; + press_sum = 0; + + _copy_to_frontend(instance, pressure, temperature); + lock.give(); + } +} + +void AP_Baro_QURT::timer(void) +{ + if (handle == 0) { + int ret = bmp280_open("/dev/i2c-2", &handle); + if (ret != 0) { + AP_HAL::panic("unable to open QURT baro"); + return; + } + last_timer_ms = AP_HAL::millis(); + return; + } + if (AP_HAL::millis() - last_timer_ms < 10) { + return; + } + if (lock.take(1)) { + last_timer_ms = AP_HAL::millis(); + struct bmp280_sensor_data data; + int ret = bmp280_get_sensor_data(handle, &data, false); + if (ret != 0 || data.sensor_read_counter == last_counter) { + lock.give(); + return; + } + last_counter = data.sensor_read_counter; + temp_sum += data.temperature_in_c; + press_sum += data.pressure_in_pa; + count++; + lock.give(); + } +} + +#endif // CONFIG_HAL_BOARD diff --git a/libraries/AP_Baro/AP_Baro_QURT.h b/libraries/AP_Baro/AP_Baro_QURT.h new file mode 100644 index 0000000000..d594e3e458 --- /dev/null +++ b/libraries/AP_Baro/AP_Baro_QURT.h @@ -0,0 +1,34 @@ +/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- +#pragma once + +#include "AP_Baro_Backend.h" +#include + +#if CONFIG_HAL_BOARD == HAL_BOARD_QURT +extern "C" { +#include "bmp280_api.h" +} + +class AP_Baro_QURT : public AP_Baro_Backend +{ +public: + // Constructor + AP_Baro_QURT(AP_Baro &baro); + + /* AP_Baro public interface: */ + void update() override; + +private: + void timer(void); + + uint32_t instance; + uint32_t handle; + uint32_t last_timer_ms; + uint64_t last_counter; + + uint32_t count; + float temp_sum; + float press_sum; + QURT::Semaphore lock; +}; +#endif // CONFIG_HAL_BOARD