diff --git a/components/single_axis_motor_control/zcr_single_axis_motor_controler.cpp b/components/single_axis_motor_control/zcr_single_axis_motor_controler.cpp new file mode 100644 index 0000000..e69de29 diff --git a/components/single_axis_motor_control/zcr_single_axis_motor_controler.hpp b/components/single_axis_motor_control/zcr_single_axis_motor_controler.hpp new file mode 100644 index 0000000..6a21446 --- /dev/null +++ b/components/single_axis_motor_control/zcr_single_axis_motor_controler.hpp @@ -0,0 +1,130 @@ +#pragma once +#include "sdk/hal/zhal.hpp" +#ifdef HAL_CAN_MODULE_ENABLED +#include + +#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 diff --git a/components/zcanreceiver/cmd.hpp b/components/zcanreceiver/cmd.hpp new file mode 100644 index 0000000..1825110 --- /dev/null +++ b/components/zcanreceiver/cmd.hpp @@ -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 + */ diff --git a/components/zcanreceiver/zcanreceiver.cpp b/components/zcanreceiver/zcanreceiver.cpp new file mode 100644 index 0000000..2c5d1eb --- /dev/null +++ b/components/zcanreceiver/zcanreceiver.cpp @@ -0,0 +1,225 @@ +#include "zcanreceiver.hpp" + +#ifdef HAL_CAN_MODULE_ENABLED +#include +#include +#include +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 diff --git a/components/zcanreceiver/zcanreceiver.hpp b/components/zcanreceiver/zcanreceiver.hpp new file mode 100644 index 0000000..d4d2e92 --- /dev/null +++ b/components/zcanreceiver/zcanreceiver.hpp @@ -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 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 \ No newline at end of file