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             : 
      16             : #include "AP_TemperatureSensor_TSYS01.h"
      17             : 
      18             : #if AP_TEMPERATURE_SENSOR_TSYS01_ENABLED
      19             : #include <utility>
      20             : #include <stdio.h>
      21             : #include <AP_HAL/I2CDevice.h>
      22             : #include <AP_Math/AP_Math.h>
      23             : 
      24             : #ifndef AP_TEMPERATURE_SENSOR_TSYS01_ENFORCE_KNOWN_VALID_I2C_ADDRESS
      25             : #define AP_TEMPERATURE_SENSOR_TSYS01_ENFORCE_KNOWN_VALID_I2C_ADDRESS 1
      26             : #endif
      27             : 
      28             : extern const AP_HAL::HAL &hal;
      29             : 
      30             : static const uint8_t TSYS01_CMD_RESET       = 0x1E;
      31             : static const uint8_t TSYS01_CMD_READ_PROM   = 0xA0;
      32             : static const uint8_t TSYS01_CMD_CONVERT     = 0x40;
      33             : static const uint8_t TSYS01_CMD_READ_ADC    = 0x00;
      34             : 
      35          80 : void AP_TemperatureSensor_TSYS01::init()
      36             : {
      37          80 :     constexpr char name[] = "TSYS01";
      38             : 
      39             : #if AP_TEMPERATURE_SENSOR_TSYS01_ENFORCE_KNOWN_VALID_I2C_ADDRESS
      40             :     // I2C Address: Default to using TSYS01_ADDR_CSB0 & Check I2C Address is Correct
      41          80 :     if ((_params.bus_address != TSYS01_ADDR_CSB0) && (_params.bus_address != TSYS01_ADDR_CSB1)) {
      42           0 :         printf("%s wrong I2C addr of 0x%2X, setting to 0x%2X", name, (unsigned)_params.bus_address.get(), (unsigned)TSYS01_ADDR_CSB0);
      43           0 :         _params.bus_address.set(TSYS01_ADDR_CSB0);
      44             :     }
      45             : #endif
      46             : 
      47          80 :     _dev = hal.i2c_mgr->get_device_ptr(_params.bus, _params.bus_address);
      48          80 :     if (!_dev) {
      49           0 :         printf("%s device is null!", name);
      50           0 :         return;
      51             :     }
      52             : 
      53          80 :     WITH_SEMAPHORE(_dev->get_semaphore());
      54             : 
      55          80 :     _dev->set_retries(10);
      56             : 
      57             :     // reset
      58          80 :     if (!_dev->transfer(&TSYS01_CMD_RESET, 1, nullptr, 0)) {
      59           0 :         printf("%s reset failed", name);
      60           0 :         return;
      61             :     }
      62             : 
      63          80 :     hal.scheduler->delay(4);
      64             : 
      65          80 :     if (!read_prom()) {
      66           0 :         printf("%s prom read failed", name);
      67           0 :         return;
      68             :     }
      69             : 
      70          80 :     start_next_sample();
      71             : 
      72             :     // lower retries for run
      73          80 :     _dev->set_retries(3);
      74             : 
      75             :     /* Request 20Hz update */
      76             :     // Max conversion time is 9.04 ms
      77          80 :     _dev->register_periodic_callback(50 * AP_USEC_PER_MSEC,
      78             :                                      FUNCTOR_BIND_MEMBER(&AP_TemperatureSensor_TSYS01::_timer, void));
      79          80 : }
      80             : 
      81             : // Register map
      82             : // prom word    Address
      83             : //      0       0xA0 -> unused
      84             : //      1       0xA2 -> _k[4]
      85             : //      2       0xA4 -> _k[3]
      86             : //      3       0xA6 -> _k[2]
      87             : //      4       0xA8 -> _k[1]
      88             : //      5       0xAA -> _k[0]
      89             : //      6       0xAC -> unused
      90             : //      7       0xAE -> unused
      91          80 : bool AP_TemperatureSensor_TSYS01::read_prom()
      92             : {
      93          80 :     bool success = false;
      94         480 :     for (uint8_t i = 0; i < ARRAY_SIZE(_k); i++) {
      95             :         // Read only the prom values that we use
      96         400 :         _k[i] = read_prom_word(ARRAY_SIZE(_k)-i);
      97         400 :         success |= (_k[i] != 0);
      98             :     }
      99          80 :     return success;
     100             : }
     101             : 
     102             : // Borrowed from MS Baro driver
     103         400 : uint16_t AP_TemperatureSensor_TSYS01::read_prom_word(const uint8_t word) const
     104             : {
     105         400 :     const uint8_t reg = TSYS01_CMD_READ_PROM + (word << 1);
     106             :     uint8_t val[2];
     107         400 :     if (!_dev->transfer(&reg, 1, val, 2)) {
     108           0 :         return 0;
     109             :     }
     110         400 :     return UINT16_VALUE(val[0], val[1]);
     111             : }
     112             : 
     113      108852 : uint32_t AP_TemperatureSensor_TSYS01::read_adc() const
     114             : {
     115             :     uint8_t val[3];
     116      108852 :     if (!_dev->transfer(&TSYS01_CMD_READ_ADC, 1, val, 3)) {
     117           0 :         return 0;
     118             :     }
     119      108852 :     return UINT32_VALUE(0,val[0],val[1],val[2]);
     120             : }
     121             : 
     122      108852 : void AP_TemperatureSensor_TSYS01::_timer(void)
     123             : {
     124      108852 :     const uint32_t adc = read_adc();
     125             : 
     126      108852 :     if (adc != 0) {
     127      108852 :         const float temp = calculate(adc);
     128      108852 :         set_temperature(temp);
     129             :     }
     130             : 
     131      108852 :     start_next_sample();
     132      108852 : }
     133             : 
     134      108932 : void AP_TemperatureSensor_TSYS01::start_next_sample()
     135             : {
     136      108932 :     _dev->transfer(&TSYS01_CMD_CONVERT, 1, nullptr, 0);
     137      108932 : }
     138             : 
     139      108852 : float AP_TemperatureSensor_TSYS01::calculate(const uint32_t adc) const
     140             : {
     141      108852 :     const float adc16 = adc/256;
     142             :     const float temperature =
     143      108852 :         -2   * _k[4] * powf(10, -21) * powf(adc16, 4) +
     144      108852 :         4    * _k[3] * powf(10, -16) * powf(adc16, 3) +
     145      108852 :         -2   * _k[2] * powf(10, -11) * powf(adc16, 2) +
     146      108852 :         1    * _k[1] * powf(10, -6)  * adc16 +
     147      108852 :         -1.5 * _k[0] * powf(10, -2);
     148             : 
     149      108852 :     return temperature;
     150             : }
     151             : 
     152             : #endif // AP_TEMPERATURE_SENSOR_TSYS01_ENABLED