|
| 1 | +#pragma once |
| 2 | + |
| 3 | +#include <stdint.h> |
| 4 | + |
| 5 | +#define LPP_DIGITAL_INPUT 0 // 1 byte |
| 6 | +#define LPP_DIGITAL_OUTPUT 1 // 1 byte |
| 7 | +#define LPP_ANALOG_INPUT 2 // 2 bytes, 0.01 signed |
| 8 | +#define LPP_ANALOG_OUTPUT 3 // 2 bytes, 0.01 signed |
| 9 | +#define LPP_GENERIC_SENSOR 100 // 4 bytes, unsigned |
| 10 | +#define LPP_LUMINOSITY 101 // 2 bytes, 1 lux unsigned |
| 11 | +#define LPP_PRESENCE 102 // 1 byte, bool |
| 12 | +#define LPP_TEMPERATURE 103 // 2 bytes, 0.1°C signed |
| 13 | +#define LPP_RELATIVE_HUMIDITY 104 // 1 byte, 0.5% unsigned |
| 14 | +#define LPP_ACCELEROMETER 113 // 2 bytes per axis, 0.001G |
| 15 | +#define LPP_BAROMETRIC_PRESSURE 115 // 2 bytes 0.1hPa unsigned |
| 16 | +#define LPP_VOLTAGE 116 // 2 bytes 0.01V unsigned |
| 17 | +#define LPP_CURRENT 117 // 2 bytes 0.001A unsigned |
| 18 | +#define LPP_FREQUENCY 118 // 4 bytes 1Hz unsigned |
| 19 | +#define LPP_PERCENTAGE 120 // 1 byte 1-100% unsigned |
| 20 | +#define LPP_ALTITUDE 121 // 2 byte 1m signed |
| 21 | +#define LPP_CONCENTRATION 125 // 2 bytes, 1 ppm unsigned |
| 22 | +#define LPP_POWER 128 // 2 byte, 1W, unsigned |
| 23 | +#define LPP_DISTANCE 130 // 4 byte, 0.001m, unsigned |
| 24 | +#define LPP_ENERGY 131 // 4 byte, 0.001kWh, unsigned |
| 25 | +#define LPP_DIRECTION 132 // 2 bytes, 1deg, unsigned |
| 26 | +#define LPP_UNIXTIME 133 // 4 bytes, unsigned |
| 27 | +#define LPP_GYROMETER 134 // 2 bytes per axis, 0.01 °/s |
| 28 | +#define LPP_COLOUR 135 // 1 byte per RGB Color |
| 29 | +#define LPP_GPS 136 // 3 byte lon/lat 0.0001 °, 3 bytes alt 0.01 meter |
| 30 | +#define LPP_SWITCH 142 // 1 byte, 0/1 |
| 31 | +#define LPP_POLYLINE 240 // 1 byte size, 1 byte delta factor, 3 byte lon/lat 0.0001° * factor, n (size-8) bytes deltas |
| 32 | + |
| 33 | +// Multipliers |
| 34 | +#define LPP_DIGITAL_INPUT_MULT 1 |
| 35 | +#define LPP_DIGITAL_OUTPUT_MULT 1 |
| 36 | +#define LPP_ANALOG_INPUT_MULT 100 |
| 37 | +#define LPP_ANALOG_OUTPUT_MULT 100 |
| 38 | +#define LPP_GENERIC_SENSOR_MULT 1 |
| 39 | +#define LPP_LUMINOSITY_MULT 1 |
| 40 | +#define LPP_PRESENCE_MULT 1 |
| 41 | +#define LPP_TEMPERATURE_MULT 10 |
| 42 | +#define LPP_RELATIVE_HUMIDITY_MULT 2 |
| 43 | +#define LPP_ACCELEROMETER_MULT 1000 |
| 44 | +#define LPP_BAROMETRIC_PRESSURE_MULT 10 |
| 45 | +#define LPP_VOLTAGE_MULT 100 |
| 46 | +#define LPP_CURRENT_MULT 1000 |
| 47 | +#define LPP_FREQUENCY_MULT 1 |
| 48 | +#define LPP_PERCENTAGE_MULT 1 |
| 49 | +#define LPP_ALTITUDE_MULT 1 |
| 50 | +#define LPP_POWER_MULT 1 |
| 51 | +#define LPP_DISTANCE_MULT 1000 |
| 52 | +#define LPP_ENERGY_MULT 1000 |
| 53 | +#define LPP_DIRECTION_MULT 1 |
| 54 | +#define LPP_UNIXTIME_MULT 1 |
| 55 | +#define LPP_GYROMETER_MULT 100 |
| 56 | +#define LPP_GPS_LAT_LON_MULT 10000 |
| 57 | +#define LPP_GPS_ALT_MULT 100 |
| 58 | +#define LPP_SWITCH_MULT 1 |
| 59 | +#define LPP_CONCENTRATION_MULT 1 |
| 60 | +#define LPP_COLOUR_MULT 1 |
| 61 | + |
| 62 | +#define LPP_ERROR_OK 0 |
| 63 | +#define LPP_ERROR_OVERFLOW 1 |
| 64 | +#define LPP_ERROR_UNKOWN_TYPE 2 |
| 65 | + |
| 66 | +class LPPReader { |
| 67 | + const uint8_t* _buf; |
| 68 | + uint8_t _len; |
| 69 | + uint8_t _pos; |
| 70 | + |
| 71 | + float getFloat(const uint8_t * buffer, uint8_t size, uint32_t multiplier, bool is_signed) { |
| 72 | + uint32_t value = 0; |
| 73 | + for (uint8_t i = 0; i < size; i++) { |
| 74 | + value = (value << 8) + buffer[i]; |
| 75 | + } |
| 76 | + |
| 77 | + int sign = 1; |
| 78 | + if (is_signed) { |
| 79 | + uint32_t bit = 1ul << ((size * 8) - 1); |
| 80 | + if ((value & bit) == bit) { |
| 81 | + value = (bit << 1) - value; |
| 82 | + sign = -1; |
| 83 | + } |
| 84 | + } |
| 85 | + return sign * ((float) value / multiplier); |
| 86 | + } |
| 87 | + |
| 88 | +public: |
| 89 | + LPPReader(const uint8_t buf[], uint8_t len) : _buf(buf), _len(len), _pos(0) { } |
| 90 | + |
| 91 | + void reset() { |
| 92 | + _pos = 0; |
| 93 | + } |
| 94 | + |
| 95 | + bool readHeader(uint8_t& channel, uint8_t& type) { |
| 96 | + if (_pos + 2 < _len) { |
| 97 | + channel = _buf[_pos++]; |
| 98 | + type = _buf[_pos++]; |
| 99 | + |
| 100 | + return channel != 0; // channel 0 is End-of-data |
| 101 | + } |
| 102 | + return false; // end-of-buffer |
| 103 | + } |
| 104 | + |
| 105 | + bool readGPS(float& lat, float& lon, float& alt) { |
| 106 | + lat = getFloat(&_buf[_pos], 3, 10000, true); _pos += 3; |
| 107 | + lon = getFloat(&_buf[_pos], 3, 10000, true); _pos += 3; |
| 108 | + alt = getFloat(&_buf[_pos], 3, 100, true); _pos += 3; |
| 109 | + return _pos <= _len; |
| 110 | + } |
| 111 | + bool readVoltage(float& voltage) { |
| 112 | + voltage = getFloat(&_buf[_pos], 2, 100, false); _pos += 2; |
| 113 | + return _pos <= _len; |
| 114 | + } |
| 115 | + bool readCurrent(float& amps) { |
| 116 | + amps = getFloat(&_buf[_pos], 2, 1000, false); _pos += 2; |
| 117 | + return _pos <= _len; |
| 118 | + } |
| 119 | + bool readPower(float& watts) { |
| 120 | + watts = getFloat(&_buf[_pos], 2, 1, false); _pos += 2; |
| 121 | + return _pos <= _len; |
| 122 | + } |
| 123 | + bool readTemperature(float& degrees_c) { |
| 124 | + degrees_c = getFloat(&_buf[_pos], 2, 10, true); _pos += 2; |
| 125 | + return _pos <= _len; |
| 126 | + } |
| 127 | + bool readPressure(float& pa) { |
| 128 | + pa = getFloat(&_buf[_pos], 2, 10, false); _pos += 2; |
| 129 | + return _pos <= _len; |
| 130 | + } |
| 131 | + bool readRelativeHumidity(float& pct) { |
| 132 | + pct = getFloat(&_buf[_pos], 1, 2, false); _pos += 1; |
| 133 | + return _pos <= _len; |
| 134 | + } |
| 135 | + bool readAltitude(float& m) { |
| 136 | + m = getFloat(&_buf[_pos], 2, 1, true); _pos += 2; |
| 137 | + return _pos <= _len; |
| 138 | + } |
| 139 | + |
| 140 | + void skipData(uint8_t type) { |
| 141 | + switch (type) { |
| 142 | + case LPP_GPS: |
| 143 | + _pos += 9; break; |
| 144 | + case LPP_POLYLINE: |
| 145 | + _pos += 8; break; // TODO: this is MINIMIUM |
| 146 | + case LPP_GYROMETER: |
| 147 | + case LPP_ACCELEROMETER: |
| 148 | + _pos += 6; break; |
| 149 | + case LPP_GENERIC_SENSOR: |
| 150 | + case LPP_FREQUENCY: |
| 151 | + case LPP_DISTANCE: |
| 152 | + case LPP_ENERGY: |
| 153 | + case LPP_UNIXTIME: |
| 154 | + _pos += 4; break; |
| 155 | + case LPP_COLOUR: |
| 156 | + _pos += 3; break; |
| 157 | + case LPP_ANALOG_INPUT: |
| 158 | + case LPP_ANALOG_OUTPUT: |
| 159 | + case LPP_LUMINOSITY: |
| 160 | + case LPP_TEMPERATURE: |
| 161 | + case LPP_CONCENTRATION: |
| 162 | + case LPP_BAROMETRIC_PRESSURE: |
| 163 | + case LPP_ALTITUDE: |
| 164 | + case LPP_VOLTAGE: |
| 165 | + case LPP_CURRENT: |
| 166 | + case LPP_DIRECTION: |
| 167 | + case LPP_POWER: |
| 168 | + _pos += 2; break; |
| 169 | + default: |
| 170 | + _pos++; |
| 171 | + } |
| 172 | + } |
| 173 | +}; |
| 174 | + |
| 175 | +class LPPWriter { |
| 176 | + uint8_t* _buf; |
| 177 | + uint8_t _max_len; |
| 178 | + uint8_t _len; |
| 179 | + |
| 180 | + void write(uint16_t value) { |
| 181 | + _buf[_len++] = (value >> 8) & 0xFF; // MSB |
| 182 | + _buf[_len++] = value & 0xFF; // LSB |
| 183 | + } |
| 184 | + |
| 185 | +public: |
| 186 | + LPPWriter(uint8_t buf[], uint8_t max_len): _buf(buf), _max_len(max_len), _len(0) { } |
| 187 | + |
| 188 | + bool writeVoltage(uint8_t channel, float voltage) { |
| 189 | + if (_len + 4 <= _max_len) { |
| 190 | + _buf[_len++] = channel; |
| 191 | + _buf[_len++] = LPP_VOLTAGE; |
| 192 | + uint16_t value = voltage * 100; |
| 193 | + write(value); |
| 194 | + return true; |
| 195 | + } |
| 196 | + return false; |
| 197 | + } |
| 198 | + |
| 199 | + bool writeGPS(uint8_t channel, float lat, float lon, float alt) { |
| 200 | + if (_len + 11 <= _max_len) { |
| 201 | + _buf[_len++] = channel; |
| 202 | + _buf[_len++] = LPP_GPS; |
| 203 | + |
| 204 | + int32_t lati = lat * 10000; // we lose some precision :-( |
| 205 | + int32_t loni = lon * 10000; |
| 206 | + int32_t alti = alt * 100; |
| 207 | + |
| 208 | + _buf[_len++] = lati >> 16; |
| 209 | + _buf[_len++] = lati >> 8; |
| 210 | + _buf[_len++] = lati; |
| 211 | + _buf[_len++] = loni >> 16; |
| 212 | + _buf[_len++] = loni >> 8; |
| 213 | + _buf[_len++] = loni; |
| 214 | + _buf[_len++] = alti >> 16; |
| 215 | + _buf[_len++] = alti >> 8; |
| 216 | + _buf[_len++] = alti; |
| 217 | + return true; |
| 218 | + } |
| 219 | + return false; |
| 220 | + } |
| 221 | + |
| 222 | + uint8_t length() { return _len; } |
| 223 | +}; |
0 commit comments