AP_TemperatureSensor: Add AP_TemperatureSensor library and TSYS01 driver

libraries/AP_TemperatureSensor/TSYS01.cpp

@@ -0,0 +1,142 @@
+#include "TSYS01.h"
+
+#include <utility>
+#include <stdio.h>
+#include <AP_HAL/AP_HAL.h>
+#include <AP_HAL/I2CDevice.h>
+#include <AP_Math/AP_Math.h>
+
+extern const AP_HAL::HAL &hal;
+
+static const uint8_t TSYS01_CMD_RESET       = 0x1E;
+static const uint8_t TSYS01_CMD_READ_PROM   = 0xA0;
+static const uint8_t TSYS01_CMD_CONVERT     = 0x40;
+static const uint8_t TSYS01_CMD_READ_ADC    = 0x00;
+
+TSYS01::TSYS01() :
+    _dev(nullptr),
+    _temperature(0),
+    _healthy(false)
+{
+    memset(&_k, 0, sizeof(_k));
+}
+
+bool TSYS01::init()
+{
+    _dev = std::move(hal.i2c_mgr->get_device(1, TSYS01_ADDR));
+    if (!_dev) {
+        printf("TSYS01 device is null!");
+        return false;
+    }
+
+    if (!_dev->get_semaphore()->take(0)) {
+        AP_HAL::panic("PANIC: TSYS01: failed to take serial semaphore for init");
+    }
+
+    _dev->set_retries(10);
+
+    if (!_reset()) {
+        printf("TSYS01 reset failed");
+        _dev->get_semaphore()->give();
+        return false;
+    }
+
+    hal.scheduler->delay(4);
+
+    if (!_read_prom()) {
+        printf("TSYS01 prom read failed");
+        _dev->get_semaphore()->give();
+        return false;
+    }
+
+    _convert();
+
+    // lower retries for run
+    _dev->set_retries(3);
+
+    _dev->get_semaphore()->give();
+
+    /* Request 20Hz update */
+    // Max conversion time is 9.04 ms
+    _dev->register_periodic_callback(50 * USEC_PER_MSEC,
+                                     FUNCTOR_BIND_MEMBER(&TSYS01::_timer, void));
+    return true;
+}
+
+bool TSYS01::_reset()
+{
+    return _dev->transfer(&TSYS01_CMD_RESET, 1, nullptr, 0);
+}
+
+// Register map
+// prom word	Address
+//      0       0xA0 -> unused
+//      1       0xA2 -> _k[4]
+//      2       0xA4 -> _k[3]
+//      3       0xA6 -> _k[2]
+//      4       0xA8 -> _k[1]
+//      5       0xAA -> _k[0]
+//      6       0xAC -> unused
+//      7       0xAE -> unused
+bool TSYS01::_read_prom()
+{
+    bool success = false;
+    for (int i = 0; i < 5; i++) {
+        // Read only the prom values that we use
+        _k[i] = _read_prom_word(5-i);
+        success |= _k[i] != 0;
+    }
+    return success;
+}
+
+// Borrowed from MS Baro driver
+uint16_t TSYS01::_read_prom_word(uint8_t word)
+{
+    const uint8_t reg = TSYS01_CMD_READ_PROM + (word << 1);
+    uint8_t val[2];
+    if (!_dev->transfer(&reg, 1, val, 2)) {
+        return 0;
+    }
+    return (val[0] << 8) | val[1];
+}
+
+bool TSYS01::_convert()
+{
+    return _dev->transfer(&TSYS01_CMD_CONVERT, 1, nullptr, 0);
+}
+
+uint32_t TSYS01::_read_adc()
+{
+    uint8_t val[3];
+    if (!_dev->transfer(&TSYS01_CMD_READ_ADC, 1, val, 3)) {
+        return 0;
+    }
+    return (val[0] << 16) | (val[1] << 8) | val[2];
+}
+
+void TSYS01::_timer(void)
+{
+    uint32_t adc = _read_adc();
+    _healthy = adc != 0;
+
+    if (_healthy) {
+        _calculate(adc);
+    } else {
+        _temperature = 0;
+    }
+
+    //printf("\nTemperature: %.2lf C", _temperature);
+
+    _convert();
+}
+
+void TSYS01::_calculate(uint32_t adc)
+{
+    float adc16 = adc/256;
+    _temperature =
+        -2   * _k[4] * pow(10, -21) * pow(adc16, 4) +
+        4    * _k[3] * pow(10, -16) * pow(adc16, 3) +
+        -2   * _k[2] * pow(10, -11) * pow(adc16, 2) +
+        1    * _k[1] * pow(10, -6)  * adc16 +
+        -1.5 * _k[0] * pow(10, -2);
+}

libraries/AP_TemperatureSensor/TSYS01.h

@@ -0,0 +1,34 @@
+/*
+ * I2C driver for Measurement Specialties MEAS TSYS01 digital temperature sensor
+ */
+
+#pragma once
+
+#include <AP_HAL/AP_HAL.h>
+#include <AP_HAL/Semaphores.h>
+#include <AP_HAL/Device.h>
+
+#define TSYS01_ADDR 0x77
+
+class TSYS01 {
+public:
+    TSYS01(void);
+
+    bool init(void);
+    float temperature(void) { return _temperature; } // temperature in degrees C
+    bool healthy(void) { return _healthy; } // do we have a valid temperature reading?
+
+    AP_HAL::OwnPtr<AP_HAL::Device> _dev;
+
+private:
+    float _temperature; // degrees C
+    bool _healthy; // we have a valid temperature reading to report
+    uint16_t _k[5]; // internal calibration for temperature calculation
+    bool _reset(void); // reset device
+    bool _read_prom(void); // read (relevant) internal calibration registers into _k
+    bool _convert(void); // begin an ADC conversion (min:7.40ms typ:8.22ms max:9.04ms)
+    uint32_t _read_adc(void);
+    uint16_t _read_prom_word(uint8_t word);
+    void _timer(void); // update the temperature, called at 20Hz
+    void _calculate(uint32_t adc); // calculate temperature using adc reading and internal calibration
+};