From 8f8150086f4e1acf7c4af7ad970bad9227a7c48a Mon Sep 17 00:00:00 2001 From: zhaohe Date: Sun, 28 Apr 2024 18:48:59 +0800 Subject: [PATCH] update --- iflytop_canbus_protocol | 2 +- usrc/base/hardware.cpp | 13 +++- usrc/protocol_impl/protocol_impl_service.cpp | 93 +++++++++++++++++++++++++++- zsdk | 2 +- 4 files changed, 105 insertions(+), 5 deletions(-) diff --git a/iflytop_canbus_protocol b/iflytop_canbus_protocol index 0a220fc..0d30ade 160000 --- a/iflytop_canbus_protocol +++ b/iflytop_canbus_protocol @@ -1 +1 @@ -Subproject commit 0a220fccdd10378ec311e17871edbc0acf089569 +Subproject commit 0d30adef26695d979d4f62a051d896aab1354734 diff --git a/usrc/base/hardware.cpp b/usrc/base/hardware.cpp index 8e73944..cb9ee15 100644 --- a/usrc/base/hardware.cpp +++ b/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); } diff --git a/usrc/protocol_impl/protocol_impl_service.cpp b/usrc/protocol_impl/protocol_impl_service.cpp index 0290cb1..f804d58 100644 --- a/usrc/protocol_impl/protocol_impl_service.cpp +++ b/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*)®val, 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); } diff --git a/zsdk b/zsdk index 2c74701..7951cee 160000 --- a/zsdk +++ b/zsdk @@ -1 +1 @@ -Subproject commit 2c74701b86a85d9a7658f2e64a34a3f9101a2306 +Subproject commit 7951cee7dfc612be73f402914c12933ddffe37cc