5 changed files with 464 additions and 0 deletions
-
0components/single_axis_motor_control/zcr_single_axis_motor_controler.cpp
-
130components/single_axis_motor_control/zcr_single_axis_motor_controler.hpp
-
19components/zcanreceiver/cmd.hpp
-
225components/zcanreceiver/zcanreceiver.cpp
-
90components/zcanreceiver/zcanreceiver.hpp
@ -0,0 +1,130 @@ |
|||||
|
#pragma once
|
||||
|
#include "sdk/hal/zhal.hpp"
|
||||
|
#ifdef HAL_CAN_MODULE_ENABLED
|
||||
|
#include <stdlib.h>
|
||||
|
|
||||
|
#include "sdk/components/step_action_scheduler/step_scheduler.hpp"
|
||||
|
#include "sdk\components\iflytop_can_slave_v1\iflytop_can_slave.hpp"
|
||||
|
#include "sdk\components\tmc\basic\tmc_ic_interface.hpp"
|
||||
|
|
||||
|
/**
|
||||
|
* @brief |
||||
|
* |
||||
|
* service: SingleAxisMotorControler |
||||
|
* |
||||
|
*/ |
||||
|
#define SINGLE_AXIS_MOTOR_CTRL_REG_NUM 99
|
||||
|
|
||||
|
#define SAMC_REG_ACT_ROTATE (0) // 速度模式控制
|
||||
|
#define SAMC_REG_ACT_MOVE_TO (1) // 位置模式控制
|
||||
|
#define SAMC_REG_ACT_MOVE_BY (2) // 相对位置模式控制
|
||||
|
#define SAMC_REG_ACT_RUN_TO_HOME (3) // 归零
|
||||
|
#define SAMC_REG_ACT_DO_NOTHING (4) // __
|
||||
|
#define SAMC_REG_ACT_STOP (8) // 停止
|
||||
|
#define SAMC_REG_ACT_CLEAR_EXCEPTION (9) // 清空异常
|
||||
|
|
||||
|
namespace iflytop { |
||||
|
using namespace std; |
||||
|
|
||||
|
class SingleAxisMotorControler : public ICPSListener { |
||||
|
public: |
||||
|
typedef enum { |
||||
|
kstate_idle = 0, |
||||
|
kstate_rotate = 1, |
||||
|
kstate_moveTo = 2, |
||||
|
kstate_moveBy = 3, |
||||
|
kstate_runToHome = 4, |
||||
|
kstate_exception = 65535, |
||||
|
} state_t; |
||||
|
|
||||
|
typedef enum { |
||||
|
kexcep_motorBlocking = 1, |
||||
|
kexcep_loseZeroPoint = 2, |
||||
|
} exception_id_t; |
||||
|
|
||||
|
class RunToHomeContext { |
||||
|
public: |
||||
|
bool stopByIrq = false; |
||||
|
int32_t zeroPointPos; |
||||
|
}; |
||||
|
|
||||
|
private: |
||||
|
const char* m_name; |
||||
|
IflytopCanProtocolStackProcesser* m_processer; |
||||
|
int m_regaddoff; |
||||
|
// res
|
||||
|
ZGPIO* m_homegpio; |
||||
|
ZGPIO* m_maxlimitgpio; |
||||
|
ZGPIO* m_minlimitgpio; |
||||
|
IStepperMotor* m_motor; |
||||
|
|
||||
|
ICPSSlaveModule* m_slave; |
||||
|
|
||||
|
state_t m_state = kstate_idle; |
||||
|
|
||||
|
icps::Reg_t* m_statusReg; |
||||
|
icps::Reg_t* m_currentVelocityReg; |
||||
|
icps::Reg_t* m_currentPosReg; |
||||
|
icps::Reg_t* m_exceptionReg; |
||||
|
|
||||
|
RunToHomeContext m_runToHomeContext; |
||||
|
|
||||
|
StepActionScheduler m_step_scheduler; |
||||
|
|
||||
|
ZGPIO::onirq_t m_gpioirqprocesser; |
||||
|
|
||||
|
public: |
||||
|
icps::Reg_t* cfg_acc; |
||||
|
icps::Reg_t* cfg_dec; |
||||
|
icps::Reg_t* cfg_velocity; |
||||
|
icps::Reg_t* cfg_zero_shift; |
||||
|
icps::Reg_t* cfg_runhome_velocity; |
||||
|
icps::Reg_t* cfg_runtohome_dec; |
||||
|
icps::Reg_t* cfg_runtohome_max_distance; |
||||
|
icps::Reg_t* cfg_runtohome_leave_zero_point_distance; |
||||
|
icps::Reg_t* cfg_min_pos; |
||||
|
icps::Reg_t* cfg_max_pos; |
||||
|
icps::Reg_t* cfg_runtohome_keep_move_distance; |
||||
|
|
||||
|
public: |
||||
|
SingleAxisMotorControler(){}; |
||||
|
virtual ~SingleAxisMotorControler(){}; |
||||
|
|
||||
|
void initialize(const char* name, IflytopCanProtocolStackProcesser* processer, int regaddoff, //
|
||||
|
ZGPIO* homegpio, //
|
||||
|
ZGPIO* mingpio, //
|
||||
|
ZGPIO* rightgpio, //
|
||||
|
IStepperMotor* motor); |
||||
|
|
||||
|
public: |
||||
|
virtual icps::error_t onHostRegisterWriteEvent(icps::WriteEvent* event); |
||||
|
|
||||
|
icps::error_t rotate(int32_t velocity); |
||||
|
icps::error_t moveTo(int32_t pos); |
||||
|
icps::error_t moveBy(int32_t pos); |
||||
|
icps::error_t runToHome(); |
||||
|
icps::error_t stop(); |
||||
|
icps::error_t clearException(); |
||||
|
|
||||
|
private: |
||||
|
void motor_moveTo(int32_t pos, int32_t velocity, int32_t acc, int32_t dec); |
||||
|
void motor_moveBy(int32_t pos, int32_t velocity, int32_t acc, int32_t dec); |
||||
|
void motor_rotate(int32_t velocity, int32_t acc, int32_t dec); |
||||
|
|
||||
|
void periodJob(ZHALCORE::Context* context); |
||||
|
void changeToState(state_t state); |
||||
|
|
||||
|
private: |
||||
|
bool motorIsStop(StepActionContext* context); |
||||
|
void regGpioIrqProcesser(ZGPIO::onirq_t irqprocesser) { |
||||
|
CriticalContext cc; |
||||
|
m_gpioirqprocesser = irqprocesser; |
||||
|
} |
||||
|
void clearGpioIrqProcesser() { |
||||
|
CriticalContext cc; |
||||
|
m_gpioirqprocesser = NULL; |
||||
|
} |
||||
|
}; |
||||
|
|
||||
|
} // namespace iflytop
|
||||
|
#endif
|
||||
@ -0,0 +1,19 @@ |
|||||
|
#pragma once
|
||||
|
#include "sdk/hal/zhal.hpp"
|
||||
|
|
||||
|
namespace iflytop { |
||||
|
namespace zcr { |
||||
|
|
||||
|
typedef struct { |
||||
|
uint16_t packetindex; |
||||
|
uint16_t cmdindex; |
||||
|
uint8_t subcmdindex; |
||||
|
uint16_t var0; |
||||
|
} cmdheader_t; |
||||
|
|
||||
|
} // namespace zcr
|
||||
|
} // namespace iflytop
|
||||
|
|
||||
|
/**
|
||||
|
* @brief |
||||
|
*/ |
||||
@ -0,0 +1,225 @@ |
|||||
|
#include "zcanreceiver.hpp"
|
||||
|
|
||||
|
#ifdef HAL_CAN_MODULE_ENABLED
|
||||
|
#include <stdio.h>
|
||||
|
#include <stdlib.h>
|
||||
|
#include <string.h>
|
||||
|
using namespace iflytop; |
||||
|
|
||||
|
#define TAG "ZCanReceiver"
|
||||
|
|
||||
|
#define OVER_TIME_MS 5
|
||||
|
|
||||
|
ZCanReceiver::CFG *ZCanReceiver::createCFG(uint8_t deviceId) { |
||||
|
CFG *cfg = new CFG(); |
||||
|
ZASSERT(cfg != NULL); |
||||
|
cfg->deviceId = deviceId; |
||||
|
#ifdef STM32F103xB
|
||||
|
cfg->canHandle = &hcan; |
||||
|
#else
|
||||
|
cfg->canHandle = &hcan1; |
||||
|
#endif
|
||||
|
cfg->canFilterIndex0 = 0; |
||||
|
cfg->maxFilterNum = 7; |
||||
|
cfg->rxfifoNum = CAN_RX_FIFO0; |
||||
|
return cfg; |
||||
|
} |
||||
|
void ZCanReceiver::init(CFG *cfg) { |
||||
|
HAL_StatusTypeDef hal_status; |
||||
|
m_config = cfg; |
||||
|
|
||||
|
/**
|
||||
|
* @brief 初始化CAN |
||||
|
*/ |
||||
|
|
||||
|
/**
|
||||
|
* @brief 初始化消息接收buf |
||||
|
*/ |
||||
|
m_canPacketRxBuffer[0].dataIsReady = false; |
||||
|
m_canPacketRxBuffer[0].id = 1; // 只接收来自主机的消息
|
||||
|
m_canPacketRxBuffer[0].m_canPacketNum = 0; |
||||
|
|
||||
|
/**
|
||||
|
* @brief 初始化过滤器 |
||||
|
*/ |
||||
|
hal_status = initializeFilter(); |
||||
|
if (hal_status != HAL_OK) { |
||||
|
ZLOGE(TAG, "start can initializeFilter fail\r\n"); |
||||
|
return; |
||||
|
} |
||||
|
/**
|
||||
|
* @brief 启动CAN |
||||
|
*/ |
||||
|
hal_status = HAL_CAN_Start(m_config->canHandle); // 开启CAN
|
||||
|
if (hal_status != HAL_OK) { |
||||
|
ZLOGE(TAG, "start can fail\r\n"); |
||||
|
return; |
||||
|
} |
||||
|
/**
|
||||
|
* @brief 监听回调 |
||||
|
*/ |
||||
|
ZCanIRQDispatcher::instance().regListener(this); |
||||
|
HAL_StatusTypeDef status = activateRxIT(); |
||||
|
if (status != HAL_OK) { |
||||
|
ZLOGE(TAG, "activateRxIT fail\r\n"); |
||||
|
return; |
||||
|
} |
||||
|
ZHALCORE::getInstance()->regPeriodJob([this](ZHALCORE::Context &context) { loop(); }, 0); |
||||
|
} |
||||
|
HAL_StatusTypeDef ZCanReceiver::initializeFilter() { |
||||
|
/**
|
||||
|
* @brief ID区帧格式 |
||||
|
* [ 27:0 ] |
||||
|
* [ STDID ] [ EXTID ] |
||||
|
* [11 :9] [8:6] [5:0] [17:16] [15:8] [7:0] |
||||
|
* 优先级 属性 帧类型 目标ID 源ID |
||||
|
*/ |
||||
|
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 = m_config->rxfifoNum; // 关联过滤器到rxfifoNum
|
||||
|
sFilterConfig.FilterActivation = ENABLE; // 激活过滤器
|
||||
|
sFilterConfig.SlaveStartFilterBank = m_config->maxFilterNum; // slave filter start index
|
||||
|
|
||||
|
/*******************************************************************************
|
||||
|
* 接收所有消息 * |
||||
|
*******************************************************************************/ |
||||
|
filterId = (0); //
|
||||
|
mask = (0); //
|
||||
|
sFilterConfig.FilterBank = m_config->canFilterIndex0; //
|
||||
|
sFilterConfig.FilterMaskIdLow = mask & 0xffff; //
|
||||
|
sFilterConfig.FilterMaskIdHigh = (mask & 0xffff0000) >> 16; //
|
||||
|
sFilterConfig.FilterIdLow = filterId & 0xffff; //
|
||||
|
sFilterConfig.FilterIdHigh = (filterId & 0xffff0000) >> 16; //
|
||||
|
HAL_Status = HAL_CAN_ConfigFilter(m_config->canHandle, &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 ZCanReceiver::registerListener(ZCanRceiverListener *listener) { m_listenerList.push_back(listener); } |
||||
|
void ZCanReceiver::sendPacket(uint8_t *packet, size_t len) {} |
||||
|
bool ZCanReceiver::getRxMessage(CAN_RxHeaderTypeDef *pHeader, uint8_t aData[] /*8byte table*/) { |
||||
|
/**
|
||||
|
* @brief 读取当前FIFO中缓存了多少帧的数据 |
||||
|
*/ |
||||
|
uint32_t level = HAL_CAN_GetRxFifoFillLevel(m_config->canHandle, m_config->rxfifoNum); |
||||
|
if (level == 0) { |
||||
|
return false; |
||||
|
} |
||||
|
HAL_StatusTypeDef HAL_RetVal; |
||||
|
HAL_RetVal = HAL_CAN_GetRxMessage(m_config->canHandle, m_config->rxfifoNum, pHeader, aData); |
||||
|
if (HAL_OK == HAL_RetVal) { |
||||
|
// 处理接收到的can总线数据
|
||||
|
return true; |
||||
|
} |
||||
|
return false; |
||||
|
} |
||||
|
void ZCanReceiver::STM32_HAL_onCAN_RxFifo0MsgPending(CAN_HandleTypeDef *canHandle) { |
||||
|
/**
|
||||
|
* @brief 中断上下文 |
||||
|
*/ |
||||
|
// ZLOG_INFO("%s\n", __FUNCTION__);
|
||||
|
// printf("------------------%s\n", __FUNCTION__);
|
||||
|
if (canHandle != m_config->canHandle) { |
||||
|
return; |
||||
|
} |
||||
|
/**
|
||||
|
* @brief 处理can接收到消息 |
||||
|
*/ |
||||
|
CAN_RxHeaderTypeDef pHeader; |
||||
|
uint8_t aData[8] /*8byte table*/; |
||||
|
while (getRxMessage(&pHeader, aData)) { |
||||
|
/**
|
||||
|
* @brief 消息格式 |
||||
|
* |
||||
|
* [2] [3bit] [8bit] [8bit] [8bit] |
||||
|
* , from frameNum frameId |
||||
|
*/ |
||||
|
uint8_t from = (pHeader.ExtId >> 16 & 0xFF); |
||||
|
uint8_t nframe = (pHeader.ExtId & 0xFF00) >> 8; |
||||
|
uint8_t frameId = (pHeader.ExtId & 0x00FF); |
||||
|
CanPacketRxBuffer *rxbuf = &m_canPacketRxBuffer[0]; |
||||
|
if (from != rxbuf->id) { |
||||
|
// 目前只接收来自主机的消息
|
||||
|
continue; |
||||
|
} |
||||
|
|
||||
|
if (rxbuf->dataIsReady) { |
||||
|
// 上次接收到的消息还没有来的急处理
|
||||
|
continue; |
||||
|
} |
||||
|
/**
|
||||
|
* @TODO:判断是否丢包 |
||||
|
*/ |
||||
|
if (frameId == 0) { |
||||
|
rxbuf->m_canPacketNum = 0; |
||||
|
} |
||||
|
|
||||
|
if (rxbuf->m_canPacketNum < 255) { |
||||
|
rxbuf->m_canPacket[rxbuf->m_canPacketNum].pHeader = pHeader; |
||||
|
memcpy(rxbuf->m_canPacket[rxbuf->m_canPacketNum].aData, aData, 8); |
||||
|
rxbuf->m_canPacketNum++; |
||||
|
} |
||||
|
if (nframe == frameId) { |
||||
|
rxbuf->dataIsReady = true; |
||||
|
} |
||||
|
} |
||||
|
|
||||
|
// deactivateRxIT();
|
||||
|
} |
||||
|
void ZCanReceiver::STM32_HAL_onCAN_Error(CAN_HandleTypeDef *canHandle) { |
||||
|
if (canHandle != m_config->canHandle) { |
||||
|
return; |
||||
|
} |
||||
|
ZLOGE(TAG, "onCAN_Error\r\n"); |
||||
|
} |
||||
|
void ZCanReceiver::loop() { |
||||
|
CanPacketRxBuffer *rxbuf = &m_canPacketRxBuffer[0]; |
||||
|
if (rxbuf->dataIsReady) { |
||||
|
int dataoff = 0; |
||||
|
for (size_t i = 0; i < rxbuf->m_canPacketNum; i++) { |
||||
|
memcpy(rxdata + dataoff, rxbuf->m_canPacket[i].aData, rxbuf->m_canPacket[i].pHeader.DLC); |
||||
|
dataoff += rxbuf->m_canPacket[i].pHeader.DLC; |
||||
|
} |
||||
|
for (auto &var : m_listenerList) { |
||||
|
var->onRceivePacket(rxbuf, rxdata, dataoff); |
||||
|
} |
||||
|
rxbuf->dataIsReady = false; |
||||
|
} |
||||
|
} |
||||
|
HAL_StatusTypeDef ZCanReceiver::activateRxIT() { |
||||
|
HAL_StatusTypeDef hal_status = HAL_ERROR; |
||||
|
if (m_config->rxfifoNum == CAN_RX_FIFO0) { |
||||
|
hal_status = HAL_CAN_ActivateNotification(m_config->canHandle, CAN_IT_RX_FIFO0_MSG_PENDING); |
||||
|
} else if (m_config->rxfifoNum == CAN_RX_FIFO1) { |
||||
|
hal_status = HAL_CAN_ActivateNotification(m_config->canHandle, 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 ZCanReceiver::deactivateRxIT() { |
||||
|
HAL_StatusTypeDef hal_status = HAL_ERROR; |
||||
|
if (m_config->rxfifoNum == CAN_RX_FIFO0) { |
||||
|
hal_status = HAL_CAN_DeactivateNotification(m_config->canHandle, CAN_IT_RX_FIFO0_MSG_PENDING); |
||||
|
} else if (m_config->rxfifoNum == CAN_RX_FIFO1) { |
||||
|
hal_status = HAL_CAN_DeactivateNotification(m_config->canHandle, 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; |
||||
|
} |
||||
|
|
||||
|
#endif
|
||||
@ -0,0 +1,90 @@ |
|||||
|
//
|
||||
|
// Created by zwsd
|
||||
|
//
|
||||
|
|
||||
|
#pragma once
|
||||
|
#include "sdk/hal/zhal.hpp"
|
||||
|
|
||||
|
namespace iflytop { |
||||
|
namespace zcr { |
||||
|
typedef enum { |
||||
|
kpacketHeader = 0, |
||||
|
kpacketData = 1, |
||||
|
kpacketTail = 2, |
||||
|
} packet_type_t; |
||||
|
|
||||
|
class CANPacket { |
||||
|
public: |
||||
|
CAN_RxHeaderTypeDef pHeader; |
||||
|
uint8_t aData[8]; /*8byte table*/ |
||||
|
}; |
||||
|
|
||||
|
class CanPacketRxBuffer { |
||||
|
public: |
||||
|
uint16_t id; |
||||
|
CANPacket m_canPacket[256]; // 用于接收can消息
|
||||
|
uint8_t m_canPacketNum = 0; |
||||
|
bool dataIsReady; |
||||
|
}; |
||||
|
}; // namespace zcr
|
||||
|
|
||||
|
using namespace zcr; |
||||
|
|
||||
|
class ZCanRceiverListener { |
||||
|
public: |
||||
|
virtual void onRceivePacket(CanPacketRxBuffer *rxbuf, uint8_t *packet, size_t len) = 0; |
||||
|
}; |
||||
|
|
||||
|
class ZCanReceiver : public ZCanIRQListener { |
||||
|
public: |
||||
|
class CFG { |
||||
|
public: |
||||
|
uint8_t deviceId; //
|
||||
|
/*******************************************************************************
|
||||
|
* CANConfig * |
||||
|
*******************************************************************************/ |
||||
|
CAN_HandleTypeDef *canHandle; // 默认使用CAN1
|
||||
|
int canFilterIndex0; // 过滤器0 接收，发给自身的消息
|
||||
|
int maxFilterNum; // 使用的过滤器数量，最大值14，默认为7
|
||||
|
int rxfifoNum; // 使用的FIFO,默认使用FIFO0
|
||||
|
int packetRxOvertime_ms; //
|
||||
|
}; |
||||
|
|
||||
|
uint8_t rxdata[255 * 8]; |
||||
|
|
||||
|
public: |
||||
|
class LoopJobContext { |
||||
|
public: |
||||
|
bool hasDoneSomething; |
||||
|
}; |
||||
|
|
||||
|
private: |
||||
|
CFG *m_config = NULL; // 配置
|
||||
|
bool m_canOnRxDataFlag = false; // 是否有数据接收，用于从中断上下文转移到MainLoop上下文
|
||||
|
uint32_t m_lastPacketTicket = 0; // 上一次接收到消息的时间，用于判断与主机是否断开连接
|
||||
|
HAL_StatusTypeDef m_lastTransmitStatus; // 上次调用can发送方法的返回值
|
||||
|
|
||||
|
list<ZCanRceiverListener *> m_listenerList; |
||||
|
CanPacketRxBuffer m_canPacketRxBuffer[1]; |
||||
|
|
||||
|
public: |
||||
|
ZCanReceiver() {} |
||||
|
CFG *createCFG(uint8_t deviceId); |
||||
|
void init(CFG *cfg); |
||||
|
|
||||
|
void registerListener(ZCanRceiverListener *listener); |
||||
|
void sendPacket(uint8_t *packet, size_t len); |
||||
|
|
||||
|
public: |
||||
|
virtual void STM32_HAL_onCAN_RxFifo0MsgPending(CAN_HandleTypeDef *can); |
||||
|
virtual void STM32_HAL_onCAN_Error(CAN_HandleTypeDef *can); |
||||
|
|
||||
|
private: |
||||
|
void loop(); |
||||
|
HAL_StatusTypeDef initializeFilter(); |
||||
|
HAL_StatusTypeDef activateRxIT(); |
||||
|
HAL_StatusTypeDef deactivateRxIT(); |
||||
|
bool getRxMessage(CAN_RxHeaderTypeDef *pHeader, uint8_t aData[] /*8byte table*/); |
||||
|
}; |
||||
|
|
||||
|
} // namespace iflytop
|
||||
Write
Preview
Loading…
Cancel
Save
Reference in new issue