mirror of
https://github.com/scottbez1/smartknob.git
synced 2025-11-04 17:19:40 +08:00
- exposed strain SCK and DO - use 12 bit PWM (instead of 16 bit) for screen brightness on s3 - added option to use uart serial for s3 - bump nanopb/Nanopb @ 0.4.7 - fixed SPI bus alias and DMA channel for s3 --------- Co-authored-by: Scott Bezek <scottbez1@gmail.com>
141 lines
3.7 KiB
C++
141 lines
3.7 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;
|
|
}
|
|
|
|
#if SENSOR_MT6701
|
|
|
|
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,
|
|
};
|
|
|
|
#ifdef CONFIG_IDF_TARGET_ESP32S3
|
|
esp_err_t ret = spi_bus_initialize(SPI3_HOST, &tx_bus_config, SPI_DMA_CH_AUTO);
|
|
#else
|
|
esp_err_t ret = spi_bus_initialize(HSPI_HOST, &tx_bus_config, 1);
|
|
#endif
|
|
|
|
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,
|
|
};
|
|
#ifdef CONFIG_IDF_TARGET_ESP32S3
|
|
ret=spi_bus_add_device(SPI3_HOST, &tx_device_config, &spi_device_);
|
|
#else
|
|
ret=spi_bus_add_device(HSPI_HOST, &tx_device_config, &spi_device_);
|
|
#endif
|
|
|
|
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 {
|
|
error_ = {
|
|
.error = true,
|
|
.received_crc = received_crc,
|
|
.calculated_crc = calculated_crc,
|
|
};
|
|
}
|
|
|
|
last_update_ = now;
|
|
}
|
|
float rad = -atan2f(y_, x_);
|
|
if (rad < 0) {
|
|
rad += 2*PI;
|
|
}
|
|
return rad;
|
|
}
|
|
|
|
MT6701Error MT6701Sensor::getAndClearError() {
|
|
MT6701Error out = error_;
|
|
error_ = {};
|
|
return out;
|
|
}
|
|
|
|
#endif |