diff options
| -rw-r--r-- | include/driver/sen5x.h | 3 | ||||
| -rw-r--r-- | src/driver/sen5x.cc | 32 |
2 files changed, 35 insertions, 0 deletions
diff --git a/include/driver/sen5x.h b/include/driver/sen5x.h index 47d92a0..bc7b54b 100644 --- a/include/driver/sen5x.h +++ b/include/driver/sen5x.h @@ -13,6 +13,9 @@ class SEN5x { unsigned char txbuf[2]; unsigned char rxbuf[24]; + unsigned char crcWord(unsigned char byte1, unsigned char byte2); + bool crcValid(unsigned char* data, unsigned char length); + public: SEN5x() {} diff --git a/src/driver/sen5x.cc b/src/driver/sen5x.cc index 9edbcd0..28a4a0b 100644 --- a/src/driver/sen5x.cc +++ b/src/driver/sen5x.cc @@ -44,6 +44,11 @@ bool SEN5x::read() if (i2c.xmit(address, 0, txbuf, 24, rxbuf)) { return false; } + + if (!crcValid(rxbuf, 24)) { + return false; + } + pm1 = (rxbuf[0] << 8) + rxbuf[1]; pm2_5 = (rxbuf[3] << 8) + rxbuf[4]; pm4 = (rxbuf[6] << 8) + rxbuf[7]; @@ -67,6 +72,10 @@ bool SEN5x::readStatus() return false; } + if (!crcValid(rxbuf, 6)) { + return false; + } + fan_speed_warning = rxbuf[1] & 0x20; fan_cleaning_active = rxbuf[1] & 0x08; gas_sensor_error = rxbuf[4] & 0x80; @@ -77,4 +86,27 @@ bool SEN5x::readStatus() return true; } +unsigned char SEN5x::crcWord(unsigned char byte1, unsigned char byte2) +{ + unsigned char crc = 0xff ^ byte1; + for (unsigned char bit = 8; bit > 0; bit--) { + crc = (crc << 1) ^ (crc & 0x80 ? 0x31 : 0); + } + crc ^= byte2; + for (unsigned char bit = 8; bit > 0; bit--) { + crc = (crc << 1) ^ (crc & 0x80 ? 0x31 : 0); + } + return crc; +} + +bool SEN5x::crcValid(unsigned char* data, unsigned char length) +{ + for (unsigned char i = 0; i < length; i += 3) { + if (crcWord(data[i], data[i+1]) != data[i+2]) { + return false; + } + } + return true; +} + SEN5x sen5x; |