Browse Source

add liquid ctrl board cmd processer

master
zhaohe 1 year ago
parent
commit
6c6a7a1299
  1. 2
      app_protocols/transmit_disfection_protocol
  2. 267
      usrc/protocol_processer_impl/liquid_ctrl_board_cmd_processer.cpp
  3. 7
      usrc/protocol_processer_impl/liquid_ctrl_board_cmd_processer.hpp
  4. 1
      usrc/protocol_processer_impl/public_cmd_processer.cpp
  5. 2
      usrc/protocol_processer_mgr/i_protocol_processer.hpp
  6. 2
      zsdk

2
app_protocols/transmit_disfection_protocol

@ -1 +1 @@
Subproject commit 591e174e21072cc71d6288d6c5cfa00f34952e9b
Subproject commit 0cbbc25f3b6402ae5e5122ca6e3d2337b985f182

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

7
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

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

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

2
zsdk

@ -1 +1 @@
Subproject commit c2a0f4d846f416a48f7cd6d4a22e2e7a8649ab40
Subproject commit ad5cb407bff7a885afbef6104cb992a680f8e828
Loading…
Cancel
Save