68 lines
1.9 KiB
C
68 lines
1.9 KiB
C
/**
|
|
* @file temperature_control.c
|
|
* @brief Temperature-control services for the laser TEC loops.
|
|
*/
|
|
|
|
#include "temperature_control.h"
|
|
|
|
uint16_t temperature_control_compute_pid(const laser_channel_config_t *channel_config,
|
|
laser_runtime_t *runtime_state,
|
|
uint8_t channel_index,
|
|
uint32_t current_tick_1ms,
|
|
uint32_t *shared_pid_reference_tick)
|
|
{
|
|
int32_t error;
|
|
float proportional_gain;
|
|
float integral_term;
|
|
int32_t output;
|
|
|
|
if ((channel_config == NULL) || (runtime_state == NULL) || (shared_pid_reference_tick == NULL))
|
|
{
|
|
return 32768u;
|
|
}
|
|
|
|
error = (int32_t)runtime_state->current_temperature_raw - (int32_t)channel_config->target_temperature_raw;
|
|
integral_term = runtime_state->integral_error;
|
|
|
|
if ((error < 3000) && (error > -3000))
|
|
{
|
|
integral_term += channel_config->pid_i *
|
|
(float)error *
|
|
(float)(current_tick_1ms - *shared_pid_reference_tick) /
|
|
100.0f;
|
|
}
|
|
|
|
proportional_gain = channel_config->pid_p;
|
|
|
|
if (integral_term > 32000.0f)
|
|
{
|
|
integral_term = 32000.0f;
|
|
}
|
|
else if (integral_term < -32000.0f)
|
|
{
|
|
integral_term = -32000.0f;
|
|
}
|
|
|
|
runtime_state->integral_error = integral_term;
|
|
|
|
output = 32768 + (int32_t)(proportional_gain * (float)error) + (int32_t)integral_term;
|
|
|
|
if (output < 1000)
|
|
{
|
|
output = 8800;
|
|
}
|
|
else if (output > 56800)
|
|
{
|
|
output = 56800;
|
|
}
|
|
|
|
/* Both PID channels use a shared timing reference updated after the
|
|
* second TEC computation. This preserves the original controller timing. */
|
|
if (channel_index == 2u)
|
|
{
|
|
*shared_pid_reference_tick = current_tick_1ms;
|
|
}
|
|
|
|
return (uint16_t)output;
|
|
}
|