From 8660d1c973d1770275fe36cd990623a5a807b204 Mon Sep 17 00:00:00 2001 From: Scott Powell Date: Tue, 8 Sep 2026 17:55:02 +1000 Subject: [PATCH] * refactor: LPP helpers moved to LPPData class --- examples/simple_sensor/SensorMesh.cpp | 126 +++----------------- src/helpers/sensors/LPPDataHelpers.h | 159 ++++++++++++++++++-------- 2 files changed, 128 insertions(+), 157 deletions(-) diff --git a/examples/simple_sensor/SensorMesh.cpp b/examples/simple_sensor/SensorMesh.cpp index 8c6b06a6d..de58e2b16 100644 --- a/examples/simple_sensor/SensorMesh.cpp +++ b/examples/simple_sensor/SensorMesh.cpp @@ -1,5 +1,6 @@ #include "SensorMesh.h" #include +#include /* ------------------------------ Config -------------------------------- */ @@ -80,103 +81,6 @@ static File openAppend(FILESYSTEM* _fs, const char* fname) { /* --------------------- Cayenne LPP helpers ----------------------------*/ -static uint8_t getDataSize(uint8_t type) { - switch (type) { - case LPP_GPS: - return 9; - case LPP_POLYLINE: - return 8; // TODO: this is MINIMIUM - case LPP_GYROMETER: - case LPP_ACCELEROMETER: - return 6; - case LPP_GENERIC_SENSOR: - case LPP_FREQUENCY: - case LPP_DISTANCE: - case LPP_ENERGY: - case LPP_UNIXTIME: - return 4; - case LPP_COLOUR: - return 3; - case LPP_ANALOG_INPUT: - case LPP_ANALOG_OUTPUT: - case LPP_LUMINOSITY: - case LPP_TEMPERATURE: - case LPP_CONCENTRATION: - case LPP_BAROMETRIC_PRESSURE: - case LPP_RELATIVE_HUMIDITY: - case LPP_ALTITUDE: - case LPP_VOLTAGE: - case LPP_CURRENT: - case LPP_DIRECTION: - case LPP_POWER: - return 2; - } - return 1; -} - -static uint32_t getMultiplier(uint8_t type) { - switch (type) { - case LPP_CURRENT: - case LPP_DISTANCE: - case LPP_ENERGY: - return 1000; - case LPP_VOLTAGE: - case LPP_ANALOG_INPUT: - case LPP_ANALOG_OUTPUT: - return 100; - case LPP_TEMPERATURE: - case LPP_BAROMETRIC_PRESSURE: - case LPP_RELATIVE_HUMIDITY: - return 10; - } - return 1; -} - -static bool isSigned(uint8_t type) { - return type == LPP_ALTITUDE || type == LPP_TEMPERATURE || type == LPP_GYROMETER || - type == LPP_ANALOG_INPUT || type == LPP_ANALOG_OUTPUT || type == LPP_GPS || type == LPP_ACCELEROMETER; -} - -static float getFloat(const uint8_t * buffer, uint8_t size, uint32_t multiplier, bool is_signed) { - uint32_t value = 0; - for (uint8_t i = 0; i < size; i++) { - value = (value << 8) + buffer[i]; - } - - int sign = 1; - if (is_signed) { - uint32_t bit = 1ul << ((size * 8) - 1); - if ((value & bit) == bit) { - value = (bit << 1) - value; - sign = -1; - } - } - return sign * ((float) value / multiplier); -} - -static uint8_t putFloat(uint8_t * dest, float value, uint8_t size, uint32_t multiplier, bool is_signed) { - // check sign - bool sign = value < 0; - if (sign) value = -value; - - // get value to store - uint32_t v = value * multiplier; - - // format an uint32_t as if it was an int32_t - if (is_signed & sign) { - uint32_t mask = (1 << (size * 8)) - 1; - v = v & mask; - if (sign) v = mask - v + 1; - } - - // add bytes (MSB first) - for (uint8_t i=1; i<=size; i++) { - dest[size - i] = (v & 0xFF); - v >>= 8; - } - return size; -} - static float findTelemValue(const uint8_t* buf, uint8_t size, uint8_t channel, uint8_t type) { uint8_t i = 0; @@ -185,10 +89,10 @@ static float findTelemValue(const uint8_t* buf, uint8_t size, uint8_t channel, u uint8_t ch = buf[i++]; // Get data type uint8_t t = buf[i++]; - uint8_t sz = getDataSize(t); + uint8_t sz = LPPData::getDataSize(t); if (ch == channel && t == type) { - return getFloat(&buf[i], sz, getMultiplier(t), isSigned(t)); + return LPPData::getFloat(&buf[i], sz, LPPData::getMultiplier(t), LPPData::isSigned(t)); } i += sz; // skip } @@ -209,10 +113,10 @@ bool SensorMesh::telemHasChanged(const uint8_t* min_deltas, uint8_t min_deltas_l uint8_t ch = buf[i++]; // Get data type uint8_t t = buf[i++]; - uint8_t sz = getDataSize(t); + uint8_t sz = LPPData::getDataSize(t); - float v = getFloat(&buf[i], sz, getMultiplier(t), isSigned(t)); - float pv = getFloat(&prev_telem[i], sz, getMultiplier(t), isSigned(t)); + float v = LPPData::getFloat(&buf[i], sz, LPPData::getMultiplier(t), LPPData::isSigned(t)); + float pv = LPPData::getFloat(&prev_telem[i], sz, LPPData::getMultiplier(t), LPPData::isSigned(t)); float min_delta = findTelemValue(min_deltas, min_deltas_len, ch, t); if (abs(v - pv) > min_delta) return true; // Yes, has changed @@ -268,12 +172,12 @@ uint8_t SensorMesh::handleRequest(ClientInfo* from, uint32_t sender_timestamp, u auto d = &data[i]; reply_data[ofs++] = d->_channel; reply_data[ofs++] = d->_lpp_type; - uint8_t sz = getDataSize(d->_lpp_type); - uint32_t mult = getMultiplier(d->_lpp_type); - bool is_signed = isSigned(d->_lpp_type); - ofs += putFloat(&reply_data[ofs], d->_min, sz, mult, is_signed); - ofs += putFloat(&reply_data[ofs], d->_max, sz, mult, is_signed); - ofs += putFloat(&reply_data[ofs], d->_avg, sz, mult, is_signed); + uint8_t sz = LPPData::getDataSize(d->_lpp_type); + uint32_t mult = LPPData::getMultiplier(d->_lpp_type); + bool is_signed = LPPData::isSigned(d->_lpp_type); + ofs += LPPData::putFloat(&reply_data[ofs], d->_min, sz, mult, is_signed); + ofs += LPPData::putFloat(&reply_data[ofs], d->_max, sz, mult, is_signed); + ofs += LPPData::putFloat(&reply_data[ofs], d->_avg, sz, mult, is_signed); } return ofs; } @@ -317,9 +221,9 @@ uint8_t SensorMesh::handleRequest(ClientInfo* from, uint32_t sender_timestamp, u from->extra.sensor.push_tag = 0; from->extra.sensor.expiry_timestamp = 0; // REVISIT: maybe return some stats, eg total number of telemetry pushes since SUBSCRIBE? - reply_data[4] = 0; // success - getRNG()->random(&reply_data[5], 3); // just some entropy for better packet-hash uniqueness - return 8; + memset(&reply_data[4], 0, 8); // success + getRNG()->random(&reply_data[12], 2); // just some entropy for better packet-hash uniqueness + return 12 + 2; } return 0; // unknown command } diff --git a/src/helpers/sensors/LPPDataHelpers.h b/src/helpers/sensors/LPPDataHelpers.h index 70a036c49..db49cade4 100644 --- a/src/helpers/sensors/LPPDataHelpers.h +++ b/src/helpers/sensors/LPPDataHelpers.h @@ -63,12 +63,66 @@ #define LPP_ERROR_OVERFLOW 1 #define LPP_ERROR_UNKOWN_TYPE 2 -class LPPReader { - const uint8_t* _buf; - uint8_t _len; - uint8_t _pos; +class LPPData { +public: + static uint8_t getDataSize(uint8_t type) { + switch (type) { + case LPP_GPS: + return 9; + case LPP_POLYLINE: + return 8; // TODO: this is MINIMIUM + case LPP_GYROMETER: + case LPP_ACCELEROMETER: + return 6; + case LPP_GENERIC_SENSOR: + case LPP_FREQUENCY: + case LPP_DISTANCE: + case LPP_ENERGY: + case LPP_UNIXTIME: + return 4; + case LPP_COLOUR: + return 3; + case LPP_ANALOG_INPUT: + case LPP_ANALOG_OUTPUT: + case LPP_LUMINOSITY: + case LPP_TEMPERATURE: + case LPP_CONCENTRATION: + case LPP_BAROMETRIC_PRESSURE: + case LPP_RELATIVE_HUMIDITY: + case LPP_ALTITUDE: + case LPP_VOLTAGE: + case LPP_CURRENT: + case LPP_DIRECTION: + case LPP_POWER: + return 2; + } + return 1; + } - float getFloat(const uint8_t * buffer, uint8_t size, uint32_t multiplier, bool is_signed) { + static uint32_t getMultiplier(uint8_t type) { + switch (type) { + case LPP_CURRENT: + case LPP_DISTANCE: + case LPP_ENERGY: + return 1000; + case LPP_VOLTAGE: + case LPP_ANALOG_INPUT: + case LPP_ANALOG_OUTPUT: + return 100; + case LPP_TEMPERATURE: + case LPP_BAROMETRIC_PRESSURE: + case LPP_RELATIVE_HUMIDITY: + return 10; + } + return 1; + } + + static bool isSigned(uint8_t type) { + return type == LPP_ALTITUDE || type == LPP_TEMPERATURE || type == LPP_GYROMETER || + type == LPP_ANALOG_INPUT || type == LPP_ANALOG_OUTPUT || type == LPP_GPS || type == LPP_ACCELEROMETER; + } + + static float getFloat(const uint8_t * buffer, uint8_t size, uint32_t multiplier, bool is_signed) { uint32_t value = 0; for (uint8_t i = 0; i < size; i++) { value = (value << 8) + buffer[i]; @@ -85,6 +139,36 @@ class LPPReader { return sign * ((float) value / multiplier); } + static uint8_t putFloat(uint8_t * dest, float value, uint8_t size, uint32_t multiplier, bool is_signed) { + // check sign + bool sign = value < 0; + if (sign) value = -value; + + // get value to store + uint32_t v = value * multiplier; + + // format an uint32_t as if it was an int32_t + if (is_signed & sign) { + uint32_t mask = (1 << (size * 8)) - 1; + v = v & mask; + if (sign) v = mask - v + 1; + } + + // add bytes (MSB first) + for (uint8_t i=1; i<=size; i++) { + dest[size - i] = (v & 0xFF); + v >>= 8; + } + return size; + } + +}; + +class LPPReader { + const uint8_t* _buf; + uint8_t _len; + uint8_t _pos; + public: LPPReader(const uint8_t buf[], uint8_t len) : _buf(buf), _len(len), _pos(0) { } @@ -103,72 +187,42 @@ public: } bool readGPS(float& lat, float& lon, float& alt) { - lat = getFloat(&_buf[_pos], 3, 10000, true); _pos += 3; - lon = getFloat(&_buf[_pos], 3, 10000, true); _pos += 3; - alt = getFloat(&_buf[_pos], 3, 100, true); _pos += 3; + lat = LPPData::getFloat(&_buf[_pos], 3, 10000, true); _pos += 3; + lon = LPPData::getFloat(&_buf[_pos], 3, 10000, true); _pos += 3; + alt = LPPData::getFloat(&_buf[_pos], 3, 100, true); _pos += 3; return _pos <= _len; } bool readVoltage(float& voltage) { - voltage = getFloat(&_buf[_pos], 2, 100, false); _pos += 2; + voltage = LPPData::getFloat(&_buf[_pos], 2, 100, false); _pos += 2; return _pos <= _len; } bool readCurrent(float& amps) { - amps = getFloat(&_buf[_pos], 2, 1000, true); _pos += 2; + amps = LPPData::getFloat(&_buf[_pos], 2, 1000, true); _pos += 2; return _pos <= _len; } bool readPower(float& watts) { - watts = getFloat(&_buf[_pos], 2, 1, false); _pos += 2; + watts = LPPData::getFloat(&_buf[_pos], 2, 1, false); _pos += 2; return _pos <= _len; } bool readTemperature(float& degrees_c) { - degrees_c = getFloat(&_buf[_pos], 2, 10, true); _pos += 2; + degrees_c = LPPData::getFloat(&_buf[_pos], 2, 10, true); _pos += 2; return _pos <= _len; } bool readPressure(float& pa) { - pa = getFloat(&_buf[_pos], 2, 10, false); _pos += 2; + pa = LPPData::getFloat(&_buf[_pos], 2, 10, false); _pos += 2; return _pos <= _len; } bool readRelativeHumidity(float& pct) { - pct = getFloat(&_buf[_pos], 1, 2, false); _pos += 1; + pct = LPPData::getFloat(&_buf[_pos], 1, 2, false); _pos += 1; return _pos <= _len; } bool readAltitude(float& m) { - m = getFloat(&_buf[_pos], 2, 1, true); _pos += 2; + m = LPPData::getFloat(&_buf[_pos], 2, 1, true); _pos += 2; return _pos <= _len; } void skipData(uint8_t type) { - switch (type) { - case LPP_GPS: - _pos += 9; break; - case LPP_POLYLINE: - _pos += 8; break; // TODO: this is MINIMUM - case LPP_GYROMETER: - case LPP_ACCELEROMETER: - _pos += 6; break; - case LPP_GENERIC_SENSOR: - case LPP_FREQUENCY: - case LPP_DISTANCE: - case LPP_ENERGY: - case LPP_UNIXTIME: - _pos += 4; break; - case LPP_COLOUR: - _pos += 3; break; - case LPP_ANALOG_INPUT: - case LPP_ANALOG_OUTPUT: - case LPP_LUMINOSITY: - case LPP_TEMPERATURE: - case LPP_CONCENTRATION: - case LPP_BAROMETRIC_PRESSURE: - case LPP_ALTITUDE: - case LPP_VOLTAGE: - case LPP_CURRENT: - case LPP_DIRECTION: - case LPP_POWER: - _pos += 2; break; - default: - _pos++; - } + _pos += LPPData::getDataSize(type); } }; @@ -185,6 +239,19 @@ class LPPWriter { public: LPPWriter(uint8_t buf[], uint8_t max_len): _buf(buf), _max_len(max_len), _len(0) { } + bool writeData(uint8_t channel, uint8_t type, float v) { + uint8_t sz = LPPData::getDataSize(type); + bool s = LPPData::isSigned(type); + uint32_t mul = LPPData::getMultiplier(type); + if (_len + 2 + sz <= _max_len) { + _buf[_len++] = channel; + _buf[_len++] = type; + _len += LPPData::putFloat(&_buf[_len], v, sz, mul, s); + return true; + } + return false; + } + bool writeVoltage(uint8_t channel, float voltage) { if (_len + 4 <= _max_len) { _buf[_len++] = channel;