diff --git a/src/sensors/buff_encoder.cpp b/src/sensors/buff_encoder.cpp index 28468ecd..79e0526b 100644 --- a/src/sensors/buff_encoder.cpp +++ b/src/sensors/buff_encoder.cpp @@ -3,13 +3,26 @@ const SPISettings BuffEncoder::m_settings = SPISettings(1000000, MT6835_BITORDER, SPI_MODE3); + +uint32_t read_count = 0; void BuffEncoder::init() { // set the SPI pins to the correct mode pinMode(config_data.spi_cs, OUTPUT); digitalWrite(config_data.spi_cs, HIGH); // set CS high to start + + while (read_zero_pos() != 0.0f && read_count < read_zero_pos_max_attempts) { + write_zero_pos(0); + read_count++; + } + read_count = 0; } void BuffEncoder::read() { + while (read_zero_pos() != 0.0f && read_count < read_zero_pos_max_attempts) { + write_zero_pos(0); + read_count++; + } + read_count = 0; uint8_t data[6] = { 0 }; // transact 48 bits @@ -25,6 +38,23 @@ void BuffEncoder::read() { digitalWrite(config_data.spi_cs, HIGH); SPI.endTransaction(); + uint8_t status = data[4] & 0x07; + uint8_t crc_received = data[5]; + uint8_t crc_computed = mt6835_crc8(&data[2], 3); + + if (crc_received != crc_computed) { + Serial.printf("Pin: %u, MT6835 CRC mismatch\n", config_data.spi_cs); + return; + } + if (status & MT6835_STATUS_UNDERVOLT) { + Serial.printf("Pin: %u, MT6835 undervoltage detected\n", config_data.spi_cs); + return; + } + if (status & MT6835_STATUS_WEAKFIELD) { + Serial.printf("Pin: %u, MT6835 weak field detected\n", config_data.spi_cs); + return; + } + // convert received angle into radians int raw_angle = (data[2] << 13) | (data[3] << 5) | (data[4] >> 3); float radians = raw_angle / (float)MT6835_CPR * (3.14159265 * 2.0); @@ -35,6 +65,107 @@ void BuffEncoder::read() { // Serial.printf("Buff Encoder %u - angle: %f\n", static_cast(config_data.encoder_name), m_angle); comms_data.m_angle = m_angle; + + // read_zero_pos(); +} + +void BuffEncoder::write_zero_pos(uint16_t zero_pos_raw) { + if (zero_pos_raw > 0x0FFF) { + Serial.printf("Pin: %u, ZERO_POS value out of range: %u\n", config_data.spi_cs, zero_pos_raw); + return; + } + + uint8_t zero_pos_high = (zero_pos_raw >> 4) & 0xFF; // ZERO_POS[11:4] -> full byte for 0x009 + uint8_t zero_pos_low = zero_pos_raw & 0x0F; // ZERO_POS[3:0] -> top nibble of 0x00A + + // --- Read back current 0x00A first, so we don't clobber Z_EDGE / Z_PUL_WID[2:0] --- + uint8_t tx_read[3] = { 0 }; + tx_read[0] = (MT6835_OP_READ << 4) | ((MT6835_REG_ZERO2 >> 8) & 0x0F); + tx_read[1] = MT6835_REG_ZERO2 & 0xFF; + tx_read[2] = 0x00; + + SPI.beginTransaction(m_settings); + digitalWrite(config_data.spi_cs, LOW); + SPI.transfer(tx_read, 3); + digitalWrite(config_data.spi_cs, HIGH); + SPI.endTransaction(); + + uint8_t current_00A = tx_read[2]; + uint8_t z_edge_and_pulwid = current_00A & 0x0F; // preserve Z_EDGE (bit3) + Z_PUL_WID[2:0] (bits2:0) + + delayMicroseconds(1); + + // --- Write 0x009: ZERO_POS[11:4] --- + uint8_t tx009[3]; + tx009[0] = (MT6835_OP_WRITE << 4) | ((MT6835_REG_ZERO1 >> 8) & 0x0F); + tx009[1] = MT6835_REG_ZERO1 & 0xFF; + tx009[2] = zero_pos_high; + + SPI.beginTransaction(m_settings); + digitalWrite(config_data.spi_cs, LOW); + SPI.transfer(tx009, 3); + digitalWrite(config_data.spi_cs, HIGH); + SPI.endTransaction(); + + delayMicroseconds(1); + + // --- Write 0x00A: ZERO_POS[3:0] | Z_EDGE | Z_PUL_WID[2:0] (preserved) --- + uint8_t tx00A[3]; + tx00A[0] = (MT6835_OP_WRITE << 4) | ((MT6835_REG_ZERO2 >> 8) & 0x0F); + tx00A[1] = MT6835_REG_ZERO2 & 0xFF; + tx00A[2] = (zero_pos_low << 4) | z_edge_and_pulwid; + + SPI.beginTransaction(m_settings); + digitalWrite(config_data.spi_cs, LOW); + SPI.transfer(tx00A, 3); + digitalWrite(config_data.spi_cs, HIGH); + SPI.endTransaction(); + + Serial.printf("Pin: %u, wrote ZERO_POS = 0x%03X (%u)\n", + config_data.spi_cs, zero_pos_raw, zero_pos_raw); +} + +float BuffEncoder::read_zero_pos() { + // --- Read register 0x009: ZERO_POS[11:4] --- + uint8_t tx009[3] = { 0 }; + tx009[0] = (MT6835_OP_READ << 4) | ((MT6835_REG_ZERO1 >> 8) & 0x0F); // command nibble + A11:A8 + tx009[1] = MT6835_REG_ZERO1 & 0xFF; // A7:A0 + tx009[2] = 0x00; // dummy byte to clock out data + + SPI.beginTransaction(m_settings); + digitalWrite(config_data.spi_cs, LOW); + SPI.transfer(tx009, 3); + digitalWrite(config_data.spi_cs, HIGH); + SPI.endTransaction(); + + uint8_t zero_pos_high = tx009[2]; // ZERO_POS[11:4] + + delayMicroseconds(10); // small gap between transactions, adjust if needed + + // --- Read register 0x00A: ZERO_POS[3:0] | Z_EDGE | Z_PUL_WID[2:0] --- + uint8_t tx00A[3] = { 0 }; + tx00A[0] = (MT6835_OP_READ << 4) | ((MT6835_REG_ZERO2 >> 8) & 0x0F); + tx00A[1] = MT6835_REG_ZERO2 & 0xFF; + tx00A[2] = 0x00; + + SPI.beginTransaction(m_settings); + digitalWrite(config_data.spi_cs, LOW); + SPI.transfer(tx00A, 3); + digitalWrite(config_data.spi_cs, HIGH); + SPI.endTransaction(); + + uint8_t reg00A = tx00A[2]; + uint8_t zero_pos_low = (reg00A >> 4) & 0x0F; // ZERO_POS[3:0] + + uint16_t zero_pos_raw = (static_cast(zero_pos_high) << 4) | zero_pos_low; // 12-bit value, 0-4095 + + if (zero_pos_raw != 0) { + Serial.printf("Pin: %u, ZERO_POS raw = 0x%03X (%u), degrees = %.3f\n", + config_data.spi_cs, zero_pos_raw, zero_pos_raw, + zero_pos_raw * (360.0f / 4096.0f)); + } + + return zero_pos_raw * (360.0f / 4096.0f); } void BuffEncoder::send_to_comms() const { @@ -46,4 +177,21 @@ void BuffEncoder::send_to_comms() const { void BuffEncoder::print() const{ Serial.printf("Buff Encoder:\n\t"); Serial.println(get_angle()); -} \ No newline at end of file +} + +uint8_t BuffEncoder::mt6835_crc8(const uint8_t* data, size_t len) const { + constexpr uint8_t poly = 0x07; // X^8 + X^2 + X + 1 + uint8_t crc = 0x00; // datasheet does not state a seed; 0x00 is the typical default for this polynomial family + + for (size_t i = 0; i < len; ++i) { + crc ^= data[i]; + for (uint8_t bit = 0; bit < 8; ++bit) { + if (crc & 0x80) { + crc = (crc << 1) ^ poly; + } else { + crc = crc << 1; + } + } + } + return crc; +} diff --git a/src/sensors/buff_encoder.hpp b/src/sensors/buff_encoder.hpp index 8ddf6a21..d7771efd 100644 --- a/src/sensors/buff_encoder.hpp +++ b/src/sensors/buff_encoder.hpp @@ -6,6 +6,7 @@ #include "sensors/sensor.hpp" #include "comms/data/buff_encoder_data.hpp" +constexpr uint32_t read_zero_pos_max_attempts = 10; // Encoder Registers and Config constexpr uint32_t MT6835_OP_READ = 0b0011; @@ -55,6 +56,14 @@ class BuffEncoder : public Sensor { /// @note Returns and sets m_angle when it reads void read() override; + /// @brief Read the ZERO_POS registers from the encoder + /// @return The ZERO_POS value in degrees (0-360) + float read_zero_pos(); + + /// @brief Write the ZERO_POS registers to the encoder + /// @param zero_pos_raw 12-bit value (0-4095) to write + void write_zero_pos(uint16_t zero_pos_raw); + /// @brief Send the current data to comms void send_to_comms() const override; @@ -69,6 +78,12 @@ class BuffEncoder : public Sensor { /// @brief Print the data for debugging void print() const; + /// @brief Compute CRC8 per MT6835 datasheet spec (poly = X^8 + X^2 + X + 1, MSB first) + /// @param data Pointer to the 3 bytes covering ANGLE[20:0] + STATUS[2:0] (i.e. data[2], data[3], data[4] from the angle burst read) + /// @param len Number of bytes (should be 3 for this chip) + /// @return Computed 8-bit CRC + uint8_t mt6835_crc8(const uint8_t* data, size_t len) const; + private: /// @brief Read angle from the encoder