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             : #include "AP_Baro_AUAV.h"
      16             : 
      17             : #if AP_BARO_AUAV_ENABLED
      18             : 
      19             : extern const AP_HAL::HAL &hal;
      20             : 
      21           0 : AP_Baro_AUAV::AP_Baro_AUAV(AP_Baro &baro, AP_HAL::Device *_dev)
      22             :     : AP_Baro_Backend(baro)
      23           0 :     , dev(_dev)
      24             : {
      25           0 : }
      26             : 
      27           0 : AP_Baro_Backend *AP_Baro_AUAV::probe(AP_Baro &baro, AP_HAL::Device &_dev)
      28             : {
      29           0 :     AP_Baro_AUAV *sensor = NEW_NOTHROW AP_Baro_AUAV(baro, &_dev);
      30           0 :     if (!sensor || !sensor->init()) {
      31           0 :         delete sensor;
      32           0 :         return nullptr;
      33             :     }
      34           0 :     return sensor;
      35             : }
      36             : 
      37           0 : bool AP_Baro_AUAV::init()
      38             : {
      39           0 :     if (!dev) {
      40           0 :         return false;
      41             :     }
      42             : 
      43             :     {
      44             :         // Take semaphore for i2c functions
      45           0 :         WITH_SEMAPHORE(dev->get_semaphore());
      46           0 :         dev->set_retries(10);
      47             : 
      48             :         // Request a measurement
      49           0 :         if (!sensor.measure()) {
      50           0 :             return false;
      51             :         }
      52           0 :         hal.scheduler->delay(40);
      53             : 
      54             :         // Test read and discard result as the compensation coefficients are not configured
      55             :         float PComp, temperature;
      56           0 :         if (sensor.collect(PComp, temperature) != AUAV_Pressure_sensor::Status::Normal) {
      57           0 :             return false;
      58             :         }
      59           0 :     }
      60             : 
      61             :     // Register sensor and set dev-id
      62           0 :     instance = _frontend.register_sensor();
      63           0 :     dev->set_device_type(DEVTYPE_BARO_AUAV);
      64           0 :     set_bus_id(instance, dev->get_bus_id());
      65             : 
      66           0 :     dev->register_periodic_callback(40000,
      67             :                                      FUNCTOR_BIND_MEMBER(&AP_Baro_AUAV::timer, void));
      68             : 
      69           0 :     return true;
      70             : }
      71             : 
      72             : //  accumulate a new sensor reading
      73           0 : void AP_Baro_AUAV::timer(void)
      74             : {
      75           0 :     if (sensor.stage != AUAV_Pressure_sensor::CoefficientStage::Done) {
      76           0 :         sensor.read_coefficients();
      77           0 :         return;
      78             :     }
      79             : 
      80           0 :     if (measurement_requested) {
      81             :         // Read in result of last measurement
      82             :         float Pcomp, temp_C;
      83           0 :         switch (sensor.collect(Pcomp, temp_C)) {
      84           0 :         case AUAV_Pressure_sensor::Status::Normal: {
      85             :             // Convert to correct units
      86           0 :             const float pressure_mbar = 250 + (1.25 * ((Pcomp-1677722)/16777216.0) * 1000.0);
      87             :             {
      88           0 :                 WITH_SEMAPHORE(_sem);
      89           0 :                 pressure_sum += pressure_mbar * 100;
      90           0 :                 temperature_sum += temp_C;
      91           0 :                 count++;
      92           0 :             }
      93           0 :             break;
      94             :         }
      95           0 :         case AUAV_Pressure_sensor::Status::Busy:
      96             :             // Don't request a new measurement
      97           0 :             return;
      98             : 
      99           0 :         case AUAV_Pressure_sensor::Status::Fault:
     100           0 :             break;
     101             :         }
     102             :     }
     103             : 
     104             :     // Request a new measurement
     105           0 :     measurement_requested = sensor.measure();
     106             : }
     107             : 
     108             : // transfer data to the frontend
     109           0 : void AP_Baro_AUAV::update(void)
     110             : {
     111           0 :     if (count == 0) {
     112           0 :         return;
     113             :     }
     114             : 
     115           0 :     WITH_SEMAPHORE(_sem);
     116             : 
     117           0 :     _copy_to_frontend(instance, pressure_sum/count, temperature_sum/count);
     118             : 
     119           0 :     pressure_sum = 0;
     120           0 :     temperature_sum = 0;
     121           0 :     count = 0;
     122           0 : }
     123             : 
     124             : #endif  // AP_BARO_AUAV_ENABLED