diff --git a/libraries/AP_TemperatureSensor/AP_TemperatureSensor.cpp b/libraries/AP_TemperatureSensor/AP_TemperatureSensor.cpp index 0eebe6dce6..857bed4f84 100644 --- a/libraries/AP_TemperatureSensor/AP_TemperatureSensor.cpp +++ b/libraries/AP_TemperatureSensor/AP_TemperatureSensor.cpp @@ -26,6 +26,7 @@ #include "AP_TemperatureSensor_TSYS01.h" #include "AP_TemperatureSensor_MCP9600.h" +#include "AP_TemperatureSensor_MAX31865.h" #include #include @@ -134,6 +135,11 @@ void AP_TemperatureSensor::init() case AP_TemperatureSensor_Params::Type::MCP9600: drivers[instance] = new AP_TemperatureSensor_MCP9600(*this, _state[instance], _params[instance]); break; +#endif +#if AP_TEMPERATURE_SENSOR_MAX31865_ENABLED + case AP_TemperatureSensor_Params::Type::MAX31865: + drivers[instance] = new AP_TemperatureSensor_MAX31865(*this, _state[instance], _params[instance]); + break; #endif case AP_TemperatureSensor_Params::Type::NONE: default: diff --git a/libraries/AP_TemperatureSensor/AP_TemperatureSensor.h b/libraries/AP_TemperatureSensor/AP_TemperatureSensor.h index ed357fd05b..14c02b3e6b 100644 --- a/libraries/AP_TemperatureSensor/AP_TemperatureSensor.h +++ b/libraries/AP_TemperatureSensor/AP_TemperatureSensor.h @@ -24,12 +24,14 @@ class AP_TemperatureSensor_Backend; class AP_TemperatureSensor_TSYS01; class AP_TemperatureSensor_MCP9600; +class AP_TemperatureSensor_MAX31865; class AP_TemperatureSensor { friend class AP_TemperatureSensor_Backend; friend class AP_TemperatureSensor_TSYS01; friend class AP_TemperatureSensor_MCP9600; + friend class AP_TemperatureSensor_MAX31865; public: diff --git a/libraries/AP_TemperatureSensor/AP_TemperatureSensor_MAX31865.cpp b/libraries/AP_TemperatureSensor/AP_TemperatureSensor_MAX31865.cpp new file mode 100644 index 0000000000..f85ec04ebe --- /dev/null +++ b/libraries/AP_TemperatureSensor/AP_TemperatureSensor_MAX31865.cpp @@ -0,0 +1,139 @@ +/* + * Copyright (C) 2023 Kraus Hamdani Aerospace Inc. All rights reserved. + * + * This file 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 file 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 . + * + * Code by Tom Pittenger + */ +/* + Implements SPI driver for MAX31865 digital RTD Temperature converter +*/ + +#include "AP_TemperatureSensor_MAX31865.h" + +#if AP_TEMPERATURE_SENSOR_MAX31865_ENABLED +#include +#include +#include + +extern const AP_HAL::HAL &hal; + +#define MAX31865_REG_WRITE_ADDR_OFFSET 0x80 + + +#define MAX31865_REG_CONFIG_READ 0x00 +#define MAX31865_REG_CONFIG_WRITE (MAX31865_REG_CONFIG_READ | MAX31865_REG_WRITE_ADDR_OFFSET) +#define MAX31865_REG_DATA_MSB 0x01 +#define MAX31865_REG_DATA_LSB 0x02 +#define MAX31865_REG_HIGH_FAULT_THRESH_MSB 0x03 +#define MAX31865_REG_HIGH_FAULT_THRESH_LSB 0x04 +#define MAX31865_REG_LOW_FAULT_THRESH_MSB 0x05 +#define MAX31865_REG_LOW_FAULT_THRESH_LSB 0x06 +#define MAX31865_REG_FAULT_STATUS_READ 0x07 +#define MAX31865_REG_FAULT_STATUS_WRITE (MAX31865_REG_FAULT_STATUS_READ | MAX31865_REG_WRITE_ADDR_OFFSET) + + +// Vbias = ON (must be on for automatic mode) +// Conversion mode: Auto +// Fault Status: 1set 1 to Clear all latched faults +#define MAX31865_CONFIG_VALUE (0b11000010) + +#define MAX31865_DEBUGGING 0 + +#if MAX31865_DEBUGGING +#ifdef HAL_BUILD_AP_PERIPH + extern "C" { + void can_printf(const char *fmt, ...); + } + //# define Debug(fmt, args ...) do { can_printf("%s:%d: " fmt "\n", __FUNCTION__, __LINE__, ## args); hal.scheduler->delay(1); } while(0) + # define Debug(fmt, args ...) do { can_printf(fmt, ## args); } while(0) +#else + # define Debug(fmt, args ...) do {hal.console->printf("%s:%d: " fmt "\n", __FUNCTION__, __LINE__, ## args); hal.scheduler->delay(1); } while(0) + #endif // HAL_BUILD_AP_PERIPH +#else + # define Debug(fmt, args ...) +#endif // MAX31865_DEBUGGING + + +void AP_TemperatureSensor_MAX31865::init() +{ + if (_dev) { + return; + } + + // also look for "max31865_n" where n is the addr param. + char name[12] = "max31865"; + if (_params.bus_address > 0 && _params.bus_address <= 9) { + name[8] = '_'; + name[9] = '0' + _params.bus_address; // convert bus_address to ascii + name[10] = 0; + } + + _dev = std::move(hal.spi->get_device(name)); + if (!_dev) { + return; + } + + WITH_SEMAPHORE(_dev->get_semaphore()); + + _dev->set_speed(AP_HAL::Device::SPEED_LOW); + + _dev->write_register(MAX31865_REG_CONFIG_WRITE, MAX31865_CONFIG_VALUE); + + _dev->set_speed(AP_HAL::Device::SPEED_HIGH); + + /* Request 10Hz update */ + _dev->register_periodic_callback(100 * AP_USEC_PER_MSEC, + FUNCTOR_BIND_MEMBER(&AP_TemperatureSensor_MAX31865::thread_tick, void)); +} + +void AP_TemperatureSensor_MAX31865::thread_tick() +{ + uint16_t raw_data; + if (!_dev->read_registers(MAX31865_REG_DATA_MSB, (uint8_t *)&raw_data, sizeof(raw_data))) { + return; + } + + // 16bit byte swap + const uint16_t data = htobe16(raw_data); + + // fault is LSB bit, temperature data is upper 15 bits + const bool is_fault = (data & 0x0001) != 0; + const uint16_t data_temperature = data >> 1; + + if (is_fault) { +#if MAX31865_DEBUGGING + uint8_t data_fault = 0; + _dev->read_registers(MAX31865_REG_FAULT_STATUS_READ, (uint8_t *)&data_fault, 1); + Debug("%d MAX31865 fault: 0x02X", _state.instance, data_fault); +#endif + // clear the fault + _dev->write_register(MAX31865_REG_CONFIG_WRITE, MAX31865_CONFIG_VALUE); + return; + } + + // For a temperature range of -100 C to +100 C, a good approximation of + // temperature can be made with simple linearization. This equation gives + // 0 C error at 0C, -1.75 C error at -100 C, and -1.4 C error at +100 C + const float temperature = ((float)data_temperature/32.0f) - 256.0f; + +#if MAX31865_DEBUGGING + Debug("%d MAX31865 %.2f C", _state.instance, temperature); +#endif + + set_temperature(temperature); +} + +#endif // AP_TEMPERATURE_SENSOR_MAX31865_ENABLED + diff --git a/libraries/AP_TemperatureSensor/AP_TemperatureSensor_MAX31865.h b/libraries/AP_TemperatureSensor/AP_TemperatureSensor_MAX31865.h new file mode 100644 index 0000000000..cf6682c78e --- /dev/null +++ b/libraries/AP_TemperatureSensor/AP_TemperatureSensor_MAX31865.h @@ -0,0 +1,40 @@ +/* + * Copyright (C) 2023 Kraus Hamdani Aerospace Inc. All rights reserved. + * + * This file 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 file 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 . + * + * Code by Tom Pittenger + */ +/* + Implements SPI driver for MAX31865 digital RTD Temperature converter +*/ + +#pragma once +#include "AP_TemperatureSensor_Backend.h" + +#if AP_TEMPERATURE_SENSOR_MAX31865_ENABLED + +class AP_TemperatureSensor_MAX31865 : public AP_TemperatureSensor_Backend { + using AP_TemperatureSensor_Backend::AP_TemperatureSensor_Backend; +public: + + void init(void) override; + + void update(void) override {}; + +private: + void thread_tick(void); +}; + +#endif // AP_TEMPERATURE_SENSOR_MAX31865_ENABLED diff --git a/libraries/AP_TemperatureSensor/AP_TemperatureSensor_Params.cpp b/libraries/AP_TemperatureSensor/AP_TemperatureSensor_Params.cpp index d3bd11bd6b..e3bed869b7 100644 --- a/libraries/AP_TemperatureSensor/AP_TemperatureSensor_Params.cpp +++ b/libraries/AP_TemperatureSensor/AP_TemperatureSensor_Params.cpp @@ -34,7 +34,7 @@ const AP_Param::GroupInfo AP_TemperatureSensor_Params::var_info[] = { // @Param: TYPE // @DisplayName: Temperature Sensor Type // @Description: Enables temperature sensors - // @Values: 0:Disabled, 1:TSYS01, 2:MCP9600 + // @Values: 0:Disabled, 1:TSYS01, 2:MCP9600, 3:MAX31865 // @User: Standard // @RebootRequired: True AP_GROUPINFO_FLAGS("TYPE", 1, AP_TemperatureSensor_Params, type, (float)Type::NONE, AP_PARAM_FLAG_ENABLE), diff --git a/libraries/AP_TemperatureSensor/AP_TemperatureSensor_Params.h b/libraries/AP_TemperatureSensor/AP_TemperatureSensor_Params.h index ce7a8dc04a..1e4bfbf22e 100644 --- a/libraries/AP_TemperatureSensor/AP_TemperatureSensor_Params.h +++ b/libraries/AP_TemperatureSensor/AP_TemperatureSensor_Params.h @@ -30,6 +30,7 @@ public: NONE = 0, TSYS01 = 1, MCP9600 = 2, + MAX31865 = 3, }; // option to map to another system component diff --git a/libraries/AP_TemperatureSensor/AP_TemperatureSensor_config.h b/libraries/AP_TemperatureSensor/AP_TemperatureSensor_config.h index da8268123f..b721cbebfd 100644 --- a/libraries/AP_TemperatureSensor/AP_TemperatureSensor_config.h +++ b/libraries/AP_TemperatureSensor/AP_TemperatureSensor_config.h @@ -16,6 +16,10 @@ #endif #endif +#ifndef AP_TEMPERATURE_SENSOR_MAX31865_ENABLED + #define AP_TEMPERATURE_SENSOR_MAX31865_ENABLED AP_TEMPERATURE_SENSOR_ENABLED +#endif + // maximum number of Temperature Sensors