zsdk_stm32_v1
/
stm32components
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

update

hand_acid_mainboard
zhaohe 12 months ago
parent
bcca882e2f
commit
d387a6d07d
17 changed files with 3262 additions and 0 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 67
      modbus/modbus_basic.cpp
  2. 28
      modbus/modbus_basic.hpp
  3. 202
      modbus/modbus_block_host.cpp
  4. 31
      modbus/modbus_block_host.hpp
  5. 5
      stm32components.hpp
  6. 153
      sysmgr/sys_mgr.cpp
  7. 50
      sysmgr/sys_mgr.hpp
  8. BIN
      tmcdriver/doc/TMC5130A_datasheet_rev1.20 (1).pdf
  9. 19
      tmcdriver/tmc/tmc_type.h
  10. 67
      tmcdriver/tmc51x0/reg/TMC5130_Constants.h
  11. 1475
      tmcdriver/tmc51x0/reg/TMC5130_Fields.h
  12. 74
      tmcdriver/tmc51x0/reg/TMC5130_Register.h
  13. 59
      tmcdriver/tmc51x0/reg/TMC5130_Type.h
  14. 437
      tmcdriver/tmc51x0/tmc51x0.cpp
  15. 123
      tmcdriver/tmc51x0/tmc51x0.hpp
  16. 427
      zcanreceiver/zcanreceiver.cpp
  17. 45
      zcanreceiver/zcanreceiver.hpp

67
modbus/modbus_basic.cpp
View File

@ -0,0 +1,67 @@
/**
* Modbus
* | add(1byte) | functionCode(1byte) | ...data... | crc1(1byte) | crc2(2byte)|
*
*
*
*
*/
#include "modbus_basic.hpp"
#include <stdint.h>
// CRC_16高8位数据区
#define ASSERT(con) \
while (!(con)) { \
}
static const uint8_t auchCRCHi[] = {
0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1,
0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40,
0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0,
0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41,
0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0,
0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41,
0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1,
0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40,
0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1,
0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40};
// CRC低位字节值表
static const uint8_t auchCRCLo[] = { // CRC_16低8位数据区
0x00, 0xC0, 0xC1, 0x01, 0xC3, 0x03, 0x02, 0xC2, 0xC6, 0x06, 0x07, 0xC7, 0x05, 0xC5, 0xC4, 0x04, 0xCC, 0x0C, 0x0D, 0xCD, 0x0F, 0xCF, 0xCE, 0x0E, 0x0A, 0xCA,
0xCB, 0x0B, 0xC9, 0x09, 0x08, 0xC8, 0xD8, 0x18, 0x19, 0xD9, 0x1B, 0xDB, 0xDA, 0x1A, 0x1E, 0xDE, 0xDF, 0x1F, 0xDD, 0x1D, 0x1C, 0xDC, 0x14, 0xD4, 0xD5, 0x15,
0xD7, 0x17, 0x16, 0xD6, 0xD2, 0x12, 0x13, 0xD3, 0x11, 0xD1, 0xD0, 0x10, 0xF0, 0x30, 0x31, 0xF1, 0x33, 0xF3, 0xF2, 0x32, 0x36, 0xF6, 0xF7, 0x37, 0xF5, 0x35,
0x34, 0xF4, 0x3C, 0xFC, 0xFD, 0x3D, 0xFF, 0x3F, 0x3E, 0xFE, 0xFA, 0x3A, 0x3B, 0xFB, 0x39, 0xF9, 0xF8, 0x38, 0x28, 0xE8, 0xE9, 0x29, 0xEB, 0x2B, 0x2A, 0xEA,
0xEE, 0x2E, 0x2F, 0xEF, 0x2D, 0xED, 0xEC, 0x2C, 0xE4, 0x24, 0x25, 0xE5, 0x27, 0xE7, 0xE6, 0x26, 0x22, 0xE2, 0xE3, 0x23, 0xE1, 0x21, 0x20, 0xE0, 0xA0, 0x60,
0x61, 0xA1, 0x63, 0xA3, 0xA2, 0x62, 0x66, 0xA6, 0xA7, 0x67, 0xA5, 0x65, 0x64, 0xA4, 0x6C, 0xAC, 0xAD, 0x6D, 0xAF, 0x6F, 0x6E, 0xAE, 0xAA, 0x6A, 0x6B, 0xAB,
0x69, 0xA9, 0xA8, 0x68, 0x78, 0xB8, 0xB9, 0x79, 0xBB, 0x7B, 0x7A, 0xBA, 0xBE, 0x7E, 0x7F, 0xBF, 0x7D, 0xBD, 0xBC, 0x7C, 0xB4, 0x74, 0x75, 0xB5, 0x77, 0xB7,
0xB6, 0x76, 0x72, 0xB2, 0xB3, 0x73, 0xB1, 0x71, 0x70, 0xB0, 0x50, 0x90, 0x91, 0x51, 0x93, 0x53, 0x52, 0x92, 0x96, 0x56, 0x57, 0x97, 0x55, 0x95, 0x94, 0x54,
0x9C, 0x5C, 0x5D, 0x9D, 0x5F, 0x9F, 0x9E, 0x5E, 0x5A, 0x9A, 0x9B, 0x5B, 0x99, 0x59, 0x58, 0x98, 0x88, 0x48, 0x49, 0x89, 0x4B, 0x8B, 0x8A, 0x4A, 0x4E, 0x8E,
0x8F, 0x4F, 0x8D, 0x4D, 0x4C, 0x8C, 0x44, 0x84, 0x85, 0x45, 0x87, 0x47, 0x46, 0x86, 0x82, 0x42, 0x43, 0x83, 0x41, 0x81, 0x80, 0x40};
/*CRC16查表计算函数*/
uint16_t modbus_gen_crc16(uint8_t *puckMsg, uint8_t usDataLen) {
volatile uint8_t uchCRCHi = 0xFF; // 高CRC字节初始化
volatile uint8_t uchCRCLo = 0xFF; // 低CRC 字节初始化
volatile uint32_t uIndex; // CRC循环中的索引
// 传输消息缓冲区
while (usDataLen--) {
// 计算CRC
uIndex = uchCRCLo ^ *puckMsg++;
uchCRCLo = uchCRCHi ^ auchCRCHi[uIndex];
uchCRCHi = auchCRCLo[uIndex];
}
// 返回结果，高位在前
return (uchCRCLo << 8 | uchCRCHi);
}
void modbus_pack_crc_to_packet(uint8_t *puckMsg, uint8_t packetlen) {
uint16_t crc = modbus_gen_crc16(puckMsg, packetlen - 2);
puckMsg[packetlen - 2] = crc >> 8;
puckMsg[packetlen - 1] = crc;
}
/*函数功能：用于校验接收到的信息是否有误
输入参数：message是接收到的待校验消息，length是消息字节的长度
函数输出：输出校验结果，没有错返回True，有错返回False
通过CRC校验校验接收的信息是否正确*/
bool modbus_checkcrc16(uint8_t *message, uint8_t length) { return (modbus_gen_crc16(message, length) == 0x00) ? true : false; }

28
modbus/modbus_basic.hpp
View File

@ -0,0 +1,28 @@
#pragma once
#include <stdbool.h>
#include <stdint.h>
/**
* @brief 生成CRC16校验码
*
* @param puckMsg 数据指针
* @param usDataLen 数据长度(不包含CRC16)
* @return uint16_t
*/
uint16_t modbus_gen_crc16(uint8_t *puckMsg, uint8_t usDataLen);
/**
* @brief 将CRC16校验码添加到数据包中
*
* @param puckMsg 数据指针，数据中预留两个位置给CRC16
* @param packetlen 数据长度
*/
void modbus_pack_crc_to_packet(uint8_t *puckMsg, uint8_t packetlen);
/**
* @brief 检查CRC16校验码
*
* @param message 数据指针
* @param length 数据长度(包含CRC16)
* @return true 校验成功
* @return false 校验失败
*/
bool modbus_checkcrc16(uint8_t *message, uint8_t length);

202
modbus/modbus_block_host.cpp
View File

@ -0,0 +1,202 @@
#include "modbus_block_host.hpp"
#include "modbus_basic.hpp"
using namespace iflytop;
#define PRV_DEBUG 0
ModbusBlockHost::ModbusBlockHost() {}
ModbusBlockHost::~ModbusBlockHost() {}
extern "C" {
void HAL_UART_RxCpltCallback(UART_HandleTypeDef *huart) {}
}
void ModbusBlockHost::initialize(UART_HandleTypeDef *huart) {
this->huart = huart;
m_modbus_lock.init();
}
void ModbusBlockHost::cleanRxBuff() { //
HAL_StatusTypeDef status;
do {
status = HAL_UART_Receive(huart, rxbuff, 1, 1);
} while (status == HAL_OK);
}
void ModbusBlockHost::uarttx(uint8_t *buff, size_t len) {
ZASSERT(len < sizeof(txbuff));
#if PRV_DEBUG
printf("uart_tx:\n");
for (size_t i = 0; i < len; i++) {
printf("%02x ", buff[i]);
}
printf("\n");
#endif
// HAL_UART_Transmit(huart, buff, len, 1000);
HAL_UART_DMAStop(huart);
HAL_StatusTypeDef ret = HAL_UART_Transmit_DMA(huart, buff, len);
ZASSERT(ret == HAL_OK);
// 等待DMA传输完成
while (true) {
if (HAL_UART_GetState(huart) == HAL_UART_STATE_READY) break;
}
return;
}
bool ModbusBlockHost::uartrx(uint8_t *buff, size_t len, int overtimems) {
HAL_StatusTypeDef status;
ZASSERT(len < sizeof(rxbuff));
status = HAL_UART_Receive_DMA(huart, buff, len);
if (status != HAL_OK) {
return false;
}
for (size_t i = 0; i < overtimems; i++) {
osDelay(3);
if (HAL_UART_GetState(huart) == HAL_UART_STATE_READY) {
#if PRV_DEBUG
if (status == HAL_OK) {
printf("uart_rx:");
for (size_t i = 0; i < len; i++) {
printf("%02x ", buff[i]);
}
printf("\n");
}
#endif
return true;
}
}
#if PRV_DEBUG
printf("uart_rx timeout\n");
#endif
HAL_UART_Abort(huart);
HAL_UART_DMAStop(huart);
return false;
}
bool ModbusBlockHost::readReg03(uint8_t slaveAddr, uint16_t regAddr, uint16_t *regVal, int overtimems) {
zlock_guard lck(m_modbus_lock);
txbuff[0] = slaveAddr;
txbuff[1] = 0x03;
txbuff[2] = regAddr >> 8;
txbuff[3] = regAddr & 0xff;
txbuff[4] = 0x00;
txbuff[5] = 0x01;
modbus_pack_crc_to_packet(txbuff, 6 + 2);
cleanRxBuff();
uarttx(txbuff, 6 + 2);
bool status;
status = uartrx(rxbuff, 5 + 2, overtimems);
if (!status) {
return false;
}
if (!modbus_checkcrc16(rxbuff, 7)) {
ZLOGE("ModbusBlockHost", "crc error");
return false;
}
*regVal = (((uint16_t)(rxbuff[3])) << 8) | rxbuff[4];
return true;
}
// static const char *hex2str(uint8_t *buff, size_t len) {
// static char str[100];
// for (size_t i = 0; i < len; i++) {
// sprintf(str + i * 2, "%02x", buff[i]);
// }
// return str;
// }
bool ModbusBlockHost::readReg03Muti(uint8_t slaveAddr, uint16_t regAddr, uint16_t *regVal, int regNum, int overtimems) {
zlock_guard lck(m_modbus_lock);
txbuff[0] = slaveAddr;
txbuff[1] = 0x03;
txbuff[2] = regAddr >> 8;
txbuff[3] = regAddr & 0xff;
txbuff[4] = 0x00;
txbuff[5] = regNum;
modbus_pack_crc_to_packet(txbuff, 6 + 2);
cleanRxBuff();
uarttx(txbuff, 6 + 2);
bool status;
// 14*2 = 28
status = uartrx(rxbuff, 5 + regNum * 2, overtimems);
if (!status) {
return false;
}
if (!modbus_checkcrc16(rxbuff, 5 + regNum * 2)) {
ZLOGE("ModbusBlockHost", "crc error");
return false;
}
for (int i = 0; i < regNum; i++) {
regVal[i] = ((uint16_t)(rxbuff[3 + i * 2]) << 8) | rxbuff[4 + i * 2];
// int16_t val = regVal[i] ;
// ZLOGI("RX", "%d", val);
}
return true;
}
bool ModbusBlockHost::writeReg06(uint8_t slaveAddr, uint16_t regAddr, uint16_t regVal, int overtimems) {
zlock_guard lck(m_modbus_lock);
txbuff[0] = slaveAddr;
txbuff[1] = 0x06;
txbuff[2] = regAddr >> 8;
txbuff[3] = regAddr & 0xff;
txbuff[4] = regVal >> 8;
txbuff[5] = regVal & 0xff;
modbus_pack_crc_to_packet(txbuff, 6 + 2);
cleanRxBuff();
uarttx(txbuff, 6 + 2);
bool status;
status = uartrx(rxbuff, 8, overtimems);
if (status && modbus_checkcrc16(rxbuff, 8)) {
return true;
}
return false;
}
bool ModbusBlockHost::writeReg10(uint8_t slaveAddr, uint16_t regAddr, uint16_t regVal, int overtimems) {
zlock_guard lck(m_modbus_lock);
txbuff[0] = slaveAddr;
txbuff[1] = 0x10;
txbuff[2] = regAddr >> 8;
txbuff[3] = regAddr & 0xff;
txbuff[4] = 0x00;
txbuff[5] = 0x01;
txbuff[6] = 0x02; // 字节数
txbuff[7] = regVal >> 8;
txbuff[8] = regVal & 0xff;
modbus_pack_crc_to_packet(txbuff, 9 + 2);
cleanRxBuff();
uarttx(txbuff, 9 + 2);
bool status;
status = uartrx(rxbuff, 8, overtimems);
if (status && modbus_checkcrc16(rxbuff, 8)) {
return true;
}
return false;
}

31
modbus/modbus_block_host.hpp
View File

@ -0,0 +1,31 @@
#pragma once
//
#include "stm32basic/zbasic.h"
#include "stm32basic/stm32basic.hpp"
namespace iflytop {
class ModbusBlockHost {
UART_HandleTypeDef *huart;
uint8_t txbuff[100];
uint8_t rxbuff[100];
zmutex m_modbus_lock;
public:
ModbusBlockHost();
~ModbusBlockHost();
void initialize(UART_HandleTypeDef *huart);
bool readReg03(uint8_t slaveAddr, uint16_t regAddr, uint16_t *regVal, int overtimems);
bool readReg03Muti(uint8_t slaveAddr, uint16_t regAddr, uint16_t *regVal, int regNum, int overtimems);
bool writeReg06(uint8_t slaveAddr, uint16_t regAddr, uint16_t regVal, int overtimems);
bool writeReg10(uint8_t slaveAddr, uint16_t regAddr, uint16_t regVal, int overtimems);
private:
void cleanRxBuff();
void uarttx(uint8_t *buff, size_t len);
bool uartrx(uint8_t *buff, size_t len, int overtimems);
};
} // namespace iflytop

5
stm32components.hpp
View File

@ -0,0 +1,5 @@
#pragma once
#include "zcanreceiver/zcanreceiver.hpp"
#include "tmcdriver/tmc51x0/tmc51x0.hpp"
#include "sysmgr/sys_mgr.hpp"
#include "modbus/modbus_block_host.hpp"

153
sysmgr/sys_mgr.cpp
View File

@ -0,0 +1,153 @@
#include "sys_mgr.hpp"
#include <stdlib.h>
using namespace iflytop;
#define TAG "SysMgr"
extern "C" {
/***********************************************************************************************************************
* STM32_CODE_ERROR *
***********************************************************************************************************************/
void SysMgr_on_Error_Handler() {
ZLOGE(TAG, "Error_Handler\n");
while (1) {
}
}
void SysMgr_on_assert_failed(uint8_t* file, uint32_t line) {
ZLOGE(TAG, "ASSERT: %s [%s:%d]\n", file, line);
while (1) {
}
}
extern uint8_t _end; /* Symbol defined in the linker script */
extern uint8_t _estack; /* Symbol defined in the linker script */
extern uint32_t _Min_Stack_Size; /* Symbol defined in the linker script */
extern uint8_t* __sbrk_heap_end;
static size_t get_free_heap_size() {
const uint32_t stack_limit = (uint32_t)&_estack - (uint32_t)&_Min_Stack_Size;
const uint8_t* max_heap = (uint8_t*)stack_limit;
uint8_t* prev_heap_end;
return max_heap - __sbrk_heap_end;
// if (__sbrk_heap_end + incr > max_heap) {
// errno = ENOMEM;
// return (void*)-1;
// }
}
/***********************************************************************************************************************
* STM32_ERROR_IRQ *
***********************************************************************************************************************/
void SysMgr_on_NMI_Handler(void) { ZLOGI(TAG, "on NMI_Handler"); }
void SysMgr_on_HardFault_Handler(void) { ZLOGI(TAG, "on HardFault_Handler"); }
void SysMgr_on_MemManage_Handler(void) { ZLOGI(TAG, "on MemManage_Handler"); }
void SysMgr_on_BusFault_Handler(void) { ZLOGI(TAG, "on BusFault_Handler"); }
void SysMgr_on_UsageFault_Handler(void) { ZLOGI(TAG, "on UsageFault_Handler"); }
/***********************************************************************************************************************
* FREERTOS_ERROR *
***********************************************************************************************************************/
void vApplicationStackOverflowHook(xTaskHandle xTask, signed char* pcTaskName) {
ZLOGE(TAG, "StackOverflowHook: %s\n", pcTaskName);
__disable_irq();
while (1) {
}
}
void vApplicationMallocFailedHook(void) {
ZLOGE(TAG, "MallocFailedHook\n");
__disable_irq();
while (1) {
}
}
}
SysMgr* SysMgr::ins() {
static SysMgr s_ins;
return &s_ins;
}
// void SysMgr::regTaskId(osThreadId id) {
// m_task[m_ntask].Id = id;
// m_ntask++;
// }
size_t SysMgr::osGetSysRunTime() { return HAL_GetTick(); }
UBaseType_t uxTaskGetSystemState(TaskStatus_t* const pxTaskStatusArray, const UBaseType_t uxArraySize, uint32_t* const pulTotalRunTime);
void SysMgr::initedFinished() { //
static TaskStatus_t pxTaskStatusArray[SDK_MAX_TASK + 1];
UBaseType_t uxArraySize = SDK_MAX_TASK + 1;
uint32_t pulTotalRunTime;
m_ntask = uxTaskGetSystemState(pxTaskStatusArray, uxArraySize, &pulTotalRunTime);
for (int i = 0; i < m_ntask; i++) {
m_task[i].Id = pxTaskStatusArray[i].xHandle;
}
ZASSERT_INFO(m_ntask < SDK_MAX_TASK, "task num is too large, please increase SDK_MAX_TASK");
}
size_t SysMgr::osGetMinimumEverFreeHeapSize() { return ::xPortGetMinimumEverFreeHeapSize(); }
size_t SysMgr::osGetFreeHeapSize() { return ::xPortGetFreeHeapSize(); }
size_t SysMgr::osGetTotalHeapSize() { return configTOTAL_HEAP_SIZE; }
size_t SysMgr::osGetFreeSysHeapSize() { return get_free_heap_size(); }
int32_t SysMgr::getTaskNum() { return m_ntask; }
void SysMgr::dumpSysInfo() {
zlog("---------------Heap Info--------------\n");
zlog("MinimumEverFreeHeapSize : %d\n", osGetMinimumEverFreeHeapSize());
zlog("RTOS FreeHeapSize : %d\n", osGetFreeHeapSize());
zlog("RTOS TotalHeapSize : %d\n", osGetTotalHeapSize());
zlog("Sys FreeHeap : %d\n", get_free_heap_size());
zlog("");
zlog("---------------Task Info--------------\n");
static char buf[40 * SDK_MAX_TASK]; // 40一个任务，最多支持10个任务
vTaskList(buf);
zlog("Name State Priority Stack Num\n");
zlog_raw(buf);
zlog("- TaskInfoEnd -\n");
}
uint32_t SysMgr::osTaskStackRemainingSize(osThreadId id) { return uxTaskGetStackHighWaterMark(id); }
const char* SysMgr::osTaskName(osThreadId id) { return pcTaskGetName(id); }
osThreadId SysMgr::osGetId(int offset) {
if (offset < m_ntask) {
return m_task[offset].Id;
}
return NULL;
}
void SysMgr::osTaskName(osThreadId id, char* name, int bufsize) {
strncpy(name, pcTaskGetName(id), bufsize);
name[bufsize - 1] = 0;
}
void SysMgr::osTaskStackRemainingSize(osThreadId id, uint16_t* remainsize) { *remainsize = uxTaskGetStackHighWaterMark(id); }
void SysMgr::osTaskPriority(osThreadId id, uint16_t* priority) { *priority = uxTaskPriorityGet(id); }
void SysMgr::osTaskGetState(osThreadId id, char* state) {
eTaskState task_state = eTaskGetState(id);
switch (task_state) {
case eRunning:
*state = 'X';
break;
case eReady:
*state = 'R';
break;
case eBlocked:
*state = 'B';
break;
case eSuspended:
*state = 'S';
break;
case eDeleted:
*state = 'D';
break;
default:
*state = '?';
break;
}
}

50
sysmgr/sys_mgr.hpp
View File

@ -0,0 +1,50 @@
#pragma once
#include "cmsis_os.h"
#include "stm32basic/stm32basic.hpp"
namespace iflytop {
class ZTaskInfo {
public:
osThreadId Id;
};
class SysMgr {
private:
/* data */
public:
ZTaskInfo m_task[30] = {0};
int m_ntask = 0;
static SysMgr* ins();
void initedFinished();
void dumpSysInfo();
size_t osGetSysRunTime();
/***********************************************************************************************************************
* HeapMgr *
***********************************************************************************************************************/
size_t osGetMinimumEverFreeHeapSize();
size_t osGetFreeHeapSize();
size_t osGetTotalHeapSize();
size_t osGetFreeSysHeapSize();
/***********************************************************************************************************************
* TaskInfo *
***********************************************************************************************************************/
osThreadId osGetId(int offset);
uint32_t osTaskStackRemainingSize(osThreadId id);
const char* osTaskName(osThreadId id);
void osTaskName(osThreadId id, char* name, int bufsize);
void osTaskStackRemainingSize(osThreadId id, uint16_t* remainsize);
void osTaskPriority(osThreadId id, uint16_t* priority);
void osTaskGetState(osThreadId id, char* state);
int32_t getTaskNum();
};
} // namespace iflytop

BIN
tmcdriver/doc/TMC5130A_datasheet_rev1.20 (1).pdf
View File

19
tmcdriver/tmc/tmc_type.h
View File

@ -0,0 +1,19 @@
#pragma once
typedef enum {
kmres_256 = 0x00,
kmres_128 = 0x01,
kmres_64 = 0x02,
kmres_32 = 0x03,
kmres_16 = 0x04,
kmres_8 = 0x05,
kmres_4 = 0x06,
kmres_2 = 0x07,
kmres_1 = 0x08,
} mres_type_t;
typedef enum {
kTMC4361A = 0x2,
kTMC2160AndTMC5160 = 0x30,
kTMC5130 = 0x11,
// kTMC5160 = 0x30,
} tmcic_id_t;

67
tmcdriver/tmc51x0/reg/TMC5130_Constants.h
View File

@ -0,0 +1,67 @@
/*
* TMC5130_Constants.h
*
* Created on: 13.06.2018
* Author: LK
*/
#ifndef TMC_IC_TMC5130_TMC5130_CONSTANTS_H_
#define TMC_IC_TMC5130_TMC5130_CONSTANTS_H_
#define TMC5130_REGISTER_COUNT 128
#define TMC5130_MOTORS 1
#define TMC5130_WRITE_BIT 0x80
#define TMC5130_ADDRESS_MASK 0x7F
#define TMC5130_MAX_VELOCITY 8388096
#define TMC5130_MAX_ACCELERATION 65535
// ramp modes (Register TMC5161_RAMPMODE)
#define TMC5130_MODE_POSITION 0
#define TMC5130_MODE_VELPOS 1
#define TMC5130_MODE_VELNEG 2
#define TMC5130_MODE_HOLD 3
// limit switch mode bits (Register TMC5130_SWMODE)
#define TMC5130_SW_STOPL_ENABLE 0x0001
#define TMC5130_SW_STOPR_ENABLE 0x0002
#define TMC5130_SW_STOPL_POLARITY 0x0004
#define TMC5130_SW_STOPR_POLARITY 0x0008
#define TMC5130_SW_SWAP_LR 0x0010
#define TMC5130_SW_LATCH_L_ACT 0x0020
#define TMC5130_SW_LATCH_L_INACT 0x0040
#define TMC5130_SW_LATCH_R_ACT 0x0080
#define TMC5130_SW_LATCH_R_INACT 0x0100
#define TMC5130_SW_LATCH_ENC 0x0200
#define TMC5130_SW_SG_STOP 0x0400
#define TMC5130_SW_SOFTSTOP 0x0800
// Status bits (Register TMC5130_RAMPSTAT)
#define TMC5130_RS_STOPL 0x0001
#define TMC5130_RS_STOPR 0x0002
#define TMC5130_RS_LATCHL 0x0004
#define TMC5130_RS_LATCHR 0x0008
#define TMC5130_RS_EV_STOPL 0x0010
#define TMC5130_RS_EV_STOPR 0x0020
#define TMC5130_RS_EV_STOP_SG 0x0040
#define TMC5130_RS_EV_POSREACHED 0x0080
#define TMC5130_RS_VELREACHED 0x0100
#define TMC5130_RS_POSREACHED 0x0200
#define TMC5130_RS_VZERO 0x0400
#define TMC5130_RS_ZEROWAIT 0x0800
#define TMC5130_RS_SECONDMOVE 0x1000
#define TMC5130_RS_SG 0x2000
// Encoderbits (Register TMC5130_ENCMODE)
#define TMC5130_EM_DECIMAL 0x0400
#define TMC5130_EM_LATCH_XACT 0x0200
#define TMC5130_EM_CLR_XENC 0x0100
#define TMC5130_EM_NEG_EDGE 0x0080
#define TMC5130_EM_POS_EDGE 0x0040
#define TMC5130_EM_CLR_ONCE 0x0020
#define TMC5130_EM_CLR_CONT 0x0010
#define TMC5130_EM_IGNORE_AB 0x0008
#define TMC5130_EM_POL_N 0x0004
#define TMC5130_EM_POL_B 0x0002
#define TMC5130_EM_POL_A 0x0001
#endif /* TMC_IC_TMC5130_TMC5130_CONSTANTS_H_ */

1475
tmcdriver/tmc51x0/reg/TMC5130_Fields.h
File diff suppressed because it is too large
View File

74
tmcdriver/tmc51x0/reg/TMC5130_Register.h
View File

@ -0,0 +1,74 @@
/*
* TMC5130_Register.h
*
* Created on: 30.09.2016
* Author: ed based on te
*/
#ifndef TMC5130_REGISTER_H
#define TMC5130_REGISTER_H
// ===== TMC5130 register set =====
#define TMC5130_GCONF 0x00
#define TMC5130_GSTAT 0x01
#define TMC5130_IFCNT 0x02
#define TMC5130_SLAVECONF 0x03
#define TMC5130_IOIN 0x04
#define TMC5130_X_COMPARE 0x05
#define TMC5130_IHOLD_IRUN 0x10
#define TMC5130_TPOWERDOWN 0x11
#define TMC5130_TSTEP 0x12
#define TMC5130_TPWMTHRS 0x13
#define TMC5130_TCOOLTHRS 0x14
#define TMC5130_THIGH 0x15
#define TMC5130_RAMPMODE 0x20
#define TMC5130_XACTUAL 0x21
#define TMC5130_VACTUAL 0x22
#define TMC5130_VSTART 0x23
#define TMC5130_A1 0x24
#define TMC5130_V1 0x25
#define TMC5130_AMAX 0x26
#define TMC5130_VMAX 0x27
#define TMC5130_DMAX 0x28
#define TMC5130_D1 0x2A
#define TMC5130_VSTOP 0x2B
#define TMC5130_TZEROWAIT 0x2C
#define TMC5130_XTARGET 0x2D
#define TMC5130_VDCMIN 0x33
#define TMC5130_SWMODE 0x34
#define TMC5130_RAMPSTAT 0x35
#define TMC5130_XLATCH 0x36
#define TMC5130_ENCMODE 0x38
#define TMC5130_XENC 0x39
#define TMC5130_ENC_CONST 0x3A
#define TMC5130_ENC_STATUS 0x3B
#define TMC5130_ENC_LATCH 0x3C
#define TMC5130_MSLUT0 0x60
#define TMC5130_MSLUT1 0x61
#define TMC5130_MSLUT2 0x62
#define TMC5130_MSLUT3 0x63
#define TMC5130_MSLUT4 0x64
#define TMC5130_MSLUT5 0x65
#define TMC5130_MSLUT6 0x66
#define TMC5130_MSLUT7 0x67
#define TMC5130_MSLUTSEL 0x68
#define TMC5130_MSLUTSTART 0x69
#define TMC5130_MSCNT 0x6A
#define TMC5130_MSCURACT 0x6B
#define TMC5130_CHOPCONF 0x6C
#define TMC5130_COOLCONF 0x6D
#define TMC5130_DCCTRL 0x6E
#define TMC5130_DRVSTATUS 0x6F
#define TMC5130_PWMCONF 0x70
#define TMC5130_PWMSTATUS 0x71
#define TMC5130_ENCM_CTRL 0x72
#define TMC5130_LOST_STEPS 0x73
#endif /* TMC5130_REGISTER_H */

59
tmcdriver/tmc51x0/reg/TMC5130_Type.h
View File

@ -0,0 +1,59 @@
#pragma once
#include <stdint.h>
#include "TMC5130_Constants.h"
#include "TMC5130_Fields.h"
#include "TMC5130_Register.h"
namespace iflytop {
class TMC5130RampStat {
public:
uint32_t m_state;
typedef enum {
/**
* @brief
* R ：只读
* R+C：只读，但读后会自动清零
*/
ktmc5130_rs_stopl = TMC5130_RS_STOPL, // (R )
ktmc5130_rs_stopr = TMC5130_RS_STOPR, // (R )
ktmc5130_rs_latchl = TMC5130_RS_LATCHL, // (R+C)
ktmc5130_rs_latchr = TMC5130_RS_LATCHR, // (R+C)
ktmc5130_rs_ev_stopl = TMC5130_RS_EV_STOPL, // (R )
ktmc5130_rs_ev_stopr = TMC5130_RS_EV_STOPR, // (R )
ktmc5130_rs_ev_stop_sg = TMC5130_RS_EV_STOP_SG, // (R+C)
ktmc5130_rs_ev_posreached = TMC5130_RS_EV_POSREACHED, // (R+C)
ktmc5130_rs_velreached = TMC5130_RS_VELREACHED, // (R )
ktmc5130_rs_posreached = TMC5130_RS_POSREACHED, // (R ) 是否到达目标位置
ktmc5130_rs_vzero = TMC5130_RS_VZERO, // (R )
ktmc5130_rs_zerowait = TMC5130_RS_ZEROWAIT, // (R )
ktmc5130_rs_secondmove = TMC5130_RS_SECONDMOVE, // (R+C)
ktmc5130_rs_sg = TMC5130_RS_SG, // (R )
} ramp_stat_bit_t;
TMC5130RampStat(uint32_t state) : m_state(state) {}
bool isSetted(ramp_stat_bit_t bit) { return (m_state & bit) != 0; }
};
typedef struct {
uint32_t sg_result : 10;
uint32_t reserved0 : 5;
uint32_t fsactive : 1;
uint32_t cs_actual : 5;
uint32_t reserved1 : 3;
uint32_t stallguard : 1;
uint32_t ot : 1; // overtemperature flag
uint32_t otpw : 1; // overtemperature pre warning flag
uint32_t s2ga : 1; // short to ground indicator phase A
uint32_t s2gb : 1; // short to ground indicator phase B
uint32_t ola : 1; // open load indicator phase A
uint32_t olb : 1; // open load indicator phase B
uint32_t stst : 1; // standstill indicator
} TMC5130DevStatusReg_t;
typedef struct {
uint32_t reset : 1; // 复位标识
uint32_t drv_err : 1; //
uint32_t uv_cp : 1;
} TMC5130GState_t;
} // namespace iflytop

437
tmcdriver/tmc51x0/tmc51x0.cpp
View File

@ -0,0 +1,437 @@
#include "tmc51x0.hpp"
#include "reg/TMC5130_Constants.h"
#include "reg/TMC5130_Fields.h"
#include "reg/TMC5130_Register.h"
//
using namespace iflytop;
#if 0
// Register access permissions:
// 0x00: none (reserved)
// 0x01: read
// 0x02: write
// 0x03: read/write
// 0x13: read/write, separate functions/values for reading or writing
// 0x21: read, flag register (read to clear)
// 0x42: write, has hardware presets on reset
static const uint8_t tmc5130_defaultRegisterAccess[TMC5130_REGISTER_COUNT] = {
// 0 1 2 3 4 5 6 7 8 9 A B C D E F
0x03, 0x21, 0x01, 0x02, 0x13, 0x02, ____, ____, ____, ____, ____, ____, ____, ____, ____, ____, // 0x00 - 0x0F
0x02, 0x02, 0x01, 0x02, 0x02, 0x02, ____, ____, ____, ____, ____, ____, ____, ____, ____, ____, // 0x10 - 0x1F
0x03, 0x03, 0x01, 0x02, 0x02, 0x02, 0x02, 0x02, 0x02, ____, 0x02, 0x02, 0x02, 0x03, ____, ____, // 0x20 - 0x2F
____, ____, ____, 0x02, 0x03, 0x21, 0x01, ____, 0x03, 0x03, 0x02, 0x21, 0x01, ____, ____, ____, // 0x30 - 0x3F
____, ____, ____, ____, ____, ____, ____, ____, ____, ____, ____, ____, ____, ____, ____, ____, // 0x40 - 0x4F
____, ____, ____, ____, ____, ____, ____, ____, ____, ____, ____, ____, ____, ____, ____, ____, // 0x50 - 0x5F
0x42, 0x42, 0x42, 0x42, 0x42, 0x42, 0x42, 0x42, 0x42, 0x42, 0x01, 0x01, 0x03, 0x02, 0x02, 0x01, // 0x60 - 0x6F
0x42, 0x01, 0x02, 0x01, ____, ____, ____, ____, ____, ____, ____, ____, ____, ____, ____, ____ // 0x70 - 0x7F
};
#endif
/***********************************************************************************************************************
* 工具宏 *
***********************************************************************************************************************/
#define FIELD_SET(data, mask, shift, value) (data = (((data) & (~(mask))) | (((value) << (shift)) & (mask))))
#define FIELD_GET(data, mask, shift) (((data) & (mask)) >> (shift))
#define BYTE(value, n) (((value) >> ((n) << 3)) & 0xFF)
#define NIBBLE(value, n) (((value) >> ((n) << 2)) & 0x0F)
#define SHORT(value, n) (((value) >> ((n) << 4)) & 0xFFFF)
#define WORD(value, n) (((value) >> ((n) << 5)) & 0xFFFFFFFF)
#define TMC51x0_ADDRESS(x) ((x) & (TMC5130_ADDRESS_MASK))
#define TAG "TMC51X0"
void TMC51X0::initialize(TMC51X0Cfg cfg) {
m_cfg = cfg;
m_hspi = cfg.hspi;
ZLOGI(TAG, "SPI:%p CSPin:%s,enPIn:%s", cfg.hspi, pinname(cfg.csnpin), pinname(cfg.ennpin));
m_mutex.init();
m_csnpin.initAsOutput(m_cfg.csnpin, kxs_gpio_nopull, false, true);
m_ennpin.initAsOutput(m_cfg.ennpin, kxs_gpio_nopull, false, true);
enable(false);
stop();
writeInt(TMC5130_PWMCONF, 0x000500C8);
writeInt(TMC5130_CHOPCONF, 0x000100c3);
writeInt(TMC5130_IHOLD_IRUN, 0x00051A00);
writeInt(TMC5130_PWMCONF, 0x000401c8);
writeInt(TMC5130_XTARGET, 0);
writeInt(TMC5130_XACTUAL, 0x00000000);
writeInt(TMC5130_VACTUAL, 0x00000000);
writeInt(TMC5130_TZEROWAIT, 0);
setVstart(100);
setA1(30);
setAmax(50);
setV1(500);
setDmax(50);
setD1(30);
setVstop(100);
setTzerowait(100);
setIHOLD_IRUN(0, 30, 100);
enable(false);
}
/*******************************************************************************
* 驱动器寄存器读写方法 *
*******************************************************************************/
void TMC51X0::readWriteArray(uint8_t *data, size_t length) {
portENTER_CRITICAL();
m_csnpin.write(false);
HAL_SPI_TransmitReceive(m_hspi, data, data, length, 100);
m_csnpin.write(true);
portEXIT_CRITICAL();
}
void TMC51X0::writeDatagram(uint8_t address, uint8_t x1, uint8_t x2, uint8_t x3, uint8_t x4) {
uint8_t data[5] = {static_cast<uint8_t>(address | static_cast<uint8_t>(TMC5130_WRITE_BIT)), x1, x2, x3, x4};
readWriteArray(&data[0], 5);
}
void TMC51X0::writeInt(uint8_t address, int32_t value) { //
writeDatagram(address, BYTE(value, 3), BYTE(value, 2), BYTE(value, 1), BYTE(value, 0));
}
void TMC51X0::writeField(uint8_t add, uint32_t mask, uint32_t shift, uint32_t value) {
uint32_t regval = readUInt(add);
regval = (regval & ~mask) | ((value << shift) & mask);
writeInt(add, regval);
}
int32_t TMC51X0::readInt(uint8_t address) {
/**
* @WARNING:
* 这里没有判断寄存器是否可读，有些寄存器是不可读所以，所以可能读回来的数值和写入数值是不相等的
*/
address = TMC51x0_ADDRESS(address);
uint8_t data[5] = {0, 0, 0, 0, 0};
data[0] = address;
readWriteArray(&data[0], 5);
data[0] = address;
readWriteArray(&data[0], 5);
return ((uint32_t)data[1] << 24) | ((uint32_t)data[2] << 16) | (data[3] << 8) | data[4];
}
uint32_t TMC51X0::readUInt(uint8_t address) {
int32_t val = readInt(address);
uint32_t ret;
memcpy(&ret, &val, sizeof(uint32_t));
return ret;
}
/***********************************************************************************************************************
* BASIC *
***********************************************************************************************************************/
bool TMC51X0::ping() {
zlock_guard lkg(m_mutex);
uint32_t regval = readUInt(TMC5130_IOIN);
uint32_t chipId = FIELD_GET(regval, TMC5130_VERSION_MASK, TMC5130_VERSION_SHIFT);
return chipId != 0;
}
void TMC51X0::enable(bool enable) {
zlock_guard lkg(m_mutex);
m_ennpin.write(!enable);
}
int32_t TMC51X0::to_motor_acc(int32_t acc) { //
int32_t val = acc / 60.0 * m_onecirclepulse; // 65535
if (val > 65535) val = 65535;
return val;
}
int32_t TMC51X0::to_motor_vel(int32_t vel) { //
int32_t val = vel / 60.0 * m_onecirclepulse;
if (val > 8388095 /*2^23-512*/) {
val = 8388095;
}
return val;
} // rpm
int32_t TMC51X0::to_motor_pos(int32_t pos) { //
int32_t val = pos * 1.0 / m_scale * m_scale_deceleration * m_onecirclepulse + 0.5;
return val;
} //
int32_t TMC51X0::to_user_pos(int32_t pos) { //
int32_t val = pos / m_onecirclepulse * m_scale / m_scale_deceleration + 0.5;
// ZLOGI("TMC5130", "to_user_pos %d,%d", pos, val);
return val;
} //
int32_t TMC51X0::to_user_vel(int32_t vel) { //
int32_t val = vel * 60.0 / m_onecirclepulse;
return val;
}
/***********************************************************************************************************************
* CTRL *
***********************************************************************************************************************/
void TMC51X0::stop() {
zlock_guard lkg(m_mutex);
rotate(0);
}
void TMC51X0::rotate(int32_t velocity) {
zlock_guard lkg(m_mutex);
velocity = to_motor_vel(velocity);
writeInt(TMC5130_VMAX, abs(velocity));
writeInt(TMC5130_RAMPMODE, (velocity >= 0) ? TMC5130_MODE_VELPOS : TMC5130_MODE_VELNEG);
}
void TMC51X0::moveTo(int32_t position, uint32_t velocityMax) {
zlock_guard lkg(m_mutex);
position = to_motor_pos(position);
velocityMax = to_motor_vel(velocityMax);
writeInt(TMC5130_RAMPMODE, TMC5130_MODE_POSITION);
writeInt(TMC5130_VMAX, velocityMax);
writeInt(TMC5130_XTARGET, position);
}
void TMC51X0::moveToEnd(int32_t direction, uint32_t velocityMax) {
zlock_guard lkg(m_mutex);
if (direction >= 0) {
writeInt(TMC5130_RAMPMODE, TMC5130_MODE_POSITION);
writeInt(TMC5130_VMAX, to_motor_vel(velocityMax));
writeInt(TMC5130_XTARGET, INT32_MAX / 1.5 - 1000);
} else {
writeInt(TMC5130_RAMPMODE, TMC5130_MODE_POSITION);
writeInt(TMC5130_VMAX, to_motor_vel(velocityMax));
writeInt(TMC5130_XTARGET, INT32_MIN / 1.5 + 1000);
}
}
void TMC51X0::moveBy(int32_t relativePosition, uint32_t velocityMax) { // determine actual position and add numbers of ticks to move
zlock_guard lkg(m_mutex);
int32_t pos = getXACTUAL();
int32_t target = pos + relativePosition;
moveTo(target, velocityMax);
}
/***********************************************************************************************************************
* GET STATE *
***********************************************************************************************************************/
int32_t TMC51X0::getXACTUAL() {
zlock_guard lkg(m_mutex);
return to_user_pos(readInt(TMC5130_XACTUAL));
}
int32_t TMC51X0::getVACTUAL() {
zlock_guard lkg(m_mutex);
return to_user_pos(readInt(TMC5130_VACTUAL));
}
int32_t TMC51X0::getEncVal() {
zlock_guard lkg(m_mutex);
int32_t enc_val = to_user_pos(readInt(TMC5130_XENC));
if (m_enc_resolution < 0) {
enc_val = -enc_val;
}
return enc_val;
}
TMC5130RampStat TMC51X0::getRampStat() {
zlock_guard lkg(m_mutex);
int32_t val = readInt(TMC5130_RAMPSTAT);
return TMC5130RampStat(val);
}
bool TMC51X0::isReachTarget(TMC5130RampStat *state) {
zlock_guard lkg(m_mutex);
int mode = readInt(TMC5130_RAMPMODE);
if (mode == TMC5130_MODE_POSITION) {
return state->isSetted(TMC5130RampStat::ktmc5130_rs_posreached);
} else {
return state->isSetted(TMC5130RampStat::ktmc5130_rs_vzero) && state->isSetted(TMC5130RampStat::ktmc5130_rs_velreached);
}
}
bool TMC51X0::isTMC5130() {
zlock_guard lkg(m_mutex);
uint32_t regval = readUInt(TMC5130_IOIN);
uint32_t chipId = FIELD_GET(regval, TMC5130_VERSION_MASK, TMC5130_VERSION_SHIFT);
return chipId == kTMC5130;
}
TMC5130DevStatusReg_t TMC51X0::getDevStatus() { // R 读后不清
zlock_guard lkg(m_mutex);
static_assert(sizeof(TMC5130DevStatusReg_t) == 4);
uint32_t value = readInt(TMC5130_DRVSTATUS);
TMC5130DevStatusReg_t val;
memcpy(&val, &value, sizeof(TMC5130DevStatusReg_t));
return val;
}
TMC5130GState_t TMC51X0::getGState() { // R+C 读后自动清除
zlock_guard lkg(m_mutex);
static_assert(sizeof(TMC5130GState_t) == 4);
uint32_t value = readInt(TMC5130_GSTAT);
TMC5130GState_t val;
memcpy(&val, &value, sizeof(TMC5130GState_t));
return val;
}
int32_t TMC51X0::readICVersion() {
zlock_guard lkg(m_mutex);
uint32_t regval = readUInt(TMC5130_IOIN);
uint32_t chipId = FIELD_GET(regval, TMC5130_VERSION_MASK, TMC5130_VERSION_SHIFT);
return chipId;
}
/***********************************************************************************************************************
* set state *
***********************************************************************************************************************/
void TMC51X0::setXACTUAL(int32_t value) {
zlock_guard lkg(m_mutex);
writeInt(TMC5130_XACTUAL, to_motor_pos(value));
}
void TMC51X0::setMotorShaft(bool reverse) {
zlock_guard lkg(m_mutex);
uint32_t regval = readUInt(TMC5130_GCONF);
FIELD_SET(regval, TMC5130_SHAFT_MASK, TMC5130_SHAFT_SHIFT, reverse ? 1 : 0);
writeInt(TMC5130_GCONF, regval);
// uint32_t readbak = readUInt(TMC5130_GCONF);
// ZLOGI(TAG, "%x,%x", regval, readbak);
}
void TMC51X0::setIHOLD_IRUN(uint8_t ihold, uint8_t irun, uint16_t iholddelay) {
zlock_guard lkg(m_mutex);
writeInt(TMC5130_IHOLD_IRUN, (iholddelay << TMC5130_IHOLDDELAY_SHIFT) | (irun << TMC5130_IRUN_SHIFT) | (ihold << TMC5130_IHOLD_SHIFT));
}
void TMC51X0::setGlobalScale(uint8_t globalscale) {
#define TMC5160_GLOBAL_SCALER 0x0B
#define TMC5160_GLOBAL_SCALER_MASK 0xFF
#define TMC5160_GLOBAL_SCALER_SHIFT 0
zlock_guard lkg(m_mutex);
if (isTMC5130()) {
return;
}
if (globalscale == 0) {
globalscale = 0;
} else if (globalscale <= 31 && globalscale >= 1) {
globalscale = 32;
} else {
globalscale = globalscale;
}
writeInt(TMC5160_GLOBAL_SCALER, (readInt(TMC5160_GLOBAL_SCALER) & ~TMC5160_GLOBAL_SCALER_MASK) | (globalscale << TMC5160_GLOBAL_SCALER_SHIFT));
}
void TMC51X0::setScale(int32_t scale) {
zlock_guard lkg(m_mutex);
m_scale = scale;
}
void TMC51X0::setScaleDenominator(int32_t scale) {
zlock_guard lkg(m_mutex);
m_scale_deceleration = scale;
}
void TMC51X0::setVstart(int32_t val) {
zlock_guard lkg(m_mutex);
writeInt(TMC5130_VSTART, to_motor_vel(val));
}
void TMC51X0::setA1(int32_t val) {
zlock_guard lkg(m_mutex);
writeInt(TMC5130_A1, to_motor_acc(val));
}
void TMC51X0::setAmax(int32_t val) {
zlock_guard lkg(m_mutex);
writeInt(TMC5130_AMAX, to_motor_acc(val));
}
void TMC51X0::setV1(int32_t val) {
zlock_guard lkg(m_mutex);
writeInt(TMC5130_V1, to_motor_vel(val));
}
void TMC51X0::setDmax(int32_t val) {
zlock_guard lkg(m_mutex);
writeInt(TMC5130_DMAX, to_motor_acc(val));
}
void TMC51X0::setD1(int32_t val) {
zlock_guard lkg(m_mutex);
writeInt(TMC5130_D1, to_motor_acc(val));
}
void TMC51X0::setVstop(int32_t val) {
zlock_guard lkg(m_mutex);
writeInt(TMC5130_VSTOP, to_motor_vel(val));
}
void TMC51X0::setTzerowait(int32_t val) {
zlock_guard lkg(m_mutex);
writeInt(TMC5130_TZEROWAIT, val);
}
bool TMC51X0::setEncResolution(int32_t enc_resolution) {
zlock_guard lkg(m_mutex);
/**
* @brief
*
* 1.假设电机是256细分
* 2.假设TMC5130_ENC_CONST是十进制模式
* 3.只支持指定分辨率的编码器
*/
int32_t enc_resolution_tmp = enc_resolution * 4;
int16_t enc_const_integral = 0;
int16_t enc_const_fractional = 0;
switch (abs(enc_resolution_tmp)) {
case 1000:
enc_const_integral = 51;
enc_const_fractional = 0.2 * 10000;
break;
case 1024:
enc_const_integral = 50;
enc_const_fractional = 0 * 10000;
break;
case 4000:
enc_const_integral = 12;
enc_const_fractional = 0.8 * 10000;
break;
case 4096:
enc_const_integral = 12;
enc_const_fractional = 0.5 * 10000;
break;
case 16384:
enc_const_integral = 3;
enc_const_fractional = 0.125 * 10000;
break;
case 0:
m_enc_resolution = 0;
return true;
default:
return false;
break;
}
m_enc_resolution = enc_resolution;
uint32_t setval = 0;
uint8_t *psetval = (uint8_t *)&setval;
memcpy(psetval + 2, &enc_const_integral, 2);
memcpy(psetval, &enc_const_fractional, 2);
writeInt(TMC5130_ENCMODE, 0x1 << 10);
writeInt(TMC5130_ENC_CONST, setval);
return true;
}
void TMC51X0::setEncVal(int32_t enc_val) {
zlock_guard lkg(m_mutex);
int32_t enc_set_val = to_motor_pos(enc_val);
if (m_enc_resolution < 0) {
enc_set_val = -enc_set_val;
}
writeInt(TMC5130_XENC, enc_set_val);
}
/***********************************************************************************************************************
* reg operation *
***********************************************************************************************************************/
void TMC51X0::writeIntExt(uint8_t address, int32_t value) {
zlock_guard lkg(m_mutex);
writeInt(address, value);
}
int32_t TMC51X0::readIntExt(uint8_t address) {
zlock_guard lkg(m_mutex);
return readInt(address);
}

123
tmcdriver/tmc51x0/tmc51x0.hpp
View File

@ -0,0 +1,123 @@
#pragma once
#include <stdbool.h>
#include <stdint.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
//
#include "../tmc/tmc_type.h"
#include "reg/TMC5130_Type.h"
#include "stm32basic/stm32basic.hpp"
namespace iflytop {
/**
* @brief
* 注意事项
*
* 1. 6点加速控制只在位置模式下有效，在速度模式下无效
* 2. 速度模式下只有AMAX有效
*
*/
class TMC51X0Cfg {
public:
SPI_HandleTypeDef *hspi = NULL;
Pin_t csnpin;
Pin_t ennpin;
TMC51X0Cfg(SPI_HandleTypeDef *hspi, Pin_t csnpin, Pin_t ennpin) : hspi(hspi), csnpin(csnpin), ennpin(ennpin) {}
TMC51X0Cfg() {}
};
class TMC51X0 {
protected:
TMC51X0Cfg m_cfg;
SPI_HandleTypeDef *m_hspi = NULL;
ZGPIO m_csnpin;
ZGPIO m_ennpin;
int32_t m_scale = 10000;
int32_t m_scale_deceleration = 1;
mres_type_t m_MRES = kmres_256;
double m_onecirclepulse = 51200;
int32_t m_enc_resolution = 0; // 编码器分辨率，默认只有在256细分的情况下有效
zmutex m_mutex;
bool m_driErr = false;
public:
void initialize(TMC51X0Cfg cfg);
private:
/*******************************************************************************
* 驱动器寄存器读写方法 *
*******************************************************************************/
void readWriteArray(uint8_t *data, size_t length);
void writeDatagram(uint8_t address, uint8_t x1, uint8_t x2, uint8_t x3, uint8_t x4);
void writeInt(uint8_t address, int32_t value);
void writeField(uint8_t add, uint32_t mask, uint32_t shift, uint32_t value);
int32_t readInt(uint8_t address);
uint32_t readUInt(uint8_t address);
public:
bool ping();
void enable(bool enable);
void setdriErr(bool val) { m_driErr = val; }
bool getdriErr() { return m_driErr; }
/***********************************************************************************************************************
* ctrl *
***********************************************************************************************************************/
void rotate(int32_t velocity);
void moveTo(int32_t position, uint32_t velocityMax);
void moveToEnd(int32_t direction, uint32_t velocityMax);
void moveBy(int32_t relativePosition, uint32_t velocityMax);
void stop();
/***********************************************************************************************************************
* get state *
***********************************************************************************************************************/
int32_t getXACTUAL(); // 当前位置
int32_t getVACTUAL(); // 当前速度
int32_t getEncVal(); // ENCVAL
TMC5130RampStat getRampStat();
TMC5130DevStatusReg_t getDevStatus(); // R 读后不清
TMC5130GState_t getGState(); // R+C 读后自动清除
int32_t readICVersion();
bool isReachTarget(TMC5130RampStat *state); // 是否到达目标位置
bool isTMC5130();
/***********************************************************************************************************************
* set state *
***********************************************************************************************************************/
void setXACTUAL(int32_t value); // 设置当前位置
void setMotorShaft(bool reverse);
void setIHOLD_IRUN(uint8_t ihold, uint8_t irun, uint16_t iholddelay);
void setGlobalScale(uint8_t globalscale); // ONLY for 5160
void setScale(int32_t scale);
void setScaleDenominator(int32_t scale);
void setVstart(int32_t val);
void setA1(int32_t val);
void setAmax(int32_t val);
void setV1(int32_t val);
void setDmax(int32_t val);
void setD1(int32_t val);
void setVstop(int32_t val);
void setTzerowait(int32_t val);
bool setEncResolution(int32_t enc_resolution);
void setEncVal(int32_t enc_val);
/***********************************************************************************************************************
* reg operation *
***********************************************************************************************************************/
void writeIntExt(uint8_t address, int32_t value);
int32_t readIntExt(uint8_t address);
private:
int32_t to_motor_acc(int32_t acc); // rpm/s^2
int32_t to_motor_vel(int32_t vel); // rpm
int32_t to_motor_pos(int32_t pos); //
int32_t to_user_pos(int32_t pos); //
int32_t to_user_vel(int32_t vel);
};
} // namespace iflytop

427
zcanreceiver/zcanreceiver.cpp
View File

@ -0,0 +1,427 @@
#include "zcanreceiver.hpp"
#include "stm32basic/mutex.hpp"
#include "stm32halport/stm32halport.hpp"
#define TAG "zcan"
#define CANHANDLER &hcan1
#define CAN_FILTER_INDEX 0
#define CAN_MAX_FILTER_NUM 7
#define CAN_FIFO_NUM CAN_RX_FIFO0
#define OVER_TIME_MS 30
#define HEART_OVERTIME (30 * 1000)
using namespace iflytop;
using namespace zscanprotocol;
static uint8_t m_deviceId;
static zcanbus_on_rx_t m_rxlistener[30];
static uint32_t m_numListener = 0;
static zcanbus_on_connected_t m_connectedlistener;
static canrxbuffer_t m_rxbufcache;
static uint16_t reportIndex;
static uint8_t rxdata[200];
static uint8_t txbuff[200];
static uint32_t lastpacket_ticket;
static bool m_is_connected;
static uint8_t m_priority;
zmutex m_lock;
static void _oncanpacket(CAN_HandleTypeDef *hcan);
static void _processOneCanPacket(CAN_RxHeaderTypeDef *pHeader, uint8_t *aData);
static bool _getRxMessage(CAN_RxHeaderTypeDef *pHeader, uint8_t aData[] /*8byte table*/);
extern "C" {
void HAL_CAN_TxMailbox0CompleteCallback(CAN_HandleTypeDef *hcan) { ZLOGI(TAG, "HAL_CAN_TxMailbox0CompleteCallback"); }
void HAL_CAN_TxMailbox1CompleteCallback(CAN_HandleTypeDef *hcan) { ZLOGI(TAG, "HAL_CAN_TxMailbox1CompleteCallback"); }
void HAL_CAN_TxMailbox2CompleteCallback(CAN_HandleTypeDef *hcan) { ZLOGI(TAG, "HAL_CAN_TxMailbox2CompleteCallback"); }
void HAL_CAN_TxMailbox0AbortCallback(CAN_HandleTypeDef *hcan) { ZLOGI(TAG, "HAL_CAN_TxMailbox0AbortCallback"); }
void HAL_CAN_TxMailbox1AbortCallback(CAN_HandleTypeDef *hcan) { ZLOGI(TAG, "HAL_CAN_TxMailbox1AbortCallback"); }
void HAL_CAN_TxMailbox2AbortCallback(CAN_HandleTypeDef *hcan) { ZLOGI(TAG, "HAL_CAN_TxMailbox2AbortCallback"); }
void HAL_CAN_RxFifo0MsgPendingCallback(CAN_HandleTypeDef *hcan) { _oncanpacket(hcan); }
void HAL_CAN_RxFifo0FullCallback(CAN_HandleTypeDef *hcan) { ZLOGI(TAG, "HAL_CAN_RxFifo0FullCallback"); }
void HAL_CAN_RxFifo1MsgPendingCallback(CAN_HandleTypeDef *hcan) { ZLOGI(TAG, "HAL_CAN_RxFifo1MsgPendingCallback"); }
void HAL_CAN_RxFifo1FullCallback(CAN_HandleTypeDef *hcan) { ZLOGI(TAG, "HAL_CAN_RxFifo1FullCallback"); }
void HAL_CAN_SleepCallback(CAN_HandleTypeDef *hcan) { ZLOGI(TAG, "HAL_CAN_SleepCallback"); }
void HAL_CAN_WakeUpFromRxMsgCallback(CAN_HandleTypeDef *hcan) { ZLOGI(TAG, "HAL_CAN_WakeUpFromRxMsgCallback"); }
void HAL_CAN_ErrorCallback(CAN_HandleTypeDef *hcan) { ZLOGI(TAG, "HAL_CAN_ErrorCallback"); }
}
/***********************************************************************************************************************
* 中断消息处理 *
***********************************************************************************************************************/
static void _oncanpacket(CAN_HandleTypeDef *hcan) {
if (CANHANDLER != hcan) return;
CAN_RxHeaderTypeDef pHeader;
uint8_t aData[8] /*8byte table*/;
while (_getRxMessage(&pHeader, aData)) {
_processOneCanPacket(&pHeader, aData);
}
}
static bool _getRxMessage(CAN_RxHeaderTypeDef *pHeader, uint8_t aData[] /*8byte table*/) {
/**
* @brief 读取当前FIFO中缓存了多少帧的数据
*/
uint32_t level = HAL_CAN_GetRxFifoFillLevel(CANHANDLER, CAN_FIFO_NUM);
if (level == 0) {
return false;
}
HAL_StatusTypeDef HAL_RetVal;
HAL_RetVal = HAL_CAN_GetRxMessage(CANHANDLER, CAN_FIFO_NUM, pHeader, aData);
if (HAL_OK == HAL_RetVal) {
// 处理接收到的can总线数据
return true;
}
return false;
}
static void _processOneCanPacket(CAN_RxHeaderTypeDef *pHeader, uint8_t *aData) {
/**
* @brief 消息格式 12bit from,1bit emergency
*
* [1] [4bit] [8bit] [8bit] [4bit/4bit]
* , from to frameNum/frameId
*/
uint8_t from = (pHeader->ExtId & 0x00FF0000) >> 16;
uint8_t to = (pHeader->ExtId & 0x0000FF00) >> 8;
uint8_t nframe = (pHeader->ExtId & 0x000000F0) >> 4;
uint8_t frameId = (pHeader->ExtId & 0x0000000F);
// ZLOGI(TAG, "from:%d to:%d nframe:%d frameId:%d", from, to, nframe, frameId);
if (pHeader->IDE == CAN_ID_STD) {
return;
}
// 只接收主机消息
if (from != 1) {
return;
}
if (to != m_deviceId && to != 0xff) {
return;
}
// 上次接收到的消息还没有来的急处理
if (m_rxbufcache.dataIsReady) {
ZLOGI(TAG, "discard rx packet ,last packet not processed");
return;
}
if (frameId == 0) {
m_rxbufcache.canPacketNum = 0;
}
if (frameId != m_rxbufcache.canPacketNum) {
m_rxbufcache.canPacketNum = 0;
ZLOGI(TAG, "discard rx packet ,due to lost packet");
return;
}
if (m_rxbufcache.canPacketNum < ZARRAY_SIZE(m_rxbufcache.canPacket)) {
if (m_rxbufcache.canPacketNum == 0) {
m_rxbufcache.header = *pHeader;
m_rxbufcache.from = from;
m_rxbufcache.to = to;
}
m_rxbufcache.canPacket[m_rxbufcache.canPacketNum].datalen = pHeader->DLC;
memcpy(m_rxbufcache.canPacket[m_rxbufcache.canPacketNum].aData, aData, 8);
m_rxbufcache.canPacketNum++;
} else {
ZLOGI(TAG, "discard rx packet ,due to buffer full");
m_rxbufcache.canPacketNum = 0;
return;
}
if (nframe == frameId + 1) {
if (m_rxbufcache.canPacketNum != nframe) {
m_rxbufcache.canPacketNum = 0;
ZLOGI(TAG, "discard rx packet ,due to lost packet");
return;
} else {
m_rxbufcache.dataIsReady = true;
}
}
}
/***********************************************************************************************************************
* FUNC *
***********************************************************************************************************************/
static HAL_StatusTypeDef activateRxIT() {
HAL_StatusTypeDef hal_status = HAL_ERROR;
if (CAN_FIFO_NUM == CAN_RX_FIFO0) {
hal_status = HAL_CAN_ActivateNotification(CANHANDLER, CAN_IT_RX_FIFO0_MSG_PENDING);
} else if (CAN_FIFO_NUM == CAN_RX_FIFO1) {
hal_status = HAL_CAN_ActivateNotification(CANHANDLER, CAN_IT_RX_FIFO1_MSG_PENDING);
} else {
ZLOGE(TAG, "start can HAL_CAN_ActivateNotification CAN_IT_RX_FIFO0_MSG_PENDING fail\r\n");
return hal_status;
}
return hal_status;
}
// static HAL_StatusTypeDef deactivateRxIT() {
// HAL_StatusTypeDef hal_status = HAL_ERROR;
// if (CAN_FIFO_NUM == CAN_RX_FIFO0) {
// hal_status = HAL_CAN_DeactivateNotification(CANHANDLER, CAN_IT_RX_FIFO0_MSG_PENDING);
// } else if (CAN_FIFO_NUM == CAN_RX_FIFO1) {
// hal_status = HAL_CAN_DeactivateNotification(CANHANDLER, CAN_IT_RX_FIFO1_MSG_PENDING);
// } else {
// ZLOGE(TAG, "start can HAL_CAN_ActivateNotification CAN_IT_RX_FIFO0_MSG_PENDING fail\r\n");
// return hal_status;
// }
// return hal_status;
// }
HAL_StatusTypeDef initializeFilter() {
HAL_StatusTypeDef HAL_Status;
CAN_FilterTypeDef sFilterConfig;
uint32_t filterId;
uint32_t mask;
memset(&sFilterConfig, 0, sizeof(sFilterConfig));
sFilterConfig.FilterMode = CAN_FILTERMODE_IDMASK; // 设为MASK模式
sFilterConfig.FilterScale = CAN_FILTERSCALE_32BIT; // CAN_FILTERSCALE_16BIT
sFilterConfig.FilterFIFOAssignment = CAN_FIFO_NUM; // 关联过滤器到rxfifoNum
sFilterConfig.FilterActivation = ENABLE; // 激活过滤器
sFilterConfig.SlaveStartFilterBank = CAN_MAX_FILTER_NUM; // slave filter start index
/*******************************************************************************
* 接收所有消息 *
*******************************************************************************/
filterId = (0); //
mask = (0); //
sFilterConfig.FilterBank = CAN_FILTER_INDEX; //
sFilterConfig.FilterMaskIdLow = mask & 0xffff; //
sFilterConfig.FilterMaskIdHigh = (mask & 0xffff0000) >> 16; //
sFilterConfig.FilterIdLow = filterId & 0xffff; //
sFilterConfig.FilterIdHigh = (filterId & 0xffff0000) >> 16; //
HAL_Status = HAL_CAN_ConfigFilter(CANHANDLER, &sFilterConfig);
if (HAL_Status != HAL_OK) {
ZLOGE(TAG, "HAL_CAN_ConfigFilter filter0 fail");
return HAL_Status;
}
// ZLOGI(TAG, "HAL_CAN_ConfigFilter filterID1 %08x", filterId >> 3);
return HAL_Status;
}
void zcanbus_init(uint8_t deviceId) {
m_deviceId = deviceId;
HAL_StatusTypeDef hal_status;
m_rxbufcache.dataIsReady = false;
m_rxbufcache.id = 0; // 只接收来自主机的消息
m_rxbufcache.canPacketNum = 0;
hal_status = initializeFilter();
if (hal_status != HAL_OK) {
ZLOGE(TAG, "start can initializeFilter fail\r\n");
return;
}
hal_status = HAL_CAN_Start(CANHANDLER); // 开启CAN
if (hal_status != HAL_OK) {
ZLOGE(TAG, "start can fail\r\n");
return;
}
m_lock.init();
HAL_StatusTypeDef status = activateRxIT();
if (status != HAL_OK) {
ZLOGE(TAG, "activateRxIT fail\r\n");
ZASSERT(0);
return;
}
ZLOGI(TAG, "zcanbus init done");
}
void zcanbus_reglistener(zcanbus_on_rx_t rxlistener) {
ZASSERT(m_numListener < ZARRAY_SIZE(m_rxlistener));
m_rxlistener[m_numListener++] = rxlistener;
}
void zcanbus_reg_on_connected_listener(zcanbus_on_connected_t connectedlistener) { m_connectedlistener = connectedlistener; }
bool zcanbus_send_packet(uint8_t to, uint8_t *packet, size_t len) { return zcanbus_send_packet(to, packet, len, OVER_TIME_MS); }
static char *hex2str(uint8_t *data, size_t len) {
static char buf[200];
memset(buf, 0, sizeof(buf));
for (size_t i = 0; i < len; i++) {
sprintf(buf + i * 2, "%02x", data[i]);
}
return buf;
}
bool zcanbus_send_packet(uint8_t to, uint8_t *packet, size_t len, int overtimems) {
ZLOGI(TAG, "sendPacket to:%d, %s(%d)", to, hex2str(packet, len), len);
int npacket = len / 8 + (len % 8 == 0 ? 0 : 1);
if (npacket > 255) {
ZLOGE(TAG, "sendPacket fail, len:%d", len);
return false;
}
int finalpacketlen = len % 8 == 0 ? 8 : len % 8;
for (uint8_t i = 0; i < npacket; i++) {
bool suc = false;
if (i == npacket - 1) {
suc = zcanbus_send_sub_packet(to, npacket, i, packet + i * 8, finalpacketlen, overtimems);
} else {
suc = zcanbus_send_sub_packet(to, npacket, i, packet + i * 8, 8, overtimems);
}
if (!suc) {
// ZLOGE(TAG, "sendPacket fail, packet(%d:%d)", npacket, i);
return false;
}
}
return true;
}
// static const char *canpacket_dump(uint8_t *data, int size) {
// static char buf[20];
// memset(buf, 0, sizeof(buf));
// for (int i = 0; i < size; i++) {
// sprintf(buf + i * 2, "%02x", data[i]);
// }
// return buf;
// }
bool zcanbus_send_sub_packet(uint8_t to, int npacket, int packetIndex, uint8_t *packet, size_t len, int overtimems) {
CAN_TxHeaderTypeDef pHeader;
uint8_t aData[8] /*8byte table*/;
uint32_t txMailBox = 0;
uint32_t enterticket = zget_ticket();
memset(&pHeader, 0, sizeof(pHeader));
memset(aData, 0, sizeof(aData));
pHeader.StdId = 0x00;
pHeader.ExtId = (m_deviceId << 16) | (to << 8) | (npacket << 4) | (packetIndex);
pHeader.ExtId |= (m_priority & 0x0f) << 24;
pHeader.ExtId |= (0x01 << 28);
pHeader.IDE = CAN_ID_EXT;
pHeader.RTR = CAN_RTR_DATA;
pHeader.DLC = len;
pHeader.TransmitGlobalTime = DISABLE;
memcpy(aData, packet, len);
// ZLOGI(TAG, "tx %s", canpacket_dump(aData, len));
HAL_StatusTypeDef lastTransmitStatus = HAL_CAN_AddTxMessage(CANHANDLER, &pHeader, aData, &txMailBox);
if (lastTransmitStatus != HAL_OK) {
ZLOGE(TAG, "HAL_CAN_AddTxMessage fail");
return false;
}
while (HAL_CAN_IsTxMessagePending(CANHANDLER, txMailBox)) {
if (zhas_passedms(enterticket) > (uint32_t)overtimems) {
lastTransmitStatus = HAL_TIMEOUT;
HAL_CAN_AbortTxRequest(CANHANDLER, txMailBox);
return false;
}
zos_delay_ms(1);
}
return true;
}
bool zcanbus_send_ack(zcanbus_packet_t *rxpacket, uint8_t *param, size_t len) {
zlock_guard l(m_lock);
zcanbus_packet_t *txpacket = (zcanbus_packet_t *)txbuff;
txpacket->index = rxpacket->index;
txpacket->function_id = rxpacket->function_id;
txpacket->ptype = kreceipt;
if (param) memcpy(txpacket->params, param, len);
return zcanbus_send_packet(1 /*mainboard*/, (uint8_t *)&txpacket[0], sizeof(zcanbus_packet_t) + len);
}
bool zcanbus_send_ack(iflytop::zscanprotocol::zcanbus_packet_t *rxpacket, int32_t val) { return zcanbus_send_ack(rxpacket, (uint8_t *)&val, sizeof(val)); }
bool zcanbus_send_ack(iflytop::zscanprotocol::zcanbus_packet_t *rxpacket, bool _val) {
int32_t val = _val ? 1 : 0;
return zcanbus_send_ack(rxpacket, (uint8_t *)&val, sizeof(val));
}
bool zcanbus_send_errorack(zcanbus_packet_t *rxpacket, int32_t errcode) {
zlock_guard l(m_lock);
m_priority = kpriority_receipt;
zcanbus_packet_t *txpacket = (zcanbus_packet_t *)txbuff;
txpacket->index = rxpacket->index;
txpacket->function_id = rxpacket->function_id;
txpacket->ptype = kerror_receipt;
memcpy(txpacket->params, &errcode, sizeof(errcode));
return zcanbus_send_packet(1 /*mainboard*/, (uint8_t *)&txpacket[0], sizeof(zcanbus_packet_t) + 4);
}
bool zcanbus_send_report(uint16_t function_id, uint8_t *param, size_t len, int32_t overtime) {
zlock_guard l(m_lock);
m_priority = kpriority_report;
zcanbus_packet_t *txpacket = (zcanbus_packet_t *)txbuff;
txpacket->index = reportIndex++;
txpacket->function_id = function_id;
txpacket->ptype = kreport;
memcpy(txpacket->params, param, len);
return zcanbus_send_packet(1 /*mainboard*/, (uint8_t *)&txpacket[0], sizeof(zcanbus_packet_t) + len, overtime);
}
bool zcanbus_send_emergency_report(uint16_t function_id, uint8_t *param, size_t len, int32_t overtime) {
zlock_guard l(m_lock);
m_priority = kpriority_emergency_report;
zcanbus_packet_t *txpacket = (zcanbus_packet_t *)txbuff;
txpacket->index = reportIndex++;
txpacket->function_id = function_id;
txpacket->ptype = kreport;
memcpy(txpacket->params, param, len);
return zcanbus_send_packet(1 /*mainboard*/, (uint8_t *)&txpacket[0], sizeof(zcanbus_packet_t) + len, overtime);
}
static void process_rx_packet(canrxbuffer_t *canrxbuf, uint8_t *rx, size_t len) {
zcanbus_packet_t *packet = (zcanbus_packet_t *)rx;
lastpacket_ticket = zget_ticket();
if (!m_is_connected) {
m_is_connected = true;
if (m_connectedlistener) m_connectedlistener(true);
}
if (packet->ptype == kcmd) {
for (size_t i = 0; i < m_numListener; i++) {
m_rxlistener[i](canrxbuf->from, canrxbuf->to, rxdata, len);
}
}
}
void zcanbus_schedule() {
canrxbuffer_t *rxbuf = &m_rxbufcache;
uint16_t fromId = 0;
uint16_t toId = 0;
if (rxbuf->dataIsReady) {
int dataoff = 0;
// rxdata[0] = rxbuf->from;
// rxdata[1] = rxbuf->to;
for (size_t i = 0; i < rxbuf->canPacketNum; i++) {
memcpy(rxdata + dataoff, rxbuf->canPacket[i].aData, rxbuf->canPacket[i].datalen);
dataoff += rxbuf->canPacket[i].datalen;
ZASSERT(dataoff < ZARRAY_SIZE(rxdata));
}
process_rx_packet(rxbuf, rxdata, dataoff);
rxbuf->dataIsReady = false;
}
if (m_is_connected && zhas_passedms(lastpacket_ticket) > HEART_OVERTIME) {
m_is_connected = false;
if (m_connectedlistener) m_connectedlistener(false);
}
}
bool zcanbus_is_connected() { return m_is_connected; }

45
zcanreceiver/zcanreceiver.hpp
View File

@ -0,0 +1,45 @@
//
// Created by zwsd
//
#pragma once
#include "project_configs.h"
#include "stm32basic/stm32basic.hpp"
#include "app_protocols/zscanprotocol/zscanprotocol.hpp"
typedef void (*zcanbus_on_rx_t)(uint8_t from, uint8_t to, uint8_t *packet, size_t len);
typedef void (*zcanbus_on_connected_t)(bool connected);
typedef struct {
public:
uint8_t aData[8]; /*8byte table*/
uint8_t datalen;
} stm32_can_packet_t;
typedef struct {
public:
uint16_t id;
CAN_RxHeaderTypeDef header;
uint8_t from;
uint8_t to;
stm32_can_packet_t canPacket[20]; // 用于接收can消息
uint8_t canPacketNum = 0;
bool dataIsReady;
} canrxbuffer_t;
void zcanbus_init(uint8_t deviceId);
void zcanbus_reglistener(zcanbus_on_rx_t rxlistener);
void zcanbus_reg_on_connected_listener(zcanbus_on_connected_t connectedlistener);
void zcanbus_schedule();
bool zcanbus_is_connected();
bool zcanbus_send_packet(uint8_t to, uint8_t *packet, size_t len);
bool zcanbus_send_packet(uint8_t to, uint8_t *packet, size_t len, int overtimems);
bool zcanbus_send_sub_packet(uint8_t to, int npacket, int packetIndex, uint8_t *packet, size_t len, int overtimems);
bool zcanbus_send_ack(iflytop::zscanprotocol::zcanbus_packet_t *rxpacket, uint8_t *param, size_t len);
bool zcanbus_send_ack(iflytop::zscanprotocol::zcanbus_packet_t *rxpacket, int32_t val);
bool zcanbus_send_ack(iflytop::zscanprotocol::zcanbus_packet_t *rxpacket, bool _val);
bool zcanbus_send_errorack(iflytop::zscanprotocol::zcanbus_packet_t *rxpacket, int32_t errcode);
bool zcanbus_send_report(uint16_t function_id, uint8_t *param, size_t len, int32_t overtime);
bool zcanbus_send_emergency_report(uint16_t function_id, uint8_t *param, size_t len, int32_t overtime);
Write Preview
Loading…
Cancel
Save