diff --git a/pid.c b/pid.c --- a/pid.c +++ b/pid.c @@ -11,8 +11,14 @@ void pid_init(pid_state_t* state) int16_t pid_update(therm_settings_t* set, therm_status_t* status, pid_state_t *state) { + + // Convert temperature to fixed point number with 1/10th resolution + int8_t temp_frac = status->temp_frac > 9 ? status->temp_frac / 10 : status->temp_frac; + temp_frac = status->temp > 0 ? temp_frac : temp_frac * -1; + int32_t temp = (status->temp * 10) + temp_frac; + // Calculate instantaneous error - int16_t error = status->setpoint - status->temp; // TODO: Use fixed point fraction + int16_t error = status->setpoint * 10 - temp; // TODO: Use fixed point fraction // Proportional component int32_t p_term = set->val.k_p * error; @@ -26,7 +32,7 @@ int16_t pid_update(therm_settings_t* set // it cant help be cold despite its best efforts) // not necessary, but this makes windup guard values // relative to the current iGain - int32_t windup_guard_res = set->val.windup_guard / set->val.k_i; + int32_t windup_guard_res = (set->val.windup_guard * 10) / set->val.k_i; // Calculate integral term with windup guard if (state->i_state > windup_guard_res) @@ -37,13 +43,12 @@ int16_t pid_update(therm_settings_t* set int32_t i_term = set->val.k_i * state->i_state; // Calculate differential term (slope since last iteration) - int32_t d_term = (set->val.k_d * (status->temp - state->last_pid_temp)); + int32_t d_term = (set->val.k_d * (temp - state->last_pid_temp)); // Save temperature for next iteration - state->last_pid_temp = status->temp; - state->last_pid_temp_frac = status->temp_frac; + state->last_pid_temp = temp; - int16_t result = p_term + i_term - d_term; + int16_t result = (p_term + i_term - d_term) / 10; // Put out tenths of percent, 0-1000. if(result > 1000)