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