72 lines
1.4 KiB
C
Executable File
72 lines
1.4 KiB
C
Executable File
#pragma once
|
|
|
|
#include <stdint.h>
|
|
#include <fixpoint/fp248.h>
|
|
#include <fixpoint/fp4816.h>
|
|
|
|
#define KI_FROM_TN(tn) (1/tn)
|
|
#define TN_FROM_KI(ki) (1/ki)
|
|
|
|
struct _pi_float
|
|
{
|
|
float limit_min,
|
|
limit_max;
|
|
float kp,
|
|
ki;
|
|
float integral;
|
|
};
|
|
typedef struct _pi_float PI_FLOAT;
|
|
|
|
static inline float pi_float_cycle(PI_FLOAT *pi,float value)
|
|
{
|
|
pi->integral += pi->ki * value;
|
|
if (pi->integral > pi->limit_max)
|
|
pi->integral = pi->limit_max;
|
|
if (pi->integral < pi->limit_min)
|
|
pi->integral = pi->limit_min;
|
|
return pi->integral + (pi->kp * value);
|
|
};
|
|
|
|
struct _pi_fp248
|
|
{
|
|
fp248_t limit_min,
|
|
limit_max;
|
|
fp248_t kp,
|
|
ki;
|
|
fp248_t integral;
|
|
};
|
|
typedef struct _pi_fp248 PI_FP248;
|
|
|
|
static inline fp248_t pi_fp248_cycle(PI_FP248 *pi,fp248_t value)
|
|
{
|
|
pi->integral += fp248_mul(pi->ki, value);
|
|
if (pi->integral > pi->limit_max)
|
|
pi->integral = pi->limit_max;
|
|
if (pi->integral < pi->limit_min)
|
|
pi->integral = pi->limit_min;
|
|
return pi->integral + fp248_mul(pi->kp, value);
|
|
};
|
|
|
|
|
|
struct _pi_fp4816
|
|
{
|
|
fp4816_t limit_min,
|
|
limit_max;
|
|
fp4816_t kp,
|
|
ki;
|
|
fp4816_t integral;
|
|
};
|
|
typedef struct _pi_fp4816 PI_FP4816;
|
|
|
|
static inline fp4816_t pi_fp4816_cycle(PI_FP4816 *pi,fp4816_t value)
|
|
{
|
|
pi->integral += fp4816_mul(pi->ki, value);
|
|
if (pi->integral > pi->limit_max)
|
|
pi->integral = pi->limit_max;
|
|
if (pi->integral < pi->limit_min)
|
|
pi->integral = pi->limit_min;
|
|
return pi->integral + fp4816_mul(pi->kp, value);
|
|
};
|
|
|
|
|