h3n3/klipper
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

angle: mt6816 added support

Browse Source 
Signed-off-by: Timofey Titovets <nefelim4ag@gmail.com>
This commit is contained in:
Timofey Titovets
2024-10-20 04:24:28 +02:00
committed by KevinOConnor
parent 896343d943
commit 90c1b82baa
3 changed files with 90 additions and 9 deletions
Download Patch File Download Diff File Expand all files Collapse all files

45
src/sensor_angle.c
View File

@@ -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);
}
}