Merge branch 'master' of 192.168.1.3:manufacturer_stm32/iflytop_stm32_os_sdk

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;

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);

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
    */

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();