24 changed files with 157 additions and 1593 deletions
-
10.cproject
-
5.gitmodules
-
2.project
-
4.settings/language.settings.xml
-
4graphite_digester_subboard.ioc
-
0hand_acid_mainboard.cfg
-
12hand_acid_mainboard.launch
-
56usrc/components/zcancmder/basic.cpp
-
64usrc/components/zcancmder/basic.hpp
-
66usrc/components/zcancmder/protocol_event_bus_sender.cpp
-
31usrc/components/zcancmder/protocol_event_bus_sender.hpp
-
392usrc/components/zcancmder/zcan_protocol_parser.cpp
-
103usrc/components/zcancmder/zcan_protocol_parser.hpp
-
418usrc/components/zcancmder/zcanreceiver.cpp
-
92usrc/components/zcancmder/zcanreceiver.hpp
-
2usrc/project_configs.h
-
42usrc/public_service/common_hardware_init.cpp
-
53usrc/public_service/ext_board_impl.cpp
-
68usrc/public_service/ext_board_impl.hpp
-
18usrc/public_service/gservice.cpp
-
34usrc/public_service/gservice.hpp
-
2usrc/public_service/public_service.hpp
@ -1,3 +1,6 @@ |
|||
[submodule "protocol"] |
|||
path = protocol |
|||
url = zwsd@192.168.1.3:p_graphite_digester_v2/graphite_digester_protocol.git |
|||
url = zwsd@192.168.1.3:p_chinagoldgroup_hand_acid/hand_acid_ble_protocol.git |
|||
[submodule "sdk"] |
|||
path = sdk |
|||
url = zwsd@192.168.1.3:p_chinagoldgroup_hand_acid/chinagoldgroup_hand_acid_stm32_sdk.git |
|||
@ -1,56 +0,0 @@ |
|||
#include "basic.hpp"
|
|||
|
|||
#include <stdio.h>
|
|||
#include <string.h>
|
|||
|
|||
using namespace iflytop; |
|||
using namespace std; |
|||
uint16_t CanPacketRxBuffer::get_packetindex() { |
|||
zcr_cmd_header_t *cmdheader = (zcr_cmd_header_t *)rxdata; |
|||
return cmdheader->packetindex; |
|||
} |
|||
uint16_t CanPacketRxBuffer::get_cmdid() { |
|||
zcr_cmd_header_t *cmdheader = (zcr_cmd_header_t *)rxdata; |
|||
|
|||
return CMDID(cmdheader->cmdMainId, cmdheader->cmdSubId); |
|||
} |
|||
uint8_t CanPacketRxBuffer::get_cmdSubId() { |
|||
zcr_cmd_header_t *cmdheader = (zcr_cmd_header_t *)rxdata; |
|||
return cmdheader->cmdSubId; |
|||
} |
|||
uint8_t CanPacketRxBuffer::get_packetType() { |
|||
zcr_cmd_header_t *cmdheader = (zcr_cmd_header_t *)rxdata; |
|||
return cmdheader->packetType; |
|||
} |
|||
uint8_t *CanPacketRxBuffer::get_params() { |
|||
zcr_cmd_header_t *cmdheader = (zcr_cmd_header_t *)rxdata; |
|||
return cmdheader->data; |
|||
} |
|||
uint16_t CanPacketRxBuffer::get_params_len() { return rxdataSize - sizeof(zcr_cmd_header_t); } |
|||
void CanPacketRxBuffer::clear() { |
|||
id = 0; |
|||
memset(&m_canPacket, 0, sizeof(m_canPacket) / sizeof(m_canPacket[0])); |
|||
m_canPacketNum = 0; |
|||
m_npacket = 0; |
|||
memset(rxdata, 0, sizeof(rxdata)); |
|||
rxdataSize = 0; |
|||
|
|||
dataIsReady = false; |
|||
isOnReceive = false; |
|||
isUsed = false; |
|||
lostpacket = false; |
|||
} |
|||
|
|||
bool CanPacketRxBuffer::iscmd(int32_t id) { |
|||
zcr_cmd_header_t *cmdheader = (zcr_cmd_header_t *)rxdata; |
|||
uint16_t maincmdid = ((uint32_t)id >> 8) & 0xFFFF; |
|||
uint8_t cmdSubId = ((uint32_t)id) & 0xFF; |
|||
return cmdheader->cmdMainId == maincmdid && cmdheader->cmdSubId == cmdSubId; |
|||
} |
|||
zcr_cmd_header_t *CanPacketRxBuffer::get_cmdheader() { |
|||
zcr_cmd_header_t *cmdheader = (zcr_cmd_header_t *)rxdata; |
|||
return cmdheader; |
|||
} |
|||
|
|||
uint8_t *CanPacketRxBuffer::get_rx_raw() { return rxdata; } |
|||
uint16_t CanPacketRxBuffer::get_rx_raw_len() { return rxdataSize; } |
|||
@ -1,64 +0,0 @@ |
|||
//
|
|||
// Created by zwsd
|
|||
//
|
|||
|
|||
#pragma once
|
|||
#include "sdk/sdk.hpp"
|
|||
#include "protocol/protocol.hpp"
|
|||
|
|||
namespace iflytop { |
|||
namespace zcr { |
|||
typedef enum { |
|||
kpacketHeader = 0, |
|||
kpacketData = 1, |
|||
kpacketTail = 2, |
|||
} packet_type_t; |
|||
|
|||
typedef struct { |
|||
// CAN_RxHeaderTypeDef pHeader;
|
|||
uint8_t dlc; |
|||
uint8_t aData[8]; /*8byte table*/ |
|||
} CANPacket; |
|||
|
|||
}; // namespace zcr
|
|||
using namespace zcr; |
|||
|
|||
class CanPacketRxBuffer { |
|||
public: |
|||
uint16_t id = 0; |
|||
CANPacket m_canPacket[100] = {0}; // 用于接收can消息
|
|||
uint8_t m_canPacketNum = 0; |
|||
uint8_t m_npacket = 0; |
|||
uint8_t rxdata[1000] = {0}; |
|||
int rxdataSize = 0; |
|||
|
|||
bool dataIsReady = false; |
|||
bool isOnReceive = false; |
|||
bool isUsed = false; |
|||
bool lostpacket = false; |
|||
bool dataIsProcessed = false; |
|||
int32_t lastrxtime = 0; |
|||
|
|||
public: |
|||
uint16_t get_packetindex(); |
|||
uint16_t get_cmdid(); |
|||
uint8_t get_cmdSubId(); |
|||
uint8_t get_packetType(); |
|||
uint8_t *get_params(); |
|||
uint16_t get_params_len(); |
|||
void clear(); |
|||
|
|||
uint8_t* get_rx_raw(); |
|||
uint16_t get_rx_raw_len(); |
|||
|
|||
zcr_cmd_header_t *get_cmdheader(); |
|||
|
|||
bool iscmd(int32_t id); |
|||
|
|||
template <typename T> |
|||
T *get_param_as() { |
|||
return (T *)get_params(); |
|||
} |
|||
}; |
|||
|
|||
} // namespace iflytop
|
|||
@ -1,66 +0,0 @@ |
|||
#include "protocol_event_bus_sender.hpp"
|
|||
namespace iflytop { |
|||
using namespace std; |
|||
|
|||
void ProtocolEventBusSender::initialize(IZCanReceiver* zcanreceiver) { |
|||
m_zcanreceiver = zcanreceiver; |
|||
lock_.init(); |
|||
} |
|||
|
|||
|
|||
void ProtocolEventBusSender::push_reg_state_change_event(int32_t moduleid, int32_t regindex, int32_t oldval, int32_t toval) { |
|||
zlock_guard l(lock_); |
|||
|
|||
zcr_cmd_header_t* cmd_header = (zcr_cmd_header_t*)txbuf; |
|||
int32_t* data = (int32_t*)cmd_header->data; |
|||
cmd_header->subModuleid = moduleid; |
|||
cmd_header->cmdMainId = zcr::kevent_bus_reg_change_report >> 8; |
|||
cmd_header->subModuleid = zcr::kevent_bus_reg_change_report & 0xff; |
|||
|
|||
data[0] = regindex; |
|||
data[1] = oldval; |
|||
data[2] = toval; |
|||
m_zcanreceiver->triggerEvent(cmd_header, (uint8_t*)data, 4 * 3); |
|||
} |
|||
|
|||
void ProtocolEventBusSender::report(int32_t moduleid, int32_t cmdindex, int32_t data0) { |
|||
zlock_guard l(lock_); |
|||
|
|||
zcr_cmd_header_t* cmd_header = (zcr_cmd_header_t*)txbuf; |
|||
int32_t* data = (int32_t*)cmd_header->data; |
|||
cmd_header->subModuleid = moduleid; |
|||
cmd_header->cmdMainId = cmdindex >> 8; |
|||
cmd_header->subModuleid = cmdindex & 0xff; |
|||
|
|||
data[0] = data0; |
|||
m_zcanreceiver->triggerEvent(cmd_header, (uint8_t*)data, 4 * 1); |
|||
} |
|||
void ProtocolEventBusSender::report(int32_t moduleid, int32_t cmdindex, int32_t data0, int32_t data1) { |
|||
zlock_guard l(lock_); |
|||
|
|||
zcr_cmd_header_t* cmd_header = (zcr_cmd_header_t*)txbuf; |
|||
int32_t* data = (int32_t*)cmd_header->data; |
|||
cmd_header->subModuleid = moduleid; |
|||
cmd_header->cmdMainId = cmdindex >> 8; |
|||
cmd_header->subModuleid = cmdindex & 0xff; |
|||
|
|||
data[0] = data0; |
|||
data[1] = data1; |
|||
m_zcanreceiver->triggerEvent(cmd_header, (uint8_t*)data, 4 * 1); |
|||
} |
|||
void ProtocolEventBusSender::report(int32_t moduleid, int32_t cmdindex, int32_t data0, int32_t data1, int32_t data2) { |
|||
zlock_guard l(lock_); |
|||
|
|||
zcr_cmd_header_t* cmd_header = (zcr_cmd_header_t*)txbuf; |
|||
int32_t* data = (int32_t*)cmd_header->data; |
|||
cmd_header->subModuleid = moduleid; |
|||
cmd_header->cmdMainId = cmdindex >> 8; |
|||
cmd_header->subModuleid = cmdindex & 0xff; |
|||
|
|||
data[0] = data0; |
|||
data[1] = data1; |
|||
data[2] = data2; |
|||
m_zcanreceiver->triggerEvent(cmd_header, (uint8_t*)data, 4 * 1); |
|||
} |
|||
|
|||
} // namespace iflytop
|
|||
@ -1,31 +0,0 @@ |
|||
#pragma once
|
|||
|
|||
#include "basic.hpp"
|
|||
|
|||
namespace iflytop { |
|||
using namespace std; |
|||
|
|||
class ProtocolEventBusSender { |
|||
IZCanReceiver* m_zcanreceiver = nullptr; |
|||
zmutex lock_; |
|||
|
|||
uint8_t txbuf[128]; |
|||
|
|||
public: |
|||
static ProtocolEventBusSender* inst() { |
|||
static ProtocolEventBusSender instance; |
|||
return &instance; |
|||
} |
|||
|
|||
void initialize(IZCanReceiver* zcanreceiver); |
|||
|
|||
void push_reg_state_change_event(int32_t moduleid, int32_t regindex, int32_t oldval, int32_t toval); |
|||
|
|||
void report(int32_t moduleid, int32_t cmdindex, int32_t data0); |
|||
void report(int32_t moduleid, int32_t cmdindex, int32_t data0, int32_t data1); |
|||
void report(int32_t moduleid, int32_t cmdindex, int32_t data0, int32_t data1, int32_t data2); |
|||
|
|||
private: |
|||
void _report(int32_t moduleid, int32_t cmdindex, int32_t ndata, int32_t* data); |
|||
}; |
|||
} // namespace iflytop
|
|||
@ -1,392 +0,0 @@ |
|||
#include "zcan_protocol_parser.hpp"
|
|||
|
|||
#include <stdio.h>
|
|||
#include <string.h>
|
|||
|
|||
|
|||
using namespace iflytop; |
|||
using namespace std; |
|||
#define TAG "PROTO"
|
|||
|
|||
#define REGFN(fn) regCmdFn(k##fn, fn);
|
|||
|
|||
void ZCanProtocolParser::initialize(IZCanReceiver* cancmder) { |
|||
m_cancmder = cancmder; |
|||
m_cancmder->registerListener(this); |
|||
|
|||
REGFN(module_ping); |
|||
REGFN(module_get_status); |
|||
REGFN(module_set_reg); |
|||
REGFN(module_get_reg); |
|||
REGFN(module_get_error); |
|||
REGFN(module_clear_error); |
|||
REGFN(module_stop); |
|||
|
|||
// REGFN(step_motor_enable);
|
|||
// REGFN(step_motor_read_pos);
|
|||
// REGFN(step_motor_easy_rotate);
|
|||
// REGFN(step_motor_easy_move_by);
|
|||
// REGFN(step_motor_easy_move_to);
|
|||
// REGFN(step_motor_easy_move_to_zero);
|
|||
// REGFN(step_motor_easy_set_current_pos);
|
|||
// REGFN(step_motor_easy_move_to_io);
|
|||
// REGFN(step_motor_active_cfg);
|
|||
// REGFN(step_motor_stop);
|
|||
// REGFN(step_motor_read_io_state);
|
|||
// REGFN(step_motor_easy_move_to_end_point);
|
|||
|
|||
// REGFN(step_motor_read_tmc5130_status);
|
|||
// REGFN(step_motor_read_tmc5130_state);
|
|||
// REGFN(step_motor_read_tmc4361a_status);
|
|||
// REGFN(step_motor_read_tmc4361a_state);
|
|||
// REGFN(step_motor_read_tmc2160_status);
|
|||
// REGFN(step_motor_read_tmc2160_state);
|
|||
// REGFN(step_motor_read_io_index_in_stm32);
|
|||
|
|||
// REGFN(mini_servo_enable);
|
|||
// REGFN(mini_servo_read_pos);
|
|||
// REGFN(mini_servo_active_cfg);
|
|||
// REGFN(mini_servo_stop);
|
|||
// REGFN(mini_servo_set_mid_point);
|
|||
// REGFN(mini_servo_read_io_state);
|
|||
|
|||
// REGFN(mini_servo_move_to);
|
|||
// REGFN(mini_servo_rotate);
|
|||
// REGFN(mini_servo_rotate_with_torque);
|
|||
|
|||
// REGFN(extboard_read_inio);
|
|||
// REGFN(extboard_write_outio);
|
|||
// REGFN(extboard_read_muti_inio);
|
|||
// REGFN(extboard_read_inio_index_in_stm32);
|
|||
// REGFN(extboard_read_outio_index_in_stm32);
|
|||
// REGFN(extboard_read_outio);
|
|||
|
|||
|
|||
} |
|||
void ZCanProtocolParser::_registerModule(uint16_t id, ZIModule* module) { m_modulers[id] = module; } |
|||
void ZCanProtocolParser::registerModule(ZIModule* module) { |
|||
int32_t moduleid = 0; |
|||
module->getid(&moduleid); |
|||
uint16_t id = moduleid; |
|||
_registerModule(id, module); |
|||
} |
|||
|
|||
void ZCanProtocolParser::onRceivePacket(zcr_cmd_header_t* rxcmd, uint8_t* data, int32_t len) { |
|||
// printf("onRceivePacket cmdid:%d subModuleid:%d cmdSubId:%d\n", rxcmd->cmdMainId, rxcmd->subModuleid, rxcmd->cmdSubId);
|
|||
uint16_t subModuleid = rxcmd->subModuleid; |
|||
auto it = m_modulers.find(subModuleid); |
|||
if (it == m_modulers.end()) { |
|||
return; |
|||
} |
|||
|
|||
ZIModule* module = it->second; |
|||
int32_t cmdid = CMDID(rxcmd->cmdMainId, rxcmd->cmdSubId); |
|||
_onRceivePacket(module, rxcmd, cmdid, rxcmd->data, len); |
|||
} |
|||
|
|||
void ZCanProtocolParser::regCmdFn(int32_t cmdid, cmdcb_t cb) { |
|||
cmdfnlist[cmdfnNum].cmdid = cmdid; |
|||
cmdfnlist[cmdfnNum].cb = cb; |
|||
cmdfnNum++; |
|||
} |
|||
ZCanProtocolParser::cmdfn_t* ZCanProtocolParser::findcmdfn(int32_t cmdid) { |
|||
for (int i = 0; i < cmdfnNum; i++) { |
|||
if (cmdfnlist[i].cmdid == cmdid) { |
|||
return &cmdfnlist[i]; |
|||
} |
|||
} |
|||
return nullptr; |
|||
} |
|||
|
|||
void ZCanProtocolParser::_onRceivePacket(ZIModule* module, zcr_cmd_header_t* rxcmd, int32_t cmdid, uint8_t* param, int32_t len) { |
|||
int paramNum = (len) / sizeof(int32_t); |
|||
int32_t* ack = (int32_t*)&ackbuf[0]; |
|||
|
|||
cmdfn_t* fn = findcmdfn(cmdid); |
|||
if (!fn) { |
|||
m_cancmder->sendErrorAck(rxcmd, err::kcmd_not_support); |
|||
return; |
|||
} |
|||
cmdcontxt_t cxt = {0}; |
|||
cxt.module = module; |
|||
cxt.rxcmd = rxcmd; |
|||
cxt.cmdid = cmdid; |
|||
cxt.params = (int32_t*)param; |
|||
cxt.paramRaw = param; |
|||
cxt.paramlen = len; |
|||
cxt.acklen = 0; |
|||
cxt.ackbuf = ackbuf; |
|||
|
|||
memset(ackbuf, 0, sizeof(ackbuf)); |
|||
|
|||
int32_t ecode = fn->cb(&cxt); |
|||
if (ecode != 0) { |
|||
m_cancmder->sendErrorAck(rxcmd, ecode); |
|||
} else { |
|||
m_cancmder->sendBufAck(rxcmd, cxt.ackbuf, cxt.acklen); |
|||
} |
|||
return; |
|||
} |
|||
|
|||
#define CHECK_AND_GET_MODULE(paraNum) \
|
|||
auto* module = dynamic_cast<MODULE_CLASS*>(cxt->module); \ |
|||
if (module == nullptr) return err::kcmd_not_support; \ |
|||
if (paraNum >= 0 && paraNum != cxt->paramlen / 4) return err::kcmd_param_num_error; |
|||
|
|||
#define GET_MODULE(paraNum) \
|
|||
auto* module = dynamic_cast<MODULE_CLASS*>(cxt->module); \ |
|||
if (module == nullptr) return err::kcmd_not_support; |
|||
|
|||
#define MODULE_CLASS ZIModule
|
|||
int32_t ZCanProtocolParser::module_ping(cmdcontxt_t* cxt) { |
|||
CHECK_AND_GET_MODULE(0); |
|||
return module->module_ping(); |
|||
} |
|||
int32_t ZCanProtocolParser::module_get_status(cmdcontxt_t* cxt) { |
|||
CHECK_AND_GET_MODULE(0); |
|||
int32_t* ack = (int32_t*)cxt->ackbuf; |
|||
cxt->acklen = 4; |
|||
return module->module_get_status(&ack[0]); |
|||
} |
|||
int32_t ZCanProtocolParser::module_set_reg(cmdcontxt_t* cxt) { |
|||
CHECK_AND_GET_MODULE(2); |
|||
return module->module_set_reg(cxt->params[0], cxt->params[1]); |
|||
} |
|||
int32_t ZCanProtocolParser::module_get_reg(cmdcontxt_t* cxt) { |
|||
CHECK_AND_GET_MODULE(1); |
|||
int32_t* ack = (int32_t*)cxt->ackbuf; |
|||
cxt->acklen = 4; |
|||
return module->module_get_reg(cxt->params[0], &ack[0]); |
|||
} |
|||
|
|||
int32_t ZCanProtocolParser::module_get_error(cmdcontxt_t* cxt) { |
|||
CHECK_AND_GET_MODULE(0); |
|||
int32_t* ack = (int32_t*)cxt->ackbuf; |
|||
cxt->acklen = 4; |
|||
return module->module_get_error(&ack[0]); |
|||
} |
|||
int32_t ZCanProtocolParser::module_clear_error(cmdcontxt_t* cxt) { |
|||
CHECK_AND_GET_MODULE(0); |
|||
return module->module_clear_error(); |
|||
} |
|||
|
|||
int32_t ZCanProtocolParser::module_stop(cmdcontxt_t* cxt) { |
|||
CHECK_AND_GET_MODULE(0); |
|||
return module->module_stop(); |
|||
} |
|||
int32_t ZCanProtocolParser::module_active_cfg(cmdcontxt_t* cxt) { |
|||
CHECK_AND_GET_MODULE(0); |
|||
return module->module_active_cfg(); |
|||
} |
|||
|
|||
#undef MODULE_CLASS
|
|||
|
|||
/***********************************************************************************************************************
|
|||
* ZIStepMotor * |
|||
***********************************************************************************************************************/ |
|||
#if 0
|
|||
#define MODULE_CLASS ZIStepMotor
|
|||
|
|||
int32_t ZCanProtocolParser::step_motor_enable(cmdcontxt_t* cxt) { |
|||
CHECK_AND_GET_MODULE(1); |
|||
return module->step_motor_enable(cxt->params[0]); |
|||
} |
|||
int32_t ZCanProtocolParser::step_motor_stop(cmdcontxt_t* cxt) { |
|||
CHECK_AND_GET_MODULE(1); |
|||
return module->step_motor_stop(cxt->params[0]); |
|||
} |
|||
int32_t ZCanProtocolParser::step_motor_read_pos(cmdcontxt_t* cxt) { |
|||
CHECK_AND_GET_MODULE(0); |
|||
int32_t* ack = (int32_t*)cxt->ackbuf; |
|||
cxt->acklen = 4; |
|||
return module->step_motor_read_pos(&ack[0]); |
|||
} |
|||
int32_t ZCanProtocolParser::step_motor_easy_rotate(cmdcontxt_t* cxt) { |
|||
CHECK_AND_GET_MODULE(1); |
|||
return module->step_motor_easy_rotate(cxt->params[0]); |
|||
} |
|||
|
|||
int32_t ZCanProtocolParser::step_motor_easy_move_by(cmdcontxt_t* cxt) { |
|||
CHECK_AND_GET_MODULE(1); |
|||
return module->step_motor_easy_move_by(cxt->params[0]); |
|||
} |
|||
int32_t ZCanProtocolParser::step_motor_easy_move_to(cmdcontxt_t* cxt) { |
|||
CHECK_AND_GET_MODULE(1); |
|||
return module->step_motor_easy_move_to(cxt->params[0]); |
|||
} |
|||
int32_t ZCanProtocolParser::step_motor_easy_move_to_zero(cmdcontxt_t* cxt) { |
|||
GET_MODULE(0); |
|||
return module->step_motor_easy_move_to_zero(); |
|||
} |
|||
int32_t ZCanProtocolParser::step_motor_easy_set_current_pos(cmdcontxt_t* cxt) { |
|||
CHECK_AND_GET_MODULE(1); |
|||
return module->step_motor_easy_set_current_pos(cxt->params[0]); |
|||
} |
|||
int32_t ZCanProtocolParser::step_motor_easy_move_to_io(cmdcontxt_t* cxt) { |
|||
CHECK_AND_GET_MODULE(2); |
|||
return module->step_motor_easy_move_to_io(cxt->params[0], cxt->params[1]); |
|||
} |
|||
|
|||
int32_t ZCanProtocolParser::step_motor_active_cfg(cmdcontxt_t* cxt) { |
|||
CHECK_AND_GET_MODULE(0); |
|||
return module->step_motor_active_cfg(); |
|||
} |
|||
int32_t ZCanProtocolParser::step_motor_read_io_state(cmdcontxt_t* cxt) { |
|||
CHECK_AND_GET_MODULE(1); |
|||
int32_t* ack = (int32_t*)cxt->ackbuf; |
|||
cxt->acklen = 4; |
|||
return module->step_motor_read_io_state(cxt->params[0], ack); |
|||
} |
|||
|
|||
int32_t ZCanProtocolParser::step_motor_easy_move_to_end_point(cmdcontxt_t* cxt) { |
|||
CHECK_AND_GET_MODULE(0); |
|||
return module->step_motor_easy_move_to_end_point(); |
|||
} |
|||
|
|||
int32_t ZCanProtocolParser::step_motor_read_tmc5130_status(cmdcontxt_t* cxt) { |
|||
CHECK_AND_GET_MODULE(0); |
|||
int32_t* ack = (int32_t*)cxt->ackbuf; |
|||
cxt->acklen = 4; |
|||
return module->step_motor_read_tmc5130_status(&ack[0]); |
|||
} |
|||
int32_t ZCanProtocolParser::step_motor_read_tmc5130_state(cmdcontxt_t* cxt) { |
|||
CHECK_AND_GET_MODULE(0); |
|||
int32_t* ack = (int32_t*)cxt->ackbuf; |
|||
cxt->acklen = 4; |
|||
return module->step_motor_read_tmc5130_state(&ack[0]); |
|||
} |
|||
int32_t ZCanProtocolParser::step_motor_read_tmc4361a_status(cmdcontxt_t* cxt) { |
|||
CHECK_AND_GET_MODULE(0); |
|||
int32_t* ack = (int32_t*)cxt->ackbuf; |
|||
cxt->acklen = 4; |
|||
return module->step_motor_read_tmc4361a_status(&ack[0]); |
|||
} |
|||
int32_t ZCanProtocolParser::step_motor_read_tmc4361a_state(cmdcontxt_t* cxt) { |
|||
CHECK_AND_GET_MODULE(0); |
|||
int32_t* ack = (int32_t*)cxt->ackbuf; |
|||
cxt->acklen = 4; |
|||
return module->step_motor_read_tmc4361a_state(&ack[0]); |
|||
} |
|||
int32_t ZCanProtocolParser::step_motor_read_tmc2160_status(cmdcontxt_t* cxt) { |
|||
CHECK_AND_GET_MODULE(0); |
|||
int32_t* ack = (int32_t*)cxt->ackbuf; |
|||
cxt->acklen = 4; |
|||
return module->step_motor_read_tmc2160_status(&ack[0]); |
|||
} |
|||
int32_t ZCanProtocolParser::step_motor_read_tmc2160_state(cmdcontxt_t* cxt) { |
|||
CHECK_AND_GET_MODULE(0); |
|||
int32_t* ack = (int32_t*)cxt->ackbuf; |
|||
cxt->acklen = 4; |
|||
return module->step_motor_read_tmc2160_state(&ack[0]); |
|||
} |
|||
int32_t ZCanProtocolParser::step_motor_read_io_index_in_stm32(cmdcontxt_t* cxt) { |
|||
CHECK_AND_GET_MODULE(1); |
|||
int32_t* ack = (int32_t*)cxt->ackbuf; |
|||
cxt->acklen = 4; |
|||
return module->step_motor_read_io_index_in_stm32(cxt->params[0], ack); |
|||
} |
|||
|
|||
#undef MODULE_CLASS
|
|||
#define MODULE_CLASS ZIMiniServo
|
|||
// virtual int32_t mini_servo_enable(int32_t enable) = 0;
|
|||
// virtual int32_t mini_servo_read_pos(int32_t* pos) = 0;
|
|||
// virtual int32_t mini_servo_active_cfg() = 0;
|
|||
// virtual int32_t mini_servo_stop(int32_t breakstop) = 0;
|
|||
|
|||
// virtual int32_t mini_servo_rotate(int32_t direction) = 0;
|
|||
// virtual int32_t mini_servo_move_to(int32_t position) = 0;
|
|||
|
|||
// virtual int32_t mini_servo_set_mid_point() = 0;
|
|||
// virtual int32_t mini_servo_read_io_state(int32_t ioindex) = 0;
|
|||
int32_t ZCanProtocolParser::mini_servo_enable(cmdcontxt_t* cxt) { |
|||
CHECK_AND_GET_MODULE(1); |
|||
return module->mini_servo_enable(cxt->params[0]); |
|||
} |
|||
int32_t ZCanProtocolParser::mini_servo_read_pos(cmdcontxt_t* cxt) { |
|||
CHECK_AND_GET_MODULE(0); |
|||
int32_t* ack = (int32_t*)cxt->ackbuf; |
|||
cxt->acklen = 4; |
|||
return module->mini_servo_read_pos(&ack[0]); |
|||
} |
|||
int32_t ZCanProtocolParser::mini_servo_active_cfg(cmdcontxt_t* cxt) { |
|||
CHECK_AND_GET_MODULE(0); |
|||
return module->mini_servo_active_cfg(); |
|||
} |
|||
int32_t ZCanProtocolParser::mini_servo_stop(cmdcontxt_t* cxt) { |
|||
CHECK_AND_GET_MODULE(1); |
|||
return module->mini_servo_stop(cxt->params[0]); |
|||
} |
|||
int32_t ZCanProtocolParser::mini_servo_set_mid_point(cmdcontxt_t* cxt) { |
|||
CHECK_AND_GET_MODULE(0); |
|||
return module->mini_servo_set_mid_point(); |
|||
} |
|||
int32_t ZCanProtocolParser::mini_servo_read_io_state(cmdcontxt_t* cxt) { |
|||
CHECK_AND_GET_MODULE(1); |
|||
int32_t* ack = (int32_t*)cxt->ackbuf; |
|||
cxt->acklen = 4; |
|||
return module->mini_servo_read_io_state(cxt->params[0], ack); |
|||
} |
|||
|
|||
int32_t ZCanProtocolParser::mini_servo_move_to(cmdcontxt_t* cxt) { |
|||
CHECK_AND_GET_MODULE(1); |
|||
return module->mini_servo_move_to(cxt->params[0]); |
|||
} |
|||
int32_t ZCanProtocolParser::mini_servo_rotate(cmdcontxt_t* cxt) { |
|||
CHECK_AND_GET_MODULE(1); |
|||
return module->mini_servo_rotate(cxt->params[0]); |
|||
} |
|||
int32_t ZCanProtocolParser::mini_servo_rotate_with_torque(cmdcontxt_t* cxt) { |
|||
CHECK_AND_GET_MODULE(1); |
|||
return module->mini_servo_rotate_with_torque(cxt->params[0]); |
|||
} |
|||
|
|||
#undef MODULE_CLASS
|
|||
|
|||
/***********************************************************************************************************************
|
|||
* ZIBoard * |
|||
***********************************************************************************************************************/ |
|||
#define MODULE_CLASS ZIBoard
|
|||
int32_t ZCanProtocolParser::extboard_read_inio(cmdcontxt_t* cxt) { |
|||
CHECK_AND_GET_MODULE(1); |
|||
int32_t* ack = (int32_t*)cxt->ackbuf; |
|||
cxt->acklen = 4; |
|||
return module->extboard_read_inio(cxt->params[0], ack); |
|||
} |
|||
int32_t ZCanProtocolParser::extboard_write_outio(cmdcontxt_t* cxt) { |
|||
CHECK_AND_GET_MODULE(2); |
|||
return module->kextboard_write_outio(cxt->params[0], cxt->params[1]); |
|||
} |
|||
|
|||
int32_t ZCanProtocolParser::extboard_read_muti_inio(cmdcontxt_t* cxt) { |
|||
CHECK_AND_GET_MODULE(0); |
|||
int32_t* ack = (int32_t*)cxt->ackbuf; |
|||
cxt->acklen = 4; |
|||
return module->extboard_read_muti_inio(ack); |
|||
} |
|||
|
|||
int32_t ZCanProtocolParser::extboard_read_inio_index_in_stm32(cmdcontxt_t* cxt) { |
|||
CHECK_AND_GET_MODULE(1); |
|||
int32_t* ack = (int32_t*)cxt->ackbuf; |
|||
cxt->acklen = 4; |
|||
return module->extboard_read_inio_index_in_stm32(cxt->params[0], ack); |
|||
} |
|||
int32_t ZCanProtocolParser::extboard_read_outio_index_in_stm32(cmdcontxt_t* cxt) { |
|||
CHECK_AND_GET_MODULE(1); |
|||
int32_t* ack = (int32_t*)cxt->ackbuf; |
|||
cxt->acklen = 4; |
|||
return module->extboard_read_outio_index_in_stm32(cxt->params[0], ack); |
|||
} |
|||
|
|||
int32_t ZCanProtocolParser::extboard_read_outio(cmdcontxt_t* cxt) { |
|||
CHECK_AND_GET_MODULE(1); |
|||
int32_t* ack = (int32_t*)cxt->ackbuf; |
|||
cxt->acklen = 4; |
|||
return module->extboard_read_outio(cxt->params[0], ack); |
|||
} |
|||
|
|||
#undef MODULE_CLASS
|
|||
#endif
|
|||
|
|||
/***********************************************************************************************************************
|
|||
* ZIPipetteCtrlModule * |
|||
***********************************************************************************************************************/ |
|||
@ -1,103 +0,0 @@ |
|||
#pragma once
|
|||
#include <map>
|
|||
|
|||
#include "basic.hpp"
|
|||
|
|||
#define CMDFN(name) static int32_t name(cmdcontxt_t* cxt)
|
|||
#define CMDFN_IMPL(name) int32_t ZCanProtocolParser::name(cmdcontxt_t* cxt)
|
|||
|
|||
namespace iflytop { |
|||
class ZCanProtocolParser : public IZCanReceiverListener { |
|||
public: |
|||
typedef struct { |
|||
ZIModule* module; |
|||
zcr_cmd_header_t* rxcmd; |
|||
int32_t cmdid; |
|||
int32_t* params; |
|||
uint8_t* paramRaw; |
|||
int32_t paramlen; |
|||
int32_t acklen; |
|||
uint8_t* ackbuf; |
|||
} cmdcontxt_t; |
|||
|
|||
typedef int32_t (*cmdcb_t)(cmdcontxt_t* cxt); |
|||
|
|||
typedef struct { |
|||
int32_t cmdid; |
|||
cmdcb_t cb; |
|||
} cmdfn_t; |
|||
|
|||
private: |
|||
IZCanReceiver* m_cancmder = nullptr; |
|||
map<uint16_t, ZIModule*> m_modulers; |
|||
|
|||
uint8_t ackbuf[ZCANCMD_READ_BUF_MAX_SIZE + 10]; |
|||
int32_t acklen = 0; |
|||
|
|||
cmdfn_t cmdfnlist[100]; |
|||
int32_t cmdfnNum = 0; |
|||
|
|||
public: |
|||
void initialize(IZCanReceiver* cancmder); |
|||
virtual void onRceivePacket(zcr_cmd_header_t* rxcmd, uint8_t* data, int32_t len); |
|||
void registerModule(ZIModule* module); |
|||
ZIModule* getModule(int id) { return m_modulers[id]; } |
|||
|
|||
private: |
|||
void _registerModule(uint16_t id, ZIModule* module); |
|||
void _onRceivePacket(ZIModule* module, zcr_cmd_header_t* rxcmd, int32_t cmdid, uint8_t* param, int32_t len); |
|||
void regCmdFn(int32_t cmdid, cmdcb_t cb); |
|||
cmdfn_t* findcmdfn(int32_t cmdid); |
|||
|
|||
private: |
|||
CMDFN(module_ping); |
|||
CMDFN(module_get_status); |
|||
CMDFN(module_set_reg); |
|||
CMDFN(module_get_reg); |
|||
CMDFN(module_get_error); |
|||
CMDFN(module_clear_error); |
|||
CMDFN(module_stop); |
|||
CMDFN(module_active_cfg); |
|||
|
|||
CMDFN(step_motor_enable); |
|||
CMDFN(step_motor_stop); |
|||
CMDFN(step_motor_read_pos); |
|||
CMDFN(step_motor_easy_rotate); |
|||
CMDFN(step_motor_easy_move_by); |
|||
CMDFN(step_motor_easy_move_to); |
|||
CMDFN(step_motor_easy_move_to_zero); |
|||
CMDFN(step_motor_easy_set_current_pos); |
|||
CMDFN(step_motor_easy_move_to_io); |
|||
CMDFN(step_motor_active_cfg); |
|||
CMDFN(step_motor_read_io_state); |
|||
CMDFN(step_motor_easy_move_to_end_point); |
|||
CMDFN(step_motor_read_tmc5130_status); |
|||
CMDFN(step_motor_read_tmc5130_state); |
|||
CMDFN(step_motor_read_tmc4361a_status); |
|||
CMDFN(step_motor_read_tmc4361a_state); |
|||
CMDFN(step_motor_read_tmc2160_status); |
|||
CMDFN(step_motor_read_tmc2160_state); |
|||
CMDFN(step_motor_read_io_index_in_stm32); |
|||
|
|||
CMDFN(mini_servo_enable); |
|||
CMDFN(mini_servo_read_pos); |
|||
CMDFN(mini_servo_active_cfg); |
|||
CMDFN(mini_servo_stop); |
|||
CMDFN(mini_servo_set_mid_point); |
|||
CMDFN(mini_servo_read_io_state); |
|||
|
|||
CMDFN(mini_servo_move_to); |
|||
CMDFN(mini_servo_rotate); |
|||
CMDFN(mini_servo_rotate_with_torque); |
|||
|
|||
CMDFN(extboard_read_inio); |
|||
CMDFN(extboard_write_outio); |
|||
CMDFN(extboard_read_muti_inio); |
|||
CMDFN(extboard_read_inio_index_in_stm32); |
|||
CMDFN(extboard_read_outio_index_in_stm32); |
|||
CMDFN(extboard_read_outio); |
|||
|
|||
|
|||
}; |
|||
|
|||
} // namespace iflytop
|
|||
@ -1,418 +0,0 @@ |
|||
#include "zcanreceiver.hpp"
|
|||
|
|||
#ifdef HAL_CAN_MODULE_ENABLED
|
|||
#include <stdio.h>
|
|||
#include <stdlib.h>
|
|||
#include <string.h>
|
|||
using namespace iflytop; |
|||
using namespace zcr; |
|||
|
|||
#define TAG "ZCanCmder"
|
|||
|
|||
#define OVER_TIME_MS 5
|
|||
|
|||
ZCanReceiver *g_receiver; |
|||
|
|||
extern "C" { |
|||
|
|||
void HAL_CAN_TxMailbox0CompleteCallback(CAN_HandleTypeDef *hcan) {} |
|||
void HAL_CAN_TxMailbox1CompleteCallback(CAN_HandleTypeDef *hcan) {} |
|||
void HAL_CAN_TxMailbox2CompleteCallback(CAN_HandleTypeDef *hcan) {} |
|||
void HAL_CAN_TxMailbox0AbortCallback(CAN_HandleTypeDef *hcan) {} |
|||
void HAL_CAN_TxMailbox1AbortCallback(CAN_HandleTypeDef *hcan) {} |
|||
void HAL_CAN_TxMailbox2AbortCallback(CAN_HandleTypeDef *hcan) {} |
|||
void HAL_CAN_RxFifo0MsgPendingCallback(CAN_HandleTypeDef *hcan) { |
|||
if (g_receiver) g_receiver->STM32_HAL_onCAN_RxFifo0MsgPending(hcan); |
|||
} |
|||
void HAL_CAN_RxFifo0FullCallback(CAN_HandleTypeDef *hcan) {} |
|||
void HAL_CAN_RxFifo1MsgPendingCallback(CAN_HandleTypeDef *hcan) {} |
|||
void HAL_CAN_RxFifo1FullCallback(CAN_HandleTypeDef *hcan) {} |
|||
void HAL_CAN_SleepCallback(CAN_HandleTypeDef *hcan) {} |
|||
void HAL_CAN_WakeUpFromRxMsgCallback(CAN_HandleTypeDef *hcan) {} |
|||
void HAL_CAN_ErrorCallback(CAN_HandleTypeDef *hcan) {} |
|||
} |
|||
|
|||
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::initialize(CFG *cfg) { |
|||
HAL_StatusTypeDef hal_status; |
|||
m_config = cfg; |
|||
m_lock.init(); |
|||
|
|||
/**
|
|||
* @brief 初始化CAN |
|||
*/ |
|||
|
|||
/**
|
|||
* @brief 初始化消息接收buf |
|||
*/ |
|||
m_canPacketRxBuffer[0].dataIsReady = false; |
|||
m_canPacketRxBuffer[0].id = 0; // 只接收来自主机的消息
|
|||
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 监听回调 |
|||
*/ |
|||
HAL_StatusTypeDef status = activateRxIT(); |
|||
if (status != HAL_OK) { |
|||
ZLOGE(TAG, "activateRxIT fail\r\n"); |
|||
return; |
|||
} |
|||
g_receiver = this; |
|||
// 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(IZCanReceiverListener *listener) { m_listenerList.push_back(listener); } |
|||
|
|||
// static inline const char *dumphex(uint8_t *packet, size_t len) {
|
|||
// static char buf[1024];
|
|||
// memset(buf, 0, sizeof(buf));
|
|||
// for (size_t i = 0; i < len; i++) {
|
|||
// sprintf(buf + i * 2, "%02x", packet[i]);
|
|||
// }
|
|||
// return buf;
|
|||
// }
|
|||
void ZCanReceiver::sendPacket(uint8_t *packet, size_t len) { |
|||
/**
|
|||
* @brief |
|||
*/ |
|||
int npacket = len / 8 + (len % 8 == 0 ? 0 : 1); |
|||
if (npacket > 255) { |
|||
ZLOGE(TAG, "sendPacket fail, len:%d", len); |
|||
return; |
|||
} |
|||
int finalpacketlen = len % 8 == 0 ? 8 : len % 8; |
|||
|
|||
for (uint8_t i = 0; i < npacket; i++) { |
|||
bool suc = false; |
|||
if (i == npacket - 1) { |
|||
suc = sendPacketSub(npacket, i, packet + i * 8, finalpacketlen, OVER_TIME_MS); |
|||
} else { |
|||
suc = sendPacketSub(npacket, i, packet + i * 8, 8, OVER_TIME_MS); |
|||
} |
|||
if (!suc) { |
|||
ZLOGE(TAG, "sendPacket fail, packet(%d:%d)", npacket, i); |
|||
return; |
|||
} |
|||
} |
|||
} |
|||
#if 0
|
|||
void ZCanReceiver::sendAck(zcr_cmd_header_t *cmdheader, uint8_t *data, size_t len) { |
|||
zlock_guard l(m_lock); |
|||
zcr_cmd_header_t *txheader = (zcr_cmd_header_t *)txbuff; |
|||
memcpy(txheader, cmdheader, sizeof(zcr_cmd_header_t)); |
|||
txheader->packetType = kptv2_ack; |
|||
memcpy(txheader->data, data, len); |
|||
sendPacket(txbuff, sizeof(zcr_cmd_header_t) + len); |
|||
} |
|||
void ZCanReceiver::sendErrorAck(zcr_cmd_header_t *cmdheader, uint16_t id, uint32_t errcode) { |
|||
zlock_guard l(m_lock); |
|||
|
|||
zcr_cmd_header_t *txheader = (zcr_cmd_header_t *)txbuff; |
|||
memcpy(txheader, cmdheader, sizeof(zcr_cmd_header_t)); |
|||
txheader->packetType = kptv2_error_ack; |
|||
zcanreceiver_error_ack_t *error_ack = (zcanreceiver_error_ack_t *)txheader->data; |
|||
error_ack->id = id; |
|||
error_ack->errorcode = errcode; |
|||
sendPacket(txbuff, sizeof(zcr_cmd_header_t) + sizeof(zcanreceiver_error_ack_t)); |
|||
} |
|||
|
|||
void ZCanReceiver::sendExecStatusReport(zcr_cmd_header_t *rxcmdheader, uint8_t *data, size_t len) { |
|||
zlock_guard l(m_lock); |
|||
|
|||
zcr_cmd_header_t *txheader = (zcr_cmd_header_t *)txbuff; |
|||
memcpy(txheader, rxcmdheader, sizeof(zcr_cmd_header_t)); |
|||
txheader->packetType = kptv2_cmd_exec_status_report; |
|||
memcpy(txheader->data, data, len); |
|||
sendPacket(txbuff, sizeof(zcr_cmd_header_t) + len); |
|||
} |
|||
void ZCanReceiver::sendStatusReport(zcr_cmd_header_t *rxcmdheader, uint8_t *data, size_t len) { |
|||
zlock_guard l(m_lock); |
|||
|
|||
zcr_cmd_header_t *txheader = (zcr_cmd_header_t *)txbuff; |
|||
memcpy(txheader, rxcmdheader, sizeof(zcr_cmd_header_t)); |
|||
txheader->packetType = kptv2_report; |
|||
memcpy(txheader->data, data, len); |
|||
sendPacket(txbuff, sizeof(zcr_cmd_header_t) + len); |
|||
} |
|||
#endif
|
|||
int32_t ZCanReceiver::sendBufAck(zcr_cmd_header_t *rx_cmd_header, uint8_t *data, int32_t len) { |
|||
zlock_guard l(m_lock); |
|||
ZASSERT(sizeof(txbuff) > sizeof(zcr_cmd_header_t) + len); |
|||
zcr_cmd_header_t *txheader = (zcr_cmd_header_t *)txbuff; |
|||
memcpy(txheader, rx_cmd_header, sizeof(zcr_cmd_header_t)); |
|||
txheader->packetType = kptv2_ack; |
|||
memcpy(txheader->data, data, len); |
|||
sendPacket(txbuff, sizeof(zcr_cmd_header_t) + len); |
|||
return 0; |
|||
} |
|||
int32_t ZCanReceiver::triggerEvent(zcr_cmd_header_t *cmd_header, uint8_t *data, int32_t len) { |
|||
zlock_guard l(m_lock); |
|||
m_reportIndex++; |
|||
ZASSERT(sizeof(txbuff) > sizeof(zcr_cmd_header_t) + len); |
|||
zcr_cmd_header_t *txheader = (zcr_cmd_header_t *)txbuff; |
|||
memcpy(txheader, cmd_header, sizeof(zcr_cmd_header_t)); |
|||
txheader->packetType = kptv2_event; |
|||
txheader->packetindex = m_reportIndex; |
|||
memcpy(txheader->data, data, len); |
|||
sendPacket(txbuff, sizeof(zcr_cmd_header_t) + len); |
|||
return 0; |
|||
} |
|||
|
|||
int32_t ZCanReceiver::sendAck(zcr_cmd_header_t *rx_cmd_header, int32_t *ackvar, int32_t nack) { |
|||
zlock_guard l(m_lock); |
|||
zcr_cmd_header_t *txheader = (zcr_cmd_header_t *)txbuff; |
|||
memcpy(txheader, rx_cmd_header, sizeof(zcr_cmd_header_t)); |
|||
txheader->packetType = kptv2_ack; |
|||
|
|||
int32_t *txackcache = (int32_t *)txheader->data; |
|||
for (int i = 0; i < nack; i++) { |
|||
txackcache[i] = ackvar[i]; |
|||
} |
|||
|
|||
sendPacket(txbuff, sizeof(zcr_cmd_header_t) + sizeof(int32_t) * nack); |
|||
return 0; |
|||
} |
|||
int32_t ZCanReceiver::sendErrorAck(zcr_cmd_header_t *rx_cmd_header, int32_t errorcode) { |
|||
zlock_guard l(m_lock); |
|||
zcr_cmd_header_t *txheader = (zcr_cmd_header_t *)txbuff; |
|||
memcpy(txheader, rx_cmd_header, sizeof(zcr_cmd_header_t)); |
|||
txheader->packetType = kptv2_error_ack; |
|||
|
|||
int32_t *txackcache = (int32_t *)txheader->data; |
|||
txackcache[0] = errorcode; |
|||
|
|||
sendPacket(txbuff, sizeof(zcr_cmd_header_t) + sizeof(int32_t)); |
|||
return 0; |
|||
} |
|||
|
|||
bool ZCanReceiver::sendPacketSub(int npacket, int packetIndex, uint8_t *packet, size_t len, int overtimems) { |
|||
zlock_guard l(m_lock); |
|||
|
|||
// ZLOGI(TAG, "sendPacketSub(%d:%d)", npacket, packetIndex);
|
|||
CAN_TxHeaderTypeDef pHeader; |
|||
uint8_t aData[8] /*8byte table*/; |
|||
uint32_t txMailBox = 0; |
|||
|
|||
uint32_t enterticket = zos_get_tick(); |
|||
|
|||
memset(&pHeader, 0, sizeof(pHeader)); |
|||
memset(aData, 0, sizeof(aData)); |
|||
pHeader.StdId = 0x00; |
|||
pHeader.ExtId = (m_config->deviceId << 16) | (npacket << 8) | packetIndex; |
|||
pHeader.IDE = CAN_ID_EXT; |
|||
pHeader.RTR = CAN_RTR_DATA; |
|||
pHeader.DLC = len; |
|||
pHeader.TransmitGlobalTime = DISABLE; |
|||
|
|||
memcpy(aData, packet, len); |
|||
|
|||
m_lastTransmitStatus = HAL_CAN_AddTxMessage(m_config->canHandle, &pHeader, aData, &txMailBox); |
|||
if (m_lastTransmitStatus != HAL_OK) { |
|||
ZLOGE(TAG, "HAL_CAN_AddTxMessage fail"); |
|||
return false; |
|||
} |
|||
|
|||
while (HAL_CAN_IsTxMessagePending(m_config->canHandle, txMailBox)) { |
|||
if (zos_haspassedms(enterticket) > (uint32_t)overtimems) { |
|||
m_lastTransmitStatus = HAL_TIMEOUT; |
|||
HAL_CAN_AbortTxRequest(m_config->canHandle, txMailBox); |
|||
return false; |
|||
} |
|||
// m_os->sleepMS(1);
|
|||
} |
|||
if (txPacketInterval_ms > 0) { |
|||
osDelay(txPacketInterval_ms); |
|||
} |
|||
return true; |
|||
} |
|||
|
|||
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 != frameId) { |
|||
rxbuf->m_canPacketNum = 0; |
|||
continue; |
|||
} |
|||
|
|||
if (rxbuf->m_canPacketNum < ZARRAY_SIZE(rxbuf->m_canPacket)) { |
|||
rxbuf->m_canPacket[rxbuf->m_canPacketNum].dlc = pHeader.DLC; |
|||
memcpy(rxbuf->m_canPacket[rxbuf->m_canPacketNum].aData, aData, 8); |
|||
rxbuf->m_canPacketNum++; |
|||
} |
|||
if (nframe == frameId + 1) { |
|||
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(rxbuf->rxdata + dataoff, rxbuf->m_canPacket[i].aData, rxbuf->m_canPacket[i].dlc); |
|||
dataoff += rxbuf->m_canPacket[i].dlc; |
|||
} |
|||
rxbuf->rxdataSize = dataoff; |
|||
// ZLOGI(TAG,"rx packet");
|
|||
for (auto &var : m_listenerList) { |
|||
if (var) var->onRceivePacket(rxbuf->get_cmdheader(), rxbuf->get_params(), rxbuf->get_params_len()); |
|||
} |
|||
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
|
|||
@ -1,92 +0,0 @@ |
|||
//
|
|||
// Created by zwsd
|
|||
//
|
|||
|
|||
#pragma once
|
|||
#include "basic.hpp"
|
|||
#include <list>
|
|||
|
|||
#ifdef HAL_CAN_MODULE_ENABLED
|
|||
namespace iflytop { |
|||
using namespace zcr; |
|||
using namespace std; |
|||
|
|||
|
|||
typedef function<void(CanPacketRxBuffer *rxcmd)> zcanreceiver_listener_t; |
|||
|
|||
class ZCanReceiver : public IZCanReceiver { |
|||
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 txbuff[2100]; |
|||
|
|||
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<IZCanReceiverListener *> m_listenerList; |
|||
CanPacketRxBuffer m_canPacketRxBuffer[1]; |
|||
|
|||
int txPacketInterval_ms = 0; |
|||
zmutex m_lock; |
|||
|
|||
int32_t m_reportIndex = 0; |
|||
|
|||
public: |
|||
ZCanReceiver() {} |
|||
CFG *createCFG(uint8_t deviceId); |
|||
void initialize(CFG *cfg); |
|||
|
|||
#if 0
|
|||
void sendExecStatusReport(zcr_cmd_header_t *rxcmdheader, uint8_t *data, size_t len); |
|||
void sendStatusReport(zcr_cmd_header_t *rxcmdheader, uint8_t *data, size_t len); |
|||
void sendAck(zcr_cmd_header_t *rxcmdheader, uint8_t *data, size_t len); |
|||
void sendErrorAck(zcr_cmd_header_t *cmdheader, uint16_t id, uint32_t errcode); |
|||
#endif
|
|||
|
|||
uint8_t getDeviceId() { return m_config->deviceId; } |
|||
void setTxPacketInterval(int interval_ms) { txPacketInterval_ms = interval_ms; } |
|||
|
|||
virtual void registerListener(IZCanReceiverListener *listener) override; |
|||
virtual int32_t sendBufAck(zcr_cmd_header_t *rx_cmd_header, uint8_t *data, int32_t len) override; |
|||
virtual int32_t sendAck(zcr_cmd_header_t *rx_cmd_header, int32_t *ackvar, int32_t nack) override; |
|||
virtual int32_t sendErrorAck(zcr_cmd_header_t *rx_cmd_header, int32_t errorcode) override; |
|||
virtual int32_t triggerEvent(zcr_cmd_header_t *cmd_header, uint8_t *data, int32_t len) override; |
|||
|
|||
void loop(); |
|||
|
|||
public: |
|||
void STM32_HAL_onCAN_RxFifo0MsgPending(CAN_HandleTypeDef *can); |
|||
void STM32_HAL_onCAN_Error(CAN_HandleTypeDef *can); |
|||
|
|||
private: |
|||
void sendPacket(uint8_t *packet, size_t len); |
|||
bool sendPacketSub(int npacket, int packetIndex, uint8_t *packet, size_t len, int overtimems); |
|||
|
|||
HAL_StatusTypeDef initializeFilter(); |
|||
HAL_StatusTypeDef activateRxIT(); |
|||
HAL_StatusTypeDef deactivateRxIT(); |
|||
bool getRxMessage(CAN_RxHeaderTypeDef *pHeader, uint8_t aData[] /*8byte table*/); |
|||
}; |
|||
|
|||
} // namespace iflytop
|
|||
#endif
|
|||
@ -1,53 +0,0 @@ |
|||
#include "ext_board_impl.hpp"
|
|||
using namespace iflytop; |
|||
|
|||
ExtBoardImpl::ExtBoardImpl(/* args */) {} |
|||
ExtBoardImpl::~ExtBoardImpl() {} |
|||
|
|||
int32_t ExtBoardImpl::getmoduleId(int off) { return zdevice_id_mgr_get_device_id() + off; } |
|||
/***********************************************************************************************************************
|
|||
* PRI * |
|||
***********************************************************************************************************************/ |
|||
int32_t ExtBoardImpl::getid(int32_t *id) { |
|||
*id = getmoduleId(0); |
|||
return 0; |
|||
} |
|||
int32_t ExtBoardImpl::module_xxx_reg(int32_t param_id, bool read, int32_t &val) { return err::kmodule_not_find_reg; } |
|||
int32_t ExtBoardImpl::extboard_read_inio(int32_t ioindex, int32_t *val) { |
|||
if (ioindex < 0 || ioindex >= ZARRAY_SIZE(IO)) return err::kparam_out_of_range; |
|||
*val = IO[ioindex].getState(); |
|||
return 0; |
|||
} |
|||
|
|||
int32_t ExtBoardImpl::extboard_read_muti_inio(int32_t *val) { |
|||
*val = 0; |
|||
for (int i = 0; i < ZARRAY_SIZE(IO); i++) { |
|||
*val |= IO[i].getState() << i; |
|||
// ZLOGI("BOARD", "IO %d %d", i, IO[i].getState());
|
|||
} |
|||
|
|||
return 0; |
|||
} |
|||
int32_t ExtBoardImpl::extboard_read_inio_index_in_stm32(int32_t ioindex, int32_t *val) { |
|||
if (ioindex < 0 || ioindex >= ZARRAY_SIZE(IO)) return err::kparam_out_of_range; |
|||
*val = IO[ioindex].getPin(); |
|||
return 0; |
|||
} |
|||
|
|||
|
|||
int32_t ExtBoardImpl::kextboard_write_outio(int32_t ioindex, int32_t val) { |
|||
if (ioindex < 0 || ioindex >= ZARRAY_SIZE(IO)) return err::kparam_out_of_range; |
|||
OutputIO[ioindex].setState(val); |
|||
return 0; |
|||
} |
|||
int32_t ExtBoardImpl::extboard_read_outio_index_in_stm32(int32_t ioindex, int32_t *val) { |
|||
if (ioindex < 0 || ioindex >= ZARRAY_SIZE(IO)) return err::kparam_out_of_range; |
|||
*val = OutputIO[ioindex].getPin(); |
|||
return 0; |
|||
} |
|||
int32_t ExtBoardImpl::extboard_read_outio(int32_t ioindex, int32_t *val) { |
|||
// ZLOGI("BOARD", "extboard_read_outio %d", ioindex);
|
|||
if (ioindex < 0 || ioindex >= ZARRAY_SIZE(IO)) return err::kparam_out_of_range; |
|||
*val = OutputIO[ioindex].getState(); |
|||
return 0; |
|||
} |
|||
@ -1,68 +0,0 @@ |
|||
#include <stddef.h>
|
|||
#include <stdint.h>
|
|||
#include <stdio.h>
|
|||
|
|||
//
|
|||
#include "protocol/protocol.hpp"
|
|||
#include "sdk/sdk.hpp"
|
|||
//
|
|||
#include "configs/device_id_mgr.hpp"
|
|||
|
|||
#define EXT_READ_IO_NUM 10
|
|||
#define EXT_WRITE_IO_NUM 10
|
|||
|
|||
#define IO_INIT() \
|
|||
IO[0].initAsInput(EXT_INPUT_IO0); \ |
|||
IO[1].initAsInput(EXT_INPUT_IO1); \ |
|||
IO[2].initAsInput(EXT_INPUT_IO2); \ |
|||
IO[3].initAsInput(EXT_INPUT_IO3); \ |
|||
IO[4].initAsInput(EXT_INPUT_IO4); \ |
|||
IO[5].initAsInput(EXT_INPUT_IO5); \ |
|||
IO[6].initAsInput(EXT_INPUT_IO6); \ |
|||
IO[7].initAsInput(EXT_INPUT_IO7); \ |
|||
IO[8].initAsInput(EXT_INPUT_IO8); \ |
|||
IO[9].initAsInput(EXT_INPUT_IO9); \ |
|||
OutputIO[0].initAsOutput(EXT_OUTPUT_IO0); \ |
|||
OutputIO[1].initAsOutput(EXT_OUTPUT_IO1); \ |
|||
OutputIO[2].initAsOutput(EXT_OUTPUT_IO2); \ |
|||
OutputIO[3].initAsOutput(EXT_OUTPUT_IO3); \ |
|||
OutputIO[4].initAsOutput(EXT_OUTPUT_IO4); \ |
|||
OutputIO[5].initAsOutput(EXT_OUTPUT_IO5); \ |
|||
OutputIO[6].initAsOutput(EXT_OUTPUT_IO6); \ |
|||
OutputIO[7].initAsOutput(EXT_OUTPUT_IO7); \ |
|||
OutputIO[8].initAsOutput(EXT_OUTPUT_IO8); \ |
|||
OutputIO[9].initAsOutput(EXT_OUTPUT_IO9); |
|||
|
|||
namespace iflytop { |
|||
class ExtBoardImpl : public ZIBoard, public ZIModule { |
|||
ENABLE_MODULE(ExtBoardImpl, kboard, PC_VERSION); |
|||
|
|||
protected: |
|||
/* data */ |
|||
ZGPIO IO[EXT_READ_IO_NUM]; |
|||
ZGPIO OutputIO[EXT_WRITE_IO_NUM]; |
|||
|
|||
public: |
|||
ExtBoardImpl(/* args */); |
|||
~ExtBoardImpl(); |
|||
|
|||
public: |
|||
/***********************************************************************************************************************
|
|||
* ZIModule * |
|||
***********************************************************************************************************************/ |
|||
virtual int32_t getid(int32_t *id) override; |
|||
virtual int32_t module_xxx_reg(int32_t param_id, bool read, int32_t &val) override; |
|||
/***********************************************************************************************************************
|
|||
* ZIBoard * |
|||
***********************************************************************************************************************/ |
|||
virtual int32_t extboard_read_inio(int32_t ioindex, int32_t *val) override; |
|||
virtual int32_t kextboard_write_outio(int32_t ioindex, int32_t val) override; |
|||
virtual int32_t extboard_read_muti_inio(int32_t *val) override; |
|||
virtual int32_t extboard_read_inio_index_in_stm32(int32_t ioindex, int32_t *val) override; |
|||
virtual int32_t extboard_read_outio_index_in_stm32(int32_t ioindex, int32_t *val) override; |
|||
virtual int32_t extboard_read_outio(int32_t ioindex, int32_t *val) override; |
|||
|
|||
virtual int32_t getmoduleId(int off); |
|||
}; |
|||
|
|||
} // namespace iflytop
|
|||
@ -1,18 +0,0 @@ |
|||
#include "gservice.hpp"
|
|||
|
|||
#include "configs/device_id_mgr.hpp"
|
|||
|
|||
using namespace iflytop; |
|||
|
|||
void GService::initialize() { //
|
|||
|
|||
ZCanReceiver::CFG* zcanreceiver_cfg = zcanreceiver.createCFG(zdevice_id_mgr_get_device_id()); |
|||
zcanreceiver.initialize(zcanreceiver_cfg); |
|||
zcan_protocol_parser.initialize(&zcanreceiver); |
|||
ProtocolEventBusSender::inst()->initialize(&zcanreceiver); |
|||
} |
|||
|
|||
GService* GService::inst() { |
|||
static GService gservice; |
|||
return &gservice; |
|||
} |
|||
@ -1,34 +0,0 @@ |
|||
#include <stddef.h>
|
|||
#include <stdio.h>
|
|||
|
|||
#include "sdk/sdk.hpp"
|
|||
#include "protocol/protocol.hpp"
|
|||
//
|
|||
#include "components/zcancmder/zcanreceiver.hpp"
|
|||
#include "components/zcancmder/zcan_protocol_parser.hpp"
|
|||
#include "components/zcancmder/protocol_event_bus_sender.hpp"
|
|||
|
|||
namespace iflytop { |
|||
|
|||
class GService { |
|||
ZCanReceiver zcanreceiver; |
|||
ZCanProtocolParser zcan_protocol_parser; |
|||
ProtocolEventBusSender evenSender; |
|||
|
|||
public: |
|||
void initialize(); |
|||
|
|||
static GService* inst(); |
|||
|
|||
ZCanReceiver* getZCanReceiver() { return &zcanreceiver; } |
|||
ZCanProtocolParser* getZCanProtocolParser() { return &zcan_protocol_parser; } |
|||
|
|||
void registerModule(ZIModule* module) { zcan_protocol_parser.registerModule(module); } |
|||
|
|||
template <typename T> |
|||
T* getModule(uint32_t moduleId) { |
|||
return dynamic_cast<T*>(zcan_protocol_parser.getModule(moduleId)); |
|||
} |
|||
}; |
|||
|
|||
} // namespace iflytop
|
|||
@ -1,6 +1,4 @@ |
|||
#pragma once
|
|||
|
|||
#include "common_hardware_init.hpp"
|
|||
#include "gservice.hpp"
|
|||
#include "ext_board_impl.hpp"
|
|||
#include "sdk/sdk.hpp"
|
|||
Write
Preview
Loading…
Cancel
Save
Reference in new issue