Add CRC check to MT6835

Author: Richard Unger

src/encoders/mt6835/MT6835.cpp

@@ -26,7 +26,39 @@ void MT6835::init(SPIClass* _spi) {
 
 
 float MT6835::getCurrentAngle(){
-    return readRawAngle21() / (float)MT6835_CPR * _2PI;
+    uint32_t rawangle = readRawAngle21();
+    if (checkcrc) {
+        if (lastcrc != calcCrc(rawangle, laststatus)) {
+            laststatus |= MT6835_CRC_ERROR;
+            return -1; // return -1 to signal CRC error - the current angle has to be non-negative otherwise
+        }
+    }
+    return rawangle / (float)MT6835_CPR * _2PI;
+};
+
+
+// calculate crc8 of 21 angle bits and 3 status bits
+// polynomial: x^8 + x^2 + x + 1 = 0x07, (0xE0 reflected) init is 0x00, no final xor
+// TOOD table-based version
+uint8_t MT6835::calcCrc(uint32_t angle, uint8_t status) {
+    uint8_t crc = 0x00;
+
+    uint8_t input = angle>>13;
+    crc ^= input;
+    for (int k = 8; k > 0; k--)
+        crc = (crc & (0x01<<7))?(crc<<1)^0x07:crc<<1;
+
+    input = (angle>>5) & 0xFF;
+    crc ^= input;
+    for (int k = 8; k > 0; k--)
+        crc = (crc & (0x01<<7))?(crc<<1)^0x07:crc<<1;
+
+    input = ((angle<<3) & 0xFF) | (status & 0x07);
+    crc ^= input;
+    for (int k = 8; k > 0; k--)
+        crc = (crc & (0x01<<7))?(crc<<1)^0x07:crc<<1;
+
+    return crc;
 };
 
 
@@ -47,6 +79,7 @@ uint32_t MT6835::readRawAngle21(){
         digitalWrite(nCS, HIGH);
     spi->endTransaction();
     laststatus = data[4]&0x07;
+    lastcrc = data[5];
     return (data[2] << 13) | (data[3] << 5) | (data[4] >> 3);
 };
 

src/encoders/mt6835/MT6835.h

@@ -22,6 +22,7 @@
 #define MT6835_STATUS_OVERSPEED 0x01
 #define MT6835_STATUS_WEAKFIELD 0x02
 #define MT6835_STATUS_UNDERVOLT 0x04
+#define MT6835_CRC_ERROR 0x08
 
 #define MT6835_WRITE_ACK 0x55
 
@@ -202,14 +203,18 @@ class MT6835 {
 	bool setZeroFromCurrentPosition();
 	bool writeEEPROM(); // wait 6s after calling this method
 
+	bool checkcrc = false;
+	
 private:
 	SPIClass* spi;
 	SPISettings settings;
 	int nCS = -1;
 	uint8_t laststatus = 0;
+	uint8_t lastcrc = 0;
 
     void transfer24(MT6835Command* outValue);
     uint8_t readRegister(uint16_t reg);
     bool writeRegister(uint16_t reg, uint8_t value);
+	uint8_t calcCrc(uint32_t angle, uint8_t status);
 
 };