From 460ee46f4bd2499d95029d5a12681d08ef437c06 Mon Sep 17 00:00:00 2001 From: zhaohe Date: Thu, 23 May 2024 19:47:06 +0800 Subject: [PATCH] update --- .gitmodules | 5 +- .settings/language.settings.xml | 4 +- sdk | 2 +- transmit_disinfection_protocol_v1 | 1 + usrc/hardware.cpp | 106 ++++++++++++++++++++++++++++++-------- usrc/hardware.hpp | 9 ++++ usrc/main.cpp | 29 ++++------- usrc/main.hpp | 3 +- 8 files changed, 112 insertions(+), 47 deletions(-) create mode 160000 transmit_disinfection_protocol_v1 diff --git a/.gitmodules b/.gitmodules index ee2589e..f67f3bb 100644 --- a/.gitmodules +++ b/.gitmodules @@ -1,3 +1,6 @@ [submodule "sdk"] path = sdk - url = zwsd@192.168.1.3:manufacturer_stm32/iflytop_sdk.git + url = zwsd@192.168.1.3:p_transmit_disinfection/transmit_disinfection_iflytop_sdk_v1.git +[submodule "transmit_disinfection_protocol_v1"] + path = transmit_disinfection_protocol_v1 + url = zwsd@192.168.1.3:p_transmit_disinfection/transmit_disinfection_protocol_v1.git diff --git a/.settings/language.settings.xml b/.settings/language.settings.xml index 4ca3985..8c27752 100644 --- a/.settings/language.settings.xml +++ b/.settings/language.settings.xml @@ -5,7 +5,7 @@ - + @@ -16,7 +16,7 @@ - + diff --git a/sdk b/sdk index 15dc8ec..b20f4cc 160000 --- a/sdk +++ b/sdk @@ -1 +1 @@ -Subproject commit 15dc8ec2ad5398c9f0becc6336285f061f5f6da8 +Subproject commit b20f4cc2e42244d6dd21566dd1a4b1a48ddc64a9 diff --git a/transmit_disinfection_protocol_v1 b/transmit_disinfection_protocol_v1 new file mode 160000 index 0000000..c39bd36 --- /dev/null +++ b/transmit_disinfection_protocol_v1 @@ -0,0 +1 @@ +Subproject commit c39bd364d809a9a6ad965c83fd3ea28c67514317 diff --git a/usrc/hardware.cpp b/usrc/hardware.cpp index a1ecbb0..1430bc6 100644 --- a/usrc/hardware.cpp +++ b/usrc/hardware.cpp @@ -5,13 +5,12 @@ #include "main.h" #include "project.hpp" +#include // // #include "sdk/components/single_axis_motor_control_v2/single_axis_motor_control_v2.hpp" -#include "sdk/components/iflytop_can_slave_modules/idcard_reader_service.hpp" -#include "sdk/components/single_axis_motor_control/single_axis_motor_control.hpp" + #include "sdk/hal/zhal.hpp" -#include "sdk\components\iflytop_can_slave_modules\io_control_service.hpp" -#include "sdk\components\iflytop_can_slave_v1\iflytop_can_slave.hpp" + #include "sdk\components\m3078\m3078_code_scaner.hpp" #include "sdk\components\tmc\ic\ztmc4361A.hpp" #include "sdk\components\tmc\ic\ztmc5130.hpp" @@ -68,6 +67,7 @@ void setmotor(TMC5130 *motor, int16_t acc_rpm2, int16_t rpm, int16_t idlepower, motor->setDeceleration(acc); motor->rotate(ppm); } + PreportionalValveCtrl m_PreportionalValveHost; int32_t preportional_valve_is_busy(int32_t *busy) { @@ -96,8 +96,8 @@ void air_compressor_read_pressure(int32_t *ack) { // void Hardware::initialize(int deviceId) { m_device_id = deviceId; - // IO_PD13_IN.initAsInput(PD13, ZGPIO::kMode_nopull, ZGPIO::kIRQ_noIrq, false /*mirror*/); - // IO_PC7_IN.initAsInput(PC7, ZGPIO::kMode_nopull, ZGPIO::kIRQ_noIrq, false /*mirror*/); + IO_PD13_IN.initAsInput(PD13, ZGPIO::kMode_nopull, ZGPIO::kIRQ_noIrq, false /*mirror*/); + IO_PC7_IN.initAsInput(PC7, ZGPIO::kMode_nopull, ZGPIO::kIRQ_noIrq, false /*mirror*/); { TMC5130::cfg_t cfg = {.hspi = &MOTOR_SPI, .enn_pin = MOTOR1_ENN, .csn_pin = MOTOR1_CSN}; @@ -214,7 +214,7 @@ int32_t Hardware::process_rx_packet(from_where_t fromwhere, uint8_t *packet, int printf("kcmd_peristaltic_pump_ctl 1 acc:%d rpm:%d idlepower:%d power:%d\n", acc, rpm, idlepower, power); - setmotor(&m_motor1, acc, rpm, idlepower, power); + setmotor1(acc, rpm, idlepower, power); receipt[0] = cmdheader->data[0]; receiptsize = 1; return 0; @@ -230,7 +230,7 @@ int32_t Hardware::process_rx_packet(from_where_t fromwhere, uint8_t *packet, int int16_t power = cmdheader->data[7]; printf("kcmd_peristaltic_pump_ctl 2 acc:%d rpm:%d idlepower:%d power:%d\n", acc, rpm, idlepower, power); - setmotor(&m_motor2, acc, rpm, idlepower, power); + setmotor2(acc, rpm, idlepower, power); receipt[0] = cmdheader->data[0]; receiptsize = 1; return 0; @@ -290,7 +290,7 @@ int32_t Hardware::process_rx_packet(from_where_t fromwhere, uint8_t *packet, int receiptsize = 4; } - if ((cmdheader->cmdid == (uint16_t)kcmd_set_proportional_valve) && (cmdheader->subcmdid == 0)) { + if ((cmdheader->cmdid == (uint16_t)kcmd_proportional_set_valve) && (cmdheader->subcmdid == 0)) { int32_t para0 = *(int32_t *)(&cmdheader->data[0]); int32_t para1 = *(int32_t *)(&cmdheader->data[4]); @@ -317,25 +317,21 @@ int32_t Hardware::process_rx_packet(from_where_t fromwhere, uint8_t *packet, int receiptsize = 4; } -#if 0 - /** - * @brief 获取蒸发仓水浸状态 - */ - PROCESS_CMD(kcmd_proportional_read_water_immersion_sensor, 0, 0) { - ((int32_t *)receipt)[0] = !IO_PC7_IN.getState(); +#if 1 + if ((cmdheader->cmdid == (uint16_t)kcmd_read_evaporation_bin_water_detection)) { + matching = true; + + ((int32_t *)receipt)[0] = IO_PC7_IN.getState(); // 高有效 receiptsize = 4; return 0; } - /** - * @brief 获取设备底盘水浸传感器 - */ - PROCESS_CMD(kcmd_proportional_read_water_immersion_sensor, 0, 1) { - ((int32_t *)receipt)[0] = !IO_PD13_IN.getState(); + if ((cmdheader->cmdid == (uint16_t)kcmd_read_device_bottom_water_detection_sensor)) { + matching = true; + ((int32_t *)receipt)[0] = !IO_PD13_IN.getState(); // 低有效 receiptsize = 4; return 0; } - #endif /** @@ -400,4 +396,70 @@ int32_t Hardware::process_rx_packet(from_where_t fromwhere, uint8_t *packet, int void Hardware::loop() {} TMC5130 *Hardware::getMotor1() { return &m_motor1; } -TMC5130 *Hardware::getMotor2() { return &m_motor2; } \ No newline at end of file +TMC5130 *Hardware::getMotor2() { return &m_motor2; } + +typedef struct motor_state_cache_t { + int16_t acc_rpm2; + int16_t rpm; + int16_t idlepower; + int16_t power; +}; + +motor_state_cache_t m_motor1_cache; +motor_state_cache_t m_motor2_cache; + +void Hardware::setmotor1(int16_t acc_rpm2, int16_t rpm, int16_t idlepower, int16_t power) { // + setmotor(&m_motor1, acc_rpm2, rpm, idlepower, power); + + m_motor1_cache.acc_rpm2 = acc_rpm2; + m_motor1_cache.rpm = rpm; + m_motor1_cache.idlepower = idlepower; + m_motor1_cache.power = power; +} +void Hardware::recovermotor1() { + m_motor1.reInitialize(); + setmotor(&m_motor1, m_motor1_cache.acc_rpm2, m_motor1_cache.rpm, m_motor1_cache.idlepower, m_motor1_cache.power); +} +bool Hardware::ismotor1error() { + DevStatusReg_t sreg1 = m_motor1.getDevStatus(); + GState_t gstate1 = m_motor1.getGState(); + + if (gstate1.reset) { + ZLOGE(TAG, "motor1 driver trigger illegal reset"); + return true; + } + + if (sreg1.ola || sreg1.olb) { + ZLOGE(TAG, "motor1 driver trigger ola olb"); + return true; + } + return false; +} + +void Hardware::setmotor2(int16_t acc_rpm2, int16_t rpm, int16_t idlepower, int16_t power) { // + setmotor(&m_motor2, acc_rpm2, rpm, idlepower, power); + + m_motor2_cache.acc_rpm2 = acc_rpm2; + m_motor2_cache.rpm = rpm; + m_motor2_cache.idlepower = idlepower; + m_motor2_cache.power = power; +} +void Hardware::recovermotor2() { + m_motor2.reInitialize(); + setmotor(&m_motor2, m_motor2_cache.acc_rpm2, m_motor2_cache.rpm, m_motor2_cache.idlepower, m_motor2_cache.power); +} +bool Hardware::ismotor2error() { + DevStatusReg_t sreg2 = m_motor2.getDevStatus(); + GState_t gstate2 = m_motor2.getGState(); + + if (gstate2.reset) { + ZLOGE(TAG, "motor2 driver trigger illegal reset"); + return true; + } + + if (sreg2.ola || sreg2.olb) { + ZLOGE(TAG, "motor2 driver trigger ola olb"); + return true; + } + return false; +} diff --git a/usrc/hardware.hpp b/usrc/hardware.hpp index 65e0302..212c8cf 100644 --- a/usrc/hardware.hpp +++ b/usrc/hardware.hpp @@ -1,4 +1,5 @@ #include + #include "sdk\components\tmc\ic\ztmc4361A.hpp" #include "sdk\components\tmc\ic\ztmc5130.hpp" @@ -15,6 +16,14 @@ class Hardware { TMC5130 *getMotor1(); TMC5130 *getMotor2(); + + void setmotor1(int16_t acc_rpm2, int16_t rpm, int16_t idlepower, int16_t power); + void recovermotor1(); + bool ismotor1error(); + + void setmotor2(int16_t acc_rpm2, int16_t rpm, int16_t idlepower, int16_t power); + void recovermotor2(); + bool ismotor2error(); }; } // namespace iflytop diff --git a/usrc/main.cpp b/usrc/main.cpp index 5c7081c..278db91 100644 --- a/usrc/main.cpp +++ b/usrc/main.cpp @@ -2,16 +2,14 @@ #include #include - +#include #include "main.h" #include "project.hpp" // // #include "sdk/components/single_axis_motor_control_v2/single_axis_motor_control_v2.hpp" -#include "sdk/components/iflytop_can_slave_modules/idcard_reader_service.hpp" -#include "sdk/components/single_axis_motor_control/single_axis_motor_control.hpp" + #include "sdk/hal/zhal.hpp" -#include "sdk\components\iflytop_can_slave_modules\io_control_service.hpp" -#include "sdk\components\iflytop_can_slave_v1\iflytop_can_slave.hpp" + #include "sdk\components\m3078\m3078_code_scaner.hpp" #include "sdk\components\tmc\ic\ztmc4361A.hpp" #include "sdk\components\tmc\ic\ztmc5130.hpp" @@ -42,7 +40,7 @@ void dumphexdata(uint8_t *data, int32_t len) { * GLOBAL * *******************************************************************************/ -IflytopCanProtocolStackProcesser m_protocolStack; + Hardware m_hardware; ZGPIO debuglight; ZCanReceiver m_canReceiver; @@ -150,20 +148,13 @@ void Main::run() { ZHALCORE::getInstance()->regPeriodJob( [this](ZHALCORE::Context &context) { // - auto *m1 = m_hardware.getMotor1(); - auto *m2 = m_hardware.getMotor2(); - - DevStatusReg_t sreg1 = m1->getDevStatus(); - GState_t gstate1 = m1->getGState(); - DevStatusReg_t sreg2 = m2->getDevStatus(); - GState_t gstate2 = m2->getGState(); - - // ZLOGI(TAG, "m1: stst:%d olb:%d ola:%d s2gb:%d s2ga:%d otpw:%d ot:%d stallguard:%d cs_actual:%d fsactive:%d sg_result:%d", sreg1.stst, sreg1.olb, - // sreg1.ola, sreg1.s2gb, sreg1.s2ga, sreg1.otpw, sreg1.ot, sreg1.stallguard, sreg1.cs_actual, sreg1.fsactive, sreg1.sg_result); + if (m_hardware.ismotor1error()) { + m_hardware.recovermotor1(); + } - ZLOGI(TAG, "m2: stst:%d olb:%d ola:%d s2gb:%d s2ga:%d otpw:%d ot:%d stallguard:%d cs_actual:%d fsactive:%d sg_result:%d", sreg2.stst, sreg2.olb, - sreg2.ola, sreg2.s2gb, sreg2.s2ga, sreg2.otpw, sreg2.ot, sreg2.stallguard, sreg2.cs_actual, sreg2.fsactive, sreg2.sg_result); - ZLOGI(TAG, "m2: reset:%d drv_err:%d uv_cp:%d", gstate2.reset, gstate2.drv_err, gstate2.uv_cp); + if (m_hardware.ismotor2error()) { + m_hardware.recovermotor2(); + } }, 1000); diff --git a/usrc/main.hpp b/usrc/main.hpp index 5767d46..0f39f0e 100644 --- a/usrc/main.hpp +++ b/usrc/main.hpp @@ -4,8 +4,7 @@ #include -#include "sdk\components\iflytop_can_slave_modules\device_base_control_service.hpp" -#include "sdk\components\iflytop_can_slave_v1\iflytop_can_slave.hpp" + #include "sdk\components\zcanreceiver\zcanreceiver.hpp" namespace iflytop { using namespace std;