From f7db3594b5cf455e3918324484f973061676bcf8 Mon Sep 17 00:00:00 2001 From: zhaohe Date: Sat, 27 Apr 2024 20:09:51 +0800 Subject: [PATCH] update --- iflytop_canbus_protocol | 2 +- usrc/protocol_impl/protocol_impl_service.cpp | 189 +++++++++++++++++++++++---- usrc/protocol_impl/protocol_impl_service.hpp | 3 - usrc/protocol_impl/report_flag_mgr.cpp | 16 +++ usrc/protocol_impl/report_flag_mgr.hpp | 32 +++++ zsdk | 2 +- 6 files changed, 217 insertions(+), 27 deletions(-) create mode 100644 usrc/protocol_impl/report_flag_mgr.cpp create mode 100644 usrc/protocol_impl/report_flag_mgr.hpp diff --git a/iflytop_canbus_protocol b/iflytop_canbus_protocol index 53b6f92..7cf6477 160000 --- a/iflytop_canbus_protocol +++ b/iflytop_canbus_protocol @@ -1 +1 @@ -Subproject commit 53b6f92696f607082404448c9f03d39a754c084c +Subproject commit 7cf64770e1e06e66488e778a994d128870852a37 diff --git a/usrc/protocol_impl/protocol_impl_service.cpp b/usrc/protocol_impl/protocol_impl_service.cpp index 3c5e3c4..9891d65 100644 --- a/usrc/protocol_impl/protocol_impl_service.cpp +++ b/usrc/protocol_impl/protocol_impl_service.cpp @@ -1,6 +1,9 @@ #include "protocol_impl_service.hpp" #include "base/device_info.hpp" +#include "report_flag_mgr.hpp" +#include "spi.h" +#include "zsdk/tmc/ztmc5130.hpp" #include "zsdk/zcanreceiver/zcanreceiver.hpp" #define TAG "PROTO" using namespace iflytop; @@ -8,6 +11,18 @@ using namespace iflytop; * FUNCTION_LIST * ***********************************************************************************************************************/ +#define CHECK_PARAM_LEN(_paramNum, expectNum) \ + if (_paramNum != expectNum) { \ + zcanbus_send_errorack(packet, kerr_invalid_param_num); \ + return; \ + } + +#define CHECK_MOTOR_INDEX(_subindex) \ + if (_subindex > ZARRAY_SIZE(m_motor)) { \ + zcanbus_send_errorack(packet, kerr_invalid_param); \ + return; \ + } + /*********************************************************************************************************************** * VAR_LIST * ***********************************************************************************************************************/ @@ -15,18 +30,19 @@ using namespace iflytop; static osTimerId HeartReportTimerId; static osThreadId PacketRxThreadId; +static ZSPI m_motor_spi; +static TMC5130 m_motor[2]; + /*********************************************************************************************************************** * FUNCTION_IMPL * ***********************************************************************************************************************/ #define GET_PARAM(buff, off) ((((int32_t*)(buff))[off])) -static void zcanbus_on_rx(uint8_t from, uint8_t to, uint8_t* rawpacket, size_t len) { // - zcanbus_packet_t* packet = (zcanbus_packet_t*)rawpacket; - ZLOGI(TAG, "rx packet from %d to %d, function_id %d, len %d", from, to, packet->function_id, len); - /*********************************************************************************************************************** - * 系统方法 * - ***********************************************************************************************************************/ +static void basic_func_impl(uint8_t from, uint8_t to, uint8_t* rawpacket, size_t len) { + zcanbus_packet_t* packet = (zcanbus_packet_t*)rawpacket; + int32_t paramLen = (len - sizeof(zcanbus_packet_t)) / 4; + // 读板子信息 if (packet->function_id == kcmd_read_board_info) { static ack_read_board_info_data_t ack; @@ -61,48 +77,177 @@ static void zcanbus_on_rx(uint8_t from, uint8_t to, uint8_t* rawpacket, size_t l zcanbus_send_ack(packet, (uint8_t*)&ack, sizeof(ack)); } - + // 心跳 ping pong else if (packet->function_id == kcmd_heart_ping) { static report_heatpacket_data_t heatpacket; heatpacket.boardType = deviceInfo_getBoardType(); heatpacket.heartIndex = GET_PARAM(packet->params, 0); - zcanbus_send_report(kreport_heatpacket, (uint8_t*)&heatpacket, sizeof(heatpacket)); + zcanbus_send_report(kreport_heatpacket_pong, (uint8_t*)&heatpacket, sizeof(heatpacket)); } - /*********************************************************************************************************************** - * 应用实现 * - ***********************************************************************************************************************/ // 触发一次强制上报事件 - else if (packet->function_id == kcmd_force_report) { + if (packet->function_id == kcmd_force_report) { + ForceReportFlagMgr::ins()->trigger(); + if (from != 0xff) zcanbus_send_ack(packet, NULL, 0); } } -static void zcanbus_on_connected(bool connected) { - if (connected) { - ZLOGI(TAG, "connected to host"); - } else { - ZLOGI(TAG, "disconnected from host"); +static void pump_func_impl(uint8_t from, uint8_t to, uint8_t* rawpacket, size_t len) { + zcanbus_packet_t* packet = (zcanbus_packet_t*)rawpacket; + int32_t paramNum = (len - sizeof(zcanbus_packet_t)) / 4; + + // 泵机转动 + if (packet->function_id == kcmd_pump_rotate) { + CHECK_PARAM_LEN(paramNum, 2); + + int32_t subindex = GET_PARAM(packet->params, 0); + int32_t velocity = GET_PARAM(packet->params, 1); + + if (subindex > ZARRAY_SIZE(m_motor)) { + zcanbus_send_errorack(packet, kerr_invalid_param); + return; + } + + m_motor[subindex].rotate(GET_PARAM(packet->params, velocity)); + zcanbus_send_ack(packet, NULL, 0); + } + + // + else if (packet->function_id == kcmd_pump_stop) { + CHECK_PARAM_LEN(paramNum, 1); + + int32_t subindex = GET_PARAM(packet->params, 0); + + if (subindex > ZARRAY_SIZE(m_motor)) { + zcanbus_send_errorack(packet, kerr_invalid_param); + return; + } + + m_motor[subindex].stop(); + zcanbus_send_ack(packet, NULL, 0); + } + + // 设置电流/保持电流/IDELAY + else if (packet->function_id == kcmd_pump_set_irun_ihold) { + CHECK_PARAM_LEN(paramNum, 4); + + int32_t subindex = GET_PARAM(packet->params, 0); + int32_t irun = GET_PARAM(packet->params, 1); + int32_t ihold = GET_PARAM(packet->params, 2); + int32_t idelay = GET_PARAM(packet->params, 3); + + if (subindex > ZARRAY_SIZE(m_motor)) { + zcanbus_send_errorack(packet, kerr_invalid_param); + return; + } + + m_motor[subindex].setIHOLD_IRUN(irun, ihold, idelay); + zcanbus_send_ack(packet, NULL, 0); + } else if (packet->function_id == kcmd_pump_set_acc) { + CHECK_PARAM_LEN(paramNum, 2); + + int32_t subindex = GET_PARAM(packet->params, 0); + int32_t acc = GET_PARAM(packet->params, 1); + + if (subindex > ZARRAY_SIZE(m_motor)) { + zcanbus_send_errorack(packet, kerr_invalid_param); + return; + } + + m_motor[subindex].setAcceleration(acc); + m_motor[subindex].setDeceleration(acc); + zcanbus_send_ack(packet, NULL, 0); + + } + + // 设置5130寄存器 + else if (packet->function_id == kcmd_pump_set_subic_reg) { + CHECK_PARAM_LEN(paramNum, 3); + + int32_t subindex = GET_PARAM(packet->params, 0); + int32_t regadd = GET_PARAM(packet->params, 1); + int32_t regval = GET_PARAM(packet->params, 2); + + if (subindex > ZARRAY_SIZE(m_motor)) { + zcanbus_send_errorack(packet, kerr_invalid_param); + return; + } + + m_motor[subindex].writeInt(regadd, regval); + } + + // 读取5130寄存器 + else if (packet->function_id == kcmd_pump_get_subic_reg) { + CHECK_PARAM_LEN(paramNum, 2); + + int32_t subindex = GET_PARAM(packet->params, 0); + int32_t regadd = GET_PARAM(packet->params, 1); + + if (subindex > ZARRAY_SIZE(m_motor)) { + zcanbus_send_errorack(packet, kerr_invalid_param); + return; + } + + int32_t regval = m_motor[subindex].readInt(regadd); + zcanbus_send_ack(packet, (uint8_t*)®val, sizeof(regval)); } } /*********************************************************************************************************************** * SCHEDULER * ***********************************************************************************************************************/ -static void onHeartReportTimer(void const* argument) { - static report_heatpacket_data_t heatpacket; - heatpacket.boardType = deviceInfo_getBoardType(); - // zcanbus_send_report(kreport_heatpacket, (uint8_t*)&heatpacket, sizeof(heatpacket)); -} static void onPacketRxThreadStart(void const* argument) { while (true) { zcanbus_schedule(); osDelay(1); } } +static void zcanbus_on_rx(uint8_t from, uint8_t to, uint8_t* rawpacket, size_t len) { // + zcanbus_packet_t* packet = (zcanbus_packet_t*)rawpacket; + ZLOGI(TAG, "rx packet from %d to %d, function_id %d, len %d", from, to, packet->function_id, len); + + basic_func_impl(from, to, rawpacket, len); + pump_func_impl(from, to, rawpacket, len); +} +static void zcanbus_on_connected(bool connected) { + if (connected) { + ZLOGI(TAG, "connected to host"); + } else { + ZLOGI(TAG, "disconnected from host"); + } +} /*********************************************************************************************************************** * EXT * ***********************************************************************************************************************/ + +void hardware_init() { + m_motor_spi.init(&MOTOR_SPI); + m_motor[0].initialize(&m_motor_spi, MOTOR1_ENN, MOTOR1_CSN); + m_motor[0].setIHOLD_IRUN(1, 20, 0); + m_motor[0].setMotorShaft(true); + m_motor[0].setAcceleration(300000); + m_motor[0].setDeceleration(300000); + + m_motor[1].initialize(&m_motor_spi, MOTOR2_ENN, MOTOR2_CSN); + m_motor[1].setIHOLD_IRUN(1, 20, 0); + m_motor[1].setMotorShaft(true); + m_motor[1].setAcceleration(300000); + m_motor[1].setDeceleration(300000); + + int32_t chipv0 = m_motor[0].readChipVERSION(); // 5130:0x11 + int32_t chipv1 = m_motor[1].readChipVERSION(); // 5130:0x11 + + m_motor[0].rotate(1000000); + m_motor[1].rotate(1000000); + + ZLOGI(TAG, "chipv0: %x, chipv1: %x", chipv0, chipv1); +} + void protocol_impl_service_init() { // + + ForceReportFlagMgr::ins()->init(); + hardware_init(); + zcanbus_init(deviceInfo_getBoardId()); zcanbus_reglistener(zcanbus_on_rx); zcanbus_reg_on_connected_listener(zcanbus_on_connected); diff --git a/usrc/protocol_impl/protocol_impl_service.hpp b/usrc/protocol_impl/protocol_impl_service.hpp index f892d18..2d7419c 100644 --- a/usrc/protocol_impl/protocol_impl_service.hpp +++ b/usrc/protocol_impl/protocol_impl_service.hpp @@ -5,6 +5,3 @@ #include "zsdk/zsdk.hpp" void protocol_impl_service_init(); - -// void zcanbus_on_rx(uint8_t from, uint8_t to, uint8_t *packet, size_t len); -// void zcanbus_on_connected(bool connected); \ No newline at end of file diff --git a/usrc/protocol_impl/report_flag_mgr.cpp b/usrc/protocol_impl/report_flag_mgr.cpp new file mode 100644 index 0000000..ee889d2 --- /dev/null +++ b/usrc/protocol_impl/report_flag_mgr.cpp @@ -0,0 +1,16 @@ +#include "report_flag_mgr.hpp" +using namespace iflytop; + +ForceReportFlagMgr* ForceReportFlagMgr::ins() { + static ForceReportFlagMgr instance; + return &instance; +} + +void ForceReportFlagMgr::init() { + m_lock.init(); + memset(&m_flag, 0, sizeof(m_flag)); +} +void ForceReportFlagMgr::trigger() { + zlock_guard lock(m_lock); + memset(&m_flag, 1, sizeof(m_flag)); +} \ No newline at end of file diff --git a/usrc/protocol_impl/report_flag_mgr.hpp b/usrc/protocol_impl/report_flag_mgr.hpp new file mode 100644 index 0000000..275416b --- /dev/null +++ b/usrc/protocol_impl/report_flag_mgr.hpp @@ -0,0 +1,32 @@ +#include +#include +#include + +#include "base/config_service.hpp" +#include "zsdk/zsdk.hpp" + +namespace iflytop { + +class ForceReportFlagMgr { + private: + typedef struct { + bool pad; + } flag_t; + + zmutex m_lock; + flag_t m_flag; + + public: + static ForceReportFlagMgr* ins(); + + void init(); + + void trigger(); + + void clearPad() { + zlock_guard lock(m_lock); + m_flag.pad = false; + } +}; + +} // namespace iflytop diff --git a/zsdk b/zsdk index 0baf700..a8bd506 160000 --- a/zsdk +++ b/zsdk @@ -1 +1 @@ -Subproject commit 0baf700046417b087f93669a8110f607e8d349d4 +Subproject commit a8bd506724276da87d8a186d8cc0e1f072492355