Line data    Source code

       1             : #include "SIM_config.h"
       2             : 
       3             : #if AP_SIM_TEMPERATURE_SHT3X_ENABLED
       4             : 
       5             : #include "SIM_Temperature_SHT3x.h"
       6             : 
       7             : #include <stdio.h>
       8             : 
       9             : using namespace SITL;
      10             : 
      11        2145 : SHT3x::SHT3x()
      12             : {
      13        2145 :     reset();
      14        2145 : }
      15             : 
      16        2145 : void SHT3x::reset()
      17             : {
      18        2145 :     status.alert_pending = 1;
      19        2145 :     status.reset_detected = 1;
      20        2145 : }
      21             : 
      22         847 : void SHT3x::pack_reading(SITL::I2C::i2c_msg &msg)
      23             : {
      24         847 :     if (msg.len != 6) {
      25           0 :         AP_HAL::panic("Unexpected length");
      26             :     }
      27             : 
      28         847 :     const uint16_t h_deconverted = (measurement.humidity*0.01) * 65535;
      29         847 :     const uint16_t t_deconverted = (((measurement.temperature + 45) / 175) * 65535);
      30             : 
      31         847 :     msg.buf[0] = t_deconverted >> 8;
      32         847 :     msg.buf[1] = t_deconverted & 0xff;
      33         847 :     msg.buf[2] = crc8_generic(&msg.buf[0], 2, 0x31, 0xff);
      34         847 :     msg.buf[3] = h_deconverted >> 8;
      35         847 :     msg.buf[4] = h_deconverted & 0xff;
      36         847 :     msg.buf[5] = crc8_generic(&msg.buf[3], 2, 0x31, 0xff);
      37         847 : }
      38             : 
      39        1703 : int SHT3x::rdwr(I2C::i2c_rdwr_ioctl_data *&data)
      40             : {
      41        1703 :     if (data->nmsgs == 2) {
      42             :         // something is expecting a response....
      43           3 :         if (data->msgs[0].flags != 0) {
      44           0 :             AP_HAL::panic("Unexpected flags");
      45             :         }
      46           3 :         if (data->msgs[1].flags != I2C_M_RD) {
      47           0 :             AP_HAL::panic("Unexpected flags");
      48             :         }
      49           3 :         const uint16_t command = data->msgs[0].buf[0] << 8 | data->msgs[0].buf[1];
      50           3 :         switch ((Command)command) {
      51           0 :         case Command::RESET:
      52             :         case Command::CLEAR_STATUS:
      53             :         case Command::MEASURE:
      54           0 :             AP_HAL::panic("Command is write-only?!");
      55           0 :         case Command::READ_STATUS:
      56           0 :             data->msgs[1].buf[0] = status.value >> 16;
      57           0 :             data->msgs[1].buf[1] = status.value & 0xff;
      58           0 :             data->msgs[1].len = 2;
      59           0 :             return 0;
      60           3 :         case Command::READ_SN:
      61           3 :             if (data->msgs[1].len != 6) {
      62           0 :                 AP_HAL::panic("Unexpwcted SN length");
      63             :             }
      64          21 :             for (uint8_t i=0; i<6; i++) {
      65          18 :                 data->msgs[1].buf[i] = i;
      66             :             }
      67           3 :             return 0;
      68             :         AP_HAL::panic("Bad command 0x%02x", (uint8_t)command);
      69             :         }
      70             :     }
      71             : 
      72        1700 :     if (data->nmsgs == 1 && data->msgs[0].flags == I2C_M_RD) {
      73             :         // driver is attempting to retrieve a measurement
      74         847 :         switch (state) {
      75           0 :         case State::UNKNOWN:
      76             :         case State::RESET:
      77             :         case State::CLEAR_STATUS:
      78             :         case State::IDLE:
      79           0 :             AP_HAL::panic("Bad state for measurement retrieval");
      80           0 :         case State::MEASURING:
      81           0 :             AP_HAL::panic("Too soon for measurement (15.5ms for high repeatability, table 4)");
      82         847 :         case State::MEASURED:
      83         847 :             pack_reading(data->msgs[0]);
      84         847 :             return 0;
      85             :         }
      86           0 :         AP_HAL::panic("Bad state");
      87             :     }
      88             : 
      89         853 :     if (data->nmsgs == 1) {
      90             :         // incoming write-only command
      91         853 :         const auto &msg = data->msgs[0];
      92         853 :         const uint16_t cmd = msg.buf[0] << 8 | msg.buf[1];
      93         853 :         switch ((Command)cmd) {
      94           3 :         case Command::RESET:
      95           3 :             set_state(State::RESET);
      96           3 :             return 0;
      97           0 :         case Command::READ_SN:
      98             :         case Command::READ_STATUS:
      99           0 :             AP_HAL::panic("Should not get here");
     100           0 :         case Command::CLEAR_STATUS:
     101           0 :             set_state(State::CLEAR_STATUS);
     102           0 :             return 0;
     103         850 :         case Command::MEASURE:
     104         850 :             set_state(State::MEASURING);
     105         850 :             return 0;
     106             :         }
     107           0 :         AP_HAL::panic("Bad command 0x%02x", (uint8_t)cmd);
     108             :     }
     109           0 :     return -1;
     110             : }
     111             : 
     112   178759942 : void SHT3x::update(const class Aircraft &aircraft)
     113             : {
     114   178759942 :     switch (state) {
     115           0 :     case State::UNKNOWN:
     116           0 :         break;
     117       10060 :     case State::RESET:
     118       10060 :         if (time_in_state_ms() > 2) {
     119        2012 :             set_state(State::IDLE);
     120             :         }
     121       10060 :         break;
     122           0 :     case State::CLEAR_STATUS:
     123           0 :         if (time_in_state_ms() < 1) {
     124           0 :             return;
     125             :         }
     126           0 :         status.value = 0;
     127           0 :         status.command_status = 1;  // cleared succesfully.  Irony.
     128           0 :         set_state(State::IDLE);
     129           0 :         break;
     130       18013 :     case State::MEASURING:
     131       18013 :         if (time_in_state_ms() > 16) {
     132         849 :             measurement.temperature = get_sim_temperature();
     133         849 :             measurement.humidity = 33.3;
     134         849 :             set_state(State::MEASURED);
     135             :         }
     136       18013 :         break;
     137       32913 :     case State::MEASURED:
     138       32913 :         break;
     139   178698956 :     case State::IDLE:
     140   178698956 :         break;
     141             :     }
     142             : }
     143             : 
     144         849 : float SHT3x::get_sim_temperature() const
     145             : {
     146         849 :     float sim_alt = AP::sitl()->state.altitude;
     147         849 :     sim_alt += 2 * rand_float();
     148             : 
     149             :     // To Do: Add a sensor board temperature offset parameter
     150         849 :     return AP_Baro::get_temperatureC_for_alt_amsl(sim_alt) + 25;
     151             : }
     152             : 
     153             : #endif  // AP_SIM_TEMPERATURE_SHT3X_ENABLED