diff --git a/libraries/AP_TemperatureSensor/AP_TemperatureSensor.cpp b/libraries/AP_TemperatureSensor/AP_TemperatureSensor.cpp
new file mode 100644
index 0000000000..a6939ff296
--- /dev/null
+++ b/libraries/AP_TemperatureSensor/AP_TemperatureSensor.cpp
@@ -0,0 +1,158 @@
+/*
+ This program is free software: you can redistribute it and/or modify
+ it under the terms of the GNU General Public License as published by
+ the Free Software Foundation, either version 3 of the License, or
+ (at your option) any later version.
+
+ This program is distributed in the hope that it will be useful,
+ but WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ GNU General Public License for more details.
+
+ You should have received a copy of the GNU General Public License
+ along with this program. If not, see .
+ */
+
+#include "AP_TemperatureSensor.h"
+
+#if AP_TEMPERATURE_SENSOR_ENABLED
+#include "AP_TemperatureSensor_TSYS01.h"
+
+#include
+#include
+
+extern const AP_HAL::HAL& hal;
+
+AP_TemperatureSensor *AP_TemperatureSensor::_singleton;
+
+const AP_Param::GroupInfo AP_TemperatureSensor::var_info[] = {
+
+ // SKIP INDEX 0
+
+ // @Param: _LOG
+ // @DisplayName: Logging
+ // @Description: Enables temperature sensor logging
+ // @Values: 0:Disabled, 1:Enabled
+ // @User: Standard
+ AP_GROUPINFO("_LOG", 1, AP_TemperatureSensor, _log_flag, 0),
+
+ // SKIP Index 2-9 to be for parameters that apply to every sensor
+
+ // @Group: 1_
+ // @Path: AP_TemperatureSensor_Params.cpp
+ AP_SUBGROUPINFO(_params[0], "1_", 10, AP_TemperatureSensor, AP_TemperatureSensor_Params),
+
+#if AP_TEMPERATURE_SENSOR_MAX_INSTANCES > 1
+ // @Group: 2_
+ // @Path: AP_TemperatureSensor_Params.cpp
+ AP_SUBGROUPINFO(_params[1], "2_", 11, AP_TemperatureSensor, AP_TemperatureSensor_Params),
+#endif
+
+#if AP_TEMPERATURE_SENSOR_MAX_INSTANCES > 2
+ // @Group: 3_
+ // @Path: AP_TemperatureSensor_Params.cpp
+ AP_SUBGROUPINFO(_params[2], "3_", 12, AP_TemperatureSensor, AP_TemperatureSensor_Params),
+#endif
+
+ AP_GROUPEND
+};
+
+// Default Constructor
+AP_TemperatureSensor::AP_TemperatureSensor()
+{
+ AP_Param::setup_object_defaults(this, var_info);
+
+ if (_singleton != nullptr) {
+ AP_HAL::panic("AP_TemperatureSensor must be singleton");
+ }
+ _singleton = this;
+}
+
+// init - instantiate the temperature sensors
+void AP_TemperatureSensor::init()
+{
+ // check init has not been called before
+ if (_num_instances != 0) {
+ return;
+ }
+
+ // For Sub set the Default: Type to TSYS01 and I2C_ADDR of 0x77
+#if APM_BUILD_TYPE(APM_BUILD_ArduSub)
+ AP_Param::set_default_by_name("TEMP1_TYPE", (int8_t)AP_TemperatureSensor::Type::TSYS01);
+ AP_Param::set_default_by_name("TEMP1_ADDR", TSYS01_ADDR_CSB0);
+#endif
+
+ // create each instance
+ for (uint8_t instance = 0; instance < AP_TEMPERATURE_SENSOR_MAX_INSTANCES; instance++) {
+
+ switch (get_type(instance)) {
+#if AP_TEMPERATURE_SENSOR_TSYS01_ENABLE
+ case Type::TSYS01:
+ drivers[instance] = new AP_TemperatureSensor_TSYS01(*this, _state[instance], _params[instance]);
+ break;
+#endif
+ case Type::NONE:
+ default:
+ break;
+ }
+
+ // call init function for each backend
+ if (drivers[instance] != nullptr) {
+ drivers[instance]->init();
+ // _num_instances is actually the index for looping over instances
+ // the user may have TEMP_TYPE=0 and TEMP2_TYPE=7, in which case
+ // there will be a gap, but as we always check for drivers[instances] being nullptr
+ // this is safe
+ _num_instances = instance + 1;
+ }
+ }
+}
+
+// update: - For all active instances update temperature and log TEMP
+void AP_TemperatureSensor::update()
+{
+ for (uint8_t i=0; i<_num_instances; i++) {
+ if (drivers[i] != nullptr && get_type(i) != Type::NONE) {
+ drivers[i]->update();
+
+#if HAL_LOGGING_ENABLED
+ const AP_Logger *logger = AP_Logger::get_singleton();
+ if (logger != nullptr && _log_flag) {
+ const uint64_t time_us = AP_HAL::micros64();
+ drivers[i]->Log_Write_TEMP(time_us);
+ }
+#endif
+ }
+ }
+}
+
+AP_TemperatureSensor::Type AP_TemperatureSensor::get_type(uint8_t instance) const
+{
+ if (instance >= AP_TEMPERATURE_SENSOR_MAX_INSTANCES) {
+ return Type::NONE;
+ }
+ return (Type)_params[instance]._type.get();
+}
+
+// returns true if there is a temperature reading
+bool AP_TemperatureSensor::temperature(float &temp, const uint8_t instance) const
+{
+ if (instance >= AP_TEMPERATURE_SENSOR_MAX_INSTANCES || drivers[instance] == nullptr) {
+ return false;
+ }
+
+ temp = _state[instance].temperature;
+
+ return drivers[instance]->healthy();
+}
+
+namespace AP {
+
+AP_TemperatureSensor &temperature_sensor()
+{
+ return *AP_TemperatureSensor::get_singleton();
+}
+
+};
+
+#endif // AP_TEMPERATURE_SENSOR_ENABLED
diff --git a/libraries/AP_TemperatureSensor/AP_TemperatureSensor.h b/libraries/AP_TemperatureSensor/AP_TemperatureSensor.h
new file mode 100644
index 0000000000..9a14b7bf24
--- /dev/null
+++ b/libraries/AP_TemperatureSensor/AP_TemperatureSensor.h
@@ -0,0 +1,106 @@
+/*
+ This program is free software: you can redistribute it and/or modify
+ it under the terms of the GNU General Public License as published by
+ the Free Software Foundation, either version 3 of the License, or
+ (at your option) any later version.
+
+ This program is distributed in the hope that it will be useful,
+ but WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ GNU General Public License for more details.
+
+ You should have received a copy of the GNU General Public License
+ along with this program. If not, see .
+ */
+#pragma once
+
+#include
+
+#ifndef AP_TEMPERATURE_SENSOR_ENABLED
+#define AP_TEMPERATURE_SENSOR_ENABLED (BOARD_FLASH_SIZE > 1024)
+#endif
+
+#if AP_TEMPERATURE_SENSOR_ENABLED
+#include "AP_TemperatureSensor_Params.h"
+
+// maximum number of Temperature Sensors
+#ifndef AP_TEMPERATURE_SENSOR_MAX_INSTANCES
+#define AP_TEMPERATURE_SENSOR_MAX_INSTANCES 3
+#endif
+
+// first sensor is always the primary sensor
+#define AP_TEMPERATURE_SENSOR_PRIMARY_INSTANCE 0
+
+// declare backend class
+class AP_TemperatureSensor_Backend;
+class AP_TemperatureSensor_TSYS01;
+
+class AP_TemperatureSensor
+{
+ friend class AP_TemperatureSensor_Backend;
+ friend class AP_TemperatureSensor_TSYS01;
+
+public:
+
+ // temperature sensor types
+ enum class Type {
+ NONE = 0,
+ TSYS01 = 1,
+ };
+
+ // Constructor
+ AP_TemperatureSensor();
+
+ CLASS_NO_COPY(AP_TemperatureSensor);
+
+ static AP_TemperatureSensor *get_singleton() { return _singleton; }
+
+ // The TemperatureSensor_State structure is filled in by the backend driver
+ struct TemperatureSensor_State {
+ const struct AP_Param::GroupInfo *var_info;
+ uint32_t last_time_micros; // time when the sensor was last read in microseconds
+ float temperature; // temperature (deg C)
+ bool healthy; // temperature sensor is communicating correctly
+ uint8_t instance; // instance number
+ };
+
+ // Return the number of temperature sensors instances
+ uint8_t num_instances(void) const { return _num_instances; }
+
+ // detect and initialise any available temperature sensors
+ void init();
+
+ // Update the temperature for all temperature sensors
+ void update();
+
+ // get_type - returns temperature sensor type
+ Type get_type() const { return get_type(AP_TEMPERATURE_SENSOR_PRIMARY_INSTANCE); }
+ Type get_type(uint8_t instance) const;
+
+ // temperature
+ bool temperature(float &temp) const { return temperature(temp, AP_TEMPERATURE_SENSOR_PRIMARY_INSTANCE); }
+ bool temperature(float &temp, const uint8_t instance) const;
+
+ static const struct AP_Param::GroupInfo var_info[];
+
+protected:
+ // parameters
+ AP_TemperatureSensor_Params _params[AP_TEMPERATURE_SENSOR_MAX_INSTANCES];
+
+private:
+ static AP_TemperatureSensor *_singleton;
+
+ TemperatureSensor_State _state[AP_TEMPERATURE_SENSOR_MAX_INSTANCES];
+ AP_TemperatureSensor_Backend *drivers[AP_TEMPERATURE_SENSOR_MAX_INSTANCES];
+
+ uint8_t _num_instances; // number of temperature sensors
+
+ // Parameters
+ AP_Int8 _log_flag; // log_flag: true if we should log all sensors data
+};
+
+namespace AP {
+ AP_TemperatureSensor &temperature_sensor();
+};
+
+#endif // AP_TEMPERATURE_SENSOR_ENABLED
diff --git a/libraries/AP_TemperatureSensor/AP_TemperatureSensor_Backend.cpp b/libraries/AP_TemperatureSensor/AP_TemperatureSensor_Backend.cpp
new file mode 100644
index 0000000000..86e3e1e73f
--- /dev/null
+++ b/libraries/AP_TemperatureSensor/AP_TemperatureSensor_Backend.cpp
@@ -0,0 +1,47 @@
+/*
+ This program is free software: you can redistribute it and/or modify
+ it under the terms of the GNU General Public License as published by
+ the Free Software Foundation, either version 3 of the License, or
+ (at your option) any later version.
+
+ This program is distributed in the hope that it will be useful,
+ but WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ GNU General Public License for more details.
+
+ You should have received a copy of the GNU General Public License
+ along with this program. If not, see .
+ */
+
+#include "AP_TemperatureSensor.h"
+
+#if AP_TEMPERATURE_SENSOR_ENABLED
+#include "AP_TemperatureSensor_Backend.h"
+
+#include
+#include
+#include
+
+/*
+ base class constructor.
+ This incorporates initialisation as well.
+*/
+AP_TemperatureSensor_Backend::AP_TemperatureSensor_Backend(AP_TemperatureSensor &front, AP_TemperatureSensor::TemperatureSensor_State &state,
+ AP_TemperatureSensor_Params ¶ms):
+ _front(front),
+ _state(state),
+ _params(params)
+{
+}
+
+void AP_TemperatureSensor_Backend::Log_Write_TEMP(uint64_t time_us) const
+{
+ AP::logger().Write("TEMP",
+ "TimeUS," "Instance," "Temp", // labels
+ "s" "#" "O" , // units
+ "F" "-" "0" , // multipliers
+ "Q" "B" "f" , // types
+ time_us, _state.instance, _state.temperature);
+}
+
+#endif // AP_TEMPERATURE_SENSOR_ENABLED
diff --git a/libraries/AP_TemperatureSensor/AP_TemperatureSensor_Backend.h b/libraries/AP_TemperatureSensor/AP_TemperatureSensor_Backend.h
new file mode 100644
index 0000000000..6e3fb2d7a2
--- /dev/null
+++ b/libraries/AP_TemperatureSensor/AP_TemperatureSensor_Backend.h
@@ -0,0 +1,45 @@
+/*
+ This program is free software: you can redistribute it and/or modify
+ it under the terms of the GNU General Public License as published by
+ the Free Software Foundation, either version 3 of the License, or
+ (at your option) any later version.
+
+ This program is distributed in the hope that it will be useful,
+ but WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ GNU General Public License for more details.
+
+ You should have received a copy of the GNU General Public License
+ along with this program. If not, see .
+ */
+#pragma once
+
+#include "AP_TemperatureSensor.h"
+
+#if AP_TEMPERATURE_SENSOR_ENABLED
+
+class AP_TemperatureSensor_Backend
+{
+public:
+ // constructor. This incorporates initialisation as well.
+ AP_TemperatureSensor_Backend(AP_TemperatureSensor &front, AP_TemperatureSensor::TemperatureSensor_State &state, AP_TemperatureSensor_Params ¶ms);
+
+ // initialise
+ virtual void init() {};
+
+ // update the latest temperature
+ virtual void update() = 0;
+
+ // do we have a valid temperature reading?
+ bool healthy(void) const { return _state.healthy; }
+
+ // logging functions
+ void Log_Write_TEMP(const uint64_t time_us) const;
+
+protected:
+ AP_TemperatureSensor &_front; // reference to front-end
+ AP_TemperatureSensor::TemperatureSensor_State &_state; // reference to this instance's state (held in the front-end)
+ AP_TemperatureSensor_Params &_params; // reference to this instance's parameters (held in the front-end)
+};
+
+#endif // AP_TEMPERATURE_SENSOR_ENABLED
diff --git a/libraries/AP_TemperatureSensor/AP_TemperatureSensor_Params.cpp b/libraries/AP_TemperatureSensor/AP_TemperatureSensor_Params.cpp
new file mode 100644
index 0000000000..449c4cdd28
--- /dev/null
+++ b/libraries/AP_TemperatureSensor/AP_TemperatureSensor_Params.cpp
@@ -0,0 +1,50 @@
+#include "AP_TemperatureSensor_Params.h"
+#include "AP_TemperatureSensor.h"
+
+#if AP_TEMPERATURE_SENSOR_ENABLED
+
+#ifndef HAL_TEMPERATURE_SENSOR_I2C_TYPE_DEFAULT
+#define HAL_TEMPERATURE_SENSOR_I2C_TYPE_DEFAULT 0
+#endif
+
+#ifndef HAL_TEMPERATURE_SENSOR_I2C_ADDR_DEFAULT
+#define HAL_TEMPERATURE_SENSOR_I2C_ADDR_DEFAULT 0
+#endif
+
+#ifndef HAL_TEMPERATURE_SENSOR_I2C_BUS_DEFAULT
+#define HAL_TEMPERATURE_SENSOR_I2C_BUS_DEFAULT 0
+#endif
+
+const AP_Param::GroupInfo AP_TemperatureSensor_Params::var_info[] = {
+ // @Param: TYPE
+ // @DisplayName: Temperature Sensor Type
+ // @Description: Enables temperature sensors
+ // @Values: 0:Disabled, 1:TSYS01
+ // @User: Standard
+ // @RebootRequired: True
+ AP_GROUPINFO_FLAGS("TYPE", 1, AP_TemperatureSensor_Params, _type, HAL_TEMPERATURE_SENSOR_I2C_TYPE_DEFAULT, AP_PARAM_FLAG_ENABLE),
+
+ // @Param: BUS
+ // @DisplayName: Temperature sensor I2C bus
+ // @Description: Temperature sensor I2C bus number
+ // @Range: 0 3
+ // @User: Advanced
+ // @RebootRequired: True
+ AP_GROUPINFO("BUS", 3, AP_TemperatureSensor_Params, _i2c_bus, HAL_TEMPERATURE_SENSOR_I2C_BUS_DEFAULT),
+
+ // @Param: ADDR
+ // @DisplayName: Temperature sensor I2C address
+ // @Description: Temperature sensor I2C address
+ // @Range: 0 127
+ // @User: Advanced
+ // @RebootRequired: True
+ AP_GROUPINFO("ADDR", 4, AP_TemperatureSensor_Params, _i2c_address, HAL_TEMPERATURE_SENSOR_I2C_ADDR_DEFAULT),
+
+ AP_GROUPEND
+};
+
+AP_TemperatureSensor_Params::AP_TemperatureSensor_Params(void) {
+ AP_Param::setup_object_defaults(this, var_info);
+}
+
+#endif // AP_TEMPERATURE_SENSOR_ENABLED
diff --git a/libraries/AP_TemperatureSensor/AP_TemperatureSensor_Params.h b/libraries/AP_TemperatureSensor/AP_TemperatureSensor_Params.h
new file mode 100644
index 0000000000..0b2051c74c
--- /dev/null
+++ b/libraries/AP_TemperatureSensor/AP_TemperatureSensor_Params.h
@@ -0,0 +1,16 @@
+#pragma once
+
+#include
+
+class AP_TemperatureSensor_Params {
+public:
+ static const struct AP_Param::GroupInfo var_info[];
+
+ AP_TemperatureSensor_Params(void);
+
+ CLASS_NO_COPY(AP_TemperatureSensor_Params);
+
+ AP_Int8 _type; // 0=disabled, others see frontend enum TYPE
+ AP_Int8 _i2c_bus; // I2C bus number
+ AP_Int8 _i2c_address; // I2C address
+};
diff --git a/libraries/AP_TemperatureSensor/TSYS01.cpp b/libraries/AP_TemperatureSensor/AP_TemperatureSensor_TSYS01.cpp
similarity index 57%
rename from libraries/AP_TemperatureSensor/TSYS01.cpp
rename to libraries/AP_TemperatureSensor/AP_TemperatureSensor_TSYS01.cpp
index 9a16ff3e29..1debf179cc 100644
--- a/libraries/AP_TemperatureSensor/TSYS01.cpp
+++ b/libraries/AP_TemperatureSensor/AP_TemperatureSensor_TSYS01.cpp
@@ -1,5 +1,6 @@
-#include "TSYS01.h"
+#include "AP_TemperatureSensor_TSYS01.h"
+#if AP_TEMPERATURE_SENSOR_TSYS01_ENABLE
#include
#include
#include
@@ -13,30 +14,37 @@ 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;
-bool TSYS01::init(uint8_t bus)
+void AP_TemperatureSensor_TSYS01::init()
{
- _dev = std::move(hal.i2c_mgr->get_device(bus, TSYS01_ADDR));
- if (!_dev) {
- printf("TSYS01 device is null!");
- return false;
+ constexpr char name[] = "TSYS01";
+
+ // I2C Address: Default to using TSYS01_ADDR_CSB0 & Check I2C Address is Correct
+ uint8_t addr = _params._i2c_address;
+ if ((addr != TSYS01_ADDR_CSB0) && (addr != TSYS01_ADDR_CSB1)) {
+ printf("%s, wrong I2C addr", name);
+ return;
}
- _dev->get_semaphore()->take_blocking();
+ _dev = std::move(hal.i2c_mgr->get_device(_params._i2c_bus, _params._i2c_address));
+ if (!_dev) {
+ printf("%s device is null!", name);
+ return;
+ }
+
+ WITH_SEMAPHORE(_dev->get_semaphore());
_dev->set_retries(10);
if (!_reset()) {
- printf("TSYS01 reset failed");
- _dev->get_semaphore()->give();
- return false;
+ printf("%s reset failed", name);
+ return;
}
hal.scheduler->delay(4);
if (!_read_prom()) {
- printf("TSYS01 prom read failed");
- _dev->get_semaphore()->give();
- return false;
+ printf("%s prom read failed", name);
+ return;
}
_convert();
@@ -44,16 +52,13 @@ bool TSYS01::init(uint8_t bus)
// 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 * AP_USEC_PER_MSEC,
- FUNCTOR_BIND_MEMBER(&TSYS01::_timer, void));
- return true;
+ FUNCTOR_BIND_MEMBER(&AP_TemperatureSensor_TSYS01::_timer, void));
}
-bool TSYS01::_reset() const
+bool AP_TemperatureSensor_TSYS01::_reset() const
{
return _dev->transfer(&TSYS01_CMD_RESET, 1, nullptr, 0);
}
@@ -68,7 +73,7 @@ bool TSYS01::_reset() const
// 5 0xAA -> _k[0]
// 6 0xAC -> unused
// 7 0xAE -> unused
-bool TSYS01::_read_prom()
+bool AP_TemperatureSensor_TSYS01::_read_prom()
{
bool success = false;
for (int i = 0; i < 5; i++) {
@@ -80,7 +85,7 @@ bool TSYS01::_read_prom()
}
// Borrowed from MS Baro driver
-uint16_t TSYS01::_read_prom_word(uint8_t word) const
+uint16_t AP_TemperatureSensor_TSYS01::_read_prom_word(uint8_t word) const
{
const uint8_t reg = TSYS01_CMD_READ_PROM + (word << 1);
uint8_t val[2];
@@ -90,12 +95,12 @@ uint16_t TSYS01::_read_prom_word(uint8_t word) const
return (val[0] << 8) | val[1];
}
-bool TSYS01::_convert() const
+bool AP_TemperatureSensor_TSYS01::_convert() const
{
return _dev->transfer(&TSYS01_CMD_CONVERT, 1, nullptr, 0);
}
-uint32_t TSYS01::_read_adc() const
+uint32_t AP_TemperatureSensor_TSYS01::_read_adc() const
{
uint8_t val[3];
if (!_dev->transfer(&TSYS01_CMD_READ_ADC, 1, val, 3)) {
@@ -104,29 +109,31 @@ uint32_t TSYS01::_read_adc() const
return (val[0] << 16) | (val[1] << 8) | val[2];
}
-void TSYS01::_timer(void)
+void AP_TemperatureSensor_TSYS01::_timer(void)
{
- uint32_t adc = _read_adc();
- _healthy = adc != 0;
+ const uint32_t adc = _read_adc();
+ _state.healthy = adc != 0;
- if (_healthy) {
+ if (_state.healthy) {
_calculate(adc);
} else {
- _temperature = 0;
+ _state.temperature = 0;
}
- //printf("\nTemperature: %.2lf C", _temperature);
+ //printf("\nTemperature: %.2lf C", _state.temperature;
_convert();
}
-void TSYS01::_calculate(uint32_t adc)
+void AP_TemperatureSensor_TSYS01::_calculate(uint32_t adc)
{
- float adc16 = adc/256;
- _temperature =
+ const float adc16 = adc/256;
+ _state.temperature =
-2 * _k[4] * powf(10, -21) * powf(adc16, 4) +
4 * _k[3] * powf(10, -16) * powf(adc16, 3) +
-2 * _k[2] * powf(10, -11) * powf(adc16, 2) +
1 * _k[1] * powf(10, -6) * adc16 +
-1.5 * _k[0] * powf(10, -2);
}
+
+#endif // AP_TEMPERATURE_SENSOR_TSYS01_ENABLE
diff --git a/libraries/AP_TemperatureSensor/AP_TemperatureSensor_TSYS01.h b/libraries/AP_TemperatureSensor/AP_TemperatureSensor_TSYS01.h
new file mode 100644
index 0000000000..23e4d43221
--- /dev/null
+++ b/libraries/AP_TemperatureSensor/AP_TemperatureSensor_TSYS01.h
@@ -0,0 +1,50 @@
+/*
+ * I2C driver for Measurement Specialties MEAS TSYS01 digital temperature sensor
+ */
+
+#pragma once
+#include "AP_TemperatureSensor_Backend.h"
+
+#ifndef AP_TEMPERATURE_SENSOR_TSYS01_ENABLE
+#define AP_TEMPERATURE_SENSOR_TSYS01_ENABLE AP_TEMPERATURE_SENSOR_ENABLED
+#endif
+
+#if AP_TEMPERATURE_SENSOR_TSYS01_ENABLE
+#include
+#include
+#include
+
+#define TSYS01_ADDR_CSB0 0x77
+#define TSYS01_ADDR_CSB1 0x76
+
+class AP_TemperatureSensor_TSYS01 : public AP_TemperatureSensor_Backend {
+ using AP_TemperatureSensor_Backend::AP_TemperatureSensor_Backend;
+
+public:
+ void init(void) override;
+
+ void update() override {};
+
+ AP_HAL::OwnPtr _dev;
+
+private:
+ uint16_t _k[5]; // internal calibration for temperature calculation
+
+ // reset device
+ bool _reset(void) const;
+
+ // read (relevant) internal calibration registers into _k
+ bool _read_prom(void);
+ uint16_t _read_prom_word(uint8_t word) const;
+
+ // begin an ADC conversion (min:7.40ms typ:8.22ms max:9.04ms)
+ bool _convert(void) const;
+ uint32_t _read_adc(void) const;
+
+ // update the temperature, called at 20Hz
+ void _timer(void);
+
+ // calculate temperature using adc reading and internal calibration
+ void _calculate(uint32_t adc);
+};
+#endif // AP_TEMPERATURE_SENSOR_TSYS01_ENABLE
diff --git a/libraries/AP_TemperatureSensor/TSYS01.h b/libraries/AP_TemperatureSensor/TSYS01.h
deleted file mode 100644
index 9c93067330..0000000000
--- a/libraries/AP_TemperatureSensor/TSYS01.h
+++ /dev/null
@@ -1,35 +0,0 @@
-/*
- * I2C driver for Measurement Specialties MEAS TSYS01 digital temperature sensor
- */
-
-#pragma once
-
-#include
-#include
-#include
-
-#define TSYS01_ADDR 0x77
-
-class TSYS01 {
-public:
-
- bool init(uint8_t bus);
- float temperature(void) const { return _temperature; } // temperature in degrees C
- bool healthy(void) const { // do we have a valid temperature reading?
- return _healthy;
- }
-
- 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) const; // reset device
- bool _read_prom(void); // read (relevant) internal calibration registers into _k
- bool _convert(void) const; // begin an ADC conversion (min:7.40ms typ:8.22ms max:9.04ms)
- uint32_t _read_adc(void) const;
- uint16_t _read_prom_word(uint8_t word) const;
- void _timer(void); // update the temperature, called at 20Hz
- void _calculate(uint32_t adc); // calculate temperature using adc reading and internal calibration
-};