First version with basic recording

This commit is contained in:
Christoph Hagen 2023-05-29 18:29:15 +02:00
commit e1852d2989
11 changed files with 1015 additions and 0 deletions

2
.gitattributes vendored Normal file
View File

@ -0,0 +1,2 @@
# Auto detect text files and perform LF normalization
* text=auto

36
.gitignore vendored Normal file
View File

@ -0,0 +1,36 @@
# Prerequisites
*.d
# Compiled Object files
*.slo
*.lo
*.o
*.obj
# Precompiled Headers
*.gch
*.pch
# Compiled Dynamic libraries
*.so
*.dylib
*.dll
# Fortran module files
*.mod
*.smod
# Compiled Static libraries
*.lai
*.la
*.a
*.lib
# Executables
*.exe
*.out
*.app
.pio
.vscode
.DS_Store

10
include/bluetooth.h Normal file
View File

@ -0,0 +1,10 @@
#pragma once
#include <stdint.h>
void bluetoothConfigure();
bool bluetoothIsConnected();
// In main.cpp
uint32_t secondsUntilNextTemperatureMeasurement();

14
include/config.h Normal file
View File

@ -0,0 +1,14 @@
#pragma once
#include <stdint.h>
#include <hal/gpio_types.h>
constexpr uint32_t serialBaudRate = 115200;
constexpr gpio_num_t wakeupButtonPin = GPIO_NUM_13;
// The time (in seconds) for which the device should stay awake after the button is pressed
// The device also stays awake as long as a bluetooth connection is active
constexpr uint32_t wakeupDurationAfterButtonPress = 30;
constexpr uint32_t temperatureMeasurementIntervalSeconds = 60;

45
include/storage.h Normal file
View File

@ -0,0 +1,45 @@
#pragma once
#include <stddef.h>
#include <stdint.h>
#include "temperature.h"
constexpr uint8_t temperatureSensorCount = 2;
constexpr size_t storageSize = 80000;
constexpr uint16_t storageIntervalInSeconds = 15;
constexpr size_t maximumStorageDurationInHours = (storageSize / TEMPERATURE_SENSOR_MAX_COUNT) * storageIntervalInSeconds / 3600;
// Max size: 7664
constexpr size_t maxRtcStorageSize = 7664;
constexpr size_t rtcStorageSize = 7632;
constexpr size_t maxEepromSize = 13350;
constexpr size_t eepromSize = 13350;
// The minimum temperature to store, in millidegrees celcius
// True minimum will be higher by 1°, since two values are reserved
constexpr long temperatureShiftForStorage = -40000;
constexpr long maximumTemperature = temperatureShiftForStorage + 255 * 500;
constexpr uint8_t temperatureMaximumValue = 255;
void storageConfigure();
/**
* @brief Save temperatures for both temperature sensors
*
* Temperatures are in millidegrees celsius
*
* @param temperatures The array of temperatures
* @param count The number of elements in the array
*/
void saveTemperatures(Temperature* temperatures);
void saveTemperatureAtCurrentIndex(Temperature temp);
uint16_t getTotalNumberOfStoredBytes();
uint16_t getRecordedBytesAtOffset(uint8_t* buffer, uint16_t offset, uint16_t count);
void discardAllRecordedBytes();

34
include/temperature.h Normal file
View File

@ -0,0 +1,34 @@
#pragma once
#include <stdint.h>
constexpr uint8_t TEMPERATURE_SENSOR_MAX_COUNT = 2;
constexpr uint8_t temperatureSensorNotAvailable = 0;
constexpr uint8_t temperatureSensorFailure = 1;
constexpr uint8_t temperatureMinimumValue = 2;
enum class TemperatureStatus {
sensorNotFound = temperatureSensorNotAvailable,
sensorError = temperatureSensorFailure,
temperatureIsValid = temperatureMinimumValue,
};
struct Temperature {
TemperatureStatus status;
/**
* @brief The temperature value, in millidegrees celsius
* This value is only valid if the status is `temperatureIsValid`
*/
long value;
};
void temperatureConfigure();
void temperaturePerformUpdate(Temperature* temperatures);

19
platformio.ini Normal file
View File

@ -0,0 +1,19 @@
; PlatformIO Project Configuration File
;
; Build options: build flags, source filter
; Upload options: custom upload port, speed and extra flags
; Library options: dependencies, extra library storages
; Advanced options: extra scripting
;
; Please visit documentation for the other options and examples
; https://docs.platformio.org/page/projectconf.html
[env:az-delivery-devkit-v4]
platform = espressif32
board = az-delivery-devkit-v4
framework = arduino
lib_deps =
pstolarz/OneWireNg @ ^0.11.2
monitor_speed = 115200
monitor_port = /dev/tty.usbserial-0001
upload_port = /dev/tty.usbserial-0001

299
src/bluetooth.cpp Normal file
View File

@ -0,0 +1,299 @@
#include <string.h>
#include <Esp.h>
#include <BLEDevice.h>
#include <BLEUtils.h>
#include <BLEServer.h>
#include <BLE2902.h>
#include "bluetooth.h"
#include "storage.h"
#include "config.h"
constexpr size_t bluetoothMaxDataSize = 200;
constexpr uint16_t DEVINFO_UUID = 0x180a;
constexpr uint16_t DEVINFO_MANUFACTURER_UUID = 0x2a29;
constexpr uint16_t DEVINFO_NAME_UUID = 0x2a24;
constexpr uint16_t DEVINFO_SERIAL_UUID = 0x2a25;
const char* deviceName = "Window";
const char* manufacturerName = "CH";
const char* serviceUUID = "22071991-feed-deaf-babe-150420870001";
const char* characteristicUUID = "22071991-feed-deaf-babe-150420870002";
bool isConnected = false;
uint8_t bluetoothOutgoingBuffer[bluetoothMaxDataSize + 1];
uint8_t* bluetoothDataBuffer;
uint8_t* bluetoothResponse;
size_t bluetoothDataCount = 0;
void bluetoothStartAdvertising();
void bluetoothDidReceiveData(uint8_t* buffer, size_t count);
class BluetoothConnection: public BLEServerCallbacks {
public:
BluetoothConnection() {}
void onConnect(BLEServer *server) override {
isConnected = true;
}
void onDisconnect(BLEServer *server) override {
isConnected = false;
bluetoothStartAdvertising();
}
};
class Receiver : public BLECharacteristicCallbacks {
public:
Receiver() {}
void onWrite(BLECharacteristic *characteristic) {
size_t bytesReceived = characteristic->getLength();
uint8_t* data = characteristic->getData();
bluetoothDidReceiveData(data, bytesReceived);
}
void onRead(BLECharacteristic *characteristic) {
characteristic->setValue(bluetoothOutgoingBuffer, bluetoothDataCount + 1); // Add 1 byte for response
}
};
BluetoothConnection bluetooth{};
BLE2902 descriptor{};
BLEServer *server;
Receiver receiver{};
void bluetoothConfigure() {
bluetoothDataBuffer = bluetoothOutgoingBuffer + 1;
bluetoothResponse = bluetoothOutgoingBuffer;
// Setup BLE Server
BLEDevice::init(deviceName);
server = BLEDevice::createServer();
server->setCallbacks(&bluetooth);
// Register message service that can receive messages and reply with a static message.
BLEService *service = server->createService(serviceUUID);
uint32_t properties = BLECharacteristic::PROPERTY_READ | BLECharacteristic::PROPERTY_NOTIFY | BLECharacteristic::PROPERTY_WRITE;
BLECharacteristic* characteristicMessage = service->createCharacteristic(characteristicUUID, properties);
characteristicMessage->setCallbacks(&receiver);
characteristicMessage->addDescriptor(&descriptor);
service->start();
// Register device info service, that contains the device's UUID, manufacturer and name.
service = server->createService(DEVINFO_UUID);
BLECharacteristic *characteristic = service->createCharacteristic(DEVINFO_MANUFACTURER_UUID, BLECharacteristic::PROPERTY_READ);
characteristic->setValue(manufacturerName);
characteristic = service->createCharacteristic(DEVINFO_NAME_UUID, BLECharacteristic::PROPERTY_READ);
characteristic->setValue(deviceName);
characteristic = service->createCharacteristic(DEVINFO_SERIAL_UUID, BLECharacteristic::PROPERTY_READ);
uint32_t id = ESP.getEfuseMac() >> 24;
char chipId[9] = {0};
sprintf(chipId, "%x", id);
characteristic->setValue(chipId);
service->start();
}
void bluetoothStartAdvertising() {
// Advertise services
BLEAdvertising *advertisement = server->getAdvertising();
BLEAdvertisementData adv;
adv.setName(deviceName);
adv.setCompleteServices(BLEUUID(serviceUUID));
advertisement->setAdvertisementData(adv);
advertisement->start();
}
bool bluetoothIsConnected() {
return isConnected;
}
enum class BluetoothRequest: uint8_t {
/**
* @brief Request the number of bytes already recorded
*
* Request:
* - No additional bytes expected
*
* Response:
* - BluetoothResponse::success, plus the number of bytes as a uint16_t (2 bytes)
*/
getNumberOfRecordedBytes = 0,
/**
* @brief Request recording data
*
* Request:
* - Bytes 1-2: Memory offset (uint16_t)
* - Bytes 3-4: Number of bytes (uint16_t)
*
* Response:
* - BluetoothResponse::success, plus the requested bytes
* - BluetoothResponse::responseTooLarge if too many bytes are requested
*/
getRecordingData = 1,
/**
* @brief Request deletion of recordings
*
* Request:
* - Bytes 1-2: Number of bytes to clear (uint16_t)
*
* Response:
* - BluetoothResponse::success
* - BluetoothResponse::invalidNumberOfBytesToDelete, if the number of bytes does not match.
* This may happen when a new temperature recording is performed in between calls
*/
clearRecordingBuffer = 2,
/**
* @brief Request the time since the device was turned on
*
* Request:
* - No additional bytes expected
*
* Response:
* - BluetoothResponse::success, plus the number of seconds as a uint32_t (4 bytes)
*/
getCurrentTime = 3,
/**
* @brief Request the number of seconds until the next measurement is performed
*
* Request:
* - No additional bytes expected
*
* Response:
* - BluetoothResponse::success, plus the number of seconds as a uint16_t (2 bytes)
*/
getSecondsUntilNextMeasurement = 4,
/**
* @brief Get the number of seconds
*
* Request:
* - No additional bytes expected
*
* Response:
* - BluetoothResponse::success, plus the number of seconds as a uint16_t (2 bytes)
*/
getMeasurementInterval = 5,
};
enum class BluetoothResponse: uint8_t {
/**
* @brief The response to the last request is provided
*
*/
success = 0,
/**
* @brief Invalid command received
*
*/
invalidCommand = 1,
responseTooLarge = 2,
unknownCommand = 3,
invalidNumberOfBytesToDelete = 4,
};
void setResponse(BluetoothResponse response) {
*bluetoothResponse = static_cast<uint8_t>(response);
}
void setResponseWithoutData(BluetoothResponse response) {
setResponse(response);
bluetoothDataCount = 0;
}
void bluetoothSetOutgoingData(BluetoothResponse response, const uint8_t* data, size_t count) {
if (count > bluetoothMaxDataSize) {
setResponseWithoutData(BluetoothResponse::responseTooLarge);
return;
}
memcpy(bluetoothDataBuffer, data, count);
bluetoothDataCount = count;
setResponse(response);
}
void setSuccessResponseWithNumber(uint16_t number) {
bluetoothSetOutgoingData(BluetoothResponse::success, (uint8_t*) &number, sizeof(uint16_t));
}
void setSuccessResponseWithUInt32(uint32_t number) {
bluetoothSetOutgoingData(BluetoothResponse::success, (uint8_t*) &number, sizeof(uint32_t));
}
uint16_t readNumberFromReceivedBuffer(uint8_t* buffer) {
return *((uint16_t*) buffer + 1);
}
void bluetoothDidReceiveData(uint8_t* buffer, size_t count) {
if (count < 1) {
setResponseWithoutData(BluetoothResponse::invalidCommand);
return;
}
uint16_t offset;
BluetoothRequest request = static_cast<BluetoothRequest>(buffer[0]);
switch (request)
{
case BluetoothRequest::getNumberOfRecordedBytes:
count = getTotalNumberOfStoredBytes();
setSuccessResponseWithNumber(count);
break;
case BluetoothRequest::clearRecordingBuffer:
count = *((uint16_t*) buffer + 1);
if (count != getTotalNumberOfStoredBytes()) {
setResponseWithoutData(BluetoothResponse::invalidNumberOfBytesToDelete);
} else {
discardAllRecordedBytes();
setResponseWithoutData(BluetoothResponse::success);
}
break;
case BluetoothRequest::getRecordingData:
if (count != 5) {
setResponseWithoutData(BluetoothResponse::invalidCommand);
break;
}
offset = *((uint16_t*) buffer + 1);
count = *((uint16_t*) buffer + 3);
if (count > bluetoothMaxDataSize) {
setResponseWithoutData(BluetoothResponse::responseTooLarge);
break;
}
count = getRecordedBytesAtOffset(bluetoothDataBuffer, offset, count);
bluetoothDataCount = count;
break;
case BluetoothRequest::getCurrentTime:
setSuccessResponseWithUInt32(time(NULL));
break;
case BluetoothRequest::getSecondsUntilNextMeasurement:
count = secondsUntilNextTemperatureMeasurement();
setSuccessResponseWithNumber(count);
break;
case BluetoothRequest::getMeasurementInterval:
setSuccessResponseWithNumber(temperatureMeasurementIntervalSeconds);
break;
default:
setResponseWithoutData(BluetoothResponse::unknownCommand);
break;
}
}

155
src/main.cpp Normal file
View File

@ -0,0 +1,155 @@
#include <Arduino.h>
#include <esp_wifi.h>
#include <driver/rtc_io.h>
#include "config.h"
#include "bluetooth.h"
#include "storage.h"
#include "temperature.h"
Temperature samples[temperatureSensorCount];
// Indicate when the next temperature measurement should be performed
// Time is measured in seconds since power-on via RTC clock
RTC_DATA_ATTR uint32_t nextTimeToMeasureTemperatureSeconds = 0;
// Indicate the time until the device should stay awake
// Updated when button is pressed
uint32_t timeUntilSleep = 0;
/**
* @brief Check if enough time has passed for the next temperature measurement
*
* @return true A measurement should be performed
* @return false Wait until more time has elapsed
*/
bool shouldMeasureTemperature() {
return time(NULL) >= nextTimeToMeasureTemperatureSeconds;
}
/**
* @brief Set the next time when temperatures should be measured
*/
void setNextTemperatureMeasurementInterval() {
nextTimeToMeasureTemperatureSeconds += temperatureMeasurementIntervalSeconds;
}
/**
* @brief Check how long to wait until the next temperature measurement
*
* @return uint32_t The number of seconds to wait
*/
uint32_t secondsUntilNextTemperatureMeasurement() {
// Time in seconds since power on
time_t currentTime = time(NULL);
if (currentTime >= nextTimeToMeasureTemperatureSeconds) {
return 0;
}
return nextTimeToMeasureTemperatureSeconds - currentTime;
}
void deepSleepUntilNextTemperatureMeasurement() {
uint32_t seconds = secondsUntilNextTemperatureMeasurement();
if (seconds == 0) {
// The time until next measurement is too short,
// so don't sleep and just wait a little
// May run the loop multiple times until the time is elapsed
delay(100);
} else {
esp_deep_sleep(seconds * 1000000);
}
}
/**
* @brief Check if the button is pressed
*
* This indicates that the device should stay awake and that Bluetooth should be enabled.
*
* @return true The button is pressed
* @return false The button is not pressed
*/
bool wakeupButtonIsPressed() {
return gpio_get_level(wakeupButtonPin) == 1;
}
void updateStayAwakeTime() {
timeUntilSleep = time(NULL) + wakeupDurationAfterButtonPress;
}
bool shouldStayAwakeDueToActivity() {
if (timeUntilSleep > time(NULL)) {
return true;
}
return bluetoothIsConnected();
}
void setup() {
// Configure LED pin
// Equivalent to:
// pinMode(1, OUTPUT);
gpio_config_t ledConfig;
ledConfig.pin_bit_mask = (1ULL << 1);
ledConfig.mode = GPIO_MODE_OUTPUT;
ledConfig.pull_down_en = GPIO_PULLDOWN_ENABLE;
//gpio_config(&ledConfig); // TODO: Enable LED
// Enable LED, as long as ESP is awake
// Equivalent to:
// digitalWrite(1, HIGH);
//gpio_set_level(GPIO_NUM_1, 1);
// Configure button pin to wake up ESP from deep sleep on high signal with pulldown
// GPIO13 -> RTC_GPIO14
rtc_gpio_pulldown_en(wakeupButtonPin);
rtc_gpio_wakeup_enable(wakeupButtonPin, GPIO_INTR_HIGH_LEVEL);
// Enable EEPROM to persist measurements
// Only needed if sufficient measurements are in RTC memory
storageConfigure();
if (wakeupButtonIsPressed()) {
bluetoothConfigure();
// Serial.println("Bluetooth configured");
}
// Configure the temperature sensors
// Happens only once per power cycle
temperatureConfigure();
// TODO: Remove
Serial.begin(serialBaudRate);
Serial.println("Setup complete");
}
void loop() {
if (shouldMeasureTemperature()) {
temperaturePerformUpdate(samples);
saveTemperatures(samples);
setNextTemperatureMeasurementInterval();
}
if (wakeupButtonIsPressed()) {
updateStayAwakeTime();
}
if (!shouldStayAwakeDueToActivity()) {
// May return, if less then one second to wait
// Otherwise control flow starts with setup() again
deepSleepUntilNextTemperatureMeasurement();
}
}
/*
255
0
0
255
0
0
*/

196
src/storage.cpp Normal file
View File

@ -0,0 +1,196 @@
#include "storage.h"
#include <EEPROM.h>
#include <esp_attr.h>
constexpr uint8_t absoluteTemperatureIndicator = 0xFF;
// The number of bytes to accumulate in RTC memory before writing it to EEPROM
constexpr uint32_t eepromChunkSize = 100;
// Storage for temperature measurements in RTC memory that survives deep sleep
RTC_DATA_ATTR uint8_t data[rtcStorageSize];
RTC_DATA_ATTR uint16_t dataIndex = 0;
// The index into EEPROM storage where the next data should be written
RTC_DATA_ATTR uint32_t eepromIndex = 0;
// Keeps the last valid temperatures for each sensor
RTC_DATA_ATTR Temperature lastTemperatures[temperatureSensorCount];
bool didSetupEEPROM = false;
// On first boot, this is set to true; Afterwards it's remembered to be false
RTC_DATA_ATTR bool shouldStartNewRecording = true;
constexpr uint16_t eepromOffset = 2; // Size of uint16
constexpr uint16_t eepromDataSize = eepromSize - eepromOffset;
void setupEEPROMIfNeeded() {
if (didSetupEEPROM) {
return;
}
bool success = EEPROM.begin(eepromSize);
if (!success) {
// Serial.println("Failed to set up EEPROM");
didSetupEEPROM = false;
}
// Serial.print("EEPROM Size ");
// Serial.print(eepromSize);
// Serial.println(" bytes");
didSetupEEPROM = true;
}
void storageConfigure() {
if (dataIndex >= eepromChunkSize) {
// Configure EEPROM at start
// May be initialized later
setupEEPROMIfNeeded();
}
// Ensure that first values are stored
for (uint8_t index = 0; index < temperatureSensorCount; index += 1) {
lastTemperatures[index].status == TemperatureStatus::sensorNotFound;
}
}
uint16_t getNumberOfBytesStoredInEEPROM() {
setupEEPROMIfNeeded();
if (shouldStartNewRecording) {
return 0; // Discard any
}
return EEPROM.readShort(0);
}
void moveDataToEEPROMIfNeeded() {
if (dataIndex < eepromChunkSize) {
return; // Wait until more bytes are available
}
// Determine EEPROM start address
uint16_t eepromIndex = getNumberOfBytesStoredInEEPROM() + eepromOffset;
if (eepromIndex >= eepromDataSize) {
return; // No more space in EEPROM, keep filling RTC memory
}
shouldStartNewRecording = false;
// Write until EEPROM is full
uint16_t bytesRemaining = eepromDataSize - eepromIndex;
uint16_t bytesToWrite = min(dataIndex, bytesRemaining);
EEPROM.writeBytes(eepromIndex, data, bytesToWrite); // TODO: Check that result == bytesToWrite
EEPROM.writeShort(0, eepromIndex + bytesToWrite);
EEPROM.commit();
// Move remaining data to front of array (if any)
uint16_t bytesToKeep = dataIndex - bytesToWrite;
memmove(data, data + bytesToWrite, bytesToKeep);
dataIndex -= bytesToWrite;
}
void saveByteAtCurrentIndex(uint8_t byte) {
data[dataIndex] = byte;
dataIndex += 1;
Serial.println(byte);
}
void saveTemperatureAtCurrentIndex(Temperature temp) {
if (temp.status != TemperatureStatus::temperatureIsValid) {
saveByteAtCurrentIndex(static_cast<uint8_t>(temp.status));
return;
}
// Convert to temperature range
long converted = (temp.value - (-40000)) / 500;
if (converted < temperatureMinimumValue) {
saveByteAtCurrentIndex(temperatureMinimumValue);
} else if (converted > temperatureMaximumValue) {
saveByteAtCurrentIndex(temperatureMaximumValue);
} else {
saveByteAtCurrentIndex(converted);
}
}
bool needsAbsoluteTemperatureRecording(Temperature* temperatures) {
for (uint8_t index = 0; index < temperatureSensorCount; index += 1) {
if (temperatures[index].status != TemperatureStatus::temperatureIsValid) {
// Error value can be encoded as differential (0x00)
continue;
}
// Note: If previously only errors, then last value is 0. This also work for differential temperatures
long diff = (lastTemperatures[index].value - temperatures[index].value) / 500 + 8;
if (diff < 1 || diff > 15) {
return true;
}
}
return false;
}
void saveTemperatures(Temperature* temperatures) {
if (needsAbsoluteTemperatureRecording(temperatures)) {
// Write absolute temperatures
saveByteAtCurrentIndex(absoluteTemperatureIndicator);
for (uint8_t sensorIndex = 0; sensorIndex < temperatureSensorCount; sensorIndex += 1) {
saveTemperatureAtCurrentIndex(temperatures[sensorIndex]);
if (temperatures[sensorIndex].status == TemperatureStatus::temperatureIsValid) {
// Only update if temperature is valid
lastTemperatures[sensorIndex] = temperatures[sensorIndex];
}
}
} else {
// Calculate temperature differences
uint8_t valueToStore = 0;
for (uint8_t index = 0; index < temperatureSensorCount; index += 1) {
uint8_t diff = 0; // Indicate sensor error
if (temperatures[index].status == TemperatureStatus::temperatureIsValid) {
diff = (lastTemperatures[index].value - temperatures[index].value) / 500 + 8;
}
if (index % 2) {
// Store second bits
valueToStore |= diff; // Valid range already ensured
saveByteAtCurrentIndex(valueToStore);
valueToStore = 0;
} else {
// Store in first four bits
valueToStore = (diff << 4);
}
if (temperatures[index].status == TemperatureStatus::temperatureIsValid) {
// Only update if temperature is valid
lastTemperatures[index] = temperatures[index];
}
}
// Ensure storage with uneven number of sensors
if (temperatureSensorCount % 2) {
saveByteAtCurrentIndex(valueToStore);
}
}
moveDataToEEPROMIfNeeded();
}
uint16_t getTotalNumberOfStoredBytes() {
return getNumberOfBytesStoredInEEPROM() + dataIndex;
}
uint16_t getRecordedBytesAtOffset(uint8_t* buffer, uint16_t offset, uint16_t count) {
// TODO: Check limits
uint16_t eepromByteCount = getNumberOfBytesStoredInEEPROM();
uint16_t endIndex = offset + count;
uint16_t eepromStart = min(offset, eepromByteCount);
uint16_t eepromEnd = min(endIndex, eepromByteCount);
uint16_t bytesToCopyFromEEPROM = eepromEnd - eepromStart;
if (bytesToCopyFromEEPROM > 0) {
EEPROM.readBytes(eepromStart, buffer, bytesToCopyFromEEPROM); // TODO: Check bytes read
// Reduce offset and count to point to RTC memory
offset -= bytesToCopyFromEEPROM;
count -= bytesToCopyFromEEPROM;
}
offset -= eepromByteCount;
// Copy remaining bytes from RTC memory
uint16_t bytesToCopyFromRTC = min(count, dataIndex);
memcpy(buffer + bytesToCopyFromEEPROM, data + offset, bytesToCopyFromRTC);
return bytesToCopyFromEEPROM + bytesToCopyFromRTC;
}
void discardAllRecordedBytes() {
shouldStartNewRecording = true;
dataIndex = 0;
}

205
src/temperature.cpp Normal file
View File

@ -0,0 +1,205 @@
#include "temperature.h"
#include <OneWireNg_CurrentPlatform.h>
#include <drivers/DSTherm.h>
#include <HardwareSerial.h>
constexpr uint8_t TEMPERATURE_RESOLUTION = 12;
constexpr int8_t TEMPERATURE_ALARM_LIMIT_LOW = -40;
constexpr int8_t TEMPERATURE_ALARM_LIMIT_HIGH = 80;
#define INVALID_SENSOR_INDEX 255
constexpr uint8_t TEMPERATURE_SENSOR_ERROR_THRESHOLD = 3;
// The number of bytes composing a temperature sensor address
constexpr int TEMPERATURE_SENSOR_ADDRESS_SIZE = 8;
constexpr bool TEMPERATURE_SENSOR_PARASITE_POWER = false;
// Note: Pin requires external 4.7kOhm pull-up
constexpr uint8_t TEMPERATURE_SENSOR_PIN = 13;
// Indicator if the temperature sensors have been configured
RTC_DATA_ATTR bool didConfigureTemperatureSensors = false;
OneWireNg_CurrentPlatform ow(TEMPERATURE_SENSOR_PIN, TEMPERATURE_SENSOR_PARASITE_POWER);
DSTherm sensorInterface = DSTherm(ow);
ALLOC_ALIGNED uint8_t scratchpadBuffer[sizeof(DSTherm::Scratchpad)];
DSTherm::Scratchpad *scratchpad;
struct Sensor {
uint8_t address[TEMPERATURE_SENSOR_ADDRESS_SIZE];
bool isSet;
bool foundDuringCurrentUpdate;
};
Sensor sensors[TEMPERATURE_SENSOR_MAX_COUNT];
uint8_t availableSensorCount = 0;
bool hasSensorAtIndex(uint8_t sensorIndex) {
return sensors[sensorIndex].isSet;
}
bool hasIdAtIndex(uint8_t sensorIndex, const OneWireNg::Id& id) {
if (!hasSensorAtIndex(sensorIndex)) {
return false;
}
for (uint8_t pos = 0; pos < TEMPERATURE_SENSOR_ADDRESS_SIZE; pos += 1) {
if (id[pos] != sensors[sensorIndex].address[pos]) {
return false;
}
}
return true;
}
uint8_t getSensorIndex(const OneWireNg::Id& id) {
for (uint8_t sensorIndex = 0; sensorIndex < TEMPERATURE_SENSOR_MAX_COUNT; sensorIndex += 1) {
if (hasIdAtIndex(sensorIndex, id)) {
return sensorIndex;
}
}
return INVALID_SENSOR_INDEX;
}
uint8_t getIndexOfFirstEmptySensorSlot() {
for (uint8_t sensorIndex = 0; sensorIndex < TEMPERATURE_SENSOR_MAX_COUNT; sensorIndex += 1) {
if (!sensors[sensorIndex].isSet) {
return false;
}
}
return INVALID_SENSOR_INDEX;
}
uint8_t addNewSensor(const OneWireNg::Id& id) {
if (availableSensorCount == TEMPERATURE_SENSOR_MAX_COUNT) {
return INVALID_SENSOR_INDEX;
}
uint8_t sensorIndex = getIndexOfFirstEmptySensorSlot();
if (sensorIndex == INVALID_SENSOR_INDEX) {
return INVALID_SENSOR_INDEX;
}
for (uint8_t pos = 0; pos < TEMPERATURE_SENSOR_ADDRESS_SIZE; pos += 1) {
sensors[sensorIndex].address[pos] = id[pos];
}
sensors[sensorIndex].isSet = true;
availableSensorCount += 1;
return sensorIndex;
}
void removeSensorAtIndex(uint8_t sensorIndex) {
sensors[sensorIndex].isSet = false;
availableSensorCount -= 1;
}
void printSensorAddress(const OneWireNg::Id& id) {
for (uint8_t i = 0; i < TEMPERATURE_SENSOR_ADDRESS_SIZE; i += 1) {
Serial.print(" 0x");
Serial.print(id[i], HEX);
}
Serial.println();
}
void printSensorAddressAtIndex(uint8_t sensorIndex) {
for (uint8_t i = 0; i < TEMPERATURE_SENSOR_ADDRESS_SIZE; i += 1) {
Serial.print(" 0x");
Serial.print(sensors[sensorIndex].address[i], HEX);
}
Serial.println();
}
void clearFoundIndicatorForAllSensors() {
for (uint8_t sensorIndex = 0; sensorIndex < TEMPERATURE_SENSOR_MAX_COUNT; sensorIndex += 1) {
sensors[sensorIndex].foundDuringCurrentUpdate = false;
}
}
void temperatureConfigure() {
if (didConfigureTemperatureSensors) {
// Only determine and configure sensors once per power cycle
return;
}
scratchpad = reinterpret_cast<DSTherm::Scratchpad*>(&scratchpadBuffer[0]);
// Write sensor resolution and set limits
if (sensorInterface.writeScratchpadAll(
TEMPERATURE_ALARM_LIMIT_HIGH,
TEMPERATURE_ALARM_LIMIT_LOW,
TEMPERATURE_RESOLUTION) != OneWireNg::ErrorCode::EC_SUCCESS) {
Serial.println("Failed to set temperature limits and resolution");
return;
}
// Clear discovery flags
clearFoundIndicatorForAllSensors();
// Read sensors and determine unknown ones
for (const auto& id: (OneWireNg&) ow) {
uint8_t sensorIndex = getSensorIndex(id);
if (sensorIndex == INVALID_SENSOR_INDEX) {
sensorIndex = addNewSensor(id);
Serial.print("Added sensor ");
printSensorAddress(id);
Serial.print(" at index ");
Serial.println(sensorIndex);
continue;
}
// Mark existing sensor
sensors[sensorIndex].foundDuringCurrentUpdate = true;
Serial.print("Discovered sensor ");
printSensorAddress(id);
Serial.print(" at index ");
Serial.println(sensorIndex);
}
// Remove missing sensors
for (uint8_t sensorIndex = 0; sensorIndex < TEMPERATURE_SENSOR_MAX_COUNT; sensorIndex += 1) {
if (!hasSensorAtIndex(sensorIndex)) {
continue;
}
if (sensors[sensorIndex].foundDuringCurrentUpdate) {
continue;
}
Serial.print("Removed sensor ");
printSensorAddressAtIndex(sensorIndex);
Serial.print(" at index ");
Serial.println(sensorIndex);
removeSensorAtIndex(sensorIndex);
}
didConfigureTemperatureSensors = true;
}
void temperaturePerformUpdate(Temperature* temperatures) {
clearFoundIndicatorForAllSensors();
// Convert temperature on all sensors connected
sensorInterface.convertTempAll(DSTherm::SCAN_BUS, TEMPERATURE_SENSOR_PARASITE_POWER);
// Read sensors one by one
for (const auto& id: (OneWireNg&) ow) {
uint8_t sensorIndex = getSensorIndex(id);
if (sensorIndex == INVALID_SENSOR_INDEX) {
// Invalid sensor id
continue;
}
sensors[sensorIndex].foundDuringCurrentUpdate = true;
if (sensorInterface.readScratchpad(id, scratchpad) != OneWireNg::EC_SUCCESS) {
// Invalid CRC
temperatures[sensorIndex].status = TemperatureStatus::sensorError;
continue;
}
temperatures[sensorIndex].value = scratchpad->getTemp(); // In millidegrees Celcius
temperatures[sensorIndex].status = TemperatureStatus::temperatureIsValid;
}
// Update missing sensors
for (uint8_t sensorIndex = 0; sensorIndex < TEMPERATURE_SENSOR_MAX_COUNT; sensorIndex += 1) {
if (!sensors[sensorIndex].foundDuringCurrentUpdate) {
temperatures[sensorIndex].status = TemperatureStatus::sensorNotFound;
}
}
}