Line data    Source code

       1             : /*
       2             :    This program is free software: you can redistribute it and/or modify
       3             :    it under the terms of the GNU General Public License as published by
       4             :    the Free Software Foundation, either version 3 of the License, or
       5             :    (at your option) any later version.
       6             : 
       7             :    This program is distributed in the hope that it will be useful,
       8             :    but WITHOUT ANY WARRANTY; without even the implied warranty of
       9             :    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
      10             :    GNU General Public License for more details.
      11             : 
      12             :    You should have received a copy of the GNU General Public License
      13             :    along with this program.  If not, see <http://www.gnu.org/licenses/>.
      14             :  */
      15             : /*
      16             :   driver for DLVR differential airspeed sensor
      17             :   https://www.allsensors.com/products/DLVR-L01D
      18             :  */
      19             : 
      20             : #include "AP_Airspeed_DLVR.h"
      21             : 
      22             : #if AP_AIRSPEED_DLVR_ENABLED
      23             : 
      24             : #include <AP_Math/AP_Math.h>
      25             : 
      26             : extern const AP_HAL::HAL &hal;
      27             : 
      28             : #define DLVR_I2C_ADDR 0x28
      29             : 
      30             : #ifdef DLVR_DEBUGGING
      31             :  # define Debug(fmt, args ...)  do {hal.console->printf("%s:%d: " fmt "\n", __FUNCTION__, __LINE__, ## args); hal.scheduler->delay(1); } while(0)
      32             : #else
      33             :  # define Debug(fmt, args ...)
      34             : #endif
      35             : 
      36             : 
      37          10 : AP_Airspeed_DLVR::AP_Airspeed_DLVR(AP_Airspeed &_frontend, uint8_t _instance, const float _range_inH2O) :
      38             :     AP_Airspeed_Backend(_frontend, _instance),
      39          10 :     range_inH2O(_range_inH2O)
      40          10 : {}
      41             : 
      42             : /*
      43             :   probe for a sensor on a given i2c address
      44             :  */
      45           0 : AP_Airspeed_Backend *AP_Airspeed_DLVR::probe(AP_Airspeed &_frontend,
      46             :                                              uint8_t _instance,
      47             :                                              AP_HAL::I2CDevice *_dev,
      48             :                                              const float _range_inH2O)
      49             : {
      50           0 :     if (!_dev) {
      51           0 :         return nullptr;
      52             :     }
      53           0 :     AP_Airspeed_DLVR *sensor = NEW_NOTHROW AP_Airspeed_DLVR(_frontend, _instance, _range_inH2O);
      54           0 :     if (!sensor) {
      55           0 :         return nullptr;
      56             :     }
      57           0 :     sensor->dev = _dev;
      58           0 :     sensor->setup();
      59           0 :     return sensor;
      60             : }
      61             : 
      62             : // initialise the sensor
      63          10 : void AP_Airspeed_DLVR::setup()
      64             : {
      65          10 :     WITH_SEMAPHORE(dev->get_semaphore());
      66          10 :     dev->set_speed(AP_HAL::Device::SPEED_LOW);
      67          10 :     dev->set_retries(2);
      68          10 :     dev->set_device_type(uint8_t(DevType::DLVR));
      69          10 :     set_bus_id(dev->get_bus_id());
      70             : 
      71          10 :     dev->register_periodic_callback(1000000UL/50U,
      72             :                                     FUNCTOR_BIND_MEMBER(&AP_Airspeed_DLVR::timer, void));
      73          10 : }
      74             : 
      75             : // probe and initialise the sensor
      76          10 : bool AP_Airspeed_DLVR::init()
      77             : {
      78          10 :     dev = hal.i2c_mgr->get_device_ptr(get_bus(), DLVR_I2C_ADDR);
      79          10 :     if (!dev) {
      80           0 :         return false;
      81             :     }
      82          10 :     setup();
      83          10 :     return true;
      84             : }
      85             : 
      86             : #define STATUS_SHIFT 30
      87             : #define TEMPERATURE_SHIFT 5
      88             : #define TEMPERATURE_MASK ((1 << 11) - 1)
      89             : #define PRESSURE_SHIFT 16
      90             : #define PRESSURE_MASK ((1 << 14) - 1)
      91             : 
      92             : #define DLVR_OFFSET 8192.0f
      93             : #define DLVR_SCALE 16384.0f
      94             : 
      95             : // 50Hz timer
      96       76858 : void AP_Airspeed_DLVR::timer()
      97             : {
      98             :     uint8_t raw_bytes[4];
      99       76858 :     if (!dev->read((uint8_t *)&raw_bytes, sizeof(raw_bytes))) {
     100       46107 :         return;
     101             :     }
     102             : 
     103       76858 :     uint32_t data = (raw_bytes[0] << 24) |
     104       76858 :                     (raw_bytes[1] << 16) |
     105       76858 :                     (raw_bytes[2] << 8)  |
     106       76858 :                     raw_bytes[3];
     107             : 
     108       76858 :     if ((data >> STATUS_SHIFT)) {
     109             :         // anything other then 00 in the status bits is an error
     110             :         Debug("DLVR: Bad status read %u", (unsigned int)(data >> STATUS_SHIFT));
     111       46107 :         return;
     112             :     }
     113             : 
     114       30751 :     uint32_t pres_raw = (data >> PRESSURE_SHIFT)    & PRESSURE_MASK;
     115       30751 :     uint32_t temp_raw = (data >> TEMPERATURE_SHIFT) & TEMPERATURE_MASK;
     116             : 
     117       30751 :     float press_h2o = 1.25f * 2.0f * range_inH2O * ((pres_raw - DLVR_OFFSET) / DLVR_SCALE);
     118             : 
     119       30751 :     if ((press_h2o > range_inH2O) || (press_h2o < -range_inH2O)) {
     120             :         Debug("DLVR: Out of range pressure %f", press_h2o);
     121           0 :         return;
     122             :     }
     123             : 
     124       30751 :     float temp = temp_raw * (200.0f / 2047.0f) - 50.0f;
     125             : 
     126       30751 :     WITH_SEMAPHORE(sem);
     127             : 
     128       30751 :     const uint32_t now = AP_HAL::millis();
     129             : 
     130             : #pragma GCC diagnostic push
     131             : #pragma GCC diagnostic ignored "-Wfloat-equal" // suppress -Wfloat-equal as we are only worried about the case where we had never read
     132       30751 :     if ((temperature != 0) && (fabsf(temperature - temp) > 10) && ((now - last_sample_time_ms) < 2000)) {
     133             :         Debug("DLVR: Temperature swing %f", temp);
     134           0 :         return;
     135             :     }
     136             : #pragma GCC diagnostic pop
     137             : 
     138       30751 :     pressure_sum += INCH_OF_H2O_TO_PASCAL * press_h2o;
     139       30751 :     temperature_sum += temp;
     140       30751 :     press_count++;
     141       30751 :     temp_count++;
     142       30751 :     last_sample_time_ms = now;
     143       30751 : }
     144             : 
     145             : // return the current differential_pressure in Pascal
     146       15317 : bool AP_Airspeed_DLVR::get_differential_pressure(float &_pressure)
     147             : {
     148       15317 :     WITH_SEMAPHORE(sem);
     149             : 
     150       15317 :     if ((AP_HAL::millis() - last_sample_time_ms) > 100) {
     151           0 :         return false;
     152             :     }
     153             : 
     154       15317 :     if (press_count > 0) {
     155       15317 :         pressure = pressure_sum / press_count;
     156       15317 :         press_count = 0;
     157       15317 :         pressure_sum = 0;
     158             :     }
     159             : 
     160       15317 :     _pressure = pressure;
     161       15317 :     return true;
     162       15317 : }
     163             : 
     164             : // return the current temperature in degrees C, if available
     165       42719 : bool AP_Airspeed_DLVR::get_temperature(float &_temperature)
     166             : {
     167       42719 :     WITH_SEMAPHORE(sem);
     168             : 
     169       42719 :     if ((AP_HAL::millis() - last_sample_time_ms) > 100) {
     170          10 :         return false;
     171             :     }
     172             : 
     173       42709 :     if (temp_count > 0) {
     174       15954 :         temperature = temperature_sum / temp_count;
     175       15954 :         temp_count = 0;
     176       15954 :         temperature_sum = 0;
     177             :     }
     178             : 
     179       42709 :     _temperature = temperature;
     180       42709 :     return true;
     181       42719 : }
     182             : 
     183             : #endif