b47fcf7da4
- Modify TLV493d library to expose frame counter in order to check for lockup, and implement auto-reset in tlv_sensor in case of lockup - Implement MT6701 SimpleFOC sensor - Make display optional - Add optional LED, strain, ALS support - Connect ALS to LED and display brightness - Hardcoded strain gauge thresholds and haptic feedback
119 lines
3.2 KiB
C++
119 lines
3.2 KiB
C++
#include "mt6701_sensor.h"
|
|
#include "driver/spi_master.h"
|
|
|
|
static const float ALPHA = 0.4;
|
|
|
|
|
|
static uint8_t tableCRC6[64] = {
|
|
0x00, 0x03, 0x06, 0x05, 0x0C, 0x0F, 0x0A, 0x09,
|
|
0x18, 0x1B, 0x1E, 0x1D, 0x14, 0x17, 0x12, 0x11,
|
|
0x30, 0x33, 0x36, 0x35, 0x3C, 0x3F, 0x3A, 0x39,
|
|
0x28, 0x2B, 0x2E, 0x2D, 0x24, 0x27, 0x22, 0x21,
|
|
0x23, 0x20, 0x25, 0x26, 0x2F, 0x2C, 0x29, 0x2A,
|
|
0x3B, 0x38, 0x3D, 0x3E, 0x37, 0x34, 0x31, 0x32,
|
|
0x13, 0x10, 0x15, 0x16, 0x1F, 0x1C, 0x19, 0x1A,
|
|
0x0B, 0x08, 0x0D, 0x0E, 0x07, 0x04, 0x01, 0x02
|
|
};
|
|
|
|
/*32-bit input data, right alignment, Calculation over 18 bits (mult. of 6) */
|
|
static uint8_t CRC6_43_18bit (uint32_t w_InputData)
|
|
{
|
|
uint8_t b_Index = 0;
|
|
uint8_t b_CRC = 0;
|
|
|
|
b_Index = (uint8_t )(((uint32_t)w_InputData >> 12u) & 0x0000003Fu);
|
|
|
|
b_CRC = (uint8_t )(((uint32_t)w_InputData >> 6u) & 0x0000003Fu);
|
|
b_Index = b_CRC ^ tableCRC6[b_Index];
|
|
|
|
b_CRC = (uint8_t )((uint32_t)w_InputData & 0x0000003Fu);
|
|
b_Index = b_CRC ^ tableCRC6[b_Index];
|
|
|
|
b_CRC = tableCRC6[b_Index];
|
|
|
|
return b_CRC;
|
|
}
|
|
|
|
|
|
|
|
MT6701Sensor::MT6701Sensor() {}
|
|
|
|
void MT6701Sensor::init() {
|
|
|
|
pinMode(PIN_MT_CSN, OUTPUT);
|
|
digitalWrite(PIN_MT_CSN, HIGH);
|
|
|
|
spi_bus_config_t tx_bus_config = {
|
|
.mosi_io_num = -1,
|
|
.miso_io_num = PIN_MT_DATA,
|
|
.sclk_io_num = PIN_MT_CLOCK,
|
|
.quadwp_io_num = -1,
|
|
.quadhd_io_num = -1,
|
|
.max_transfer_sz = 1000,
|
|
};
|
|
esp_err_t ret = spi_bus_initialize(HSPI_HOST, &tx_bus_config, 1);
|
|
ESP_ERROR_CHECK(ret);
|
|
|
|
spi_device_interface_config_t tx_device_config = {
|
|
.command_bits=0,
|
|
.address_bits=0,
|
|
.dummy_bits=0,
|
|
.mode=1,
|
|
.duty_cycle_pos=0,
|
|
.cs_ena_pretrans=4,
|
|
.cs_ena_posttrans=0,
|
|
.clock_speed_hz=4000000,
|
|
.input_delay_ns=0,
|
|
.spics_io_num=PIN_MT_CSN,
|
|
.flags = 0,
|
|
.queue_size=1,
|
|
.pre_cb=NULL,
|
|
.post_cb=NULL,
|
|
};
|
|
ret=spi_bus_add_device(HSPI_HOST, &tx_device_config, &spi_device_);
|
|
ESP_ERROR_CHECK(ret);
|
|
|
|
|
|
spi_transaction_.flags = SPI_TRANS_USE_RXDATA;
|
|
spi_transaction_.length = 24;
|
|
spi_transaction_.rxlength = 24;
|
|
spi_transaction_.tx_buffer = NULL;
|
|
spi_transaction_.rx_buffer = NULL;
|
|
}
|
|
|
|
float MT6701Sensor::getSensorAngle() {
|
|
uint32_t now = micros();
|
|
if (now - last_update_ > 100) {
|
|
|
|
esp_err_t ret=spi_device_polling_transmit(spi_device_, &spi_transaction_);
|
|
assert(ret==ESP_OK);
|
|
|
|
uint32_t spi_32 = (spi_transaction_.rx_data[0] << 16) | (spi_transaction_.rx_data[1] << 8) | spi_transaction_.rx_data[2];
|
|
uint32_t angle_spi = spi_32 >> 10;
|
|
|
|
uint8_t field_status = (spi_32 >> 6) & 0x3;
|
|
uint8_t push_status = (spi_32 >> 8) & 0x1;
|
|
uint8_t loss_status = (spi_32 >> 9) & 0x1;
|
|
|
|
uint8_t received_crc = spi_32 & 0x3F;
|
|
uint8_t calculated_crc = CRC6_43_18bit(spi_32 >> 6);
|
|
|
|
if (received_crc == calculated_crc) {
|
|
float new_angle = (float)angle_spi * 2 * PI / 16384;
|
|
float new_x = cosf(new_angle);
|
|
float new_y = sinf(new_angle);
|
|
x_ = new_x * ALPHA + x_ * (1-ALPHA);
|
|
y_ = new_y * ALPHA + y_ * (1-ALPHA);
|
|
} else {
|
|
Serial.printf("Bad CRC. expected %d, actual %d\n", calculated_crc, received_crc);
|
|
}
|
|
|
|
last_update_ = now;
|
|
}
|
|
float rad = -atan2f(y_, x_);
|
|
if (rad < 0) {
|
|
rad += 2*PI;
|
|
}
|
|
return rad;
|
|
}
|