summaryrefslogtreecommitdiff
diff options
context:
space:
mode:
-rw-r--r--include/driver/sen5x.h3
-rw-r--r--src/driver/sen5x.cc32
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;