From 6c6a7a1299676f9612bbee7675eedf5ae5047f33 Mon Sep 17 00:00:00 2001 From: zhaohe Date: Thu, 8 Aug 2024 14:26:33 +0800 Subject: [PATCH] add liquid ctrl board cmd processer --- app_protocols/transmit_disfection_protocol | 2 +- .../liquid_ctrl_board_cmd_processer.cpp | 267 +++++++++++++-------- .../liquid_ctrl_board_cmd_processer.hpp | 7 +- .../public_cmd_processer.cpp | 1 + .../i_protocol_processer.hpp | 2 +- zsdk | 2 +- 6 files changed, 176 insertions(+), 105 deletions(-) diff --git a/app_protocols/transmit_disfection_protocol b/app_protocols/transmit_disfection_protocol index 591e174..0cbbc25 160000 --- a/app_protocols/transmit_disfection_protocol +++ b/app_protocols/transmit_disfection_protocol @@ -1 +1 @@ -Subproject commit 591e174e21072cc71d6288d6c5cfa00f34952e9b +Subproject commit 0cbbc25f3b6402ae5e5122ca6e3d2337b985f182 diff --git a/usrc/protocol_processer_impl/liquid_ctrl_board_cmd_processer.cpp b/usrc/protocol_processer_impl/liquid_ctrl_board_cmd_processer.cpp index 8b46fd3..b52423b 100644 --- a/usrc/protocol_processer_impl/liquid_ctrl_board_cmd_processer.cpp +++ b/usrc/protocol_processer_impl/liquid_ctrl_board_cmd_processer.cpp @@ -3,6 +3,17 @@ #include "board/liquid_ctrl_board.hpp" using namespace iflytop; +#define TAG "LiquidCtrl" +#define ThisClass LiquidCtrlBoardCmdProcesser +#define DEVICE LiquidCtrlBoard::ins() + +/*********************************************************************************************************************** + * 压力主动上报定时器 * + ***********************************************************************************************************************/ + +/*********************************************************************************************************************** + * 电机异常监控 * + ***********************************************************************************************************************/ static osTimerId MotorMonitorTimerId; // 压力传感器数值上报 static bool motorErrorFlagCache[10]; // 电机异常状态上报标志位 static zmutex motorErrorFlagCacheLock; @@ -23,155 +34,186 @@ static void motorErrorFlag_set(int subindex, bool val) { } } -#define DEVICE LiquidCtrlBoard::ins() +static void onMotorMonitorTimer(void const* argument) { + // 电机异常检查 + + report_exeception_data_t data; + for (size_t i = 0; i < DEVICE->motorNum(); i++) { + bool estate = motorErrorFlag_get(i); + + if (!DEVICE->motor(i)->ping()) { + data.subid = i; + data.ecode = kerr_motor_subdevice_offline; + + if (!estate) { + motorErrorFlag_set(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 = DEVICE->motor(i)->getGState(); + bool flag = gstate.reset || gstate.drv_err || gstate.uv_cp; + if (!flag && estate) { + motorErrorFlag_set(i, false); + } else if (flag && !estate) { + 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; + } else { + data.ecode = kerr_motor_unkown_error; + } + data.subid = i; + motorErrorFlag_set(i, true); + zcanbus_send_emergency_report(kreport_exception_error, (uint8_t*)&data, sizeof(data), 100); + } + } + } +} -void LiquidCtrlBoardCmdProcesser::initialize() { motorErrorFlagCacheLock.init(); } +/*********************************************************************************************************************** + * LiquidCtrlBoardCmdProcesser * + ***********************************************************************************************************************/ + +void LiquidCtrlBoardCmdProcesser::initialize() { + REG_FN(evaporation_tank_water_sensor_read_state); + REG_FN(device_bottom_water_sensor_read_state); + + REG_FN(pump_rotate); + REG_FN(pump_stop); + REG_FN(pump_set_ihold_irun_idelay); + REG_FN(pump_set_acc); + REG_FN(pump_set_ramp); + REG_FN(pump_set_tzw); + REG_FN(pump_set_subic_reg); + REG_FN(pump_get_subic_reg); + REG_FN(pump_ping); + + REG_FN(triple_warning_light_ctl); + REG_FN(pressure_sensor_bus_read_data); + + motorErrorFlagCacheLock.init(); + osTimerDef(MotorMonitorTimer, onMotorMonitorTimer); + MotorMonitorTimerId = osTimerCreate(osTimer(MotorMonitorTimer), osTimerPeriodic, NULL); + osTimerStart(MotorMonitorTimerId, 1000); +} bool LiquidCtrlBoardCmdProcesser::isSupportThisCmd(ProcessContext* cxt) { return false; } /*********************************************************************************************************************** * PUMP * ***********************************************************************************************************************/ +#define MOTOR_CHECK() \ + if (GET_PARAM(0) >= DEVICE->motorNum()) { \ + zcanbus_send_errorack(cxt->packet, kerr_invalid_param); \ + return; \ + } \ + if (!DEVICE->motor(GET_PARAM(0))->ping()) { \ + zcanbus_send_errorack(cxt->packet, kerr_motor_subdevice_offline); \ + return; \ + } + void LiquidCtrlBoardCmdProcesser::pump_rotate(ProcessContext* cxt) { CHECK_PARAM_LEN(PRAAM_LEN(), 2); + MOTOR_CHECK(); - int32_t subindex = GET_PARAM(0); int32_t velocity = GET_PARAM(1); - if (subindex >= DEVICE->motorNum()) { - zcanbus_send_errorack(cxt->packet, kerr_invalid_param); - return; - } - if (!DEVICE->motor(subindex)->ping()) { - zcanbus_send_errorack(cxt->packet, kerr_motor_subdevice_offline); - return; - } + ZLOGI(TAG, "pump_rotate: %d %d", GET_PARAM(0), velocity); - DEVICE->motor(subindex)->enable(false); - DEVICE->motor(subindex)->enable(true); - DEVICE->motor(subindex)->moveToEnd(velocity > 0 ? 1 : -1, abs(velocity)); + DEVICE->motor(GET_PARAM(0))->enable(false); + DEVICE->motor(GET_PARAM(0))->enable(true); + DEVICE->motor(GET_PARAM(0))->moveToEnd(velocity > 0 ? 1 : -1, abs(velocity)); zcanbus_send_ack(cxt->packet, NULL, 0); - motorErrorFlag_set(subindex, false); + motorErrorFlag_set(GET_PARAM(0), false); } void LiquidCtrlBoardCmdProcesser::pump_stop(ProcessContext* cxt) { CHECK_PARAM_LEN(PRAAM_LEN(), 1); + MOTOR_CHECK(); - int32_t subindex = GET_PARAM(0); - - if (subindex >= DEVICE->motorNum()) { - zcanbus_send_errorack(cxt->packet, kerr_invalid_param); - return; - } - - if (!DEVICE->motor(subindex)->ping()) { - zcanbus_send_errorack(cxt->packet, kerr_motor_subdevice_offline); - return; - } - - DEVICE->motor(subindex)->stop(); + ZLOGI(TAG, "pump_stop: %d", GET_PARAM(0)); + DEVICE->motor(GET_PARAM(0))->stop(); zcanbus_send_ack(cxt->packet, NULL, 0); } void LiquidCtrlBoardCmdProcesser::pump_set_ihold_irun_idelay(ProcessContext* cxt) { CHECK_PARAM_LEN(PRAAM_LEN(), 4); + MOTOR_CHECK(); - int32_t subindex = GET_PARAM(0); - int32_t ihold = GET_PARAM(1); - int32_t irun = GET_PARAM(2); - int32_t idelay = GET_PARAM(3); + int32_t ihold = GET_PARAM(1); + int32_t irun = GET_PARAM(2); + int32_t idelay = GET_PARAM(3); - if (subindex >= DEVICE->motorNum()) { - zcanbus_send_errorack(cxt->packet, kerr_invalid_param); - return; - } - - if (!DEVICE->motor(subindex)->ping()) { - zcanbus_send_errorack(cxt->packet, kerr_motor_subdevice_offline); - return; - } + ZLOGI(TAG, "pump_set_ihold_irun_idelay: %d ihold:%d irun:%d idelay:%d", GET_PARAM(0), ihold, irun, idelay); + DEVICE->motor(GET_PARAM(0))->setIHOLD_IRUN(ihold, irun, idelay); - DEVICE->motor(subindex)->setIHOLD_IRUN(ihold, irun, idelay); zcanbus_send_ack(cxt->packet, NULL, 0); } void LiquidCtrlBoardCmdProcesser::pump_set_acc(ProcessContext* cxt) { CHECK_PARAM_LEN(PRAAM_LEN(), 2); + MOTOR_CHECK(); - int32_t subindex = GET_PARAM(0); - int32_t acc = GET_PARAM(1); + int32_t acc = GET_PARAM(1); - if (subindex >= DEVICE->motorNum()) { - zcanbus_send_errorack(cxt->packet, kerr_invalid_param); - return; - } + DEVICE->motor(GET_PARAM(0))->setAmax(acc); + DEVICE->motor(GET_PARAM(0))->setDmax(acc); - if (!DEVICE->motor(subindex)->ping()) { - zcanbus_send_errorack(cxt->packet, kerr_motor_subdevice_offline); - return; - } + ZLOGI(TAG, "pump_set_acc: %d acc:%d", GET_PARAM(0), acc); - DEVICE->motor(subindex)->setAmax(acc); - DEVICE->motor(subindex)->setDmax(acc); zcanbus_send_ack(cxt->packet, NULL, 0); } -void LiquidCtrlBoardCmdProcesser::pump_set_subic_reg(ProcessContext* cxt) { - CHECK_PARAM_LEN(PRAAM_LEN(), 3); - int32_t subindex = GET_PARAM(0); - int32_t regadd = GET_PARAM(1); - int32_t regval = GET_PARAM(2); - - if (subindex >= DEVICE->motorNum()) { - zcanbus_send_errorack(cxt->packet, kerr_invalid_param); - return; - } - - if (!DEVICE->motor(subindex)->ping()) { - zcanbus_send_errorack(cxt->packet, kerr_motor_subdevice_offline); - return; - } +void LiquidCtrlBoardCmdProcesser::pump_set_ramp(ProcessContext* cxt) { + CHECK_PARAM_LEN(PRAAM_LEN(), 5); + MOTOR_CHECK(); + + // index,vs,a1,amx,v1 + int32_t vs = GET_PARAM(1); + int32_t a1 = GET_PARAM(2); + int32_t amx = GET_PARAM(3); + int32_t v1 = GET_PARAM(4); + + ZLOGI(TAG, "pump_set_ramp: %d vs:%d a1:%d amx:%d v1:%d", GET_PARAM(0), vs, a1, amx, v1); + + DEVICE->motor(GET_PARAM(0))->setVstart(vs); + DEVICE->motor(GET_PARAM(0))->setVstop(vs); + DEVICE->motor(GET_PARAM(0))->setA1(a1); + DEVICE->motor(GET_PARAM(0))->setD1(a1); + DEVICE->motor(GET_PARAM(0))->setAmax(amx); + DEVICE->motor(GET_PARAM(0))->setDmax(amx); + DEVICE->motor(GET_PARAM(0))->setV1(v1); + zcanbus_send_ack(cxt->packet, NULL, 0); +} +void LiquidCtrlBoardCmdProcesser::pump_set_tzw(ProcessContext* cxt) {} - DEVICE->motor(subindex)->writeIntExt(regadd, regval); +void LiquidCtrlBoardCmdProcesser::pump_set_subic_reg(ProcessContext* cxt) { + CHECK_PARAM_LEN(PRAAM_LEN(), 3); + MOTOR_CHECK(); + ZLOGI(TAG, "pump_set_subic_reg: %d %d %x", GET_PARAM(0), GET_PARAM(1), GET_PARAM(2)); + DEVICE->motor(GET_PARAM(0))->writeIntExt(GET_PARAM(1), GET_PARAM(2)); + zcanbus_send_ack(cxt->packet, NULL, 0); } void LiquidCtrlBoardCmdProcesser::pump_get_subic_reg(ProcessContext* cxt) { CHECK_PARAM_LEN(PRAAM_LEN(), 2); + MOTOR_CHECK(); + int32_t val = DEVICE->motor(GET_PARAM(0))->readIntExt(GET_PARAM(1)); + ZLOGI(TAG, "pump_get_subic_reg: %d %x -> %x ", GET_PARAM(0), GET_PARAM(1), val); - int32_t subindex = GET_PARAM(0); - int32_t regadd = GET_PARAM(1); - - if (subindex >= DEVICE->motorNum()) { - zcanbus_send_errorack(cxt->packet, kerr_invalid_param); - return; - } - - if (!DEVICE->motor(subindex)->ping()) { - zcanbus_send_errorack(cxt->packet, kerr_motor_subdevice_offline); - return; - } - - int32_t val = DEVICE->motor(subindex)->readIntExt(regadd); zcanbus_send_ack(cxt->packet, (uint8_t*)&val, sizeof(val)); } void LiquidCtrlBoardCmdProcesser::pump_ping(ProcessContext* cxt) { CHECK_PARAM_LEN(PRAAM_LEN(), 1); - - int32_t subindex = GET_PARAM(0); - - if (subindex >= DEVICE->motorNum()) { - zcanbus_send_errorack(cxt->packet, kerr_invalid_param); - return; - } - - if (!DEVICE->motor(subindex)->ping()) { - zcanbus_send_errorack(cxt->packet, kerr_motor_subdevice_offline); - return; - } + MOTOR_CHECK(); zcanbus_send_ack(cxt->packet, NULL, 0); } /*********************************************************************************************************************** * WS * ***********************************************************************************************************************/ -void LiquidCtrlBoardCmdProcesser::evaporation_bin_water_sensor_read_state(ProcessContext* cxt) { +void LiquidCtrlBoardCmdProcesser::evaporation_tank_water_sensor_read_state(ProcessContext* cxt) { int32_t val = DEVICE->m_evaporation_bin_water_sensor.read(); zcanbus_send_ack(cxt->packet, (uint8_t*)&val, sizeof(val)); } @@ -183,10 +225,35 @@ void LiquidCtrlBoardCmdProcesser::device_bottom_water_sensor_read_state(ProcessC /*********************************************************************************************************************** * PRESSURE * ***********************************************************************************************************************/ -void LiquidCtrlBoardCmdProcesser::pressure_sensor_bus_read_data(ProcessContext* cxt) {} -void LiquidCtrlBoardCmdProcesser::pressure_sensor_bus_set_report_period_ms(ProcessContext* cxt) {} +void LiquidCtrlBoardCmdProcesser::pressure_sensor_bus_read_data(ProcessContext* cxt) { + CHECK_PARAM_LEN(PRAAM_LEN(), 1); + int32_t index = GET_PARAM(0); + + int16_t val = 0; + int32_t reportVal = 0; + bool suc = DEVICE->pressureSensorBus()->readData(index, &val); + + reportVal = val; + + if (suc) { + zcanbus_send_ack(cxt->packet, (uint8_t*)&reportVal, sizeof(reportVal)); + } else { + zcanbus_send_errorack(cxt->packet, kerr_subdevice_offline); + } +} /*********************************************************************************************************************** * WARNING LIGHT * ***********************************************************************************************************************/ -void LiquidCtrlBoardCmdProcesser::triple_warning_light_ctl(ProcessContext* cxt) {} +void LiquidCtrlBoardCmdProcesser::triple_warning_light_ctl(ProcessContext* cxt) { + CHECK_PARAM_LEN(PRAAM_LEN(), 4); + + int32_t r = GET_PARAM(0); + int32_t g = GET_PARAM(1); + int32_t b = GET_PARAM(2); + int32_t warning = GET_PARAM(3); + + ZLOGI(TAG, "triple_warning_light_ctl: r:%d g:%d b:%d warning:%d", r, g, b, warning); + DEVICE->setRGB(r, g, b, warning); + zcanbus_send_ack(cxt->packet, NULL, 0); +} diff --git a/usrc/protocol_processer_impl/liquid_ctrl_board_cmd_processer.hpp b/usrc/protocol_processer_impl/liquid_ctrl_board_cmd_processer.hpp index 9242e94..61ffdb4 100644 --- a/usrc/protocol_processer_impl/liquid_ctrl_board_cmd_processer.hpp +++ b/usrc/protocol_processer_impl/liquid_ctrl_board_cmd_processer.hpp @@ -24,18 +24,21 @@ class LiquidCtrlBoardCmdProcesser : public IProtocolProcesser { virtual void process(ProcessContext* cxt) override; private: - void evaporation_bin_water_sensor_read_state(ProcessContext* cxt); + void evaporation_tank_water_sensor_read_state(ProcessContext* cxt); void device_bottom_water_sensor_read_state(ProcessContext* cxt); + void pump_rotate(ProcessContext* cxt); void pump_stop(ProcessContext* cxt); void pump_set_ihold_irun_idelay(ProcessContext* cxt); void pump_set_acc(ProcessContext* cxt); + void pump_set_ramp(ProcessContext* cxt); + void pump_set_tzw(ProcessContext* cxt); void pump_set_subic_reg(ProcessContext* cxt); void pump_get_subic_reg(ProcessContext* cxt); void pump_ping(ProcessContext* cxt); + void triple_warning_light_ctl(ProcessContext* cxt); void pressure_sensor_bus_read_data(ProcessContext* cxt); - void pressure_sensor_bus_set_report_period_ms(ProcessContext* cxt); }; } // namespace iflytop \ No newline at end of file diff --git a/usrc/protocol_processer_impl/public_cmd_processer.cpp b/usrc/protocol_processer_impl/public_cmd_processer.cpp index 812faa0..87838a0 100644 --- a/usrc/protocol_processer_impl/public_cmd_processer.cpp +++ b/usrc/protocol_processer_impl/public_cmd_processer.cpp @@ -3,6 +3,7 @@ using namespace iflytop; using namespace zscanprotocol; using namespace transmit_disfection_protocol; +#define ThisClass PublicCmdProcesser void PublicCmdProcesser::initialize() { REG_FN(read_board_info); diff --git a/usrc/protocol_processer_mgr/i_protocol_processer.hpp b/usrc/protocol_processer_mgr/i_protocol_processer.hpp index 729e8a8..0d74bbf 100644 --- a/usrc/protocol_processer_mgr/i_protocol_processer.hpp +++ b/usrc/protocol_processer_mgr/i_protocol_processer.hpp @@ -16,7 +16,7 @@ using namespace std; using namespace zscanprotocol; using namespace transmit_disfection_protocol; -#define REG_FN(fn) cmdprocesser.push_back(CmdProcesser(kfn_##fn, bind(&PublicCmdProcesser::fn, this, placeholders::_1))) +#define REG_FN(fn) cmdprocesser.push_back(CmdProcesser(kfn_##fn, bind(&ThisClass::fn, this, placeholders::_1))) #define GET_PARAM(off) ((((int32_t*)(cxt->packet->params))[off])) #define PRAAM_LEN() ((cxt->packetlen - sizeof(zcanbus_packet_t)) / 4) diff --git a/zsdk b/zsdk index c2a0f4d..ad5cb40 160000 --- a/zsdk +++ b/zsdk @@ -1 +1 @@ -Subproject commit c2a0f4d846f416a48f7cd6d4a22e2e7a8649ab40 +Subproject commit ad5cb407bff7a885afbef6104cb992a680f8e828