From a70210f24904212f7a5332213694e43aead6b11f Mon Sep 17 00:00:00 2001 From: zhaohe Date: Wed, 4 Oct 2023 20:02:48 +0800 Subject: [PATCH] update --- components/pipette_module/pipette_ctrl_module.cpp | 107 ++++++++----------- components/pipette_module/pipette_ctrl_module.hpp | 14 ++- components/sensors/smtp2/smtp2.cpp | 34 +++++- components/sensors/smtp2/smtp2.hpp | 9 +- .../step_motor_ctrl_module.cpp | 114 +++++++++------------ .../step_motor_ctrl_module.hpp | 20 ++-- .../zcan_step_motor_ctrl_module.cpp | 44 ++++---- components/zprotocols/errorcode | 2 +- components/zprotocols/zcancmder | 2 +- os/zthread.cpp | 10 +- os/zthread.hpp | 2 + 11 files changed, 179 insertions(+), 179 deletions(-) diff --git a/components/pipette_module/pipette_ctrl_module.cpp b/components/pipette_module/pipette_ctrl_module.cpp index a246d49..325861b 100644 --- a/components/pipette_module/pipette_ctrl_module.cpp +++ b/components/pipette_module/pipette_ctrl_module.cpp @@ -19,97 +19,74 @@ int32_t PipetteModule::stop(u8 stop_type) { return 0; } -int32_t PipetteModule::zero_pos_calibrate(function exec_complete_cb) { +int32_t PipetteModule::zero_pos_calibrate(action_cb_status_t status_cb) { m_thread.stop(); - m_thread.start([this, exec_complete_cb]() { - action_cb_status_t action_status = {0}; - - int32_t exec_ret = m_stepMotor->move_to_zero_with_calibrate(0, [&](I_StepMotorCtrlModule::move_to_zero_with_calibrate_cb_status_t &status) { - action_status.exec_status = status.exec_status; - if (exec_complete_cb) exec_complete_cb(action_status); + m_thread.start([this, status_cb]() { + int32_t exec_ret = do_motor_action_block([this]() { // + return m_stepMotor->move_to_zero_with_calibrate(0, nullptr); }); - if (exec_ret != 0) { ZLOGE(TAG, "move_to_zero_with_calibrate fail, exec_ret = %d", exec_ret); - m_stepMotor->stop(0); - action_status.exec_status = exec_ret; - if (exec_complete_cb) exec_complete_cb(action_status); + m_lastexecstatus = exec_ret; + if (status_cb) status_cb(exec_ret); return; } - - while (!m_thread.getExitFlag()) { - ZLOGI(TAG, "Waiting for SMTP2 to complete the reset"); - if (!m_stepMotor->isbusy()) break; - m_thread.sleep(1000); - } - ZLOGI(TAG, "zero_pos_calibrate complete"); - m_stepMotor->stop(0); }); return 0; } -int32_t PipetteModule::reset_device(function exec_complete_cb) { +int32_t PipetteModule::reset_device(action_cb_status_t status_cb) { m_thread.stop(); - m_thread.start([this, exec_complete_cb]() { - action_cb_status_t report_status = {0}; - + m_thread.start([this, status_cb]() { /******************************************************************************* * m_smtp2 * *******************************************************************************/ // 移液枪复位 - int ret = m_smtp2->init_device(); + int ret = m_smtp2->doaction_block(&m_thread, [this]() { return m_smtp2->init_device(); }); if (ret != 0) { - ZLOGE(TAG, "init_device fail"); - report_status.exec_status = ret; - if (exec_complete_cb) exec_complete_cb(report_status); - return; - } - - // 等待移液枪复位完成 - while (!m_thread.getExitFlag()) { - int32_t state = m_smtp2->getState(); - if (state == 0) { - break; - } - m_thread.sleep(1000); - ZLOGI(TAG, "Waiting for SMTP2 to complete the reset"); - } - m_smtp2->stop(); - - ZLOGI(TAG, "SMTP2 reset complete"); - if (m_thread.getExitFlag()) { - ZLOGW(TAG, "break reset_device"); + ZLOGE(TAG, "init_device fail,%d", ret); + m_lastexecstatus = ret; + if (status_cb) status_cb(ret); return; } + ZLOGI(TAG, "SMTP2 init_device complete"); /******************************************************************************* * m_stepMotor * *******************************************************************************/ - ret = m_stepMotor->move_to_zero_with_calibrate(0, [&](I_StepMotorCtrlModule::move_to_zero_with_calibrate_cb_status_t &status) { // - ret = status.exec_status; - }); - - while (!m_thread.getExitFlag()) { - if (!m_stepMotor->isbusy()) break; - m_thread.sleep(1000); - ZLOGI(TAG, "Waiting for Z axis to complete the reset"); - } - m_stepMotor->stop(0); - if (m_thread.getExitFlag()) { - ZLOGW(TAG, "break reset_device"); - return; - } + ret = do_motor_action_block( // + [this]() { return m_stepMotor->move_to_zero(nullptr); }); if (ret != 0) { - ZLOGE(TAG, "move_to_zero_with_calibrate fail, ret = %d", ret); - report_status.exec_status = ret; - if (exec_complete_cb) exec_complete_cb(report_status); + ZLOGE(TAG, "move_to_zero fail, ret = %d", ret); + m_lastexecstatus = ret; + if (status_cb) status_cb(m_lastexecstatus); return; } - ZLOGI(TAG, "Z axis reset complete"); - report_status.exec_status = 0; - if (exec_complete_cb) exec_complete_cb(report_status); + m_lastexecstatus = ret; + if (status_cb) status_cb(m_lastexecstatus); }); return 0; -} \ No newline at end of file +} + +int32_t PipetteModule::do_motor_action_block(function action) { + if (action == nullptr) return -1; + int32_t ret = action(); + if (ret != 0) { + m_stepMotor->stop(0); + return ret; + } + + while (!m_thread.getExitFlag()) { + if (!m_stepMotor->isbusy()) break; + m_thread.sleep(1000); + } + + if (m_thread.getExitFlag()) { + m_stepMotor->stop(0); + return err::kcommon_error_break_by_user; + } + + return m_stepMotor->get_last_exec_status(); +} diff --git a/components/pipette_module/pipette_ctrl_module.hpp b/components/pipette_module/pipette_ctrl_module.hpp index b1282b0..9448074 100644 --- a/components/pipette_module/pipette_ctrl_module.hpp +++ b/components/pipette_module/pipette_ctrl_module.hpp @@ -17,6 +17,7 @@ class PipetteModule : public I_PipetteModule { StepMotorCtrlModule *m_stepMotor; ZThread m_thread; + int32_t m_lastexecstatus = 0; public: void initialize(SMTP2 *smtp2, StepMotorCtrlModule *stepMotor); @@ -24,13 +25,16 @@ class PipetteModule : public I_PipetteModule { virtual int32_t enable(u8 enable); virtual int32_t stop(u8 stop_type); - virtual int32_t zero_pos_calibrate(function exec_complete_cb); - virtual int32_t reset_device(function exec_complete_cb); + virtual int32_t zero_pos_calibrate(action_cb_status_t status_cb); + virtual int32_t reset_device(action_cb_status_t status_cb); - virtual int32_t take_tip(int tipid, function exec_complete_cb); - virtual int32_t remove_tip(function exec_complete_cb); + virtual int32_t take_tip(int tipid, action_cb_status_t status_cb); + virtual int32_t remove_tip(action_cb_status_t status_cb); // 取液(平台参数，试管参数, 取样体积，取样高度，摇匀次数，摇匀体积) virtual int32_t take_and_split_liquid(u8 tube_id, s16 liquid_volume, s16 zhight, s16 abs_zhight, s16 shake_times, s16 shake_volume, // - function exec_complete_cb); + action_cb_status_t status_cb); + + public: + int32_t do_motor_action_block(function action); }; } // namespace iflytop \ No newline at end of file diff --git a/components/sensors/smtp2/smtp2.cpp b/components/sensors/smtp2/smtp2.cpp index bd1b08c..12ceb9d 100644 --- a/components/sensors/smtp2/smtp2.cpp +++ b/components/sensors/smtp2/smtp2.cpp @@ -1,8 +1,8 @@ #include "smtp2.hpp" +#include +#include #include -#include -#include #include "sdk\components\zprotocols\errorcode\errorcode.hpp" using namespace iflytop; @@ -269,8 +269,8 @@ int SMTP2::sendcmd(const char* cmd, char* rxbuf, size_t rxbuflen, size_t& rxlen) int SMTP2::sendcmd(const char* cmd, size_t txlen, char* rxbuf, size_t rxbuflen, size_t& rxlen) { if (!m_uart) return -1; - int ret = 0; - //size_t rxlen = 0; + int ret = 0; + // size_t rxlen = 0; #ifdef DUMP_HEX printf("tx:%s\n", cmd); @@ -333,3 +333,29 @@ int SMTP2::sendcmd_block(const char* cmd, size_t txlen, char* rxbuf, size_t rxbu rxlen = rxnum; return 0; } + +int32_t SMTP2::doaction_block(ZThread* thread, function action) { + // 移液枪复位 + int ret = action(); + if (ret != 0) { + return ret; + } + // 等待移液枪复位完成 + while (true) { + int32_t state = getState(); + if (state != err::kcommon_error_device_is_busy) { + if (state == 0) { + stop(); + return 0; + } else { + stop(); + return ret; + } + } + if (thread->getExitFlag()) { + stop(); + return err::kcommon_error_break_by_user; + } + thread->sleep(1000); + } +} diff --git a/components/sensors/smtp2/smtp2.hpp b/components/sensors/smtp2/smtp2.hpp index 8c4f2e9..50f0e6b 100644 --- a/components/sensors/smtp2/smtp2.hpp +++ b/components/sensors/smtp2/smtp2.hpp @@ -31,8 +31,6 @@ class SMTP2 { } error_t; #endif - - #if 0 typedef enum { kIdle = 0, @@ -45,9 +43,8 @@ class SMTP2 { uint8_t m_id = 0; // const char* m_name = nullptr; - - DMA_HandleTypeDef* m_hdma_rx; - DMA_HandleTypeDef* m_hdma_tx; + DMA_HandleTypeDef* m_hdma_rx; + DMA_HandleTypeDef* m_hdma_tx; char m_rxbuf[20] = {0}; char m_rxprocessbuf[20] = {0}; @@ -122,6 +119,8 @@ class SMTP2 { */ void stop(); + int32_t doaction_block(ZThread* thread, function action); + private: int32_t doaction(char* cmd); diff --git a/components/step_motor_ctrl_module/step_motor_ctrl_module.cpp b/components/step_motor_ctrl_module/step_motor_ctrl_module.cpp index c92558b..3720149 100644 --- a/components/step_motor_ctrl_module/step_motor_ctrl_module.cpp +++ b/components/step_motor_ctrl_module/step_motor_ctrl_module.cpp @@ -29,7 +29,7 @@ void StepMotorCtrlModule::initialize(int id, IStepperMotor* stepM, ZGPIO* zero_g bool StepMotorCtrlModule::isbusy() { return m_thread.isworking(); } -int32_t StepMotorCtrlModule::move_to(int32_t tox, function status_cb) { // +int32_t StepMotorCtrlModule::move_to(int32_t tox, action_cb_status_t status_cb) { // zlock_guard lock(m_lock); ZLOGI(TAG, "m%d move_to %d", m_id, tox); @@ -42,26 +42,24 @@ int32_t StepMotorCtrlModule::move_to(int32_t tox, function status_cb) { +int32_t StepMotorCtrlModule::move_by(int32_t dx, action_cb_status_t status_cb) { zlock_guard lock(m_lock); ZLOGI(TAG, "m%d move_by %d", m_id, dx); m_thread.stop(); @@ -70,58 +68,46 @@ int32_t StepMotorCtrlModule::move_by(int32_t dx, function status_cb) { +int32_t StepMotorCtrlModule::move_to_zero(action_cb_status_t status_cb) { zlock_guard lock(m_lock); ZLOGI(TAG, "move_to_zero"); m_thread.stop(); - updateStatus(1); - m_thread.start([this, status_cb]() { - int32_t dx; - move_to_zero_cb_status_t ret_status = {0}; - int32_t erro = exec_move_to_zero_task(dx); - if (erro != 0) { - ZLOGI(TAG, "exec_move_to_zero_task failed:%d", erro); - _motor_stop(); - ret_status.exec_status = erro; - if (status_cb) status_cb(ret_status); - updateStatus(0); - return; - } - - m_stepM1->setXACTUAL(0); - ret_status.exec_status = 0; - if (status_cb) status_cb(ret_status); - updateStatus(0); - return; - }); + m_thread.start( + [this, status_cb]() { + int32_t dx; + int32_t erro = exec_move_to_zero_task(dx); + m_lastexecstatus = erro; + }, + [this, status_cb]() { + _motor_stop(); + if (m_lastexecstatus == 0) { + m_stepM1->setXACTUAL(0); + } + if (status_cb) status_cb(m_lastexecstatus); + return; + }); return 0; } -int32_t StepMotorCtrlModule::move_to_zero_with_calibrate(int32_t nowx, function status_cb) { +int32_t StepMotorCtrlModule::move_to_zero_with_calibrate(int32_t nowx, action_cb_status_t status_cb) { zlock_guard lock(m_lock); ZLOGI(TAG, "move_to_zero_with_calibrate x:%d", nowx); m_thread.stop(); - updateStatus(1); m_thread.start([this, nowx, status_cb]() { - int32_t dx; - move_to_zero_with_calibrate_cb_status_t ret_status = {0}; + int32_t dx; int32_t erro = exec_move_to_zero_task(dx); if (erro != 0) { ZLOGI(TAG, "exec_move_to_zero_task failed:%d", erro); _motor_stop(); - ret_status.exec_status = erro; - if (status_cb) status_cb(ret_status); - updateStatus(0); + m_lastexecstatus = erro; + if (status_cb) status_cb(erro); return; } @@ -130,10 +116,8 @@ int32_t StepMotorCtrlModule::move_to_zero_with_calibrate(int32_t nowx, function< m_zero_shift_x = calibrate_x; m_stepM1->setXACTUAL(0); - ret_status.exec_status = 0; - ret_status.zero_shift_x = m_zero_shift_x; - if (status_cb) status_cb(ret_status); - updateStatus(0); + m_lastexecstatus = 0; + if (status_cb) status_cb(m_lastexecstatus); return; }); @@ -160,7 +144,7 @@ int32_t StepMotorCtrlModule::force_change_current_pos(int32_t xpos) { m_stepM1->setXACTUAL(motor_pos); return 0; } -int32_t StepMotorCtrlModule::rotate(int32_t speed, int32_t lastforms, function status_cb) { // +int32_t StepMotorCtrlModule::rotate(int32_t speed, int32_t lastforms, action_cb_status_t status_cb) { // zlock_guard l(m_lock); ZLOGI(TAG, "rotate speed:%d lastforms:%d acc:%d dec:%d", speed, lastforms, m_cfg_acc, m_cfg_dec); @@ -174,9 +158,7 @@ int32_t StepMotorCtrlModule::rotate(int32_t speed, int32_t lastforms, functionsetAcceleration(m_cfg_acc); m_stepM1->setDeceleration(m_cfg_dec); m_stepM1->rotate(speed); @@ -192,11 +174,9 @@ int32_t StepMotorCtrlModule::rotate(int32_t speed, int32_t lastforms, functionstop(); - updateStatus(0); return; }); return 0; @@ -205,14 +185,14 @@ int32_t StepMotorCtrlModule::rotate(int32_t speed, int32_t lastforms, functiongetState() ? 0x01 : 0x00; if (m_end_gpio) status.io_state |= m_end_gpio->getState() ? 0x02 : 0x00; getnowpos(status.x); return 0; } int32_t StepMotorCtrlModule::read_detailed_status(detailed_status_t& debug_info) { - debug_info.status = m_status; + debug_info.status = m_thread.isworking() ? 1 : 0; if (m_Xgpio) debug_info.io_state |= m_Xgpio->getState() ? 0x01 : 0x00; if (m_end_gpio) debug_info.io_state |= m_end_gpio->getState() ? 0x02 : 0x00; getnowpos(debug_info.x); @@ -407,7 +387,9 @@ void StepMotorCtrlModule::forward_kinematics(int32_t x, int32_t& motor_pos) { x -= m_zero_shift_x; motor_pos = x; } +#if 0 void StepMotorCtrlModule::updateStatus(uint8_t status) { zlock_guard lock(m_statu_lock); m_status = status; -} \ No newline at end of file +} +#endif \ No newline at end of file diff --git a/components/step_motor_ctrl_module/step_motor_ctrl_module.hpp b/components/step_motor_ctrl_module/step_motor_ctrl_module.hpp index 9ca9aa9..61eba76 100644 --- a/components/step_motor_ctrl_module/step_motor_ctrl_module.hpp +++ b/components/step_motor_ctrl_module/step_motor_ctrl_module.hpp @@ -16,7 +16,7 @@ class StepMotorCtrlModule : public I_StepMotorCtrlModule { ZThread m_thread; - uint8_t m_status = 0; + // uint8_t m_status = 0; int32_t m_zero_shift_x = 0; @@ -44,19 +44,23 @@ class StepMotorCtrlModule : public I_StepMotorCtrlModule { float m_cfg_distance_scale = 1.0f; // bool m_x_shaft = false; + int32_t m_lastexecstatus = 0; + public: void initialize(int id, IStepperMotor* stepM, ZGPIO* zero_gpio, ZGPIO* end_gpio); - virtual bool isbusy() override; + virtual bool isbusy() override; + virtual int32_t get_last_exec_status() override { return m_lastexecstatus; }; + + virtual int32_t move_to(int32_t tox, action_cb_status_t status_cb); + virtual int32_t move_by(int32_t dx, action_cb_status_t status_cb); + virtual int32_t move_to_zero(action_cb_status_t status_cb); + virtual int32_t move_to_zero_with_calibrate(int32_t x, action_cb_status_t status_cb); + virtual int32_t rotate(int32_t speed, int32_t lastforms, action_cb_status_t status_cb); - virtual int32_t move_to(int32_t x, function status_cb) override; - virtual int32_t move_by(int32_t dx, function status_cb) override; - virtual int32_t move_to_zero(function status_cb) override; - virtual int32_t move_to_zero_with_calibrate(int32_t x, function status_cb) override; virtual int32_t enable(bool venable) override; virtual int32_t stop(uint8_t stopType) override; virtual int32_t force_change_current_pos(int32_t x) override; - virtual int32_t rotate(int32_t speed, int32_t lastforms, function status_cb) override; virtual int32_t read_version(version_t& version) override; virtual int32_t read_status(status_t& status) override; @@ -75,7 +79,7 @@ class StepMotorCtrlModule : public I_StepMotorCtrlModule { int32_t set_warning_limit_param(warning_limit_param_t& param) { return set_warning_limit_param(kset_cmd_type_set, param, param); } private: - void updateStatus(uint8_t status); + // void updateStatus(uint8_t status); void getnowpos(int32_t& pos); diff --git a/components/zcancmder_module/zcan_step_motor_ctrl_module.cpp b/components/zcancmder_module/zcan_step_motor_ctrl_module.cpp index 0f3977b..804b555 100644 --- a/components/zcancmder_module/zcan_step_motor_ctrl_module.cpp +++ b/components/zcancmder_module/zcan_step_motor_ctrl_module.cpp @@ -10,7 +10,7 @@ void ZCanStepMotorCtrlModule::initialize(ZCanCmder* cancmder, int id, I_StepMoto cancmder->registerListener(this); } void ZCanStepMotorCtrlModule::onRceivePacket(CanPacketRxBuffer* rxcmd) { -// printf("ZCanStepMotorCtrlModule::onRceivePacket %d %d\n", rxcmd->get_cmdheader()->cmdid, rxcmd->get_cmdheader()->subcmdid); + // printf("ZCanStepMotorCtrlModule::onRceivePacket %d %d\n", rxcmd->get_cmdheader()->cmdid, rxcmd->get_cmdheader()->subcmdid); PROCESS_PACKET(kcmd_step_motor_ctrl_enable, m_id) { errorcode = m_module->enable(cmd->enable); } END_PP(); @@ -19,51 +19,52 @@ void ZCanStepMotorCtrlModule::onRceivePacket(CanPacketRxBuffer* rxcmd) { END_PP(); PROCESS_PACKET(kcmd_step_motor_ctrl_move_to_zero, m_id) { - errorcode = m_module->move_to_zero([this, cmdheader](I_StepMotorCtrlModule::move_to_zero_cb_status_t& status) { + errorcode = m_module->move_to_zero([this, cmdheader](int32_t status) { osDelay(5); // 用来保证回执消息在前面 kcmd_step_motor_ctrl_move_to_zero_report_t report = {0}; - ZLOGI(TAG, "kcmd_step_motor_ctrl_move_to_zero exec_status:%d", status.exec_status); + ZLOGI(TAG, "kcmd_step_motor_ctrl_move_to_zero exec_status:%d", status); - report.id = m_id; - report.status = status; + report.id = m_id; + report.exec_status = status; m_cancmder->sendExecStatusReport(cmdheader, (uint8_t*)&report, sizeof(report)); }); } END_PP(); PROCESS_PACKET(kcmd_step_motor_ctrl_move_to_zero_with_calibrate, m_id) { - errorcode = m_module->move_to_zero_with_calibrate(cmd->nowx, [this, cmdheader](I_StepMotorCtrlModule::move_to_zero_with_calibrate_cb_status_t& status) { + errorcode = m_module->move_to_zero_with_calibrate(cmd->nowx, [this, cmdheader](int32_t status) { osDelay(5); // 用来保证回执消息在前面 kcmd_step_motor_ctrl_move_to_zero_with_calibrate_report_t report = {0}; - ZLOGI(TAG, "kcmd_step_motor_ctrl_move_to_zero_with_calibrate exec_status:%d %d", status.exec_status, status.zero_shift_x); + ZLOGI(TAG, "kcmd_step_motor_ctrl_move_to_zero_with_calibrate exec_status:%d", status); - report.id = m_id; - report.status = status; + report.id = m_id; + report.exec_status = status; m_cancmder->sendExecStatusReport(cmdheader, (uint8_t*)&report, sizeof(report)); }); } END_PP(); PROCESS_PACKET(kcmd_step_motor_ctrl_move_to, m_id) { - errorcode = m_module->move_to(cmd->x, [this, cmdheader](I_StepMotorCtrlModule::move_to_cb_status_t& status) { + errorcode = m_module->move_to(cmd->x, [this, cmdheader](int32_t status) { osDelay(5); // 用来保证回执消息在前面 kcmd_step_motor_ctrl_move_to_report_t report = {0}; - ZLOGI(TAG, "kcmd_step_motor_ctrl_move_to exec_status:%d %d", status.exec_status, status.tox); + ZLOGI(TAG, "kcmd_step_motor_ctrl_move_to exec_status:%d ", status); - report.id = m_id; - report.status = status; + report.id = m_id; + report.exec_status = status; m_cancmder->sendExecStatusReport(cmdheader, (uint8_t*)&report, sizeof(report)); }); } END_PP(); PROCESS_PACKET(kcmd_step_motor_ctrl_move_by, m_id) { - errorcode = m_module->move_by(cmd->dx, [this, cmdheader](I_StepMotorCtrlModule::move_by_cb_status_t& status) { + errorcode = m_module->move_by(cmd->dx, [this, cmdheader](int32_t status) { osDelay(5); // 用来保证回执消息在前面 kcmd_step_motor_ctrl_move_by_report_t report = {0}; - ZLOGI(TAG, "kcmd_step_motor_ctrl_move_by exec_status:%d %d", status.exec_status, status.dx); + // ZLOGI(TAG, "kcmd_step_motor_ctrl_move_by exec_status:%d %d", status.exec_status, status.dx); + ZLOGI(TAG, "kcmd_step_motor_ctrl_move_by exec_status:%d ", status); - report.id = m_id; - report.status = status; + report.id = m_id; + report.exec_status = status; m_cancmder->sendExecStatusReport(cmdheader, (uint8_t*)&report, sizeof(report)); }); } @@ -73,13 +74,14 @@ void ZCanStepMotorCtrlModule::onRceivePacket(CanPacketRxBuffer* rxcmd) { END_PP(); PROCESS_PACKET(kcmd_step_motor_ctrl_rotate, m_id) { - errorcode = m_module->rotate(cmd->speed, cmd->run_time, [this, cmdheader](I_StepMotorCtrlModule::rotate_cb_status_t& status) { + errorcode = m_module->rotate(cmd->speed, cmd->run_time, [this, cmdheader](int32_t status) { osDelay(5); // 用来保证回执消息在前面 kcmd_step_motor_ctrl_rotate_report_t report = {0}; - ZLOGI(TAG, "kcmd_step_motor_ctrl_rotate exec_status:%d %d", status.exec_status, status.lastforms); + // ZLOGI(TAG, "kcmd_step_motor_ctrl_rotate exec_status:%d %d", status.exec_status, status.lastforms); + ZLOGI(TAG, "kcmd_step_motor_ctrl_rotate exec_status:%d ", status); - report.id = m_id; - report.status = status; + report.id = m_id; + report.exec_status = status; m_cancmder->sendExecStatusReport(cmdheader, (uint8_t*)&report, sizeof(report)); }); } diff --git a/components/zprotocols/errorcode b/components/zprotocols/errorcode index 072751f..862b423 160000 --- a/components/zprotocols/errorcode +++ b/components/zprotocols/errorcode @@ -1 +1 @@ -Subproject commit 072751f7f4573591d229a337fb16181dd58aa7bc +Subproject commit 862b4234aae500ad9c45a3fc71d5306a9f2b8f67 diff --git a/components/zprotocols/zcancmder b/components/zprotocols/zcancmder index ce0623d..358d158 160000 --- a/components/zprotocols/zcancmder +++ b/components/zprotocols/zcancmder @@ -1 +1 @@ -Subproject commit ce0623df5d4dd3ab8ffe1e068fa7af86e8d8c44a +Subproject commit 358d158302546707f113d4028d4fa66e1da29d89 diff --git a/os/zthread.cpp b/os/zthread.cpp index 2edca3b..6510ab1 100644 --- a/os/zthread.cpp +++ b/os/zthread.cpp @@ -15,7 +15,8 @@ void ZThread::threadcb() { while (true) { if (m_threadisworkingFlagCallSide) { m_status = kworking; - m_taskfunction(); + if (m_taskfunction) m_taskfunction(); + if (m_taskfunction_exitcb) m_taskfunction_exitcb(); m_status = kdead; while (m_threadisworkingFlagCallSide) { @@ -44,10 +45,13 @@ void ZThread::init(const char *threadname, int stack_size, osPriority priority) m_defaultTaskHandle = osThreadCreate(osThread(zosthread_default_task), this); ZASSERT(m_defaultTaskHandle != NULL); } -void ZThread::start(zosthread_cb_t cb) { +void ZThread::start(zosthread_cb_t cb) { start(cb, nullptr); } + +void ZThread::start(zosthread_cb_t cb, zosthread_cb_t exitcb) { stop(); - m_taskfunction = cb; + m_taskfunction = cb; + m_taskfunction_exitcb = exitcb; ZASSERT(m_taskfunction); xSemaphoreTake(m_lock, portMAX_DELAY); diff --git a/os/zthread.hpp b/os/zthread.hpp index db4a7e2..efa1ae2 100644 --- a/os/zthread.hpp +++ b/os/zthread.hpp @@ -24,6 +24,7 @@ class ZThread { bool m_threadisworkingFlagCallSide = false; zosthread_cb_t m_taskfunction; + zosthread_cb_t m_taskfunction_exitcb; EventGroupHandle_t m_zthreadstartworkevent; osThreadId m_defaultTaskHandle; @@ -33,6 +34,7 @@ class ZThread { public: void init(const char* threadname, int stack_size = 1024, osPriority priority = osPriorityNormal); void start(zosthread_cb_t cb); + void start(zosthread_cb_t cb, zosthread_cb_t exitcb); void stop(); bool getExitFlag() { return !m_threadisworkingFlagCallSide; }