update

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

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                                   *
    *******************************************************************************/

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

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

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();
   }
-}
+}

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

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:

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() {

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

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(){};