firmware-nucleo/App/Src/task_temp_ctrl.c (4471B)
1 /* 2 * Copyright (c) 2026 Martin Javier Klöckner <mklockner@fi.uba.ar> 3 * 4 * See file `LICENSE` for full details 5 */ 6 7 #include <stdio.h> 8 #include <stdint.h> 9 #include <stdbool.h> 10 #include <string.h> 11 12 #include "main.h" 13 #include "app.h" 14 #include "task_temp_ctrl.h" 15 #include "logger.h" 16 17 #define G_TASK_TEMP_CTRL_TICK_CNT_INI 0ul 18 19 #define G_TASK_TEMP_CTRL_PID_KP 40.0f 20 #define G_TASK_TEMP_CTRL_PID_KI 0.75f 21 #define G_TASK_TEMP_CTRL_PID_KD 0.01f 22 #define G_TASK_TEMP_CTRL_PID_DT 250 // 100 ms 23 24 // Private data definition 25 typedef struct { 26 TIM_HandleTypeDef *htim; 27 uint32_t tim_channel; 28 } task_temp_ctrl_cfg_t; 29 30 typedef struct { 31 float pid_kp, pid_ki, pid_kd; 32 float pid_p, pid_i, pid_d; 33 float error, prev_error; 34 uint32_t tick; 35 } task_temp_ctrl_pid_t; 36 37 static const task_temp_ctrl_cfg_t task_temp_ctrl_cfg = {&htim1, TIM_CHANNEL_1}; 38 static task_temp_ctrl_pid_t task_temp_ctrl_pid = { 39 G_TASK_TEMP_CTRL_PID_KP, G_TASK_TEMP_CTRL_PID_KI, G_TASK_TEMP_CTRL_PID_KD 40 }; 41 42 // Private functions declaration 43 static inline int16_t clampf(float val, float min, float max); 44 static void task_temp_ctrl_set_duty(TIM_HandleTypeDef *htim, uint32_t ch, uint8_t duty); 45 46 // Public data definition 47 volatile uint32_t g_task_temp_ctrl_tick_cnt; // Incremented by HAL_SysTick callback 48 49 // Public functions definition 50 void task_temp_ctrl_init(void *parameters) 51 { 52 LOGGER_INFO("Initializing `task_temp_ctrl`..."); 53 54 const task_temp_ctrl_cfg_t *p_task_cfg = &task_temp_ctrl_cfg; 55 shared_data_type *p_shared_data = (shared_data_type *)parameters; 56 57 task_temp_ctrl_set_duty(p_task_cfg->htim, 58 p_task_cfg->tim_channel, 59 p_shared_data->temp_ctrl_pwm_dc); 60 61 // __HAL_TIM_SET_COMPARE(p_task_cfg->htim, 62 // p_task_cfg->tim_channel, 63 // p_shared_data->temp_ctrl_pwm_dc); 64 HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_1); 65 66 LOGGER_INFO("Done initializing `task_temp_ctrl`"); 67 } 68 69 void task_temp_ctrl_update(void *parameters) 70 { 71 bool b_time_update_required; 72 shared_data_type *p_shared_data = (shared_data_type *)parameters; 73 const task_temp_ctrl_cfg_t *p_task_cfg = &task_temp_ctrl_cfg; 74 task_temp_ctrl_pid_t *p_pid = &task_temp_ctrl_pid; 75 76 __asm("CPSID i"); 77 if (G_TASK_TEMP_CTRL_TICK_CNT_INI < g_task_temp_ctrl_tick_cnt) 78 { 79 g_task_temp_ctrl_tick_cnt--; 80 b_time_update_required = true; 81 } 82 __asm("CPSIE i"); 83 84 while (b_time_update_required) 85 { 86 __asm("CPSID i"); 87 if (G_TASK_TEMP_CTRL_TICK_CNT_INI < g_task_temp_ctrl_tick_cnt) 88 { 89 g_task_temp_ctrl_tick_cnt--; 90 b_time_update_required = true; 91 } 92 else 93 { 94 b_time_update_required = false; 95 } 96 __asm("CPSIE i"); 97 98 if (false == p_shared_data->temp_ctrl_enabled) 99 { 100 if (0 != __HAL_TIM_GET_COMPARE(p_task_cfg->htim, p_task_cfg->tim_channel)) 101 { 102 task_temp_ctrl_set_duty(p_task_cfg->htim, p_task_cfg->tim_channel, 0); 103 } 104 } 105 else 106 { 107 if (0 < p_pid->tick) 108 { 109 p_pid->tick--; 110 } 111 else 112 { 113 p_pid->tick = G_TASK_TEMP_CTRL_PID_DT; 114 float dt = (p_pid->tick / 1000.0f); 115 116 p_pid->prev_error = p_pid->error; 117 p_pid->error = p_shared_data->temp_set_point - p_shared_data->temp_sensor; 118 119 p_pid->pid_p = p_pid->error * p_pid->pid_kp; 120 p_pid->pid_i = (p_pid->pid_i + (p_pid->error*dt)) * p_pid->pid_ki; 121 p_pid->pid_d = ((p_pid->error - p_pid->prev_error)/dt) * p_pid->pid_kd; 122 123 p_shared_data->temp_ctrl_pwm_dc = clampf( 124 p_pid->pid_p + p_pid->pid_i + p_pid->pid_d, 125 0.0f, 126 100.0f); 127 128 task_temp_ctrl_set_duty(p_task_cfg->htim, 129 p_task_cfg->tim_channel, 130 p_shared_data->temp_ctrl_pwm_dc); 131 } 132 } 133 } 134 } 135 136 // Public functions declaration 137 static inline int16_t clampf(float val, float min, float max) 138 { 139 return (val > max) ? max : ((val < min) ? min : val); 140 } 141 142 static void task_temp_ctrl_set_duty(TIM_HandleTypeDef *htim, uint32_t ch, uint8_t duty) 143 { 144 uint32_t ARR = __HAL_TIM_GET_AUTORELOAD(htim); // Auto Reload Reg 145 uint32_t CCR = (uint32_t)((ARR + 1) * duty / 100); // Capture/Compare Reg 146 __HAL_TIM_SET_COMPARE(htim, ch, CCR); 147 }