From 372a2f158e8dcf36a4dbd42e784b2a223c544113 Mon Sep 17 00:00:00 2001 From: zhaohe Date: Mon, 16 Oct 2023 21:01:30 +0800 Subject: [PATCH] fix some bug --- components/mini_servo_motor/feite_servo_motor.cpp | 57 +++++++++++++++++------ components/mini_servo_motor/feite_servo_motor.hpp | 2 + components/zcancmder/zcanreceiver_master.cpp | 2 + components/zcancmder/zcanreceiver_master.hpp | 2 + 4 files changed, 48 insertions(+), 15 deletions(-) diff --git a/components/mini_servo_motor/feite_servo_motor.cpp b/components/mini_servo_motor/feite_servo_motor.cpp index e4fe23e..d4336e4 100644 --- a/components/mini_servo_motor/feite_servo_motor.cpp +++ b/components/mini_servo_motor/feite_servo_motor.cpp @@ -26,8 +26,10 @@ void FeiTeServoMotor::initialize(UART_HandleTypeDef* uart, DMA_HandleTypeDef* hd m_uart = uart; m_hdma_rx = hdma_rx; m_hdma_tx = hdma_tx; + m_mutex.init(); } bool FeiTeServoMotor::ping(uint8_t id) { + zlock_guard l(m_mutex); ping_cmd_t ping_cmd; ping_resp_t ping_resp; ping_cmd.header = 0xffff; @@ -53,8 +55,14 @@ static int16_t getcalibrate(int16_t nowpos, int16_t aftercalibratepos) { return calibrate; } -bool FeiTeServoMotor::setmode(uint8_t id, run_mode_e runmode) { return write_u8(id, kRegServoRunMode, (uint8_t)runmode); } -bool FeiTeServoMotor::getServoCalibration(uint8_t id, int16_t& poscalibration) { return read_s16(id, kRegServoCalibration, 11, poscalibration); } +bool FeiTeServoMotor::setmode(uint8_t id, run_mode_e runmode) { + zlock_guard l(m_mutex); + return write_u8(id, kRegServoRunMode, (uint8_t)runmode); +} +bool FeiTeServoMotor::getServoCalibration(uint8_t id, int16_t& poscalibration) { + zlock_guard l(m_mutex); + return read_s16(id, kRegServoCalibration, 11, poscalibration); +} run_mode_e FeiTeServoMotor::getmode(uint8_t id) { uint8_t data = 0; @@ -67,22 +75,33 @@ run_mode_e FeiTeServoMotor::getmode(uint8_t id) { } bool FeiTeServoMotor::getmode(uint8_t id, run_mode_e& runmode) { - uint8_t data = 0; - bool suc = read_u8(id, kRegServoRunMode, data); - runmode = (run_mode_e)data; + zlock_guard l(m_mutex); + uint8_t data = 0; + bool suc = read_u8(id, kRegServoRunMode, data); + runmode = (run_mode_e)data; return suc; } -bool FeiTeServoMotor::setTorqueSwitch(uint8_t id, bool on) { return write_u8(id, kRegServoTorqueSwitch, on ? 1 : 0); } +bool FeiTeServoMotor::setTorqueSwitch(uint8_t id, bool on) { + zlock_guard l(m_mutex); + return write_u8(id, kRegServoTorqueSwitch, on ? 1 : 0); +} bool FeiTeServoMotor::getTorqueSwitch(uint8_t id, bool& on) { - uint8_t data = 0; - bool suc = read_u8(id, kRegServoTorqueSwitch, data); - on = data; + zlock_guard l(m_mutex); + uint8_t data = 0; + bool suc = read_u8(id, kRegServoTorqueSwitch, data); + on = data; return suc; } -bool FeiTeServoMotor::getNowPos(uint8_t id, int16_t& pos) { return read_s16(id, kRegServoCurrentPos, 15, pos); } -bool FeiTeServoMotor::setTargetPos(uint8_t id, int16_t pos) { return write_s16(id, kRegServoTargetPos, 15, pos); } +bool FeiTeServoMotor::getNowPos(uint8_t id, int16_t& pos) { + zlock_guard l(m_mutex); + return read_s16(id, kRegServoCurrentPos, 15, pos); +} +bool FeiTeServoMotor::setTargetPos(uint8_t id, int16_t pos) { + zlock_guard l(m_mutex); + return write_s16(id, kRegServoTargetPos, 15, pos); +} bool FeiTeServoMotor::triggerAysncWrite(uint8_t id) { cmd_header_t* cmd_header = (cmd_header_t*)m_txbuf; @@ -97,6 +116,7 @@ bool FeiTeServoMotor::triggerAysncWrite(uint8_t id) { } bool FeiTeServoMotor::rotate(uint8_t id, int16_t speed, uint16_t torque) { + zlock_guard l(m_mutex); DO(setmode(id, kMotorMode)); if (torque == 0) torque = 1000; DO(write_u16(id, kRegServoTorqueLimit, torque)); @@ -105,6 +125,7 @@ bool FeiTeServoMotor::rotate(uint8_t id, int16_t speed, uint16_t torque) { } bool FeiTeServoMotor::moveTo(uint8_t id, int16_t pos, int16_t speed, uint16_t torque) { + zlock_guard l(m_mutex); /** * @brief ÉèÖÃŤ¾Ø */ @@ -117,7 +138,7 @@ bool FeiTeServoMotor::moveTo(uint8_t id, int16_t pos, int16_t speed, uint16_t to DO(setTargetPos(id, pos)); return true; } -uint16_t abs16(int16_t val) { +static uint16_t abs16(int16_t val) { if (val < 0) { return -val; } else { @@ -125,6 +146,7 @@ uint16_t abs16(int16_t val) { } } bool FeiTeServoMotor::moveWithTorque(uint8_t id, int16_t torque) { + zlock_guard l(m_mutex); DO(setmode(id, kOpenMotorMode)); if (torque == 0) torque = 1000; DO(write_u16(id, kRegServoTorqueLimit, abs16(torque))); @@ -144,13 +166,15 @@ static int16_t tosign16(uint16_t* d, int signoff) { bool FeiTeServoMotor::read_status(uint8_t id, status_t* status) { // kRegServoCurrentPos - bool suc = read_reg(id, kRegServoCurrentPos, (uint8_t*)status, sizeof(status_t)); - status->vel = tosign16((uint16_t*)&status->vel, 15); + zlock_guard l(m_mutex); + bool suc = read_reg(id, kRegServoCurrentPos, (uint8_t*)status, sizeof(status_t)); + status->vel = tosign16((uint16_t*)&status->vel, 15); if (!suc) return false; return true; } bool FeiTeServoMotor::read_detailed_status(uint8_t id, detailed_status_t* detailed_status) { - bool suc = read_reg(id, kRegServoCurrentPos, (uint8_t*)detailed_status, sizeof(*detailed_status)); + zlock_guard l(m_mutex); + bool suc = read_reg(id, kRegServoCurrentPos, (uint8_t*)detailed_status, sizeof(*detailed_status)); if (!suc) return false; detailed_status->vel = tosign16((uint16_t*)&detailed_status->vel, 15); detailed_status->torque = tosign16((uint16_t*)&detailed_status->torque, 10); @@ -270,6 +294,7 @@ bool FeiTeServoMotor::write_s16(uint8_t id, feite::reg_add_e add, uint8_t signbi } bool FeiTeServoMotor::write_reg(uint8_t id, bool async, uint8_t add, uint8_t* data, uint8_t len) { // // ZLOGI(TAG, "write_reg id:%d add:%d len:%d", id, add, len); + zlock_guard l(m_mutex); cmd_header_t* cmd_header = (cmd_header_t*)m_txbuf; receipt_header_t* receipt_header = (receipt_header_t*)m_rxbuf; @@ -300,6 +325,8 @@ bool FeiTeServoMotor::write_reg(uint8_t id, bool async, uint8_t add, uint8_t* da } bool FeiTeServoMotor::read_reg(uint8_t id, uint8_t add, uint8_t* data, uint8_t len) { // return false; + zlock_guard l(m_mutex); + cmd_header_t* cmd_header = (cmd_header_t*)m_txbuf; receipt_header_t* receipt_header = (receipt_header_t*)m_rxbuf; cmd_header->header = 0xffff; diff --git a/components/mini_servo_motor/feite_servo_motor.hpp b/components/mini_servo_motor/feite_servo_motor.hpp index 5ea01c8..756be0e 100644 --- a/components/mini_servo_motor/feite_servo_motor.hpp +++ b/components/mini_servo_motor/feite_servo_motor.hpp @@ -125,6 +125,8 @@ class FeiTeServoMotor { uint8_t m_txbuf[256] = {0}; uint8_t m_rxbuf[256] = {0}; + zmutex m_mutex; + public: void initialize(UART_HandleTypeDef* uart, DMA_HandleTypeDef* hdma_rx, DMA_HandleTypeDef* hdma_tx); bool ping(uint8_t id); diff --git a/components/zcancmder/zcanreceiver_master.cpp b/components/zcancmder/zcanreceiver_master.cpp index 7a2f4a0..996cc63 100644 --- a/components/zcancmder/zcanreceiver_master.cpp +++ b/components/zcancmder/zcanreceiver_master.cpp @@ -33,6 +33,7 @@ void ZCanCommnaderMaster::init(CFG *cfg) { HAL_StatusTypeDef hal_status; m_config = cfg; m_on_packet_map_lock.init(); + txlock.init(); /** * @brief ³õʼ»¯CAN @@ -279,6 +280,7 @@ uint16_t ZCanCommnaderMaster::generateFreeIndex() { } void ZCanCommnaderMaster::sendPacket(uint8_t *packet, size_t len) { + zlock_guard txlock_guard(txlock); /** * @brief */ diff --git a/components/zcancmder/zcanreceiver_master.hpp b/components/zcancmder/zcanreceiver_master.hpp index 7f47887..d400273 100644 --- a/components/zcancmder/zcanreceiver_master.hpp +++ b/components/zcancmder/zcanreceiver_master.hpp @@ -87,6 +87,8 @@ class ZCanCommnaderMaster : public ZCanIRQListener { zmutex m_on_packet_map_lock; uint16_t m_index_off = 0; + zmutex txlock; + public: ZCanCommnaderMaster() {} CFG *createCFG();