Add configurable repeater telemetry history
This commit is contained in:
mikecarper committed
1 parent
bb348ac83c
commit
3ba0fb1847
8 files changed
+1334
No files matched your search
@@ -0,0 +1,688 @@
|
||||
#pragma once
|
||||
|
||||
#include <math.h>
|
||||
#include <stddef.h>
|
||||
#include <stdint.h>
|
||||
#include <stdio.h>
|
||||
#include <stdlib.h>
|
||||
#include <string.h>
|
||||
|
||||
namespace mesh {
|
||||
|
||||
// A bounded, boot-local telemetry history for repeater diagnostics.
|
||||
// Samples are aligned to 30-minute UTC buckets. Temperature and voltage keep
|
||||
// seven days; GPS keeps three days by default and can grow to 30 days. Missing
|
||||
// buckets are inserted explicitly so a temperature or voltage page always
|
||||
// describes the same 48 half-hour positions. GPS pages contain 24 half-hour
|
||||
// positions.
|
||||
class TelemetryHistory {
|
||||
struct GpsSample {
|
||||
int32_t latitude_e7;
|
||||
int32_t longitude_e7;
|
||||
};
|
||||
static_assert(sizeof(GpsSample) == 8, "GPS sample must remain eight bytes");
|
||||
|
||||
public:
|
||||
static constexpr uint32_t SAMPLE_INTERVAL_SECONDS = 30UL * 60UL;
|
||||
static constexpr uint8_t SAMPLES_PER_DAY = 48;
|
||||
static constexpr uint16_t TV_RETENTION_SAMPLES = 7U * SAMPLES_PER_DAY;
|
||||
static constexpr uint8_t GPS_DEFAULT_RETENTION_DAYS = 3;
|
||||
static constexpr uint8_t GPS_MAX_RETENTION_DAYS = 30;
|
||||
static constexpr uint16_t GPS_RETENTION_SAMPLES =
|
||||
GPS_DEFAULT_RETENTION_DAYS * SAMPLES_PER_DAY;
|
||||
static constexpr uint8_t GPS_SAMPLES_PER_PAGE = 24;
|
||||
static constexpr uint8_t TV_PAGE_COUNT = 7;
|
||||
static constexpr uint8_t GPS_DEFAULT_PAGE_COUNT =
|
||||
GPS_DEFAULT_RETENTION_DAYS * 2U;
|
||||
static constexpr size_t GPS_HEAP_BYTES_PER_DAY =
|
||||
SAMPLES_PER_DAY * sizeof(GpsSample);
|
||||
|
||||
static constexpr uint8_t TEMPERATURE_PAYLOAD_TYPE_V1 = 0x11;
|
||||
static constexpr uint8_t VOLTAGE_PAYLOAD_TYPE_V1 = 0x12;
|
||||
static constexpr uint8_t GPS_PAYLOAD_TYPE_V1 = 0x13;
|
||||
|
||||
enum Series : uint8_t {
|
||||
SERIES_TEMPERATURE = 0,
|
||||
SERIES_VOLTAGE = 1,
|
||||
SERIES_GPS = 2,
|
||||
};
|
||||
|
||||
enum TemperatureStatus : uint8_t {
|
||||
TEMPERATURE_NONE = 0,
|
||||
TEMPERATURE_VALUE = 1,
|
||||
TEMPERATURE_LOW = 2,
|
||||
TEMPERATURE_HIGH = 3,
|
||||
};
|
||||
|
||||
TelemetryHistory()
|
||||
: _gps_samples(_gps_default_samples),
|
||||
_gps_capacity(GPS_RETENTION_SAMPLES),
|
||||
_gps_allocated_capacity(0) {
|
||||
clear();
|
||||
}
|
||||
|
||||
~TelemetryHistory() {
|
||||
if (_gps_allocated_capacity != 0) free(_gps_samples);
|
||||
}
|
||||
|
||||
TelemetryHistory(const TelemetryHistory&) = delete;
|
||||
TelemetryHistory& operator=(const TelemetryHistory&) = delete;
|
||||
|
||||
void clear() {
|
||||
memset(_temperature, 0, sizeof(_temperature));
|
||||
memset(_temperature_status, 0, sizeof(_temperature_status));
|
||||
memset(_voltage, 0, sizeof(_voltage));
|
||||
memset(_gps_samples, 0, gpsPhysicalCapacity() * sizeof(GpsSample));
|
||||
_tv_next = 0;
|
||||
_tv_count = 0;
|
||||
_gps_next = 0;
|
||||
_gps_count = 0;
|
||||
_last_bucket = 0;
|
||||
_has_bucket = false;
|
||||
}
|
||||
|
||||
bool sampleDue(uint32_t epoch_seconds) const {
|
||||
return !_has_bucket || epoch_seconds / SAMPLE_INTERVAL_SECONDS != _last_bucket;
|
||||
}
|
||||
|
||||
void record(uint32_t epoch_seconds, int16_t temperature_c, bool temperature_valid,
|
||||
uint16_t battery_mv,
|
||||
int32_t gps_lat_e7, int32_t gps_lon_e7, bool gps_valid) {
|
||||
const uint32_t bucket = epoch_seconds / SAMPLE_INTERVAL_SECONDS;
|
||||
uint8_t temperature_status;
|
||||
const uint8_t temperature = encodeTemperature(temperature_c, temperature_valid,
|
||||
temperature_status);
|
||||
const uint8_t voltage = encodeVoltage(battery_mv);
|
||||
gps_valid = gps_valid
|
||||
&& gps_lat_e7 >= -900000000 && gps_lat_e7 <= 900000000
|
||||
&& gps_lon_e7 >= -1800000000 && gps_lon_e7 <= 1800000000
|
||||
&& (gps_lat_e7 != 0 || gps_lon_e7 != 0);
|
||||
|
||||
if (!_has_bucket) {
|
||||
append(temperature, temperature_status, voltage,
|
||||
gps_lat_e7, gps_lon_e7, gps_valid);
|
||||
_last_bucket = bucket;
|
||||
_has_bucket = true;
|
||||
return;
|
||||
}
|
||||
|
||||
if (bucket == _last_bucket) return;
|
||||
|
||||
const uint32_t maximum_retention = _gps_capacity > TV_RETENTION_SAMPLES
|
||||
? _gps_capacity : TV_RETENTION_SAMPLES;
|
||||
if (bucket < _last_bucket || bucket - _last_bucket > maximum_retention) {
|
||||
clear();
|
||||
append(temperature, temperature_status, voltage,
|
||||
gps_lat_e7, gps_lon_e7, gps_valid);
|
||||
_last_bucket = bucket;
|
||||
_has_bucket = true;
|
||||
return;
|
||||
}
|
||||
|
||||
const uint32_t skipped = bucket - _last_bucket - 1U;
|
||||
for (uint32_t i = 0; i < skipped; i++) {
|
||||
append(0, TEMPERATURE_NONE, 0, 0, 0, false);
|
||||
}
|
||||
append(temperature, temperature_status, voltage,
|
||||
gps_lat_e7, gps_lon_e7, gps_valid);
|
||||
_last_bucket = bucket;
|
||||
}
|
||||
|
||||
// The seven-bit value is an exact whole-degree offset: 0=-50 C, 127=+77 C.
|
||||
// A separate two-bit status carries none/value/low/high because seven bits
|
||||
// alone cannot represent 128 temperatures plus three sentinel states.
|
||||
static uint8_t encodeTemperature(int16_t temperature_c, bool valid,
|
||||
uint8_t& status) {
|
||||
if (!valid) {
|
||||
status = TEMPERATURE_NONE;
|
||||
return 0;
|
||||
}
|
||||
if (temperature_c < -50) {
|
||||
status = TEMPERATURE_LOW;
|
||||
return 0;
|
||||
}
|
||||
if (temperature_c > 77) {
|
||||
status = TEMPERATURE_HIGH;
|
||||
return 127;
|
||||
}
|
||||
status = TEMPERATURE_VALUE;
|
||||
return (uint8_t)(temperature_c + 50);
|
||||
}
|
||||
|
||||
// Voltage uses all 256 eight-bit codes:
|
||||
// 0 no reading
|
||||
// 1 below 1.88 V
|
||||
// 2..254 1.88 V through 4.40 V in 0.01 V steps
|
||||
// 255 above 4.40 V
|
||||
static uint8_t encodeVoltage(uint16_t battery_mv) {
|
||||
if (battery_mv == 0) return 0;
|
||||
if (battery_mv < 1880) return 1;
|
||||
if (battery_mv > 4400) return 255;
|
||||
return (uint8_t)(2U + (battery_mv - 1880U + 5U) / 10U);
|
||||
}
|
||||
|
||||
uint8_t gpsRetentionDays() const {
|
||||
return (uint8_t)(_gps_capacity / SAMPLES_PER_DAY);
|
||||
}
|
||||
|
||||
uint8_t gpsPageCount() const {
|
||||
return (uint8_t)(_gps_capacity / GPS_SAMPLES_PER_PAGE);
|
||||
}
|
||||
|
||||
// Changes the logical GPS retention and preserves the newest samples. The
|
||||
// heap budget is the maximum additional allocation allowed for this call.
|
||||
// Requests above the budget are reduced one day at a time. The returned
|
||||
// value is the number of days actually available.
|
||||
uint8_t resizeGpsDays(uint8_t requested_days,
|
||||
size_t max_additional_heap_bytes) {
|
||||
if (requested_days < 1U) requested_days = 1U;
|
||||
if (requested_days > GPS_MAX_RETENTION_DAYS) {
|
||||
requested_days = GPS_MAX_RETENTION_DAYS;
|
||||
}
|
||||
|
||||
normalizeGpsRing();
|
||||
const uint8_t current_days = gpsRetentionDays();
|
||||
if (requested_days == current_days) return current_days;
|
||||
|
||||
if (requested_days <= GPS_DEFAULT_RETENTION_DAYS) {
|
||||
resizeGpsToDefaultStorage(requested_days);
|
||||
return gpsRetentionDays();
|
||||
}
|
||||
|
||||
if (_gps_allocated_capacity != 0
|
||||
&& requested_days * SAMPLES_PER_DAY <= _gps_allocated_capacity) {
|
||||
resizeGpsLogicalCapacity((uint16_t)(requested_days * SAMPLES_PER_DAY));
|
||||
shrinkGpsAllocation();
|
||||
return gpsRetentionDays();
|
||||
}
|
||||
|
||||
if (_gps_allocated_capacity == 0
|
||||
&& _gps_capacity < GPS_RETENTION_SAMPLES) {
|
||||
resizeGpsLogicalCapacity(GPS_RETENTION_SAMPLES);
|
||||
}
|
||||
|
||||
const uint16_t old_allocated_capacity = _gps_allocated_capacity;
|
||||
const size_t old_heap_bytes =
|
||||
(size_t)old_allocated_capacity * sizeof(GpsSample);
|
||||
for (uint8_t days = requested_days;
|
||||
days > gpsRetentionDays(); days--) {
|
||||
const uint16_t candidate_capacity = (uint16_t)(days * SAMPLES_PER_DAY);
|
||||
const size_t candidate_bytes =
|
||||
(size_t)candidate_capacity * sizeof(GpsSample);
|
||||
const size_t additional_bytes = candidate_bytes > old_heap_bytes
|
||||
? candidate_bytes - old_heap_bytes : 0;
|
||||
if (additional_bytes > max_additional_heap_bytes) continue;
|
||||
if (expandGpsStorage(candidate_capacity)) return days;
|
||||
}
|
||||
return gpsRetentionDays();
|
||||
}
|
||||
|
||||
// Formats a complete CLI reply in at most 139 bytes, including NUL:
|
||||
// get telemetry.temp [page] (page 1..7)
|
||||
// get telemetry.volt [page] (page 1..7)
|
||||
// get telemetry.gps [page] (page 1..configured days * 2)
|
||||
// Page 1 is always the newest page. Temperature and voltage pages hold one
|
||||
// day; GPS pages hold half a day.
|
||||
// The reply is "> " followed by standard padded Base64.
|
||||
bool formatPageReply(Series series, const char* args,
|
||||
char* reply, size_t reply_size) const {
|
||||
if (reply == NULL || reply_size == 0) return false;
|
||||
reply[0] = 0;
|
||||
if (!_has_bucket) {
|
||||
copyReply(reply, reply_size, "Err - telemetry history is empty");
|
||||
return false;
|
||||
}
|
||||
|
||||
const char* cursor = skipSpaces(args == NULL ? "" : args);
|
||||
char token[12];
|
||||
unsigned page = 1;
|
||||
|
||||
if (readToken(cursor, token, sizeof(token))) {
|
||||
if (!parseUnsigned(token, page)) return formatUsage(series, reply, reply_size);
|
||||
}
|
||||
|
||||
cursor = skipSpaces(cursor);
|
||||
if (*cursor != 0) return formatUsage(series, reply, reply_size);
|
||||
|
||||
if (series == SERIES_GPS) {
|
||||
if (page < 1U || page > gpsPageCount()) {
|
||||
snprintf(reply, reply_size, "Err - telemetry.gps page must be 1-%u",
|
||||
(unsigned)gpsPageCount());
|
||||
return false;
|
||||
}
|
||||
return formatGpsPage((uint8_t)page, reply, reply_size);
|
||||
}
|
||||
|
||||
if (page < 1U || page > TV_PAGE_COUNT) {
|
||||
copyReply(reply, reply_size, series == SERIES_TEMPERATURE
|
||||
? "Err - telemetry.temp page must be 1-7"
|
||||
: "Err - telemetry.volt page must be 1-7");
|
||||
return false;
|
||||
}
|
||||
return series == SERIES_TEMPERATURE
|
||||
? formatTemperaturePage((uint8_t)page, reply, reply_size)
|
||||
: formatVoltagePage((uint8_t)page, reply, reply_size);
|
||||
}
|
||||
|
||||
private:
|
||||
class BitWriter {
|
||||
uint8_t* _dest;
|
||||
size_t _capacity;
|
||||
size_t _bits;
|
||||
|
||||
public:
|
||||
BitWriter(uint8_t* dest, size_t capacity)
|
||||
: _dest(dest), _capacity(capacity), _bits(0) {
|
||||
memset(dest, 0, capacity);
|
||||
}
|
||||
|
||||
bool write(uint32_t value, uint8_t width) {
|
||||
if (_bits + width > _capacity * 8U) return false;
|
||||
for (int bit = width - 1; bit >= 0; bit--) {
|
||||
if (value & (1UL << bit)) {
|
||||
_dest[_bits / 8U] |= (uint8_t)(1U << (7U - (_bits % 8U)));
|
||||
}
|
||||
_bits++;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
};
|
||||
|
||||
uint8_t _temperature[TV_RETENTION_SAMPLES];
|
||||
uint8_t _temperature_status[(TV_RETENTION_SAMPLES * 2U + 7U) / 8U];
|
||||
uint8_t _voltage[TV_RETENTION_SAMPLES];
|
||||
GpsSample _gps_default_samples[GPS_RETENTION_SAMPLES];
|
||||
GpsSample* _gps_samples;
|
||||
uint16_t _gps_capacity;
|
||||
uint16_t _gps_allocated_capacity;
|
||||
uint16_t _tv_next;
|
||||
uint16_t _tv_count;
|
||||
uint16_t _gps_next;
|
||||
uint16_t _gps_count;
|
||||
uint32_t _last_bucket;
|
||||
bool _has_bucket;
|
||||
|
||||
static const char* skipSpaces(const char* value) {
|
||||
while (*value == ' ') value++;
|
||||
return value;
|
||||
}
|
||||
|
||||
static bool readToken(const char*& cursor, char* token, size_t token_size) {
|
||||
cursor = skipSpaces(cursor);
|
||||
if (*cursor == 0) return false;
|
||||
|
||||
size_t len = 0;
|
||||
while (*cursor != 0 && *cursor != ' ') {
|
||||
if (len + 1U >= token_size) {
|
||||
while (*cursor != 0 && *cursor != ' ') cursor++;
|
||||
token[0] = 0;
|
||||
return true;
|
||||
}
|
||||
token[len++] = *cursor++;
|
||||
}
|
||||
token[len] = 0;
|
||||
return true;
|
||||
}
|
||||
|
||||
static bool parseUnsigned(const char* token, unsigned& value) {
|
||||
if (token == NULL || *token == 0) return false;
|
||||
unsigned parsed = 0;
|
||||
while (*token != 0) {
|
||||
if (*token < '0' || *token > '9') return false;
|
||||
if (parsed > 1000U) return false;
|
||||
parsed = parsed * 10U + (unsigned)(*token++ - '0');
|
||||
}
|
||||
value = parsed;
|
||||
return true;
|
||||
}
|
||||
|
||||
static void copyReply(char* reply, size_t reply_size, const char* text) {
|
||||
if (reply_size == 0) return;
|
||||
snprintf(reply, reply_size, "%s", text);
|
||||
}
|
||||
|
||||
static bool formatUsage(Series series, char* reply, size_t reply_size) {
|
||||
const char* command = series == SERIES_TEMPERATURE ? "telemetry.temp"
|
||||
: series == SERIES_VOLTAGE ? "telemetry.volt" : "telemetry.gps";
|
||||
snprintf(reply, reply_size, "Err - use: get %s [page]", command);
|
||||
return false;
|
||||
}
|
||||
|
||||
uint16_t gpsPhysicalCapacity() const {
|
||||
return _gps_allocated_capacity != 0
|
||||
? _gps_allocated_capacity : GPS_RETENTION_SAMPLES;
|
||||
}
|
||||
|
||||
static void reverseGpsSamples(GpsSample* samples,
|
||||
uint16_t begin, uint16_t end) {
|
||||
while (begin < end && begin < --end) {
|
||||
const GpsSample saved = samples[begin];
|
||||
samples[begin++] = samples[end];
|
||||
samples[end] = saved;
|
||||
}
|
||||
}
|
||||
|
||||
void normalizeGpsRing() {
|
||||
if (_gps_count == 0) {
|
||||
_gps_next = 0;
|
||||
return;
|
||||
}
|
||||
|
||||
const uint16_t oldest = (uint16_t)(
|
||||
(_gps_next + _gps_capacity - _gps_count) % _gps_capacity);
|
||||
if (oldest != 0) {
|
||||
reverseGpsSamples(_gps_samples, 0, oldest);
|
||||
reverseGpsSamples(_gps_samples, oldest, _gps_capacity);
|
||||
reverseGpsSamples(_gps_samples, 0, _gps_capacity);
|
||||
}
|
||||
_gps_next = _gps_count == _gps_capacity ? 0 : _gps_count;
|
||||
}
|
||||
|
||||
void resizeGpsLogicalCapacity(uint16_t new_capacity) {
|
||||
const uint16_t keep_count = _gps_count < new_capacity
|
||||
? _gps_count : new_capacity;
|
||||
if (keep_count < _gps_count) {
|
||||
memmove(_gps_samples, &_gps_samples[_gps_count - keep_count],
|
||||
(size_t)keep_count * sizeof(GpsSample));
|
||||
}
|
||||
_gps_capacity = new_capacity;
|
||||
_gps_count = keep_count;
|
||||
_gps_next = keep_count == new_capacity ? 0 : keep_count;
|
||||
}
|
||||
|
||||
void resizeGpsToDefaultStorage(uint8_t days) {
|
||||
const uint16_t new_capacity = (uint16_t)(days * SAMPLES_PER_DAY);
|
||||
if (_gps_allocated_capacity == 0) {
|
||||
resizeGpsLogicalCapacity(new_capacity);
|
||||
return;
|
||||
}
|
||||
|
||||
const uint16_t keep_count = _gps_count < new_capacity
|
||||
? _gps_count : new_capacity;
|
||||
memset(_gps_default_samples, 0, sizeof(_gps_default_samples));
|
||||
memcpy(_gps_default_samples, &_gps_samples[_gps_count - keep_count],
|
||||
(size_t)keep_count * sizeof(GpsSample));
|
||||
free(_gps_samples);
|
||||
_gps_samples = _gps_default_samples;
|
||||
_gps_allocated_capacity = 0;
|
||||
_gps_capacity = new_capacity;
|
||||
_gps_count = keep_count;
|
||||
_gps_next = keep_count == new_capacity ? 0 : keep_count;
|
||||
}
|
||||
|
||||
void shrinkGpsAllocation() {
|
||||
if (_gps_allocated_capacity == 0
|
||||
|| _gps_allocated_capacity == _gps_capacity) return;
|
||||
GpsSample* resized = static_cast<GpsSample*>(
|
||||
realloc(_gps_samples, (size_t)_gps_capacity * sizeof(GpsSample)));
|
||||
if (resized != NULL) {
|
||||
_gps_samples = resized;
|
||||
_gps_allocated_capacity = _gps_capacity;
|
||||
}
|
||||
}
|
||||
|
||||
bool expandGpsStorage(uint16_t new_capacity) {
|
||||
GpsSample* resized;
|
||||
if (_gps_allocated_capacity == 0) {
|
||||
resized = static_cast<GpsSample*>(
|
||||
malloc((size_t)new_capacity * sizeof(GpsSample)));
|
||||
if (resized == NULL) return false;
|
||||
memset(resized, 0, (size_t)new_capacity * sizeof(GpsSample));
|
||||
memcpy(resized, _gps_samples, (size_t)_gps_count * sizeof(GpsSample));
|
||||
} else {
|
||||
resized = static_cast<GpsSample*>(
|
||||
realloc(_gps_samples, (size_t)new_capacity * sizeof(GpsSample)));
|
||||
if (resized == NULL) return false;
|
||||
}
|
||||
|
||||
_gps_samples = resized;
|
||||
_gps_capacity = new_capacity;
|
||||
_gps_allocated_capacity = new_capacity;
|
||||
_gps_next = _gps_count == new_capacity ? 0 : _gps_count;
|
||||
return true;
|
||||
}
|
||||
|
||||
void setTemperatureStatus(uint16_t index, uint8_t status) {
|
||||
const uint8_t shift = (uint8_t)((index & 3U) * 2U);
|
||||
const uint8_t mask = (uint8_t)(0x03U << shift);
|
||||
_temperature_status[index / 4U] = (uint8_t)(
|
||||
(_temperature_status[index / 4U] & (uint8_t)~mask)
|
||||
| ((status & 0x03U) << shift));
|
||||
}
|
||||
|
||||
uint8_t temperatureStatus(uint16_t index) const {
|
||||
const uint8_t shift = (uint8_t)((index & 3U) * 2U);
|
||||
return (uint8_t)((_temperature_status[index / 4U] >> shift) & 0x03U);
|
||||
}
|
||||
|
||||
void append(uint8_t temperature, uint8_t temperature_status, uint8_t voltage,
|
||||
int32_t gps_lat_e7,
|
||||
int32_t gps_lon_e7, bool gps_valid) {
|
||||
_temperature[_tv_next] = temperature;
|
||||
setTemperatureStatus(_tv_next, temperature_status);
|
||||
_voltage[_tv_next] = voltage;
|
||||
_tv_next = (uint16_t)((_tv_next + 1U) % TV_RETENTION_SAMPLES);
|
||||
if (_tv_count < TV_RETENTION_SAMPLES) _tv_count++;
|
||||
|
||||
_gps_samples[_gps_next].latitude_e7 = gps_valid ? gps_lat_e7 : 0;
|
||||
_gps_samples[_gps_next].longitude_e7 = gps_valid ? gps_lon_e7 : 0;
|
||||
_gps_next = (uint16_t)((_gps_next + 1U) % _gps_capacity);
|
||||
if (_gps_count < _gps_capacity) _gps_count++;
|
||||
}
|
||||
|
||||
bool tvAtOffset(uint16_t offset, uint8_t& temperature,
|
||||
uint8_t& temperature_status, uint8_t& voltage) const {
|
||||
if (offset >= _tv_count) {
|
||||
temperature = 0;
|
||||
temperature_status = TEMPERATURE_NONE;
|
||||
voltage = 0;
|
||||
return false;
|
||||
}
|
||||
const uint16_t index = (uint16_t)((_tv_next + TV_RETENTION_SAMPLES - 1U - offset)
|
||||
% TV_RETENTION_SAMPLES);
|
||||
temperature = _temperature[index];
|
||||
temperature_status = temperatureStatus(index);
|
||||
voltage = _voltage[index];
|
||||
return true;
|
||||
}
|
||||
|
||||
bool gpsAtOffset(uint16_t offset, int32_t& latitude_e7, int32_t& longitude_e7) const {
|
||||
if (offset >= _gps_count) {
|
||||
latitude_e7 = 0;
|
||||
longitude_e7 = 0;
|
||||
return false;
|
||||
}
|
||||
const uint16_t index = (uint16_t)((_gps_next + _gps_capacity - 1U - offset)
|
||||
% _gps_capacity);
|
||||
latitude_e7 = _gps_samples[index].latitude_e7;
|
||||
longitude_e7 = _gps_samples[index].longitude_e7;
|
||||
return latitude_e7 != 0 || longitude_e7 != 0;
|
||||
}
|
||||
|
||||
static void putUint32LE(uint8_t* dest, uint32_t value) {
|
||||
dest[0] = (uint8_t)value;
|
||||
dest[1] = (uint8_t)(value >> 8);
|
||||
dest[2] = (uint8_t)(value >> 16);
|
||||
dest[3] = (uint8_t)(value >> 24);
|
||||
}
|
||||
|
||||
static void putInt32LE(uint8_t* dest, int32_t value) {
|
||||
putUint32LE(dest, (uint32_t)value);
|
||||
}
|
||||
|
||||
static size_t base64Encode(const uint8_t* source, size_t source_len,
|
||||
char* dest, size_t dest_size) {
|
||||
static const char alphabet[] =
|
||||
"ABCDEFGHIJKLMNOPQRSTUVWXYZabcdefghijklmnopqrstuvwxyz0123456789+/";
|
||||
const size_t required = 4U * ((source_len + 2U) / 3U);
|
||||
if (dest_size <= required) return 0;
|
||||
|
||||
size_t in = 0;
|
||||
size_t out = 0;
|
||||
while (in < source_len) {
|
||||
const size_t remaining = source_len - in;
|
||||
const uint32_t a = source[in++];
|
||||
const uint32_t b = remaining > 1U ? source[in++] : 0U;
|
||||
const uint32_t c = remaining > 2U ? source[in++] : 0U;
|
||||
const uint32_t value = (a << 16) | (b << 8) | c;
|
||||
|
||||
dest[out++] = alphabet[(value >> 18) & 0x3FU];
|
||||
dest[out++] = alphabet[(value >> 12) & 0x3FU];
|
||||
dest[out++] = remaining > 1U ? alphabet[(value >> 6) & 0x3FU] : '=';
|
||||
dest[out++] = remaining > 2U ? alphabet[value & 0x3FU] : '=';
|
||||
}
|
||||
dest[out] = 0;
|
||||
return out;
|
||||
}
|
||||
|
||||
static bool formatBase64Reply(const uint8_t* payload, size_t payload_len,
|
||||
char* reply, size_t reply_size) {
|
||||
if (reply_size < 3U) return false;
|
||||
reply[0] = '>';
|
||||
reply[1] = ' ';
|
||||
if (base64Encode(payload, payload_len, &reply[2], reply_size - 2U) == 0) {
|
||||
copyReply(reply, reply_size, "Err - telemetry reply buffer too small");
|
||||
return false;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
uint32_t startEpochForOffset(uint16_t oldest_offset) const {
|
||||
const uint32_t start_bucket = _last_bucket >= oldest_offset
|
||||
? _last_bucket - oldest_offset : 0;
|
||||
return start_bucket * SAMPLE_INTERVAL_SECONDS;
|
||||
}
|
||||
|
||||
bool formatTemperaturePage(uint8_t page, char* reply, size_t reply_size) const {
|
||||
static constexpr size_t HEADER_SIZE = 7;
|
||||
static constexpr size_t STATUS_SIZE = (SAMPLES_PER_DAY * 2U) / 8U;
|
||||
static constexpr size_t DATA_SIZE = (SAMPLES_PER_DAY * 7U) / 8U;
|
||||
uint8_t payload[HEADER_SIZE + STATUS_SIZE + DATA_SIZE];
|
||||
memset(payload, 0, sizeof(payload));
|
||||
|
||||
const uint16_t oldest_offset = (uint16_t)(page * SAMPLES_PER_DAY - 1U);
|
||||
payload[0] = TEMPERATURE_PAYLOAD_TYPE_V1;
|
||||
putUint32LE(&payload[1], startEpochForOffset(oldest_offset));
|
||||
payload[5] = 30;
|
||||
payload[6] = SAMPLES_PER_DAY;
|
||||
|
||||
BitWriter status_writer(&payload[HEADER_SIZE], STATUS_SIZE);
|
||||
BitWriter writer(&payload[HEADER_SIZE + STATUS_SIZE], DATA_SIZE);
|
||||
for (uint8_t slot = 0; slot < SAMPLES_PER_DAY; slot++) {
|
||||
uint8_t temperature = 0;
|
||||
uint8_t temperature_status = TEMPERATURE_NONE;
|
||||
uint8_t voltage = 0;
|
||||
tvAtOffset((uint16_t)(oldest_offset - slot), temperature,
|
||||
temperature_status, voltage);
|
||||
status_writer.write(temperature_status, 2);
|
||||
writer.write(temperature, 7);
|
||||
}
|
||||
return formatBase64Reply(payload, sizeof(payload), reply, reply_size);
|
||||
}
|
||||
|
||||
bool formatVoltagePage(uint8_t page, char* reply, size_t reply_size) const {
|
||||
static constexpr size_t HEADER_SIZE = 7;
|
||||
uint8_t payload[HEADER_SIZE + SAMPLES_PER_DAY];
|
||||
memset(payload, 0, sizeof(payload));
|
||||
|
||||
const uint16_t oldest_offset = (uint16_t)(page * SAMPLES_PER_DAY - 1U);
|
||||
payload[0] = VOLTAGE_PAYLOAD_TYPE_V1;
|
||||
putUint32LE(&payload[1], startEpochForOffset(oldest_offset));
|
||||
payload[5] = 30;
|
||||
payload[6] = SAMPLES_PER_DAY;
|
||||
|
||||
for (uint8_t slot = 0; slot < SAMPLES_PER_DAY; slot++) {
|
||||
uint8_t temperature = 0;
|
||||
uint8_t temperature_status = TEMPERATURE_NONE;
|
||||
uint8_t voltage = 0;
|
||||
tvAtOffset((uint16_t)(oldest_offset - slot), temperature,
|
||||
temperature_status, voltage);
|
||||
payload[HEADER_SIZE + slot] = voltage;
|
||||
}
|
||||
return formatBase64Reply(payload, sizeof(payload), reply, reply_size);
|
||||
}
|
||||
|
||||
static int clampGpsDelta(long value, bool& clipped) {
|
||||
if (value < -8192L) {
|
||||
clipped = true;
|
||||
return -8192;
|
||||
}
|
||||
if (value > 8191L) {
|
||||
clipped = true;
|
||||
return 8191;
|
||||
}
|
||||
return (int)value;
|
||||
}
|
||||
|
||||
bool formatGpsPage(uint8_t page, char* reply, size_t reply_size) const {
|
||||
static constexpr size_t HEADER_SIZE = 17;
|
||||
static constexpr size_t DATA_SIZE = (GPS_SAMPLES_PER_PAGE * 28U) / 8U;
|
||||
static constexpr double METERS_PER_DEGREE = 111320.0;
|
||||
static constexpr double DEGREES_TO_RADIANS = 0.017453292519943295;
|
||||
uint8_t payload[HEADER_SIZE + DATA_SIZE];
|
||||
memset(payload, 0, sizeof(payload));
|
||||
|
||||
const uint16_t newest_offset = (uint16_t)((page - 1U) * GPS_SAMPLES_PER_PAGE);
|
||||
const uint16_t oldest_offset = (uint16_t)(newest_offset
|
||||
+ GPS_SAMPLES_PER_PAGE - 1U);
|
||||
|
||||
payload[0] = GPS_PAYLOAD_TYPE_V1;
|
||||
putUint32LE(&payload[1], startEpochForOffset(oldest_offset));
|
||||
payload[5] = 30;
|
||||
payload[6] = GPS_SAMPLES_PER_PAGE;
|
||||
payload[15] = 0xFF; // origin slot; 0xFF means no GPS fix on this page
|
||||
|
||||
int32_t origin_lat_e7 = 0;
|
||||
int32_t origin_lon_e7 = 0;
|
||||
for (uint8_t slot = 0; slot < GPS_SAMPLES_PER_PAGE; slot++) {
|
||||
int32_t latitude_e7;
|
||||
int32_t longitude_e7;
|
||||
if (gpsAtOffset((uint16_t)(oldest_offset - slot), latitude_e7, longitude_e7)) {
|
||||
origin_lat_e7 = latitude_e7;
|
||||
origin_lon_e7 = longitude_e7;
|
||||
payload[15] = slot;
|
||||
break;
|
||||
}
|
||||
}
|
||||
putInt32LE(&payload[7], origin_lat_e7);
|
||||
putInt32LE(&payload[11], origin_lon_e7);
|
||||
|
||||
double reference_lat = (double)origin_lat_e7 / 10000000.0;
|
||||
double reference_lon = (double)origin_lon_e7 / 10000000.0;
|
||||
bool clipped = false;
|
||||
BitWriter writer(&payload[HEADER_SIZE], DATA_SIZE);
|
||||
for (uint8_t slot = 0; slot < GPS_SAMPLES_PER_PAGE; slot++) {
|
||||
int north_units = 0;
|
||||
int east_units = 0;
|
||||
int32_t latitude_e7;
|
||||
int32_t longitude_e7;
|
||||
const bool valid = payload[15] != 0xFF && slot >= payload[15]
|
||||
&& gpsAtOffset((uint16_t)(oldest_offset - slot), latitude_e7, longitude_e7);
|
||||
|
||||
if (valid && slot != payload[15]) {
|
||||
const double latitude = (double)latitude_e7 / 10000000.0;
|
||||
const double longitude = (double)longitude_e7 / 10000000.0;
|
||||
const double mean_latitude = (latitude + reference_lat) * 0.5;
|
||||
const double north_m = (latitude - reference_lat) * METERS_PER_DEGREE;
|
||||
const double east_m = (longitude - reference_lon) * METERS_PER_DEGREE
|
||||
* cos(mean_latitude * DEGREES_TO_RADIANS);
|
||||
north_units = clampGpsDelta(lround(north_m / 10.0), clipped);
|
||||
east_units = clampGpsDelta(lround(east_m / 10.0), clipped);
|
||||
|
||||
reference_lat += (double)north_units * 10.0 / METERS_PER_DEGREE;
|
||||
const double longitude_scale = METERS_PER_DEGREE
|
||||
* cos(reference_lat * DEGREES_TO_RADIANS);
|
||||
if (fabs(longitude_scale) > 0.001) {
|
||||
reference_lon += (double)east_units * 10.0 / longitude_scale;
|
||||
}
|
||||
}
|
||||
|
||||
writer.write((uint16_t)north_units & 0x3FFFU, 14);
|
||||
writer.write((uint16_t)east_units & 0x3FFFU, 14);
|
||||
}
|
||||
if (clipped) payload[16] |= 0x01;
|
||||
return formatBase64Reply(payload, sizeof(payload), reply, reply_size);
|
||||
}
|
||||
};
|
||||
|
||||
} // namespace mesh
|
||||
Reference in new issue
Block a user