pid.jal

var sbyte*FXWIDTH proportional_constant = word_to_fx(4)
var sbyte*FXWIDTH integration_time      = word_to_fx(10)
var sbyte*FXWIDTH differentiation_time  = word_to_fx(2)
var sbyte*FXWIDTH alpha                 = fx_tensfracs[0]
var sbyte*FXWIDTH beta                  = fx_one
var sbyte*FXWIDTH gamma                 = 0
var sbyte*FXWIDTH filter_time           = mult_fx(alpha, differentiation_time)
var sbyte*FXWIDTH output_value          = 0
var sbyte*FXWIDTH last_last_error_d_f   = 0
var sbyte*FXWIDTH last_error_d_f        = 0
var sbyte*FXWIDTH last_error_p          = 0



-- if we multiply clock_absolute by (2**32/1000 = 4292967.295 = 0x00418937.4b851eb8) we get fractional secs
--var sbyte*FXWIDTH now_mult = word_to_fx(4292967)
--now_mult = now_mult + 0x4b851eb8 
--return mult_fx(sbyte*8(clock_absolute), now_mult) -- BAD ** accumulates fractional error. 

function now return sbyte*FXWIDTH is
	return div_fx(sbyte*FXWIDTH(clock_absolute) << FXFRACBITS, word_to_fx(1000))
end function


var sbyte*FXWIDTH lasttime = now()

function get_process_value return sbyte*FXWIDTH is
	return word_to_fx(98)
end function


procedure update_pid (volatile byte out device, sbyte*FXWIDTH in new_setpoint) is
	-- ideas from http://www.flightgear.org/Docs/XMLAutopilot/node3.html#SECTION00031000000000000000
	-- http://en.wikipedia.org/wiki/PID_controller#Discrete_implementation
	var sbyte*FXWIDTH curtime = now()
	var sbyte*FXWIDTH curval = get_process_value()
	var sbyte*FXWIDTH delta_t = curtime - lasttime
	var sbyte*FXWIDTH d_factor1 = div_fx(delta_t, filter_time)
	var sbyte*FXWIDTH d_factor2 = d_factor1 + fx_one
	var sbyte*FXWIDTH d_factor3 = div_fx(d_factor1, d_factor2)
	var sbyte*FXWIDTH error = new_setpoint - curval
	--var sbyte*FXWIDTH error_p = mult_fx(beta, new_setpoint) - curval)
	var sbyte*FXWIDTH error_p = mult_fx(beta, error)	
	--var sbyte*FXWIDTH error_d = mult_fx(gamma, new_setpoint) - curval)
	var sbyte*FXWIDTH error_d = mult_fx(gamma, error)
	var sbyte*FXWIDTH error_d_f = (div_fx(last_error_d_f, d_factor2) + 
                                mult_fx(error_d, d_factor3))  
                                
    var sbyte*FXWIDTH output_delta = mult_fx(proportional_constant,
                                        
                                        (error_p - last_error_p)
                                        
                                        + mult_fx( 
                                            div_fx( delta_t, integration_time),
                                            error
                                            )
                                            
                                            
                                        + mult_fx( 
                                            div_fx( differentiation_time, delta_t) ,
                                            (error_d_f
                                                - mult_fx(
                                                    word_to_fx(2),
                                                    last_error_d_f
                                                    )
                                                + last_last_error_d_f
                                                )
                                                )
                                            
                                        )
                                        
    
	last_last_error_d_f = last_error_d_f
	last_error_d_f = error_d_f
	last_error_p = error_p
	lasttime = curtime
	output_value = output_value + output_delta	
end procedure