From c97a4f6f0136f194761ddc7c14efea0a8a172cb2 Mon Sep 17 00:00:00 2001 From: zhaohe Date: Sun, 19 Nov 2023 18:11:22 +0800 Subject: [PATCH] update --- chip/zpwm_generator_muti_channel.cpp | 2 +- chip/zpwm_generator_muti_channel.hpp | 3 +- components/algorithm/pid_module.cpp | 25 ++--- components/algorithm/pid_module.hpp | 2 +- .../subcanmodule/zcancmder_subboard_initer.cpp | 3 +- components/ti/drv8710.cpp | 5 +- components/ti/drv8710.hpp | 1 + .../pwm_ctrl_module.cpp | 21 ++-- .../water_cooling_temperature_control_module.cpp | 113 +++++++++++++++------ .../water_cooling_temperature_control_module.hpp | 3 + 10 files changed, 124 insertions(+), 54 deletions(-) diff --git a/chip/zpwm_generator_muti_channel.cpp b/chip/zpwm_generator_muti_channel.cpp index 9d87534..7381499 100644 --- a/chip/zpwm_generator_muti_channel.cpp +++ b/chip/zpwm_generator_muti_channel.cpp @@ -75,7 +75,7 @@ void ZPWMGeneratorMutiChannel::updatePWMState(int32_t channelindex /*1 void ZPWMGeneratorMutiChannel::startPWM(int32_t channelindex /*1，2，3，4*/, float duty) { if (channelindex < 1 || channelindex > 4) return; m_pwmstate[channelindex] = true; - if (m_enablelog) ZEARLY_LOGI(TAG, "[%s] setpwm tim:%s ch:%d duty:%f", m_cfg.name, chip_tim_get_name(m_htim->Instance), channelindex, duty); + if (m_cfg.calltrace) ZEARLY_LOGI(TAG, "[%s] setpwm tim:%s ch:%d duty:%f", m_cfg.name, chip_tim_get_name(m_htim->Instance), channelindex, duty); updatePWMState(channelindex, duty); } void ZPWMGeneratorMutiChannel::stopPWM(int32_t channelindex) { diff --git a/chip/zpwm_generator_muti_channel.hpp b/chip/zpwm_generator_muti_channel.hpp index cdf834b..260eb3e 100644 --- a/chip/zpwm_generator_muti_channel.hpp +++ b/chip/zpwm_generator_muti_channel.hpp @@ -27,7 +27,6 @@ class ZPWMGeneratorMutiChannel { float m_freq; bool m_pwmstate[4] = {0}; - bool m_enablelog = false; hardware_config_t m_cfg; @@ -41,7 +40,7 @@ class ZPWMGeneratorMutiChannel { * @param Polarity 1:高为有效电平，0：低为有效电平 */ void initialize(hardware_config_t *cfg); - void settrace(bool enable) { m_enablelog = enable; } + void settrace(bool enable) { m_cfg.calltrace = enable; } /******************************************************************************* * PWM_MODE * *******************************************************************************/ diff --git a/components/algorithm/pid_module.cpp b/components/algorithm/pid_module.cpp index c48d200..2a3e44f 100644 --- a/components/algorithm/pid_module.cpp +++ b/components/algorithm/pid_module.cpp @@ -19,34 +19,35 @@ void PidModule::initialize(config_t *cfg) { int32_t PidModule::reset() { m_last_output = 0; m_previous_err1 = 0; - m_previous_err2 = 0; m_integral_err = 0; return 0; } -float PidModule::pid_calc_incremental(float error) { +#define MAX(a,b) ((a) > (b) ? (a) : (b)) +#define MIN(a,b) ((a) < (b) ? (a) : (b)) +float PidModule::pid_calc_positional(float error) { float output = 0; + /* Add current error to the integral error */ + m_integral_err += error; + /* If the integral error is out of the range, it will be limited */ + m_integral_err = MIN(m_integral_err, m_cfg.max_integral); + m_integral_err = MAX(m_integral_err, m_cfg.min_integral); - /* Calculate the pid control value by increment formula */ - /* du(k) = (e(k)-e(k-1))*Kp + (e(k)-2*e(k-1)+e(k-2))*Kd + e(k)*Ki */ - /* u(k) = du(k) + u(k-1) */ - output = (error - m_previous_err1) * m_cfg.kp + (error - 2 * m_previous_err1 + m_previous_err2) * m_cfg.kd + error * m_cfg.ki + m_last_output; + /* Calculate the pid control value by location formula */ + /* u(k) = e(k)*Kp + (e(k)-e(k-1))*Kd + integral*Ki */ + output = error * m_cfg.kp + (error - m_previous_err1) * m_cfg.kd + m_integral_err * m_cfg.ki; - /* If the output is beyond the range, it will be limited */ + /* If the output is out of the range, it will be limited */ output = MIN(output, m_cfg.max_output); output = MAX(output, m_cfg.min_output); /* Update previous error */ - m_previous_err2 = m_previous_err1; m_previous_err1 = error; - /* Update last output */ - m_last_output = output; - return output; } int32_t PidModule::compute(float input_error, float *ret_result) { - *ret_result = pid_calc_incremental(input_error); + *ret_result = pid_calc_positional(input_error); return 0; } diff --git a/components/algorithm/pid_module.hpp b/components/algorithm/pid_module.hpp index aedba2d..50a9ded 100644 --- a/components/algorithm/pid_module.hpp +++ b/components/algorithm/pid_module.hpp @@ -49,6 +49,6 @@ class PidModule { void set_integral_err(float v); private: - float pid_calc_incremental(float error); + float pid_calc_positional(float error); }; } // namespace iflytop diff --git a/components/subcanmodule/zcancmder_subboard_initer.cpp b/components/subcanmodule/zcancmder_subboard_initer.cpp index 56cb66d..87920d3 100644 --- a/components/subcanmodule/zcancmder_subboard_initer.cpp +++ b/components/subcanmodule/zcancmder_subboard_initer.cpp @@ -83,8 +83,9 @@ void ZCancmderSubboardIniter::register_module(ZIModule* module) { void ZCancmderSubboardIniter::loop() { while (true) { + OSDefaultSchduler::getInstance()->loop(); zcanCmder.loop(); cmder.schedule(); } -} \ No newline at end of file +} diff --git a/components/ti/drv8710.cpp b/components/ti/drv8710.cpp index 9668d8b..9a7f233 100644 --- a/components/ti/drv8710.cpp +++ b/components/ti/drv8710.cpp @@ -18,7 +18,8 @@ void DRV8710::initialize(config_t* cfg) { // m_in2.initAsOutput(cfg->in2, ZGPIO::kMode_nopull, false, false); m_in2.enableTrace(cfg->enableTrace); - enable(false); + osDelay(5); + enable(true); } void DRV8710::moveForward(int32_t duty) { @@ -42,6 +43,8 @@ void DRV8710::moveBackward(int32_t duty) { bool DRV8710::isFault() { return !m_nfault.getState(); } +void DRV8710::clearFault() { move(0); } + void DRV8710::move(int32_t duty) { if (duty >= 0) { moveForward(duty); diff --git a/components/ti/drv8710.hpp b/components/ti/drv8710.hpp index aaa382c..973b304 100644 --- a/components/ti/drv8710.hpp +++ b/components/ti/drv8710.hpp @@ -62,6 +62,7 @@ class DRV8710 { void move(int32_t duty); // -100->100 bool isFault(); + void clearFault() ; bool getSensePinState() { return m_nsensePin.getState(); } private: diff --git a/components/water_cooling_temperature_control_module/pwm_ctrl_module.cpp b/components/water_cooling_temperature_control_module/pwm_ctrl_module.cpp index 2083cbc..79fe8fc 100644 --- a/components/water_cooling_temperature_control_module/pwm_ctrl_module.cpp +++ b/components/water_cooling_temperature_control_module/pwm_ctrl_module.cpp @@ -37,7 +37,6 @@ void PWMSpeedCtrlModule::initialize(config_t* pcfg) { } cfg.pwm_cfg.calltrace = pcfg->enableTrace; m_fanCtrlPwm.initialize(&cfg.pwm_cfg); - m_fanCtrlPwm.settrace(pcfg->enableTrace); OSDefaultSchduler::getInstance()->regPeriodJob([this](OSDefaultSchduler::Context&) { checkfanState(); }, 1000); } @@ -51,12 +50,22 @@ int32_t PWMSpeedCtrlModule::startModule(int32_t duty) { // ZLOGI("startModule duty:%d", duty); zlock_guard lck(m_mutex); if (duty < 0) duty = 0; - fanisworking = true; + if (duty > 100) duty = 100; + if (duty == 0) { + stopModule(); + return 0; + } + + if (!fanisworking) { + fanisworking = true; + clearError(); + startFanPWM(duty); + powerON(); + m_lastcheckTime = zos_get_tick(); + } else { + startFanPWM(duty); + } - clearError(); - startFanPWM(duty); - powerON(); - m_lastcheckTime = zos_get_tick(); return 0; } int32_t PWMSpeedCtrlModule::stopModule() { diff --git a/components/water_cooling_temperature_control_module/water_cooling_temperature_control_module.cpp b/components/water_cooling_temperature_control_module/water_cooling_temperature_control_module.cpp index b60aa52..4c2c726 100644 --- a/components/water_cooling_temperature_control_module/water_cooling_temperature_control_module.cpp +++ b/components/water_cooling_temperature_control_module/water_cooling_temperature_control_module.cpp @@ -30,26 +30,40 @@ void WaterCoolingTemperatureControlModule::initialize(int32_t id, config_t* cfg, m_pidmodule.initialize(&cfg->pid_cfg); m_thread.init("WaterCoolingTemperatureControlModule"); - m_cfg = *cfg; + m_cfg = *cfg; + m_target_temperature = 25; + m_temperature_sensor_mutex.init(); + m_fan_ctrl->stop(); + m_pelter_ctrl->stop(); + m_pump_ctrl->stop(); + module_clear_error(); ZLOGI(TAG, "WaterCoolingTemperatureControlModule initialized..."); } int32_t WaterCoolingTemperatureControlModule::getid(int32_t* id) { *id = m_id; return 0; } -int32_t WaterCoolingTemperatureControlModule::module_clear_error() { return 0; } +int32_t WaterCoolingTemperatureControlModule::module_clear_error() { + m_com_reg.module_errorcode = 0; + m_com_reg.module_errorbitflag0 = 0; + m_fan_ctrl->clearError(); + m_pelter_ctrl->clearError(); + m_pump_ctrl->clearError(); + return 0; +} void WaterCoolingTemperatureControlModule::createDefaultConfig(config_t* cfg) { cfg->fanlevel = 90; + cfg->min_fanlevel = 30; cfg->pumplevel = 100; - cfg->pid_cfg.kp = 9; - cfg->pid_cfg.ki = 0.600; + cfg->pid_cfg.kp = 10; + cfg->pid_cfg.ki = 0.5; cfg->pid_cfg.kd = 0; - cfg->pid_cfg.max_output = 90; - cfg->pid_cfg.min_output = -90; - cfg->pid_cfg.max_integral = 100; - cfg->pid_cfg.min_integral = -100; + cfg->pid_cfg.max_output = 100; + cfg->pid_cfg.min_output = -100; + cfg->pid_cfg.max_integral = 200; + cfg->pid_cfg.min_integral = -200; cfg->pidcompute_periodms = 1000; } @@ -120,7 +134,17 @@ int32_t WaterCoolingTemperatureControlModule::do_action(int32_t actioncode) { int32_t WaterCoolingTemperatureControlModule::module_factory_reset() { return 0; } int32_t WaterCoolingTemperatureControlModule::module_flush_cfg() { return 0; } -int32_t WaterCoolingTemperatureControlModule::module_active_cfg() { return 0; } +int32_t WaterCoolingTemperatureControlModule::module_active_cfg() { + // m_pidmodule + m_pidmodule.update_kp(m_cfg.pid_cfg.kp); + m_pidmodule.update_kd(m_cfg.pid_cfg.kd); + m_pidmodule.update_ki(m_cfg.pid_cfg.ki); + m_pidmodule.update_max_output(m_cfg.pid_cfg.max_output); + m_pidmodule.update_min_output(m_cfg.pid_cfg.min_output); + m_pidmodule.update_max_integral(m_cfg.pid_cfg.max_integral); + m_pidmodule.update_min_integral(m_cfg.pid_cfg.min_integral); + return 0; +} int32_t WaterCoolingTemperatureControlModule::module_start() { m_thread.stop(); @@ -148,32 +172,41 @@ void WaterCoolingTemperatureControlModule::workloop() { pump_start(m_cfg.pumplevel); fan_start(m_cfg.fanlevel); - + int32_t i = 0; + uint32_t startticket = zos_get_tick(); while (!m_thread.getExitFlag()) { - if (checkdevice() != 0) { - ZLOGE(TAG, "somedevice is not working, stop the module"); - break; + if (zos_haspassedms(startticket) > 1000) { + if (checkdevice() != 0) { + ZLOGE(TAG, "somedevice is not working, stop the module"); + break; + } } - // m_thread.sleep(m_cfg.pidcompute_periodms); - m_thread.sleep(100); + m_thread.sleep(m_cfg.pidcompute_periodms); - // float val = read_pid_temperature(); + float val = read_pid_temperature(); - // float error = m_target_temperature - val; - // float out = 0; - // m_pidmodule.compute(error, &out); + float error = m_target_temperature - val; + float out = 0; + m_pidmodule.compute(error, &out); if (m_enable_log) { - // ZLOGI(TAG, "temperature: %.2f %.2f integral_err:%.2f out:%d", m_target_temperature, val, m_pidmodule.get_integral_err(), (int32_t)out); - ZLOGI(TAG, "temperature: %.2f", read_pid_temperature()); + ZLOGI(TAG, "temperature: %.2f %.2f integral_err:%.2f out:%d", m_target_temperature, val, m_pidmodule.get_integral_err(), (int32_t)out); } - // peltier_set_power_level(out); + peltier_set_power_level(out); + i++; + if (i % 10 == 0) { + int32_t fanspeed = out * 2; + if (fanspeed > 100) fanspeed = 100; + if (fanspeed < m_cfg.min_fanlevel) fanspeed = m_cfg.min_fanlevel; + fan_start(fanspeed); + } } pump_stop(); fan_stop(); + peltier_set_power_level(0); } #define BIT(x, n) ((x >> n) & 0x01) @@ -181,8 +214,11 @@ void WaterCoolingTemperatureControlModule::workloop() { int32_t WaterCoolingTemperatureControlModule::checkdevice() { m_com_reg.module_errorbitflag0 = geterrorbitflag0(); if (m_com_reg.module_errorbitflag0 != 0) { - ZLOGE(TAG, "checkdevice errorbitflag0:%d %d %d", // - BIT(m_com_reg.module_errorbitflag0, 0), BIT(m_com_reg.module_errorbitflag0, 1), BIT(m_com_reg.module_errorbitflag0, 2)); + ZLOGE(TAG, "checkdevice errorbitflag0:%d %d %d %d", // + BIT(m_com_reg.module_errorbitflag0, 0), // + BIT(m_com_reg.module_errorbitflag0, 1), // + BIT(m_com_reg.module_errorbitflag0, 2), // + BIT(m_com_reg.module_errorbitflag0, 3)); m_pelter_ctrl->dumpErrorInfo(); m_com_reg.module_errorcode = err::khwardware_error; return 1; @@ -195,13 +231,26 @@ int32_t WaterCoolingTemperatureControlModule::checkdevice() { *******************************************************************************/ float WaterCoolingTemperatureControlModule::read_pid_temperature() { + zlock_guard lock(m_temperature_sensor_mutex); ZITemperatureSensor* sensor = m_temperature_sensor[m_hardwared_config.temp_fb_index]; - float val = sensor->getTemperature(); - if (sensor->isError()) { - ZLOGW(TAG, "get temperature fail %s", sensor->errorInfo()); - return 0; + float val[3] = {0}; + for (int32_t i = 0; i < 3; i++) { + val[i] = sensor->getTemperature(); + osDelay(10); + } + // 对温度进行排序 + for (int32_t i = 0; i < 3; i++) { + for (int32_t j = i + 1; j < 3; j++) { + if (val[i] > val[j]) { + float tmp = val[i]; + val[i] = val[j]; + val[j] = tmp; + } + } } - return val; + + float retval = val[1]; + return retval; } void WaterCoolingTemperatureControlModule::pump_start(int32_t pump_speed) { @@ -230,7 +279,7 @@ void WaterCoolingTemperatureControlModule::fan_stop() { } void WaterCoolingTemperatureControlModule::peltier_set_power_level(int32_t level) { - ZLOGI(TAG, "peltier_set_power_level %d", level); + // ZLOGI(TAG, "peltier_set_power_level %d", level); m_pelter_ctrl->setSpeed(level); // for (int32_t i = 0; i < m_peltier_ctrl_num; i++) { // m_peltier_ctrl[i]->move(level); @@ -261,10 +310,13 @@ int32_t WaterCoolingTemperatureControlModule::geterrorbitflag0() { /** * @brief */ + ZITemperatureSensor* sensor = m_temperature_sensor[m_hardwared_config.temp_fb_index]; + int32_t errorbitflag = 0; errorbitflag |= m_fan_ctrl->isError() << 0; errorbitflag |= m_pump_ctrl->isError() << 1; errorbitflag |= m_pelter_ctrl->isError() << 2; + errorbitflag |= sensor->isError() << 3; // if (m_fanTable[1]) errorbitflag |= m_fanTable[1]->isError() << 1; // if (m_fanTable[2]) errorbitflag |= m_fanTable[2]->isError() << 2; @@ -278,6 +330,7 @@ int32_t WaterCoolingTemperatureControlModule::geterrorbitflag0() { } float WaterCoolingTemperatureControlModule::getTemperatureSensorVal(int32_t index) { + zlock_guard lock(m_temperature_sensor_mutex); if (index < 0 || index >= m_n_temperature_sensor) { return 0; } diff --git a/components/water_cooling_temperature_control_module/water_cooling_temperature_control_module.hpp b/components/water_cooling_temperature_control_module/water_cooling_temperature_control_module.hpp index 496c72d..55213ac 100644 --- a/components/water_cooling_temperature_control_module/water_cooling_temperature_control_module.hpp +++ b/components/water_cooling_temperature_control_module/water_cooling_temperature_control_module.hpp @@ -40,6 +40,7 @@ class WaterCoolingTemperatureControlModule : public ZIModule { typedef struct { PidModule::config_t pid_cfg; int32_t fanlevel; + int32_t min_fanlevel; int32_t pumplevel; int32_t pidcompute_periodms; } config_t; @@ -70,6 +71,8 @@ class WaterCoolingTemperatureControlModule : public ZIModule { int32_t m_enable_log = 0; float m_target_temperature = 0; + zmutex m_temperature_sensor_mutex; + public: WaterCoolingTemperatureControlModule(){}; virtual ~WaterCoolingTemperatureControlModule(){};