Line data    Source code

       1             : #include "SIM_config.h"
       2             : 
       3             : #if AP_SIM_TSYS03_ENABLED
       4             : 
       5             : #include "SIM_Temperature_TSYS03.h"
       6             : 
       7             : #include <stdio.h>
       8             : 
       9             : #include <GCS_MAVLink/GCS.h>
      10             : 
      11             : constexpr const uint8_t SITL::TSYS03::serial[3];
      12             : 
      13           0 : int SITL::TSYS03::rdwr_handle_read(I2C::i2c_rdwr_ioctl_data *&data)
      14             : {
      15             :     // something is expecting a response....
      16           0 :     if (data->msgs[0].flags != 0) {
      17           0 :         AP_HAL::panic("Unexpected flags");
      18             :     }
      19           0 :     if (data->msgs[1].flags != I2C_M_RD) {
      20           0 :         AP_HAL::panic("Unexpected flags");
      21             :     }
      22           0 :     const uint8_t command = data->msgs[0].buf[0];
      23           0 :     switch ((Command)command) {
      24           0 :     case Command::RESET:
      25           0 :         AP_HAL::panic("Bad RESET");
      26           0 :     case Command::READ_SERIAL: {
      27           0 :         if (state != State::RESET) {
      28             :             // not sure if this is illegal or not?
      29           0 :             AP_HAL::panic("reading serial outside RESET state");
      30             :         }
      31           0 :         if (data->msgs[1].len != 4) {
      32           0 :             AP_HAL::panic("Unexpected serial read length");
      33             :         }
      34           0 :         memcpy(data->msgs[1].buf, serial, 3);
      35           0 :         uint8_t crc = 0;
      36           0 :         for (uint8_t i=0; i<3; i++) {
      37           0 :             crc = crc8_dvb(crc, serial[i], 0x31);
      38             :         }
      39           0 :         data->msgs[1].buf[3] = crc;
      40           0 :         break;
      41             :     }
      42           0 :     case Command::CONVERT:
      43           0 :         AP_HAL::panic("Bad CONVERT");
      44           0 :     case Command::READ_ADC: {
      45           0 :         if (data->msgs[1].len != 3) {
      46           0 :             AP_HAL::panic("Unexpected adc read length");
      47             :         }
      48           0 :         if (state == State::CONVERTING) {
      49             :             // we've been asked for values while still converting.
      50             :             // Return zeroes
      51           0 :         } else if (state == State::CONVERTED) {
      52           0 :             data->msgs[1].buf[1] = adc & 0xff;
      53           0 :             data->msgs[1].buf[0] = adc >> 8;
      54             : 
      55           0 :             uint8_t crc = 0;
      56           0 :             for (uint8_t i=0; i<2; i++) {
      57           0 :                 crc = crc8_dvb(crc, data->msgs[1].buf[i], 0x31);
      58             :             }
      59           0 :             data->msgs[1].buf[2] = crc;
      60             : 
      61           0 :             set_state(State::IDLE);
      62             :         } else {
      63             :             // AP_HAL::panic("READ_ADC in bad state");
      64             :             // this happens at startup
      65           0 :             return -1;
      66             :         }
      67           0 :         break;
      68             :     }
      69             :     }
      70           0 :     return 0;
      71             : }
      72             : 
      73           0 : int SITL::TSYS03::rdwr_handle_write(I2C::i2c_rdwr_ioctl_data *&data)
      74             : {
      75             :     // incoming write-only command
      76           0 :     const auto &msg = data->msgs[0];
      77           0 :     const uint8_t cmd = msg.buf[0];
      78             : 
      79           0 :     switch ((Command)cmd) {
      80           0 :     case Command::RESET:
      81           0 :         set_state(State::RESET);
      82           0 :         break;
      83           0 :     case Command::READ_SERIAL:
      84           0 :         AP_HAL::panic("bad serial read");
      85           0 :     case Command::CONVERT:
      86           0 :         if (state != State::RESET &&
      87           0 :             state != State::CONVERTING &&
      88           0 :             state != State::IDLE &&
      89           0 :             state != State::READ_SERIAL) {
      90           0 :             AP_HAL::panic("Convert outside reset/idle");
      91             :         }
      92           0 :         set_state(State::CONVERTING);
      93           0 :         break;
      94           0 :     case Command::READ_ADC:
      95           0 :         AP_HAL::panic("bad READ_ADC");
      96             :     }
      97           0 :     return 0;
      98             : }
      99             : 
     100           0 : int SITL::TSYS03::rdwr(I2C::i2c_rdwr_ioctl_data *&data)
     101             : {
     102           0 :     if (data->nmsgs == 2) {
     103           0 :         return rdwr_handle_read(data);
     104             :     }
     105             : 
     106           0 :     if (data->nmsgs == 1) {
     107           0 :         return rdwr_handle_write(data);
     108             :     }
     109           0 :     return -1;
     110             : }
     111             : 
     112             : // swiped from the driver:
     113           0 : float SITL::TSYS03::temperature_for_adc(const uint16_t _adc) const
     114             : {
     115           0 :     const float temperature = -40.0 + _adc * 165 / (powf(2, 16) - 1.0);
     116             : 
     117           0 :     return temperature;
     118             : }
     119             : 
     120           0 : uint16_t SITL::TSYS03::calculate_adc(float temperature) const
     121             : {
     122             :     // bisect to find the adc24 value:
     123           0 :     uint16_t min_adc = 0;
     124           0 :     uint16_t max_adc = 0xffff;
     125           0 :     uint16_t current_adc = (min_adc+(uint64_t)max_adc)/2;
     126           0 :     float current_error = fabsf(temperature_for_adc(current_adc) - temperature);
     127           0 :     bool bisect_down = false;
     128             : 
     129             :     // temperature_for_adc(9378708);  // should be 10.59
     130             : 
     131           0 :     while (labs(int32_t(max_adc - min_adc)) > 1 && current_error > 0.05) {
     132             :         uint16_t candidate_adc;
     133           0 :         if (bisect_down) {
     134           0 :             candidate_adc = (min_adc+(uint64_t)current_adc)/2;
     135             :         } else {
     136           0 :             candidate_adc = (max_adc+(uint64_t)current_adc)/2;
     137             :         }
     138           0 :         const float candidate_temp = temperature_for_adc(candidate_adc);
     139           0 :         const float candidate_error = fabsf(candidate_temp - temperature);
     140           0 :         if (candidate_error > current_error) {
     141             :             // worse result
     142           0 :             if (bisect_down) {
     143           0 :                 min_adc = candidate_adc;
     144           0 :                 bisect_down = false;
     145             :             } else {
     146           0 :                 max_adc = candidate_adc;
     147           0 :                 bisect_down = true;
     148             :             }
     149             :         } else {
     150             :             // better result
     151           0 :             if (bisect_down) {
     152           0 :                 max_adc = current_adc;
     153           0 :                 bisect_down = false;
     154             :             } else {
     155           0 :                 min_adc = current_adc;
     156           0 :                 bisect_down = true;
     157             :             }
     158           0 :             current_adc = candidate_adc;
     159           0 :             current_error = candidate_error;
     160             :         }
     161             :     }
     162           0 :     return current_adc;
     163             : }
     164             : 
     165   178759942 : void SITL::TSYS03::update(const class Aircraft &aircraft)
     166             : {
     167   178759942 :     switch (state) {
     168           0 :     case State::UNKNOWN:
     169           0 :         break;
     170       10045 :     case State::RESET:
     171       10045 :         if (time_in_state_ms() > 2) {
     172        2009 :             set_state(State::IDLE);
     173             :         }
     174       10045 :         break;
     175           0 :     case State::READ_SERIAL:
     176           0 :         break;
     177   178749897 :     case State::IDLE:
     178   178749897 :         break;
     179           0 :     case State::CONVERTING:
     180           0 :         if (time_in_state_ms() > 5) {
     181           0 :             const float temperature_degC = get_sim_temperature_degC(aircraft);
     182           0 :             if (!is_equal(last_temperature_degC, temperature_degC)) {
     183           0 :                 last_temperature_degC = temperature_degC;
     184           0 :                 adc = calculate_adc(temperature_degC);
     185             :             }
     186           0 :             set_state(State::CONVERTED);
     187             :         }
     188           0 :         break;
     189           0 :     case State::CONVERTED:
     190           0 :         break;
     191             :     }
     192   178759942 : }
     193             : 
     194           0 : float SITL::TSYS03::get_sim_temperature_degC(const Aircraft &aircraft) const
     195             : {
     196           0 :     return aircraft.get_battery_temperature_degC();
     197             : }
     198             : 
     199             : #endif  // AP_SIM_TSYS03_ENABLED