// File: jkingan[P]ladyheather/heathmsc.cpp
// Function: temp_pid_filter()
void temp_pid_filter()
{
DATA_SIZE pwm_width;
char cmd[SLEN];

   #ifdef DEBUG_TEMP_CONTROL
      if(pid_debug || bang_bang) {
         show_pid_values();
      }
   #endif

   last_PID_error = PID_error;

   // PID_error is the curent temperature error (desired_temp - current_temp)
   // Negative values indicate the temperature is too high and needs to be 
   // lowered.  Positive values indicate the temperature is too low and needs
   // to be raised.
   //
   // FILTER_OFFSET is a temperature offset constant: negative values raise 
   // the temperature curve,  positive values lower it
   PID_error = (desired_temp + FILTER_OFFSET) - this_temperature;

   // integral_step is the clipped temperature error
   integral_step = PID_error; 
   if(integral_step > MAX_INTEGRAL) integral_step = MAX_INTEGRAL;  
   else if(integral_step < (-MAX_INTEGRAL)) integral_step = (-MAX_INTEGRAL);

   if((PID_out >= MAX_PID) && (integral_step > 0.0)) integral_step *= (-integrator_reset);
   else if((PID_out <= (-MAX_PID)) && (integral_step < 0.0)) integral_step *= (-integrator_reset);

   integrator += (integral_step * k4);
   if((integrator+k6) > (DATA_SIZE) 1.0) integrator = ((DATA_SIZE) 1.0 - k6);    // integrator value is maxed out
   else if((integrator+k6) < ((DATA_SIZE) -1.0)) integrator = ((DATA_SIZE) -1.0 - k6);

   PID_out = (k1*PID_error) + (k2*last_PID_error) + (k3*last_PID_out);
   last_PID_out = PID_out;
   if(PID_out > (DATA_SIZE) 0.0) PID_out -= k7;   // autotune debug - manual step size
   else                          PID_out += k7;
   PID_out += (integrator + k6);

   // clamp the filter response
   if(PID_out > (DATA_SIZE) MAX_PID) PID_out = (DATA_SIZE) MAX_PID;
   else if(PID_out < (DATA_SIZE) (-MAX_PID)) PID_out = (DATA_SIZE) (-MAX_PID);

   // convert filter output to PWM duty cycle times
   pwm_width = (-PID_out) * (DATA_SIZE) PWM_CYCLE_TIME;
   cool_time = (DATA_SIZE) PWM_CYCLE_TIME + pwm_width;
   heat_time = (DATA_SIZE) PWM_CYCLE_TIME - pwm_width;

//sprintf(debug_text, "width:%f  heat:%f(%d)  cool:%f(%d)", pwm_width, heat_time,(int)heat_time,cool_time, (int) cool_time);
   sprintf(cmd, "$CTRL %04d", (int) ((pwm_width/PWM_CYCLE_TIME)*(-1000.0)));
   send_fan_cmd(cmd);

   #ifdef DEBUG_TEMP_CONTROL
      if(pid_debug || bang_bang) {
         sprintf(debug_text,  "pwm=%.2f  heat:%.1f ms  cool:%.1f ms   PID(%.5f)  err(%.5f)  int=%.5f",
            pwm_width, heat_time,cool_time,  
            PID_out,  PID_error*((DATA_SIZE) -1.0),  integrator);
         last_PID_display =  PID_out;
      }
   #endif
}

// Assembly:
pushq %rbp
movq %rsp, %rbp
subq $0x110, %rsp            # imm = 0x110
leaq 0x234cae(%rip), %rax    # 0x39e820
cmpb $0x0, (%rax)
jne 0x169b83
leaq 0x234c9e(%rip), %rax    # 0x39e81c
cmpl $0x0, (%rax)
je 0x169b88
callq 0x1697c0
movsd 0x34b170(%rip), %xmm0   # 0x4b4d00
movsd %xmm0, 0x34b170(%rip)   # 0x4b4d08
leaq 0x235409(%rip), %rax    # 0x39efa8
movsd (%rax), %xmm0
leaq 0x234c9e(%rip), %rax    # 0x39e848
addsd (%rax), %xmm0
subsd 0x34b042(%rip), %xmm0   # 0x4b4bf8
movsd %xmm0, 0x34b142(%rip)   # 0x4b4d00
movsd 0x34b13a(%rip), %xmm0   # 0x4b4d00
movsd %xmm0, 0x34b14a(%rip)   # 0x4b4d18
movsd 0x34b142(%rip), %xmm0   # 0x4b4d18
movsd 0x5572(%rip), %xmm1     # 0x16f150
ucomisd %xmm1, %xmm0
jbe 0x169bf6
movsd 0x5564(%rip), %xmm0     # 0x16f150
movsd %xmm0, 0x34b124(%rip)   # 0x4b4d18
jmp 0x169c1a
movsd 0x7ba2(%rip), %xmm0     # 0x1717a0
ucomisd 0x34b112(%rip), %xmm0   # 0x4b4d18
jbe 0x169c18
movsd 0x7b90(%rip), %xmm0     # 0x1717a0
movsd %xmm0, 0x34b100(%rip)   # 0x4b4d18
jmp 0x169c1a
movsd 0x34b0ce(%rip), %xmm0   # 0x4b4cf0
movsd 0x25556(%rip), %xmm1    # 0x18f180
ucomisd %xmm1, %xmm0
jb 0x169c72
movsd 0x34b0e0(%rip), %xmm0   # 0x4b4d18
xorps %xmm1, %xmm1
ucomisd %xmm1, %xmm0
jbe 0x169c72
movsd 0x34b10f(%rip), %xmm0   # 0x4b4d58
movq %xmm0, %rax
movabsq $-0x8000000000000000, %rcx # imm = 0x8000000000000000
xorq %rcx, %rax
movq %rax, %xmm0
mulsd 0x34b0b0(%rip), %xmm0   # 0x4b4d18
movsd %xmm0, 0x34b0a8(%rip)   # 0x4b4d18
jmp 0x169cc2
movsd 0x2550e(%rip), %xmm0    # 0x18f188
ucomisd 0x34b06e(%rip), %xmm0   # 0x4b4cf0
jb 0x169cc0
xorps %xmm0, %xmm0
ucomisd 0x34b089(%rip), %xmm0   # 0x4b4d18
jbe 0x169cc0
movsd 0x34b0bf(%rip), %xmm0   # 0x4b4d58
movq %xmm0, %rax
movabsq $-0x8000000000000000, %rcx # imm = 0x8000000000000000
xorq %rcx, %rax
movq %rax, %xmm0
mulsd 0x34b060(%rip), %xmm0   # 0x4b4d18
movsd %xmm0, 0x34b058(%rip)   # 0x4b4d18
jmp 0x169cc2
movsd 0x34b04e(%rip), %xmm0   # 0x4b4d18
movsd 0x34b06e(%rip), %xmm2   # 0x4b4d40
movsd 0x34b046(%rip), %xmm1   # 0x4b4d20
mulsd %xmm2, %xmm0
addsd %xmm1, %xmm0
movsd %xmm0, 0x34b036(%rip)   # 0x4b4d20
movsd 0x34b02e(%rip), %xmm0   # 0x4b4d20
addsd 0x34b04e(%rip), %xmm0   # 0x4b4d48
movsd 0x5366(%rip), %xmm1     # 0x16f068
ucomisd %xmm1, %xmm0
jbe 0x169d22
movsd 0x5358(%rip), %xmm0     # 0x16f068
subsd 0x34b030(%rip), %xmm0   # 0x4b4d48
movsd %xmm0, 0x34b000(%rip)   # 0x4b4d20
jmp 0x169d5a
movsd 0x34aff6(%rip), %xmm1   # 0x4b4d20
addsd 0x34b016(%rip), %xmm1   # 0x4b4d48
movsd 0x53de(%rip), %xmm0     # 0x16f118
ucomisd %xmm1, %xmm0
jbe 0x169d58
movsd 0x53d0(%rip), %xmm0     # 0x16f118
subsd 0x34aff8(%rip), %xmm0   # 0x4b4d48
movsd %xmm0, 0x34afc8(%rip)   # 0x4b4d20
jmp 0x169d5a
movsd 0x34afc6(%rip), %xmm1   # 0x4b4d28
movsd 0x34af96(%rip), %xmm2   # 0x4b4d00
movsd 0x34afbe(%rip), %xmm0   # 0x4b4d30
mulsd 0x34af8e(%rip), %xmm0   # 0x4b4d08
mulsd %xmm2, %xmm1
addsd %xmm0, %xmm1
movsd 0x34afae(%rip), %xmm0   # 0x4b4d38
movsd 0x34af66(%rip), %xmm2   # 0x4b4cf8
mulsd %xmm2, %xmm0
addsd %xmm1, %xmm0
movsd %xmm0, 0x34af4e(%rip)   # 0x4b4cf0
movsd 0x34af46(%rip), %xmm0   # 0x4b4cf0
movsd %xmm0, 0x34af46(%rip)   # 0x4b4cf8
movsd 0x34af36(%rip), %xmm0   # 0x4b4cf0
xorps %xmm1, %xmm1
ucomisd %xmm1, %xmm0
jbe 0x169de1
movsd 0x34af85(%rip), %xmm1   # 0x4b4d50
movsd 0x34af1d(%rip), %xmm0   # 0x4b4cf0
subsd %xmm1, %xmm0
movsd %xmm0, 0x34af11(%rip)   # 0x4b4cf0
jmp 0x169df9
movsd 0x34af67(%rip), %xmm0   # 0x4b4d50
addsd 0x34aeff(%rip), %xmm0   # 0x4b4cf0
movsd %xmm0, 0x34aef7(%rip)   # 0x4b4cf0
movsd 0x34af1f(%rip), %xmm0   # 0x4b4d20
addsd 0x34af3f(%rip), %xmm0   # 0x4b4d48
addsd 0x34aedf(%rip), %xmm0   # 0x4b4cf0
movsd %xmm0, 0x34aed7(%rip)   # 0x4b4cf0
movsd 0x34aecf(%rip), %xmm0   # 0x4b4cf0
movsd 0x25357(%rip), %xmm1    # 0x18f180
ucomisd %xmm1, %xmm0
jbe 0x169e41
movsd 0x25349(%rip), %xmm0    # 0x18f180
movsd %xmm0, 0x34aeb1(%rip)   # 0x4b4cf0
jmp 0x169e65
movsd 0x2533f(%rip), %xmm0    # 0x18f188
ucomisd 0x34ae9f(%rip), %xmm0   # 0x4b4cf0
jbe 0x169e63
movsd 0x2532d(%rip), %xmm0    # 0x18f188
movsd %xmm0, 0x34ae8d(%rip)   # 0x4b4cf0
jmp 0x169e65
movsd 0x34ae83(%rip), %xmm0   # 0x4b4cf0
movq %xmm0, %rax
movabsq $-0x8000000000000000, %rcx # imm = 0x8000000000000000
xorq %rcx, %rax
movq %rax, %xmm0
movsd 0x25304(%rip), %xmm1    # 0x18f190
mulsd %xmm1, %xmm0
movsd %xmm0, -0x8(%rbp)
movsd 0x252f3(%rip), %xmm0    # 0x18f190
addsd -0x8(%rbp), %xmm0
movsd %xmm0, 0x34ad8e(%rip)   # 0x4b4c38
movsd 0x252de(%rip), %xmm0    # 0x18f190
subsd -0x8(%rbp), %xmm0
movsd %xmm0, 0x34ad71(%rip)   # 0x4b4c30
leaq -0x110(%rbp), %rdi
movsd -0x8(%rbp), %xmm0
movsd 0x252bd(%rip), %xmm1    # 0x18f190
divsd %xmm1, %xmm0
movsd 0x5221(%rip), %xmm1     # 0x16f100
mulsd %xmm1, %xmm0
cvttsd2si %xmm0, %edx
leaq 0x2eb8c(%rip), %rsi     # 0x198a7a
movb $0x0, %al
callq 0x9060
leaq -0x110(%rbp), %rdi
callq 0x83980
leaq 0x234918(%rip), %rax    # 0x39e820
cmpb $0x0, (%rax)
jne 0x169f19
leaq 0x234908(%rip), %rax    # 0x39e81c
cmpl $0x0, (%rax)
je 0x169f73
movsd -0x8(%rbp), %xmm0
movsd 0x34ad0a(%rip), %xmm1   # 0x4b4c30
movsd 0x34ad0a(%rip), %xmm2   # 0x4b4c38
movsd 0x34adba(%rip), %xmm3   # 0x4b4cf0
movsd 0x51da(%rip), %xmm4     # 0x16f118
mulsd 0x34adba(%rip), %xmm4   # 0x4b4d00
movsd 0x34add2(%rip), %xmm5   # 0x4b4d20
leaq 0x233c2b(%rip), %rdi    # 0x39db80
leaq 0x2eb29(%rip), %rsi     # 0x198a85
movb $0x6, %al
callq 0x9060
movsd 0x34ad85(%rip), %xmm0   # 0x4b4cf0
movsd %xmm0, 0x34ad9d(%rip)   # 0x4b4d10
addq $0x110, %rsp            # imm = 0x110
popq %rbp
retq
nopl (%rax)