angle: mt6816 added support

Signed-off-by: Timofey Titovets <nefelim4ag@gmail.com>

1 files changed, 40 insertions, 5 deletions

diff --git a/src/sensor_angle.c b/src/sensor_angle.c
index 54caecc2..3dfd6009 100644
--- a/src/sensor_angle.c
+++ b/src/sensor_angle.c
@@ -13,11 +13,18 @@
 #include "sensor_bulk.h" // sensor_bulk_report
 #include "spicmds.h" // spidev_transfer
 
-enum { SA_CHIP_A1333, SA_CHIP_AS5047D, SA_CHIP_TLE5012B, SA_CHIP_MAX };
+enum {
+    SA_CHIP_A1333,
+    SA_CHIP_AS5047D,
+    SA_CHIP_TLE5012B,
+    SA_CHIP_MT6816,
+    SA_CHIP_MAX
+};
 
 DECL_ENUMERATION("spi_angle_type", "a1333", SA_CHIP_A1333);
 DECL_ENUMERATION("spi_angle_type", "as5047d", SA_CHIP_AS5047D);
 DECL_ENUMERATION("spi_angle_type", "tle5012b", SA_CHIP_TLE5012B);
+DECL_ENUMERATION("spi_angle_type", "mt6816", SA_CHIP_MT6816);
 
 enum { TCODE_ERROR = 0xff };
 enum {
@@ -131,6 +138,15 @@ a1333_query(struct spi_angle *sa, uint32_t stime)
         angle_add_data(sa, stime, mtime1, (msg[0] << 9) | (msg[1] << 1));
 }
 
+static int bit_parity(uint8_t *msg)
+{
+    uint_fast8_t parity = msg[0] ^ msg[1];
+    parity ^= parity >> 4;
+    parity ^= parity >> 2;
+    parity ^= parity >> 1;
+    return parity;
+}
+
 // as5047d sensor query
 static void
 as5047d_query(struct spi_angle *sa, uint32_t stime)
@@ -147,10 +163,7 @@ as5047d_query(struct spi_angle *sa, uint32_t stime)
     msg[0] = 0xC0;
     msg[1] = 0x00;
     spidev_transfer(sa->spi, 1, sizeof(msg), msg);
-    uint_fast8_t parity = msg[0] ^ msg[1];
-    parity ^= parity >> 4;
-    parity ^= parity >> 2;
-    parity ^= parity >> 1;
+    uint_fast8_t parity = bit_parity(msg);
     if (parity & 1)
         angle_add_error(sa, SE_CRC);
     else if (msg[0] & 0x40)
@@ -159,6 +172,26 @@ as5047d_query(struct spi_angle *sa, uint32_t stime)
         angle_add_data(sa, stime, mtime2, (msg[0] << 10) | (msg[1] << 2));
 }
 
+static void mt6816_query(struct spi_angle *sa, uint32_t stime)
+{
+    uint8_t msg[3] = {0x83, 0x00, 0x00};
+    uint32_t mtime1 = timer_read_time();
+    spidev_transfer(sa->spi, 1, sizeof(msg), msg);
+    uint32_t mtime2 = timer_read_time();
+    // Data is latched on first sclk edge of response
+    if (mtime2 - mtime1 > MAX_SPI_READ_TIME) {
+        angle_add_error(sa, SE_SPI_TIME);
+        return;
+    }
+    uint_fast8_t parity = bit_parity(&msg[1]);
+    if (parity & 1)
+        angle_add_error(sa, SE_CRC);
+    else if (msg[2] & 0x02)
+        angle_add_error(sa, SE_NO_ANGLE);
+    else
+        angle_add_data(sa, stime, mtime2, (msg[1] << 8) | (msg[2] & 0xfc));
+}
+
 #define TLE_READ 0x80
 #define TLE_READ_LATCH (TLE_READ | 0x04)
 #define TLE_REG_AVAL 0x02
@@ -301,6 +334,8 @@ spi_angle_task(void)
             as5047d_query(sa, stime);
         else if (chip == SA_CHIP_TLE5012B)
             tle5012b_query(sa, stime);
+        else if (chip == SA_CHIP_MT6816)
+            mt6816_query(sa, stime);
         angle_check_report(sa, oid);
     }
 }