493 lines
14 KiB
C
493 lines
14 KiB
C
/*******************************************************************************
|
|
* Copyright © 2019 TRINAMIC Motion Control GmbH & Co. KG
|
|
* (now owned by Analog Devices Inc.),
|
|
*
|
|
* Copyright © 2023 Analog Devices Inc. All Rights Reserved. This software is
|
|
* proprietary & confidential to Analog Devices, Inc. and its licensors.
|
|
*******************************************************************************/
|
|
|
|
|
|
#include "TMC2209.h"
|
|
#include "tmc/hal/UART.h"
|
|
#include "tmc/helpers/CRC.h"
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
typedef struct {
|
|
uint8_t table[256];
|
|
uint8_t polynomial;
|
|
bool isReflected;
|
|
} CRCTypeDef;
|
|
|
|
CRCTypeDef CRCTables[CRC_TABLE_COUNT] = { 0 };
|
|
|
|
static uint8_t flipByte(uint8_t value);
|
|
static uint32_t flipBitsInBytes(uint32_t value);
|
|
|
|
/* This function generates the Lookup table used for CRC calculations.
|
|
*
|
|
* Arguments:
|
|
* uint8_t polynomial: The CRC polynomial for which the table will be generated.
|
|
* bool isReflected: Indicator whether the CRC table will be reflected or not.
|
|
* uint8_t index: The index of the table to be filled.
|
|
*
|
|
* How it works:
|
|
* A CRC calculation of a byte can be done by taking the byte to be CRC'd,
|
|
* shifting it left by one (appending a 0) and - if a 1 has been shifted out -
|
|
* XOR-ing in the CRC polynomial. After 8 iterations the result will be the
|
|
* CRC of the Byte.
|
|
*
|
|
* The function below does this in a compact way, by using all 4 bytes of a
|
|
* uint32_t to do 4 separate CRC bytes at once.
|
|
* For this to work without the Byte shifting interfering with adjacent bytes,
|
|
* the polynomial has the 8th bit (0x100) set. That way, if the shifted-out bit
|
|
* is 1, the following XOR-ing with the CRC polynomial will set that 1 to a 0,
|
|
* resulting in the shifted-in 0 for the adjacent byte.
|
|
* This process will go from the the lowest to the highest byte, resulting in
|
|
* fully independent byte-wise CRC calculations. For the highest byte, the value
|
|
* of the shifted-out byte needs to be stored before shifting the bytes (isMSBSet).
|
|
*
|
|
* The for-loop that iterates over all uint8_t values starts out with the
|
|
* uint8_t values 3 to 0 stored in one uint32_t: 0x03020100
|
|
* for each iteration each uint8_t value will increase by 4..
|
|
* 0 -> 4 -> 8 -> C -> ...
|
|
* 1 -> 5 -> 9 -> D -> ...
|
|
* 2 -> 6 -> A -> E -> ...
|
|
* 3 -> 7 -> B -> F -> ...
|
|
* ..resulting in an increase of the uint32_t by 0x04040404:
|
|
* 0x03020100 -> 0x07060504 -> 0x0B0A0908 -> 0x0F0E0D0C -> ...
|
|
* The loop ends as soon as we have iterated over all uint8_t values.
|
|
* We detect that by looking for the byte-wise overflow into the next byte:
|
|
* 0xFFFEFDFC <- last uint32_t value to be calculated
|
|
* 0xFF, 0xFE, 0xFD, 0xFC <- the corresponding uint8_t values
|
|
* 0x103, 0x102, 0x101, 0x100 <- incremented uint8_t values (overflow into the next byte!)
|
|
* 0x04030200 <- uint32_t value with the overflowed bytes
|
|
*
|
|
* We have the lower uint8_t values at the lower bytes of the uint32_t.
|
|
* This allows us to simply store the lowest byte of the uint32_t,
|
|
* right-shift the uint32_t by 8 and increment the table pointer.
|
|
* After 4 iterations of that all 4 bytes of the uint32_t are stored in the table.
|
|
*/
|
|
uint8_t tmc_fillCRC8Table(uint8_t polynomial, bool isReflected, uint8_t index)
|
|
{
|
|
uint32_t CRCdata;
|
|
// Helper pointer for traversing the result table
|
|
uint8_t *table;
|
|
|
|
if(index >= CRC_TABLE_COUNT)
|
|
return 0;
|
|
|
|
CRCTables[index].polynomial = polynomial;
|
|
CRCTables[index].isReflected = isReflected;
|
|
table = &CRCTables[index].table[0];
|
|
|
|
// Extend the polynomial to correct byte MSBs shifting into next bytes
|
|
uint32_t poly = (uint32_t) polynomial | 0x0100;
|
|
|
|
// Iterate over all 256 possible uint8_t values, compressed into a uint32_t (see detailed explanation above)
|
|
uint32_t i;
|
|
for(i = 0x03020100; i != 0x04030200; i+=0x04040404)
|
|
{
|
|
// For reflected table: Flip the bits of each input byte
|
|
CRCdata = (isReflected)? flipBitsInBytes(i) : i;
|
|
|
|
// Iterate over 8 Bits
|
|
int j;
|
|
for(j = 0; j < 8; j++)
|
|
{
|
|
// Store value of soon-to-be shifted out byte
|
|
uint8_t isMSBSet = (CRCdata & 0x80000000)? 1:0;
|
|
|
|
// CRC Shift
|
|
CRCdata <<= 1;
|
|
|
|
// XOR the bytes when required, lowest to highest
|
|
CRCdata ^= (CRCdata & 0x00000100)? (poly ) : 0;
|
|
CRCdata ^= (CRCdata & 0x00010000)? (poly << 8 ) : 0;
|
|
CRCdata ^= (CRCdata & 0x01000000)? (poly << 16) : 0;
|
|
CRCdata ^= (isMSBSet)? (poly << 24) : 0;
|
|
}
|
|
|
|
// For reflected table: Flip the bits of each output byte
|
|
CRCdata = (isReflected)? flipBitsInBytes(CRCdata) : CRCdata;
|
|
// Store the CRC result bytes in the table array
|
|
*table++ = (uint8_t) CRCdata;
|
|
CRCdata >>= 8;
|
|
*table++ = (uint8_t) CRCdata;
|
|
CRCdata >>= 8;
|
|
*table++ = (uint8_t) CRCdata;
|
|
CRCdata >>= 8;
|
|
*table++ = (uint8_t) CRCdata;
|
|
}
|
|
|
|
return 1;
|
|
}
|
|
|
|
/* This function calculates the CRC from a data buffer
|
|
*
|
|
* Arguments:
|
|
* uint8_t *data: A pointer to the data that will be CRC'd.
|
|
* uint32_t bytes: The length of the data buffer.
|
|
* uint8_t index: The index of the CRC table to be used.
|
|
*/
|
|
uint8_t tmc_CRC8(uint8_t *data, uint32_t bytes, uint8_t index)
|
|
{
|
|
uint8_t result = 0;
|
|
uint8_t *table;
|
|
|
|
if(index >= CRC_TABLE_COUNT)
|
|
return 0;
|
|
|
|
table = &CRCTables[index].table[0];
|
|
|
|
while(bytes--)
|
|
result = table[result ^ *data++];
|
|
|
|
return (CRCTables[index].isReflected)? flipByte(result) : result;
|
|
}
|
|
|
|
uint8_t tmc_tableGetPolynomial(uint8_t index)
|
|
{
|
|
if(index >= CRC_TABLE_COUNT)
|
|
return 0;
|
|
|
|
return CRCTables[index].polynomial;
|
|
}
|
|
|
|
bool tmc_tableIsReflected(uint8_t index)
|
|
{
|
|
if(index >= CRC_TABLE_COUNT)
|
|
return false;
|
|
|
|
return CRCTables[index].isReflected;
|
|
}
|
|
|
|
// Helper functions
|
|
static uint8_t flipByte(uint8_t value)
|
|
{
|
|
// swap odd and even bits
|
|
value = ((value >> 1) & 0x55) | ((value & 0x55) << 1);
|
|
// swap consecutive pairs
|
|
value = ((value >> 2) & 0x33) | ((value & 0x33) << 2);
|
|
// swap nibbles ...
|
|
value = ((value >> 4) & 0x0F) | ((value & 0x0F) << 4);
|
|
|
|
return value;
|
|
}
|
|
|
|
/* This helper function switches all bits within each byte.
|
|
* The byte order remains the same:
|
|
* [b31 b30 b29 b28 b27 b26 b25 b24 .. b7 b6 b5 b4 b3 b2 b1 b0]
|
|
* ||
|
|
* \||/
|
|
* \/
|
|
* [b24 b25 b26 b27 b28 b29 b30 b31 .. b0 b1 b2 b3 b4 b5 b6 b7]
|
|
*/
|
|
static uint32_t flipBitsInBytes(uint32_t value)
|
|
{
|
|
// swap odd and even bits
|
|
value = ((value >> 1) & 0x55555555) | ((value & 0x55555555) << 1);
|
|
// swap consecutive pairs
|
|
value = ((value >> 2) & 0x33333333) | ((value & 0x33333333) << 2);
|
|
// swap nibbles ...
|
|
value = ((value >> 4) & 0x0F0F0F0F) | ((value & 0x0F0F0F0F) << 4);
|
|
|
|
return value;
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
typedef union {
|
|
TMC2209TypeDef tmc2209;
|
|
} DriverBoards;
|
|
|
|
DriverBoards driverBoards;
|
|
#define TMC2209_CRC(data, length) tmc_CRC8(data, length, 1)
|
|
|
|
#define TMC2209 (driverBoards.tmc2209)
|
|
#define BUFFER_SIZE 32
|
|
#define INTR_PRI 6
|
|
#define UART_TIMEOUT_VALUE 10
|
|
#define WRITE_READ_DELAY 10
|
|
// #include "tmc/boards/Board.h"
|
|
// #include "tmc/tmc/StepDir.h"
|
|
// // => UART wrapper
|
|
// extern void tmc2209_readWriteArray(uint8_t channel, uint8_t *data, size_t writeLength, size_t readLength);
|
|
// // <= UART wrapper
|
|
|
|
// // => CRC wrapper
|
|
// extern uint8_t tmc2209_CRC8(uint8_t *data, size_t length);
|
|
// // <= CRC wrapper
|
|
|
|
static UART_Config *TMC2209_UARTChannel;
|
|
|
|
static inline TMC2209TypeDef *motorToIC(uint8_t motor)
|
|
{
|
|
UNUSED(motor);
|
|
|
|
return &TMC2209;
|
|
}
|
|
|
|
static inline UART_Config *channelToUART(uint8_t channel)
|
|
{
|
|
UNUSED(channel);
|
|
|
|
return TMC2209_UARTChannel;
|
|
}
|
|
|
|
// => UART wrapper
|
|
|
|
int32_t UART_readWrite(UART_Config *uart, uint8_t *data, size_t writeLength, uint8_t readLength)
|
|
{
|
|
uart->rxtx.clearBuffers();
|
|
uart->rxtx.txN(data, writeLength);
|
|
/* Workaround: Give the UART time to send. Otherwise another write/readRegister can do clearBuffers()
|
|
* before we're done. This currently is an issue with the IDE when using the Register browser and the
|
|
* periodic refresh of values gets requested right after the write request.
|
|
*/
|
|
wait(WRITE_READ_DELAY);
|
|
|
|
// Abort early if no data needs to be read back
|
|
if (readLength <= 0)
|
|
return 0;
|
|
|
|
// Wait for reply with timeout limit
|
|
uint32_t timestamp = 0;
|
|
// while(uart->rxtx.bytesAvailable() < readLength)
|
|
// {
|
|
// if(timeSince(timestamp) > UART_TIMEOUT_VALUE)
|
|
// {
|
|
// Abort on timeout
|
|
// return -1;
|
|
// }
|
|
// }
|
|
|
|
uart->rxtx.rxN(data, readLength);
|
|
|
|
return 0;
|
|
}
|
|
|
|
void tmc2209_readWriteArray(uint8_t channel, uint8_t *data, size_t writeLength, size_t readLength)
|
|
{
|
|
UART_readWrite(channelToUART(channel), data, writeLength, readLength);
|
|
}
|
|
// <= UART wrapper
|
|
|
|
// => CRC wrapper
|
|
// Return the CRC8 of [length] bytes of data stored in the [data] array.
|
|
uint8_t tmc2209_CRC8(uint8_t *data, size_t length)
|
|
{
|
|
return TMC2209_CRC(data, length);
|
|
}
|
|
// <= CRC wrapper
|
|
|
|
void tmc2209_writeRegister(uint8_t motor, uint16_t address, int32_t value)
|
|
{
|
|
tmc2209_writeInt(motorToIC(motor), (uint8_t) address, value);
|
|
|
|
}
|
|
|
|
void tmc2209_readRegister(uint8_t motor, uint16_t address, int32_t *value)
|
|
{
|
|
*value = tmc2209_readInt(motorToIC(motor), (uint8_t) address);
|
|
}
|
|
|
|
|
|
void tmc2209_writeInt(TMC2209TypeDef *tmc2209, uint8_t address, int32_t value)
|
|
{
|
|
uint8_t data[8];
|
|
|
|
data[0] = 0x05;
|
|
data[1] = tmc2209->slaveAddress;
|
|
data[2] = address | TMC_WRITE_BIT;
|
|
data[3] = (value >> 24) & 0xFF;
|
|
data[4] = (value >> 16) & 0xFF;
|
|
data[5] = (value >> 8 ) & 0xFF;
|
|
data[6] = (value ) & 0xFF;
|
|
data[7] = tmc2209_CRC8(data, 7);
|
|
|
|
tmc2209_readWriteArray(tmc2209->config->channel, &data[0], 8, 0);
|
|
|
|
// Write to the shadow register and mark the register dirty
|
|
address = TMC_ADDRESS(address);
|
|
tmc2209->config->shadowRegister[address] = value;
|
|
tmc2209->registerAccess[address] |= TMC_ACCESS_DIRTY;
|
|
}
|
|
|
|
int32_t tmc2209_readInt(TMC2209TypeDef *tmc2209, uint8_t address)
|
|
{
|
|
uint8_t data[8] = { 0 };
|
|
|
|
address = TMC_ADDRESS(address);
|
|
|
|
if (!TMC_IS_READABLE(tmc2209->registerAccess[address]))
|
|
return tmc2209->config->shadowRegister[address];
|
|
|
|
data[0] = 0x05;
|
|
data[1] = tmc2209->slaveAddress;
|
|
data[2] = address;
|
|
data[3] = tmc2209_CRC8(data, 3);
|
|
|
|
tmc2209_readWriteArray(tmc2209->config->channel, data, 4, 8);
|
|
|
|
// Byte 0: Sync nibble correct?
|
|
if (data[0] != 0x05)
|
|
return 0;
|
|
|
|
// Byte 1: Master address correct?
|
|
if (data[1] != 0xFF)
|
|
return 0;
|
|
|
|
// Byte 2: Address correct?
|
|
if (data[2] != address)
|
|
return 0;
|
|
|
|
// Byte 7: CRC correct?
|
|
if (data[7] != tmc2209_CRC8(data, 7))
|
|
return 0;
|
|
|
|
return ((uint32_t)data[3] << 24) | ((uint32_t)data[4] << 16) | (data[5] << 8) | data[6];
|
|
}
|
|
|
|
void tmc2209_init(TMC2209TypeDef *tmc2209, uint8_t channel, uint8_t slaveAddress, ConfigurationTypeDef *tmc2209_config, const int32_t *registerResetState)
|
|
{
|
|
tmc2209->slaveAddress = slaveAddress;
|
|
|
|
tmc2209->config = tmc2209_config;
|
|
tmc2209->config->callback = NULL;
|
|
tmc2209->config->channel = channel;
|
|
tmc2209->config->configIndex = 0;
|
|
tmc2209->config->state = CONFIG_READY;
|
|
|
|
for(size_t i = 0; i < TMC2209_REGISTER_COUNT; i++)
|
|
{
|
|
tmc2209->registerAccess[i] = tmc2209_defaultRegisterAccess[i];
|
|
tmc2209->registerResetState[i] = registerResetState[i];
|
|
}
|
|
}
|
|
|
|
static void writeConfiguration(TMC2209TypeDef *tmc2209)
|
|
{
|
|
uint8_t *ptr = &tmc2209->config->configIndex;
|
|
const int32_t *settings;
|
|
|
|
if(tmc2209->config->state == CONFIG_RESTORE)
|
|
{
|
|
settings = tmc2209->config->shadowRegister;
|
|
// Find the next restorable register
|
|
while((*ptr < TMC2209_REGISTER_COUNT) && !TMC_IS_RESTORABLE(tmc2209->registerAccess[*ptr]))
|
|
{
|
|
(*ptr)++;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
settings = tmc2209->registerResetState;
|
|
// Find the next resettable register
|
|
while((*ptr < TMC2209_REGISTER_COUNT) && !TMC_IS_RESETTABLE(tmc2209->registerAccess[*ptr]))
|
|
{
|
|
(*ptr)++;
|
|
}
|
|
}
|
|
|
|
if(*ptr < TMC2209_REGISTER_COUNT)
|
|
{
|
|
tmc2209_writeInt(tmc2209, *ptr, settings[*ptr]);
|
|
(*ptr)++;
|
|
}
|
|
else // Finished configuration
|
|
{
|
|
if(tmc2209->config->callback)
|
|
{
|
|
((tmc2209_callback)tmc2209->config->callback)(tmc2209, tmc2209->config->state);
|
|
}
|
|
|
|
tmc2209->config->state = CONFIG_READY;
|
|
}
|
|
}
|
|
|
|
void tmc2209_periodicJob(TMC2209TypeDef *tmc2209, uint32_t tick)
|
|
{
|
|
UNUSED(tick);
|
|
|
|
if(tmc2209->config->state != CONFIG_READY)
|
|
{
|
|
writeConfiguration(tmc2209);
|
|
return;
|
|
}
|
|
}
|
|
|
|
void tmc2209_setRegisterResetState(TMC2209TypeDef *tmc2209, const int32_t *resetState)
|
|
{
|
|
for(size_t i = 0; i < TMC2209_REGISTER_COUNT; i++)
|
|
tmc2209->registerResetState[i] = resetState[i];
|
|
}
|
|
|
|
void tmc2209_setCallback(TMC2209TypeDef *tmc2209, tmc2209_callback callback)
|
|
{
|
|
tmc2209->config->callback = (tmc_callback_config) callback;
|
|
}
|
|
|
|
uint8_t tmc2209_reset(TMC2209TypeDef *tmc2209)
|
|
{
|
|
if(tmc2209->config->state != CONFIG_READY)
|
|
return false;
|
|
|
|
// Reset the dirty bits and wipe the shadow registers
|
|
for(size_t i = 0; i < TMC2209_REGISTER_COUNT; i++)
|
|
{
|
|
tmc2209->registerAccess[i] &= ~TMC_ACCESS_DIRTY;
|
|
tmc2209->config->shadowRegister[i] = 0;
|
|
}
|
|
|
|
tmc2209->config->state = CONFIG_RESET;
|
|
tmc2209->config->configIndex = 0;
|
|
|
|
return true;
|
|
}
|
|
|
|
uint8_t tmc2209_restore(TMC2209TypeDef *tmc2209)
|
|
{
|
|
if(tmc2209->config->state != CONFIG_READY)
|
|
return false;
|
|
|
|
tmc2209->config->state = CONFIG_RESTORE;
|
|
tmc2209->config->configIndex = 0;
|
|
|
|
return true;
|
|
}
|
|
|
|
uint8_t tmc2209_get_slave(TMC2209TypeDef *tmc2209)
|
|
{
|
|
return tmc2209->slaveAddress;
|
|
}
|
|
|
|
void tmc2209_set_slave(TMC2209TypeDef *tmc2209, uint8_t slaveAddress)
|
|
{
|
|
tmc2209->slaveAddress = slaveAddress;
|
|
}
|