To whom it may interest:

I'm encounter the problem that the initial setting is entirely different direction for the setpoint that initially should start from north, however, as the heading now is facing to south
Does anybody can helps me work out why does the opposite facing, thank very much in advance

key = (PORTB >> 4) & 0x03;
switch(key){
// Heading setting initial at zero
case 0:
setpoint = 270;
break;
case 1:
setpoint = 90;
break;
case 2:
setpoint = 180;
break;
case 3:
setpoint = 0;
break;
}

Furthermore, regarding to the setting of setpoint, the push button just jump into the case loop and out of control
Could anybody gives me the advice? Thank you very much

Additionally, in terms of derivative term, there is a number of books illustrated the previous error = error
However, as the definition of derivative terms, d_error = error - p_error, this would cause the derivative terms
to zero, thus, what should I set the p_error instead of equal to error? Is the logic of derivative term correct? Then do the initial setting of Kp = 1, Kd = 0.5

error = setpoint - heading;

// 0-359 so must wraparound
if(error > 180)
error -= 360;
else if(error <= -179)
error += 360;

a_error += error;
d_error = error - p_error;

p_error = error;

kp = 1; // Set proportional gain value
ki = 0; // Set integral gain value
kd = 0.5; // Set derivative gain value

prop = kp * error; // Calculate proportional term
integ = ki * a_error; // Calculate integral term
deri = kd * d_error; // Calculate derivative term

pid_out = prop + integ + deri; // Calculate the pid result for each of the three bytes

if(pid_out < 0) pid_out = -pid_out;
/*
Out = (int)pid_out;

// Avoid integral windup
if(Out > UpLimit) Out = UpLimit; // Upper Limit
if(Out < LowLimit)Out = LowLimit; // Low Limit
return Out;
*/
Command.VertThruster = 0;
Command.PortThruster = pid_out;
Command.StbdThruster = pid_out;
//Command.PortThruster = Out;
//Command.StbdThruster = Out;