update

iflytop_canbus_protocol

@@ -1 +1 @@
-Subproject commit 0a220fccdd10378ec311e17871edbc0acf089569
+Subproject commit 0d30adef26695d979d4f62a051d896aab1354734

usrc/base/hardware.cpp

@@ -28,10 +28,21 @@ void Hardware::init() {
 
   int32_t chipv0 = m_motor[0].readChipVERSION();  // 5130:0x11
   int32_t chipv1 = m_motor[1].readChipVERSION();  // 5130:0x11
-
   // m_motor[0].rotate(500000);
   // m_motor[1].rotate(500000);
   ZLOGI(TAG, "chipv0: %x, chipv1: %x", chipv0, chipv1);
 
+  auto gstate0 = m_motor[0].getGState();
+  auto gstate1 = m_motor[1].getGState();
+
+  ZLOGI(TAG, "motor0: reset:%d  drv_err:%d uv_cp:%d", gstate0.reset, gstate0.drv_err, gstate0.uv_cp);
+  ZLOGI(TAG, "motor1: reset:%d  drv_err:%d uv_cp:%d", gstate1.reset, gstate1.drv_err, gstate1.uv_cp);
+
+   gstate0 = m_motor[0].getGState();
+   gstate1 = m_motor[1].getGState();
+
+  ZLOGI(TAG, "motor0: reset:%d  drv_err:%d uv_cp:%d", gstate0.reset, gstate0.drv_err, gstate0.uv_cp);
+  ZLOGI(TAG, "motor1: reset:%d  drv_err:%d uv_cp:%d", gstate1.reset, gstate1.drv_err, gstate1.uv_cp);
+
   m_pressureSensorBus.init(&m_modbusBlockHost);
 }

usrc/protocol_impl/protocol_impl_service.cpp

@@ -24,6 +24,9 @@ using namespace iflytop;
   }
 #define GET_PARAM(buff, off) ((((int32_t*)(buff))[off]))
 
+#define CLEAR_BIT(val, bit) (val &= ~(1 << bit))
+#define SET_BIT(val, bit)   (val |= (1 << bit))
+
 /***********************************************************************************************************************
  *                                                      VAR_LIST                                                       *
  ***********************************************************************************************************************/
@@ -34,6 +37,10 @@ static osThreadId PacketRxThreadId;
 static osTimerId PressureSensorDataReportTimerId;  // 压力传感器数值上报
 static uint32_t  m_pressureSensorDataReportPeriodMs = 3000;
 
+static osTimerId MotorMonitorTimerId;  // 压力传感器数值上报
+
+static uint8_t m_dflag;
+
 /***********************************************************************************************************************
  *                                                    FUNCTION_IMPL                                                    *
  ***********************************************************************************************************************/
@@ -81,8 +88,14 @@ static void basic_func_impl(uint8_t from, uint8_t to, uint8_t* rawpacket, size_t
     static report_heatpacket_data_t heatpacket;
     heatpacket.boardType  = deviceInfo_getBoardType();
     heatpacket.heartIndex = GET_PARAM(packet->params, 0);
+    heatpacket.flag       = m_dflag;
     zcanbus_send_report(kreport_heatpacket_pong, (uint8_t*)&heatpacket, sizeof(heatpacket), 30);
   }
+
+  else if (packet->function_id == kcmd_clear_reset_flag) {
+    CLEAR_BIT(m_dflag, 0);
+    zcanbus_send_ack(packet, NULL, 0);
+  }
   // 触发一次强制上报事件
   if (packet->function_id == kcmd_force_report) {
     ForceReportFlagMgr::ins()->trigger();
@@ -105,11 +118,18 @@ static void pump_func_impl(uint8_t from, uint8_t to, uint8_t* rawpacket, size_t
       zcanbus_send_errorack(packet, kerr_invalid_param);
       return;
     }
+    if (!Hardware::ins().motor(subindex)->ping()) {
+      zcanbus_send_errorack(packet, kerr_motor_subdevice_offline);
+      return;
+    }
+
+    Hardware::ins().motor(subindex)->enableIC(false);
+    Hardware::ins().motor(subindex)->enableIC(true);
     Hardware::ins().motor(subindex)->rotate(velocity);
     zcanbus_send_ack(packet, NULL, 0);
   }
 
-  //
+  // 泵机停止
   else if (packet->function_id == kcmd_pump_stop) {
     CHECK_PARAM_LEN(paramNum, 1);
 
@@ -120,6 +140,11 @@ static void pump_func_impl(uint8_t from, uint8_t to, uint8_t* rawpacket, size_t
       return;
     }
 
+    if (!Hardware::ins().motor(subindex)->ping()) {
+      zcanbus_send_errorack(packet, kerr_motor_subdevice_offline);
+      return;
+    }
+
     Hardware::ins().motor(subindex)->stop();
     zcanbus_send_ack(packet, NULL, 0);
   }
@@ -138,9 +163,16 @@ static void pump_func_impl(uint8_t from, uint8_t to, uint8_t* rawpacket, size_t
       return;
     }
 
+    if (!Hardware::ins().motor(subindex)->ping()) {
+      zcanbus_send_errorack(packet, kerr_motor_subdevice_offline);
+      return;
+    }
+
     Hardware::ins().motor(subindex)->setIHOLD_IRUN(ihold, irun, idelay);
     zcanbus_send_ack(packet, NULL, 0);
-  } else if (packet->function_id == kcmd_pump_set_acc) {
+  }
+  // 设置加速度
+  else if (packet->function_id == kcmd_pump_set_acc) {
     CHECK_PARAM_LEN(paramNum, 2);
 
     int32_t subindex = GET_PARAM(packet->params, 0);
@@ -151,6 +183,11 @@ static void pump_func_impl(uint8_t from, uint8_t to, uint8_t* rawpacket, size_t
       return;
     }
 
+    if (!Hardware::ins().motor(subindex)->ping()) {
+      zcanbus_send_errorack(packet, kerr_motor_subdevice_offline);
+      return;
+    }
+
     Hardware::ins().motor(subindex)->setAcceleration(acc);
     Hardware::ins().motor(subindex)->setDeceleration(acc);
     zcanbus_send_ack(packet, NULL, 0);
@@ -170,6 +207,11 @@ static void pump_func_impl(uint8_t from, uint8_t to, uint8_t* rawpacket, size_t
       return;
     }
 
+    if (!Hardware::ins().motor(subindex)->ping()) {
+      zcanbus_send_errorack(packet, kerr_motor_subdevice_offline);
+      return;
+    }
+
     Hardware::ins().motor(subindex)->writeInt(regadd, regval);
   }
 
@@ -185,6 +227,11 @@ static void pump_func_impl(uint8_t from, uint8_t to, uint8_t* rawpacket, size_t
       return;
     }
 
+    if (!Hardware::ins().motor(subindex)->ping()) {
+      zcanbus_send_errorack(packet, kerr_motor_subdevice_offline);
+      return;
+    }
+
     int32_t regval = Hardware::ins().motor(subindex)->readInt(regadd);
     zcanbus_send_ack(packet, (uint8_t*)&regval, sizeof(regval));
   }
@@ -298,6 +345,42 @@ static void onPressureSensorDataReportTimer(void const* argument) {
                       sizeof(report_pressure_data_t) + sensorNum * sizeof(report->data[0]), 10);
 }
 
+static void onMotorMonitorTimer(void const* argument) {
+  // 电机异常检查
+  static bool motorErrorFlagCache[10];
+
+  report_exeception_data_t data;
+  for (size_t i = 0; i < Hardware::ins().motorNum(); i++) {
+    if (!Hardware::ins().motor(i)->ping()) {
+      data.subid = i;
+      data.ecode = kerr_motor_subdevice_offline;
+
+      if (!motorErrorFlagCache[i]) {
+        motorErrorFlagCache[i] = true;
+        ZLOGE(TAG, "motor %d offline error", i);
+        zcanbus_send_emergency_report(kreport_exception_error, (uint8_t*)&data, sizeof(data), 100);
+      }
+    } else {
+      auto gstate = Hardware::ins().motor(i)->getGState();
+      bool flag   = gstate.reset || gstate.drv_err || gstate.uv_cp;
+      if (!flag && motorErrorFlagCache[i]) {
+        motorErrorFlagCache[i] = false;
+      } else if (flag && !motorErrorFlagCache[i]) {
+        ZLOGE(TAG, "motor %d error, reset %d, drv_err %d, uv_cp %d", i, gstate.reset, gstate.drv_err, gstate.uv_cp);
+        if (gstate.reset) {
+          data.ecode = kerr_motor_reset_error;
+        } else if (gstate.uv_cp) {
+          data.ecode = kerr_motor_undervoltage_error;
+        } else if (gstate.drv_err) {
+          data.ecode = kerr_motor_driver_error;
+        }
+        data.subid = i;
+        zcanbus_send_emergency_report(kreport_exception_error, (uint8_t*)&data, sizeof(data), 100);
+      }
+    }
+  }
+}
+
 /***********************************************************************************************************************
  *                                                         EXT                                                         *
  ***********************************************************************************************************************/
@@ -305,6 +388,8 @@ static void onPressureSensorDataReportTimer(void const* argument) {
 void protocol_impl_service_init() {  //
   Hardware::ins().init();
   ForceReportFlagMgr::ins()->init();
+  m_dflag = 0x00;
+  SET_BIT(m_dflag, 0);
 
   zcanbus_init(deviceInfo_getBoardId());
   zcanbus_reglistener(zcanbus_on_rx);
@@ -314,6 +399,10 @@ void protocol_impl_service_init() {  //
   PressureSensorDataReportTimerId = osTimerCreate(osTimer(PressureSensorDataReportTimer), osTimerPeriodic, NULL);
   osTimerStart(PressureSensorDataReportTimerId, m_pressureSensorDataReportPeriodMs);
 
+  osTimerDef(MotorMonitorTimer, onMotorMonitorTimer);
+  MotorMonitorTimerId = osTimerCreate(osTimer(MotorMonitorTimer), osTimerPeriodic, NULL);
+  osTimerStart(MotorMonitorTimerId, 1000);
+
   osThreadDef(PacketRxThread, onPacketRxThreadStart, osPriorityNormal, 0, 1024);
   PacketRxThreadId = osThreadCreate(osThread(PacketRxThread), NULL);
 }

zsdk

@@ -1 +1 @@
-Subproject commit 2c74701b86a85d9a7658f2e64a34a3f9101a2306
+Subproject commit 7951cee7dfc612be73f402914c12933ddffe37cc