Line data    Source code

       1             : #include "SIM_config.h"
       2             : 
       3             : #if AP_SIM_TEMPERATURE_TSYS01_ENABLED
       4             : 
       5             : #include "SIM_Temperature_TSYS01.h"
       6             : 
       7             : #include <stdio.h>
       8             : 
       9             : constexpr const int32_t SITL::TSYS01::_k[6];
      10             : 
      11      220642 : int SITL::TSYS01::rdwr(I2C::i2c_rdwr_ioctl_data *&data)
      12             : {
      13      220642 :     if (data->nmsgs == 2) {
      14             :         // something is expecting a response....
      15      111548 :         if (data->msgs[0].flags != 0) {
      16           0 :             AP_HAL::panic("Unexpected flags");
      17             :         }
      18      111548 :         if (data->msgs[1].flags != I2C_M_RD) {
      19           0 :             AP_HAL::panic("Unexpected flags");
      20             :         }
      21      111548 :         const uint8_t command = data->msgs[0].buf[0];
      22      111548 :         switch ((Command)command) {
      23           0 :         case Command::RESET:
      24           0 :             AP_HAL::panic("Bad RESET");
      25         892 :         case Command::READ_PROM0:
      26             :         case Command::READ_PROM1:
      27             :         case Command::READ_PROM2:
      28             :         case Command::READ_PROM3:
      29             :         case Command::READ_PROM4:
      30             :         case Command::READ_PROM5: {
      31         892 :             if (state != State::RESET) {
      32           0 :                 AP_HAL::panic("reading prom outside RESET state");
      33             :             }
      34         892 :             if (data->msgs[1].len != 2) {
      35           0 :                 AP_HAL::panic("Unexpected prom read length");
      36             :             }
      37         892 :             const uint8_t prom_idx = (uint8_t(command) - uint8_t(Command::READ_PROM0))/2;
      38         892 :             if (prom_idx == 0) {
      39             :                 // PROM0 is the unused CRC register; not requested by the driver
      40          82 :                 data->msgs[1].buf[0] = 0;
      41          82 :                 data->msgs[1].buf[1] = 0;
      42          82 :                 break;
      43             :             }
      44             :             // prom_idx 1..5 → _k[5].._k[1] (reversed to match driver's read_prom ordering)
      45         810 :             const uint16_t k = _k[ARRAY_SIZE(_k) - prom_idx];
      46         810 :             data->msgs[1].buf[0] = k >> 8;
      47         810 :             data->msgs[1].buf[1] = k & 0xFF;
      48         810 :             break;
      49             :         }
      50           0 :         case Command::CONVERT:
      51           0 :             AP_HAL::panic("Bad CONVERT");
      52      108852 :         case Command::READ_ADC: {
      53      108852 :             uint8_t registers[3] {};
      54      108852 :             if (data->msgs[1].len != sizeof(registers)) {
      55           0 :                 AP_HAL::panic("Unexpected prom read length");
      56             :             }
      57      108852 :             if (state == State::CONVERTING) {
      58             :                 // we've been asked for values while still converting.
      59             :                 // Return zeroes per data sheet
      60      108852 :             } else if (state == State::CONVERTED) {
      61      108852 :                 uint32_t value = adc;
      62      108852 :                 registers[2] = value & 0xff;
      63      108852 :                 value >>= 8;
      64      108852 :                 registers[1] = value & 0xff;
      65      108852 :                 value >>= 8;
      66      108852 :                 registers[0] = value & 0xff;
      67      108852 :                 set_state(State::IDLE);
      68             :             } else {
      69             :                 // AP_HAL::panic("READ_ADC in bad state");
      70             :                 // this happens at startup
      71           0 :                 return -1;
      72             :             }
      73      435408 :             for (uint8_t i=0; i<ARRAY_SIZE(registers); i++) {
      74      326556 :                 data->msgs[1].buf[i] = registers[i];
      75             :             }
      76      108852 :             break;
      77             :         }
      78        1804 :         default:
      79             :             // NAK commands we do not understand.  The MS5611 probe
      80             :             // sends us prom reads for words 6 and 7 (0xAC and 0xAE);
      81             :             // failing the transfer here ensures the response buffer
      82             :             // is not returned uninitialised.
      83        1804 :             return -1;
      84             :         }
      85      109744 :         return 0;
      86             :     }
      87             : 
      88      109094 :     if (data->nmsgs == 1) {
      89             :         // incoming write-only command
      90      109094 :         const auto &msg = data->msgs[0];
      91      109094 :         const uint8_t cmd = msg.buf[0];
      92             : 
      93      109094 :         switch ((Command)cmd) {
      94         162 :         case Command::RESET:
      95         162 :             set_state(State::RESET);
      96         162 :             break;
      97           0 :         case Command::READ_PROM0:
      98             :         case Command::READ_PROM1:
      99             :         case Command::READ_PROM2:
     100             :         case Command::READ_PROM3:
     101             :         case Command::READ_PROM4:
     102             :         case Command::READ_PROM5:
     103           0 :             AP_HAL::panic("bad prom read");
     104      108932 :         case Command::CONVERT:
     105      108932 :             if (state != State::RESET &&
     106      108852 :                 state != State::CONVERTING &&
     107      108852 :                 state != State::IDLE &&
     108           0 :                 state != State::READ_PROM) {
     109           0 :                 AP_HAL::panic("Convert outside reset/idle");
     110             :             }
     111      108932 :             set_state(State::CONVERTING);
     112      108932 :             break;
     113           0 :         case Command::READ_ADC:
     114           0 :             AP_HAL::panic("bad READ_ADC");
     115           0 :         default:
     116             :             // NAK commands we do not understand
     117           0 :             return -1;
     118             :         }
     119      109094 :         return 0;
     120             :     }
     121           0 :     return -1;
     122             : }
     123             : 
     124             : // swiped from the driver:
     125     1921525 : float SITL::TSYS01::temperature_for_adc(uint32_t _adc) const
     126             : {
     127     1921525 :     const float adc16 = _adc/256.0;
     128             :     // Note that the offsets used here start from 1 (PROM1), not like
     129             :     // in the main codebase where they start from 0 (PROM1)!  The main
     130             :     // code does not read PROM0 from the device (real or simulated),
     131             :     // but the array in the simulator here reserves a space for it.
     132             :     return
     133     1921525 :         -2   * _k[5] * powf(10, -21) * powf(adc16, 4) +
     134     1921525 :         4    * _k[4] * powf(10, -16) * powf(adc16, 3) +
     135     1921525 :         -2   * _k[3] * powf(10, -11) * powf(adc16, 2) +
     136     1921525 :         1    * _k[2] * powf(10, -6)  * adc16 +
     137     1921525 :         -1.5 * _k[1] * powf(10, -2);
     138             : }
     139             : 
     140      108773 : uint32_t SITL::TSYS01::calculate_adc(float temperature) const
     141             : {
     142             :     // bisect to find the adc24 value:
     143      108773 :     uint32_t min_adc = 0;
     144      108773 :     uint32_t max_adc = 1<<24;
     145      108773 :     uint32_t current_adc = (min_adc+(uint64_t)max_adc)/2;
     146      108773 :     float current_error = fabsf(temperature_for_adc(current_adc) - temperature);
     147      108773 :     bool bisect_down = false;
     148             : 
     149             :     // temperature_for_adc(9378708);  // should be 10.59
     150             : 
     151     1921525 :     while (labs(int32_t(max_adc - min_adc)) > 1 && current_error > 0.05) {
     152             :         uint32_t candidate_adc;
     153     1812752 :         if (bisect_down) {
     154      875549 :             candidate_adc = (min_adc+(uint64_t)current_adc)/2;
     155             :         } else {
     156      937203 :             candidate_adc = (max_adc+(uint64_t)current_adc)/2;
     157             :         }
     158     1812752 :         const float candidate_temp = temperature_for_adc(candidate_adc);
     159     1812752 :         const float candidate_error = fabsf(candidate_temp - temperature);
     160     1812752 :         if (candidate_error > current_error) {
     161             :             // worse result
     162     1258438 :             if (bisect_down) {
     163      629219 :                 min_adc = candidate_adc;
     164      629219 :                 bisect_down = false;
     165             :             } else {
     166      629219 :                 max_adc = candidate_adc;
     167      629219 :                 bisect_down = true;
     168             :             }
     169             :         } else {
     170             :             // better result
     171      554314 :             if (bisect_down) {
     172      246330 :                 max_adc = current_adc;
     173      246330 :                 bisect_down = false;
     174             :             } else {
     175      307984 :                 min_adc = current_adc;
     176      307984 :                 bisect_down = true;
     177             :             }
     178      554314 :             current_adc = candidate_adc;
     179      554314 :             current_error = candidate_error;
     180             :         }
     181             :     }
     182      108773 :     return current_adc;
     183             : }
     184             : 
     185   178759942 : void SITL::TSYS01::update(const class Aircraft &aircraft)
     186             : {
     187   178759942 :     switch (state) {
     188           0 :     case State::UNKNOWN:
     189           0 :         break;
     190       31536 :     case State::RESET:
     191       31536 :         if (time_in_state_ms() > 10) {
     192        2018 :             set_state(State::READ_PROM);
     193             :         }
     194       31536 :         break;
     195   172192317 :     case State::READ_PROM:
     196   172192317 :         break;
     197           0 :     case State::IDLE:
     198           0 :         break;
     199      889798 :     case State::CONVERTING:
     200      889798 :         if (time_in_state_ms() > 5) {
     201      108906 :             const float temperature = get_sim_temperature();
     202      108906 :             if (!is_equal(last_temperature, temperature)) {
     203      108773 :                 last_temperature = temperature;
     204      108773 :                 adc = calculate_adc(temperature);
     205             :             }
     206      108906 :             set_state(State::CONVERTED);
     207             :         }
     208      889798 :         break;
     209     5646291 :     case State::CONVERTED:
     210     5646291 :         break;
     211             :     }
     212   178759942 : }
     213             : 
     214      108906 : float SITL::TSYS01::get_sim_temperature() const
     215             : {
     216      108906 :     float sim_alt = AP::sitl()->state.altitude;
     217      108906 :     sim_alt += 2 * rand_float();
     218             : 
     219             :     // To Do: Add a sensor board temperature offset parameter
     220      108906 :     return AP_Baro::get_temperatureC_for_alt_amsl(sim_alt) + 25;
     221             : }
     222             : 
     223             : #endif  // AP_SIM_TEMPERATURE_TSYS01_ENABLED