Line data    Source code

       1             : #include "SIM_config.h"
       2             : 
       3             : #if AP_SIM_AIRSPEED_DLVR_ENABLED
       4             : 
       5             : #include "SIM_Airspeed_DLVR.h"
       6             : 
       7             : #include "SITL.h"
       8             : 
       9       76858 : int SITL::Airspeed_DLVR::rdwr(I2C::i2c_rdwr_ioctl_data *&data)
      10             : {
      11       76858 :     struct I2C::i2c_msg &msg = data->msgs[0];
      12       76858 :     if (msg.flags == I2C_M_RD) {
      13             :         // driver is attempting to receive reading...
      14       76858 :         if (msg.len != 4) {
      15           0 :             AP_HAL::panic("Unexpected message length (%u)", msg.len);
      16             :         }
      17             : 
      18       76858 :         uint8_t status = 0;
      19       76858 :         if (last_sent_ms == last_update_ms) {
      20       46107 :             status |= 0b10;
      21             :         }
      22       76858 :         last_sent_ms = last_update_ms;
      23             :         // if (electrical_fault_or_bad_config) {
      24             :         //     status |= 0b11;
      25             :         // }
      26             : 
      27             :         // calculation of packed-pressure value:
      28             : 
      29             :         // this is TYPE_I2C_DLVR_5IN
      30       76858 :         const uint8_t range_inH2O = 5;
      31             : 
      32             : #define DLVR_OFFSET 8192.0f
      33             : #define DLVR_SCALE 16384.0f
      34             : 
      35             : //        const float pressure_pascals = 1555.2f;  // maximum transportable
      36       76858 :         const float press_h2o = pressure * (1.0f/INCH_OF_H2O_TO_PASCAL);
      37       76858 :         const uint32_t pressure_raw = ((press_h2o / (1.25f * 2.0f * range_inH2O)) * DLVR_SCALE) + DLVR_OFFSET;
      38             : 
      39             :         // calculation of packed-temperature value
      40       76858 :         const uint32_t temp_raw = (temperature + 50.0f)  * (2047.0f/200.0f);
      41             : 
      42       76858 :         const uint32_t packed =
      43       76858 :             (status << 30) |
      44       76858 :             ((pressure_raw & 0x3fff) << 16) |
      45       76858 :             ((temp_raw & 0x7ff) << 5);
      46             : 
      47       76858 :         msg.buf[0] = (packed >> 24) & 0xff;
      48       76858 :         msg.buf[1] = (packed >> 16) & 0xff;
      49       76858 :         msg.buf[2] = (packed >>  8) & 0xff;
      50       76858 :         msg.buf[3] = (packed >>  0) & 0xff;
      51             : 
      52       76858 :         return 0;
      53             :     }
      54             : 
      55           0 :     AP_HAL::panic("Should never be written to");
      56             : }
      57             : 
      58             : 
      59   178759942 : void SITL::Airspeed_DLVR::update(const class Aircraft &aircraft)
      60             : {
      61   178759942 :     const uint32_t now_ms = AP_HAL::millis();
      62   178759942 :     if (now_ms - last_update_ms < 50) { // 20Hz
      63   175783114 :         return;
      64             :     }
      65     2976828 :     last_update_ms = now_ms;
      66             : 
      67     2976828 :     pressure = AP::sitl()->state.airspeed_raw_pressure[0];
      68             : 
      69     2976828 :     float sim_alt = AP::sitl()->state.altitude;
      70     2976828 :     sim_alt += 2 * rand_float();
      71             : 
      72             :     // To Do: Add a sensor board temperature offset parameter
      73     2976828 :     temperature = AP_Baro::get_temperatureC_for_alt_amsl(sim_alt);
      74             : }
      75             : 
      76             : #endif  // AP_SIM_AIRSPEED_DLVR_ENABLED