start work on PID controller
This commit is contained in:
parent
f7f5a16d64
commit
0200e47327
88
fw/display.c
88
fw/display.c
@ -6,15 +6,17 @@
|
||||
#include "lib_i2c.h"
|
||||
|
||||
|
||||
/*
|
||||
* GPIO and delay callback for u8g2/u8x8 display driver
|
||||
*/
|
||||
|
||||
|
||||
static u8g2_t u8g2;
|
||||
static const char digits_lut[] =
|
||||
"0001020304050607080910111213141516171819"
|
||||
"2021222324252627282930313233343536373839"
|
||||
"4041424344454647484950515253545556575859"
|
||||
"6061626364656667686970717273747576777879"
|
||||
"8081828384858687888990919293949596979899";
|
||||
|
||||
|
||||
uint8_t u8x8_gpio_and_delay(u8x8_t *u8x8, uint8_t msg, uint8_t arg_int, void *arg_ptr)
|
||||
// GPIO and delay callback for u8g2/u8x8 display driver
|
||||
static uint8_t u8x8_gpio_and_delay(u8x8_t *u8x8, uint8_t msg, uint8_t arg_int, void *arg_ptr)
|
||||
{
|
||||
switch(msg) {
|
||||
case U8X8_MSG_GPIO_AND_DELAY_INIT:
|
||||
@ -129,7 +131,7 @@ uint8_t u8x8_gpio_and_delay(u8x8_t *u8x8, uint8_t msg, uint8_t arg_int, void *ar
|
||||
}
|
||||
|
||||
|
||||
uint8_t u8x8_byte_i2c(u8x8_t *u8x8, uint8_t msg, uint8_t arg_int, void *arg_ptr)
|
||||
static uint8_t u8x8_byte_i2c(u8x8_t *u8x8, uint8_t msg, uint8_t arg_int, void *arg_ptr)
|
||||
{
|
||||
static uint8_t buffer[32];
|
||||
static uint8_t buffer_idx;
|
||||
@ -173,5 +175,77 @@ u8g2_t* display_init(void)
|
||||
u8g2_InitDisplay(&u8g2);
|
||||
u8g2_SetPowerSave(&u8g2, 0);
|
||||
u8g2_SetContrast(&u8g2, 255);
|
||||
|
||||
u8g2_SetBitmapMode(&u8g2, 1);
|
||||
u8g2_SetFontMode(&u8g2, 1);
|
||||
|
||||
return &u8g2;
|
||||
}
|
||||
|
||||
|
||||
const char* u16toa(uint16_t value)
|
||||
{
|
||||
// Max uint16_t is 65535 (5 digits) + null terminator
|
||||
static char buf[6];
|
||||
char *p = &buf[5];
|
||||
*p = '\0';
|
||||
|
||||
// Process two digits at a time using the LUT
|
||||
while (value >= 100) {
|
||||
const unsigned int idx = (value % 100) * 2;
|
||||
value /= 100;
|
||||
*--p = digits_lut[idx + 1];
|
||||
*--p = digits_lut[idx];
|
||||
}
|
||||
|
||||
// Handle the remaining value (< 100)
|
||||
if (value < 10) {
|
||||
*--p = (char)('0' + value);
|
||||
} else {
|
||||
const unsigned int idx = value * 2;
|
||||
*--p = digits_lut[idx + 1];
|
||||
*--p = digits_lut[idx];
|
||||
}
|
||||
|
||||
return p;
|
||||
}
|
||||
|
||||
|
||||
const char* i16toa(int16_t value)
|
||||
{
|
||||
static char buf[7];
|
||||
uint16_t uval;
|
||||
bool negative = false;
|
||||
|
||||
if (value < 0) {
|
||||
negative = true;
|
||||
uval = (uint16_t)-value;
|
||||
} else {
|
||||
uval = (uint16_t)value;
|
||||
}
|
||||
|
||||
char *p = &buf[6];
|
||||
*p = '\0';
|
||||
|
||||
// Same LUT logic as unsigned
|
||||
while (uval >= 100) {
|
||||
const unsigned int idx = (uval % 100) * 2;
|
||||
uval /= 100;
|
||||
*--p = digits_lut[idx + 1];
|
||||
*--p = digits_lut[idx];
|
||||
}
|
||||
|
||||
if (uval < 10) {
|
||||
*--p = (char)('0' + uval);
|
||||
} else {
|
||||
const unsigned int idx = uval * 2;
|
||||
*--p = digits_lut[idx + 1];
|
||||
*--p = digits_lut[idx];
|
||||
}
|
||||
|
||||
if (negative) {
|
||||
*--p = '-';
|
||||
}
|
||||
|
||||
return p;
|
||||
}
|
||||
|
||||
@ -8,5 +8,8 @@
|
||||
#define PIN_DISP_RST PA7 // display reset
|
||||
|
||||
u8g2_t* display_init(void);
|
||||
const char* i16toa(int16_t value);
|
||||
const char* u16toa(uint16_t value);
|
||||
|
||||
|
||||
#endif // _DISPLAY_H
|
||||
|
||||
75
fw/filter.h
75
fw/filter.h
@ -1,20 +1,79 @@
|
||||
#ifndef _FILTER_H
|
||||
#define _FILTER_H
|
||||
|
||||
#include <stdint.h>
|
||||
|
||||
|
||||
#define MIN(a, b) ((a) < (b) ? (a) : (b))
|
||||
#define MAX(a, b) ((a) > (b) ? (a) : (b))
|
||||
|
||||
static_assert(-4 >> 1 == -2, ">> doesn't do sign extension");
|
||||
|
||||
// Fixed-Point Exponential Moving Average
|
||||
// alpha = 1/2^k
|
||||
// x: output value
|
||||
// s: current sample
|
||||
#define U16_FP_EMA_K2(x, s) (u16)((((u32)(x)<<2) - (x) + (s)) >> 2)
|
||||
#define U16_FP_EMA_K4(x, s) (u16)((((u32)(x)<<4) - (x) + (s)) >> 4)
|
||||
#define U16_FP_EMA_K8(x, s) (u16)((((u32)(x)<<8) - (x) + (s)) >> 8)
|
||||
#define U16_FP_EMA_K16(x, s) (u16)((((u32)(x)<<16) - (x) + (s)) >> 16)
|
||||
#define U16_FP_EMA_K2(x, s) (uint16_t)((((uint32_t)(x)<<2) - (x) + (s)) >> 2)
|
||||
#define U16_FP_EMA_K4(x, s) (uint16_t)((((uint32_t)(x)<<4) - (x) + (s)) >> 4)
|
||||
#define U16_FP_EMA_K8(x, s) (uint16_t)((((uint32_t)(x)<<8) - (x) + (s)) >> 8)
|
||||
#define U16_FP_EMA_K16(x, s) (uint16_t)((((uint32_t)(x)<<16) - (x) + (s)) >> 16)
|
||||
|
||||
#define I16_FP_EMA_K2(x, s) (s16)((((s32)(x)<<2) - (x) + (s)) >> 2)
|
||||
#define I16_FP_EMA_K4(x, s) (s16)((((s32)(x)<<4) - (x) + (s)) >> 4)
|
||||
#define I16_FP_EMA_K8(x, s) (s16)((((s32)(x)<<8) - (x) + (s)) >> 8)
|
||||
#define I16_FP_EMA_K16(x, s) (s16)((((s32)(x)<<16) - (x) + (s)) >> 16)
|
||||
#define I16_FP_EMA_K2(x, s) (int16_t)((((int32_t)(x)<<2) - (x) + (s)) >> 2)
|
||||
#define I16_FP_EMA_K4(x, s) (int16_t)((((int32_t)(x)<<4) - (x) + (s)) >> 4)
|
||||
#define I16_FP_EMA_K8(x, s) (int16_t)((((int32_t)(x)<<8) - (x) + (s)) >> 8)
|
||||
#define I16_FP_EMA_K16(x, s) (int16_t)((((int32_t)(x)<<16) - (x) + (s)) >> 16)
|
||||
|
||||
/* ------------------------- FIXED POINT OPERATIONS ------------------------- */
|
||||
|
||||
// Fixed-point number in 8.8 format
|
||||
typedef int16_t fp16_t;
|
||||
#define F32_TO_FP16(f) ((fp16_t)((f) * 256.0f))
|
||||
#define I16_TO_FP16(i) ((fp16_t)((i) << 8))
|
||||
#define U16_TO_FP16(u) ((fp16_t)((u) << 8))
|
||||
|
||||
// Intger part of fp16_t
|
||||
#define I(f) ((f) >> 8)
|
||||
// Decimal part of fp16_t
|
||||
#define D(f) ((f) & 0xFF)
|
||||
|
||||
static inline int16_t i16_mul_fp16(int16_t v, fp16_t f)
|
||||
{
|
||||
return (((int32_t)(v)<<8) * f) >> 8;
|
||||
}
|
||||
|
||||
static inline uint16_t u16_mul_fp16(uint16_t v, fp16_t f)
|
||||
{
|
||||
return (((uint32_t)(v)<<8) * f) >> 8;
|
||||
}
|
||||
|
||||
// v * a + b for signed 16-bit
|
||||
#define I16_LINCAL(v, a, b) ((int16_t)((((int32_t)(v)<<8) * (a) + (b)) >> 8))
|
||||
// v * a + b for unsigned 16-bit
|
||||
#define U16_LINCAL(v, a, b) ((uint16_t)((((uint32_t)(v)<<8) * (a) + (b)) >> 8))
|
||||
|
||||
|
||||
// Fixed-point number in 24.8 format
|
||||
typedef uint32_t fp24_8_t;
|
||||
|
||||
static inline fp24_8_t f32_to_fp24_8(float f)
|
||||
{
|
||||
return ((fp24_8_t)((f) * 256.0f));
|
||||
}
|
||||
|
||||
static inline fp24_8_t i16_to_fp24_8(int16_t i)
|
||||
{
|
||||
return ((fp24_8_t)(i)) << 8;
|
||||
}
|
||||
|
||||
static inline fp24_8_t u16_to_fp24_8(uint16_t u)
|
||||
{
|
||||
return ((fp24_8_t)(u)) << 8;
|
||||
}
|
||||
|
||||
static inline fp24_8_t fp24_8_mul(fp24_8_t a, fp24_8_t b)
|
||||
{
|
||||
return ((fp24_8_t)((uint32_t)a * b) >> 8);
|
||||
}
|
||||
|
||||
|
||||
#endif // _FILTER_H
|
||||
|
||||
22
fw/lincal.h
22
fw/lincal.h
@ -1,22 +0,0 @@
|
||||
#ifndef _LINCAL_H
|
||||
#define _LINCAL_H
|
||||
|
||||
#include <ch32fun.h>
|
||||
|
||||
|
||||
// Fixed-point number in 8.8 format
|
||||
typedef int16_t fp16_t;
|
||||
#define F32_TO_FP16(f) ((fp16_t)((f) * 256.0f))
|
||||
|
||||
// Intger part of fp16_t
|
||||
#define I(f) ((f) >> 8)
|
||||
// Decimal part of fp16_t
|
||||
#define D(f) ((f) & 0xFF)
|
||||
|
||||
// v * a + b for signed 16-bit
|
||||
#define I16_LINCAL(v, a, b) ((int16_t)((((int32_t)(v) * (a)) >> 8) + (b)))
|
||||
// v * a + b for unsigned 16-bit
|
||||
#define U16_LINCAL(v, a, b) ((uint16_t)((((uint32_t)(v) * (a)) >> 8) + (b)))
|
||||
|
||||
|
||||
#endif
|
||||
41
fw/main.c
41
fw/main.c
@ -94,6 +94,7 @@ void update_encoder(void)
|
||||
|
||||
// FIXME: even with interrupts disabled and debounce, a single detent triggers
|
||||
// multiple interrupts, leading to multiple encoder updates in a short time.
|
||||
if (funDigitalRead(PIN_BTN) == 0) return;
|
||||
bool a = funDigitalRead(PIN_ENC_A);
|
||||
bool b = funDigitalRead(PIN_ENC_B);
|
||||
Delay_Us(100);
|
||||
@ -354,9 +355,6 @@ static inline uint16_t isqrt(uint32_t x)
|
||||
}
|
||||
|
||||
|
||||
#define MIN(a, b) ((a) < (b) ? (a) : (b))
|
||||
|
||||
|
||||
__attribute__((noreturn)) int main(void)
|
||||
{
|
||||
SystemInit();
|
||||
@ -419,8 +417,6 @@ __attribute__((noreturn)) int main(void)
|
||||
sc7a20_init();
|
||||
|
||||
u8g2_ClearBuffer(u8g2);
|
||||
u8g2_SetBitmapMode(u8g2, 1);
|
||||
u8g2_SetFontMode(u8g2, 1);
|
||||
u8g2_SetFont(u8g2, u8g2_font_5x8_tr);
|
||||
u8g2_DrawStr(u8g2, x_off+0, y_off+7, "Negotiating...");
|
||||
u8g2_SendBuffer(u8g2);
|
||||
@ -429,8 +425,6 @@ __attribute__((noreturn)) int main(void)
|
||||
bool has_pd = pd_negotiate(eUSBPD_VCC_3V3);
|
||||
if (has_pd == false) {
|
||||
u8g2_ClearBuffer(u8g2);
|
||||
u8g2_SetBitmapMode(u8g2, 1);
|
||||
u8g2_SetFontMode(u8g2, 1);
|
||||
u8g2_SetFont(u8g2, u8g2_font_5x8_tr);
|
||||
u8g2_DrawStr(u8g2, x_off+0, y_off+7, "Negotiation FAILED");
|
||||
u8g2_DrawStr(u8g2, x_off+0, y_off+14, USBPD_ResultToStr(pd_get_result()));
|
||||
@ -449,8 +443,6 @@ __attribute__((noreturn)) int main(void)
|
||||
, 100);
|
||||
|
||||
u8g2_ClearBuffer(u8g2);
|
||||
u8g2_SetBitmapMode(u8g2, 1);
|
||||
u8g2_SetFontMode(u8g2, 1);
|
||||
u8g2_SetFont(u8g2, u8g2_font_5x8_tr);
|
||||
// Display tip temperature
|
||||
u8g2_DrawStr(u8g2, x_off+0, y_off+7, "A:");
|
||||
@ -478,8 +470,6 @@ __attribute__((noreturn)) int main(void)
|
||||
poll_input(); // usb
|
||||
|
||||
u8g2_ClearBuffer(u8g2);
|
||||
u8g2_SetBitmapMode(u8g2, 1);
|
||||
u8g2_SetFontMode(u8g2, 1);
|
||||
u8g2_SetFont(u8g2, u8g2_font_5x8_tr);
|
||||
|
||||
static bool pwm = false; // PWM status
|
||||
@ -493,6 +483,7 @@ __attribute__((noreturn)) int main(void)
|
||||
static uint16_t vbus_mv, current_ma;
|
||||
static int16_t temp_c, tip_temp_c;
|
||||
static uint16_t power;
|
||||
static fp24_8_t e;
|
||||
vbus_mv = U16_FP_EMA_K4(vbus_mv, ((u32)adc_buffer[0]*VCC_MV*11)/4096);
|
||||
current_ma = U16_FP_EMA_K4(current_ma, get_current_ma(adc_buffer[1]));
|
||||
temp_c = I16_FP_EMA_K4(temp_c, get_temp_c(adc_buffer[2]));
|
||||
@ -528,16 +519,17 @@ __attribute__((noreturn)) int main(void)
|
||||
case STATE_HEATING:
|
||||
// Display tip temperature
|
||||
u8g2_DrawStr(u8g2, x_off+0, y_off+7, "TIP:");
|
||||
u8g2_DrawStr(u8g2, x_off+20, y_off+7, u8g2_u16toa(tip_temp_c, 4));
|
||||
u8g2_DrawStr(u8g2, x_off+20, y_off+7, u16toa(tip_temp_c));
|
||||
// Display bus voltage
|
||||
u8g2_DrawStr(u8g2, x_off+45, y_off+7, "V:");
|
||||
u8g2_DrawStr(u8g2, x_off+55, y_off+7, u8g2_u16toa(vbus_mv/1000, 2));
|
||||
u8g2_DrawStr(u8g2, x_off+55, y_off+7, u16toa(vbus_mv/1000));
|
||||
// Display power
|
||||
u8g2_DrawStr(u8g2, x_off+0, y_off+15, "W:");
|
||||
u8g2_DrawStr(u8g2, x_off+10, y_off+15, u8g2_u16toa(power, 3));
|
||||
//u8g2_DrawStr(u8g2, x_off+0, y_off+15, "W:");
|
||||
//u8g2_DrawStr(u8g2, x_off+10, y_off+15, u8g2_u16toa(power, 3));
|
||||
u8g2_DrawStr(u8g2, x_off+0, y_off+15, i16toa(pd_profile.set_temp - tip_temp_c));
|
||||
// Display current
|
||||
u8g2_DrawStr(u8g2, x_off+45, y_off+15, "A:");
|
||||
u8g2_DrawStr(u8g2, x_off+55, y_off+15, u8g2_u16toa(current_ma, 5));
|
||||
u8g2_DrawStr(u8g2, x_off+55, y_off+15, u16toa(current_ma));
|
||||
|
||||
|
||||
if (enabled) {
|
||||
@ -566,8 +558,23 @@ __attribute__((noreturn)) int main(void)
|
||||
|
||||
if (pwm) {
|
||||
const uint16_t tim_max = FUNCONF_SYSTEM_CORE_CLOCK / PWM_FREQ_HZ - 1;
|
||||
static int16_t err_p, err_i, err_d, prev_delta;
|
||||
|
||||
int16_t delta = pd_profile.set_temp - tip_temp_c;
|
||||
uint16_t duty = MIN((25*pd_profile.max_duty*delta)/(pd_profile.set_temp*10), pd_profile.max_duty);
|
||||
err_p = delta;
|
||||
err_i += delta;
|
||||
err_i = MAX(-1000, MIN(1000, err_i));
|
||||
err_d = delta - prev_delta;
|
||||
prev_delta = delta;
|
||||
|
||||
const fp24_8_t kp = f32_to_fp24_8(0.8f);
|
||||
const fp24_8_t ki = f32_to_fp24_8(0.15f);
|
||||
const fp24_8_t kd = f32_to_fp24_8(0.0f);
|
||||
e = fp24_8_mul(i16_to_fp24_8(err_p), kp) +
|
||||
fp24_8_mul(i16_to_fp24_8(err_i), ki) +
|
||||
fp24_8_mul(i16_to_fp24_8(err_d), kd);
|
||||
uint16_t duty = MAX(0, MIN(I(e), pd_profile.max_duty));
|
||||
|
||||
pwm_set(((u32)duty*tim_max)/100);
|
||||
u8g2_DrawBox(u8g2, x_off+92, y_off+12, 4, 4);
|
||||
} else {
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user