Fixed-Point PID Algorithm

fixedpoint-pid.c

// Fixed-Point PID Algorithm
// Ported from: https://gist.github.com/bradley219/5373998
// Author: Li "oldrev" Wei <[email protected]>

#include <stdio.h>
#include <stdint.h>
#include <string.h>
#include <stdio.h>

#define FIXED32_Q (16)
#define FIXED32_FMASK (((Fixed32)1 << FIXED32_Q) - 1)

typedef int32_t Fixed32;
typedef int64_t Fixed64;

Fixed32 Fixed32_FromInt(int32_t n) {
	return (Fixed32)((Fixed32)n << FIXED32_Q);
}

int32_t Fixed32_Frac(Fixed32 a){
	return a & FIXED32_FMASK;
}

// Optional
Fixed32 Fixed32_FromFloat(float f) {
	return (Fixed32)((f) * (((Fixed64)1 << FIXED32_Q) + ((f) >= 0 ? 0.5 : -0.5)));
}

// Optional
Fixed32 Fixed32_ToFloat(float T) {
	return (float)((T)*((float)(1)/(float)(1 << FIXED32_Q)));
}

Fixed32 Fixed32_Mul(Fixed32 a, Fixed32 b) {
	return (Fixed32)(((Fixed64)a * (Fixed64)b) >> FIXED32_Q);	
}

Fixed32 Fixed32_Div(Fixed32 a, Fixed32 b) {
	return (Fixed32)(((Fixed64)a << FIXED32_Q) / (Fixed64)b);
}

typedef struct {
	Fixed32 Dt;
	Fixed32 Max;
	Fixed32 Min;
	Fixed32 Kp;
	Fixed32 Kd;
	Fixed32 Ki;
	Fixed32 PrevError;
	Fixed32 Integral;
} FixedPid;


void FixedPid_Init(FixedPid* self) {
	memset(self, 0, sizeof(FixedPid)); 
}

Fixed32 FixedPid_Calculate(FixedPid* self, Fixed32 setpoint, Fixed32 pv) {
    // Calculate error
    Fixed32 error = setpoint - pv; //setpoint - pv;

    // Proportional term
    Fixed32 Pout = Fixed32_Mul(self->Kp, error); // self.Kp * error;

    // Integral term
    self->Integral += Fixed32_Mul(error, self->Dt);  // self.Integral += error * self.Dt;
    Fixed32 Iout = Fixed32_Mul(self->Ki, self->Integral);

    // Derivative term
    Fixed32 derivative = Fixed32_Div(error - self->PrevError, self->Dt);
    Fixed32 Dout = Fixed32_Mul(self->Kd, derivative);

    // Calculate total output
    Fixed32 output = Pout + Iout + Dout;
	
    // Restrict to max/min
    if(output > self->Max) {
        output = self->Max;
	}
    else if(output < self->Min){
        output = self->Min;
	}

    // Save error to previous error
    self->PrevError = error;
    return output;
}


int main() {
	FixedPid pid;
	FixedPid_Init(&pid);

	pid.Dt = Fixed32_FromFloat(0.1);
	pid.Max = Fixed32_FromFloat(100);
	pid.Min = Fixed32_FromFloat(-100);
	pid.Kp = Fixed32_FromFloat(0.1);
	pid.Kd = Fixed32_FromFloat(0.01);
	pid.Ki = Fixed32_FromFloat(0.5);
	float float_val = 20;
	Fixed32 val = Fixed32_FromFloat(float_val);
	printf("max=%d    min=%d    dt=%d\n", pid.Max, pid.Min, pid.Dt);
	for(int i = 0; i < 100; i++) {
		Fixed32 inc = FixedPid_Calculate(&pid, 0, val);
		float float_inc = Fixed32_ToFloat(inc);
		float_val = Fixed32_ToFloat(val);
        	printf("%d - val:% 7.8f inc:% 7.8f\n", i, float_val, float_inc);
		val += inc;
	}
	return 0;
}

@oldrev thanks for sharing this, i was amazed to find your fixed point functions, although i dont understand how they shall work at all :-D

I tried them on a microcontroller and found they dont seem to work properly, so now i put it in godbold and at least for x86 i also get strange results: https://godbolt.org/z/P7E9c4Men

I see the following output:

float vs. Fixed32 results differ for 1.234000 * 6.789000 = 8.377625 (float) vs. 8.000000 (fixed)
float vs. Fixed32 results differ for 1.234000 / 6.789000 = 0.181765 (float) vs. 0.000000 (fixed)
float vs. Fixed32 results differ for 100.099998 * 1000.200012 = 100120.023438 (float) vs. -30950.000000 (fixed)
float vs. Fixed32 results differ for 100.099998 / 1000.200012 = 0.100080 (float) vs. 0.000000 (fixed)
float vs. Fixed32 results differ for 1000.454529 * -765.654419 = -766002.437500 (float) vs. 20429.000000 (fixed)
float vs. Fixed32 results differ for 1000.454529 / -765.654419 = -1.306666 (float) vs. -1.000000 (fixed)
float vs. Fixed32 results differ for -234324.406250 * 34.119999 = -7995148.500000 (float) vs. -3940.000000 (fixed)
float vs. Fixed32 results differ for -234324.406250 / 34.119999 = -6867.655762 (float) vs. -960.000000 (fixed)

Could you share where those functions come from (so i can read up more maybe) and on which architecture you tried this?

@oldrev thanks for sharing this, i was amazed to find your fixed point functions, although i dont understand how they shall work at all :-D

I tried them on a microcontroller and found they dont seem to work properly, so now i put it in godbold and at least for x86 i also get strange results: https://godbolt.org/z/P7E9c4Men

I see the following output:

float vs. Fixed32 results differ for 1.234000 * 6.789000 = 8.377625 (float) vs. 8.000000 (fixed)
float vs. Fixed32 results differ for 1.234000 / 6.789000 = 0.181765 (float) vs. 0.000000 (fixed)
float vs. Fixed32 results differ for 100.099998 * 1000.200012 = 100120.023438 (float) vs. -30950.000000 (fixed)
float vs. Fixed32 results differ for 100.099998 / 1000.200012 = 0.100080 (float) vs. 0.000000 (fixed)
float vs. Fixed32 results differ for 1000.454529 * -765.654419 = -766002.437500 (float) vs. 20429.000000 (fixed)
float vs. Fixed32 results differ for 1000.454529 / -765.654419 = -1.306666 (float) vs. -1.000000 (fixed)
float vs. Fixed32 results differ for -234324.406250 * 34.119999 = -7995148.500000 (float) vs. -3940.000000 (fixed)
float vs. Fixed32 results differ for -234324.406250 / 34.119999 = -6867.655762 (float) vs. -960.000000 (fixed)

Could you share where those functions come from (so i can read up more maybe) and on which architecture you tried this?

You can directly have ChatGPT generate integer-based PID algorithm code. The fixed-point algorithm I ported is not flexible enough.