Real-time collaboration for Jupyter Notebooks, Linux Terminals, LaTeX, VS Code, R IDE, and more,
all in one place. Commercial Alternative to JupyterHub.
Real-time collaboration for Jupyter Notebooks, Linux Terminals, LaTeX, VS Code, R IDE, and more,
all in one place. Commercial Alternative to JupyterHub.
Path: blob/master/libraries/AP_Airspeed/AP_Airspeed_DLVR.cpp
Views: 1798
/*1This program is free software: you can redistribute it and/or modify2it under the terms of the GNU General Public License as published by3the Free Software Foundation, either version 3 of the License, or4(at your option) any later version.56This program is distributed in the hope that it will be useful,7but WITHOUT ANY WARRANTY; without even the implied warranty of8MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the9GNU General Public License for more details.1011You should have received a copy of the GNU General Public License12along with this program. If not, see <http://www.gnu.org/licenses/>.13*/14/*15driver for DLVR differential airspeed sensor16https://www.allsensors.com/products/DLVR-L01D17*/1819#include "AP_Airspeed_DLVR.h"2021#if AP_AIRSPEED_DLVR_ENABLED2223#include <AP_Math/AP_Math.h>2425extern const AP_HAL::HAL &hal;2627#define DLVR_I2C_ADDR 0x282829#ifdef DLVR_DEBUGGING30# define Debug(fmt, args ...) do {hal.console->printf("%s:%d: " fmt "\n", __FUNCTION__, __LINE__, ## args); hal.scheduler->delay(1); } while(0)31#else32# define Debug(fmt, args ...)33#endif343536AP_Airspeed_DLVR::AP_Airspeed_DLVR(AP_Airspeed &_frontend, uint8_t _instance, const float _range_inH2O) :37AP_Airspeed_Backend(_frontend, _instance),38range_inH2O(_range_inH2O)39{}4041/*42probe for a sensor on a given i2c address43*/44AP_Airspeed_Backend *AP_Airspeed_DLVR::probe(AP_Airspeed &_frontend,45uint8_t _instance,46AP_HAL::OwnPtr<AP_HAL::I2CDevice> _dev,47const float _range_inH2O)48{49if (!_dev) {50return nullptr;51}52AP_Airspeed_DLVR *sensor = NEW_NOTHROW AP_Airspeed_DLVR(_frontend, _instance, _range_inH2O);53if (!sensor) {54return nullptr;55}56sensor->dev = std::move(_dev);57sensor->setup();58return sensor;59}6061// initialise the sensor62void AP_Airspeed_DLVR::setup()63{64WITH_SEMAPHORE(dev->get_semaphore());65dev->set_speed(AP_HAL::Device::SPEED_LOW);66dev->set_retries(2);67dev->set_device_type(uint8_t(DevType::DLVR));68set_bus_id(dev->get_bus_id());6970dev->register_periodic_callback(1000000UL/50U,71FUNCTOR_BIND_MEMBER(&AP_Airspeed_DLVR::timer, void));72}7374// probe and initialise the sensor75bool AP_Airspeed_DLVR::init()76{77dev = hal.i2c_mgr->get_device(get_bus(), DLVR_I2C_ADDR);78if (!dev) {79return false;80}81setup();82return true;83}8485#define STATUS_SHIFT 3086#define TEMPERATURE_SHIFT 587#define TEMPERATURE_MASK ((1 << 11) - 1)88#define PRESSURE_SHIFT 1689#define PRESSURE_MASK ((1 << 14) - 1)9091#define DLVR_OFFSET 8192.0f92#define DLVR_SCALE 16384.0f9394// 50Hz timer95void AP_Airspeed_DLVR::timer()96{97uint8_t raw_bytes[4];98if (!dev->read((uint8_t *)&raw_bytes, sizeof(raw_bytes))) {99return;100}101102uint32_t data = (raw_bytes[0] << 24) |103(raw_bytes[1] << 16) |104(raw_bytes[2] << 8) |105raw_bytes[3];106107if ((data >> STATUS_SHIFT)) {108// anything other then 00 in the status bits is an error109Debug("DLVR: Bad status read %u", (unsigned int)(data >> STATUS_SHIFT));110return;111}112113uint32_t pres_raw = (data >> PRESSURE_SHIFT) & PRESSURE_MASK;114uint32_t temp_raw = (data >> TEMPERATURE_SHIFT) & TEMPERATURE_MASK;115116float press_h2o = 1.25f * 2.0f * range_inH2O * ((pres_raw - DLVR_OFFSET) / DLVR_SCALE);117118if ((press_h2o > range_inH2O) || (press_h2o < -range_inH2O)) {119Debug("DLVR: Out of range pressure %f", press_h2o);120return;121}122123float temp = temp_raw * (200.0f / 2047.0f) - 50.0f;124125WITH_SEMAPHORE(sem);126127const uint32_t now = AP_HAL::millis();128129#pragma GCC diagnostic push130#pragma GCC diagnostic ignored "-Wfloat-equal" // suppress -Wfloat-equal as we are only worried about the case where we had never read131if ((temperature != 0) && (fabsf(temperature - temp) > 10) && ((now - last_sample_time_ms) < 2000)) {132Debug("DLVR: Temperature swing %f", temp);133return;134}135#pragma GCC diagnostic pop136137pressure_sum += INCH_OF_H2O_TO_PASCAL * press_h2o;138temperature_sum += temp;139press_count++;140temp_count++;141last_sample_time_ms = now;142}143144// return the current differential_pressure in Pascal145bool AP_Airspeed_DLVR::get_differential_pressure(float &_pressure)146{147WITH_SEMAPHORE(sem);148149if ((AP_HAL::millis() - last_sample_time_ms) > 100) {150return false;151}152153if (press_count > 0) {154pressure = pressure_sum / press_count;155press_count = 0;156pressure_sum = 0;157}158159_pressure = pressure;160return true;161}162163// return the current temperature in degrees C, if available164bool AP_Airspeed_DLVR::get_temperature(float &_temperature)165{166WITH_SEMAPHORE(sem);167168if ((AP_HAL::millis() - last_sample_time_ms) > 100) {169return false;170}171172if (temp_count > 0) {173temperature = temperature_sum / temp_count;174temp_count = 0;175temperature_sum = 0;176}177178_temperature = temperature;179return true;180}181182#endif183184185