From ca603e151746d872b50aadc5f3a1bc0f7b6aaacd Mon Sep 17 00:00:00 2001 From: Jacob Walser Date: Fri, 13 Jan 2017 11:25:22 -0500 Subject: [PATCH] AP_TemperatureSensor: Add AP_TemperatureSensor library and TSYS01 driver --- libraries/AP_TemperatureSensor/TSYS01.cpp | 142 ++++++++++++++++++++++ libraries/AP_TemperatureSensor/TSYS01.h | 34 ++++++ 2 files changed, 176 insertions(+) create mode 100644 libraries/AP_TemperatureSensor/TSYS01.cpp create mode 100644 libraries/AP_TemperatureSensor/TSYS01.h diff --git a/libraries/AP_TemperatureSensor/TSYS01.cpp b/libraries/AP_TemperatureSensor/TSYS01.cpp new file mode 100644 index 0000000000..845e5c552d --- /dev/null +++ b/libraries/AP_TemperatureSensor/TSYS01.cpp @@ -0,0 +1,142 @@ +#include "TSYS01.h" + +#include +#include +#include +#include +#include + +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(®, 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); +} diff --git a/libraries/AP_TemperatureSensor/TSYS01.h b/libraries/AP_TemperatureSensor/TSYS01.h new file mode 100644 index 0000000000..a2ceee6550 --- /dev/null +++ b/libraries/AP_TemperatureSensor/TSYS01.h @@ -0,0 +1,34 @@ +/* + * I2C driver for Measurement Specialties MEAS TSYS01 digital temperature sensor + */ + +#pragma once + +#include +#include +#include + +#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 _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 +};