From 3c50fcd6954c052d1d5fd1587837884cf93e4aff Mon Sep 17 00:00:00 2001 From: Alessandro Mauri Date: Sun, 10 May 2026 12:58:25 +0200 Subject: [PATCH] fix FR_math impl --- fw/Makefile | 2 +- fw/display.c | 5 +++++ fw/display.h | 2 ++ fw/main.c | 40 +++++++++++++++++++++++++--------------- 4 files changed, 33 insertions(+), 16 deletions(-) diff --git a/fw/Makefile b/fw/Makefile index 6e76936..7cd0dd5 100644 --- a/fw/Makefile +++ b/fw/Makefile @@ -44,7 +44,7 @@ $(BUILD_DIR)/%.o : %.c $(HEADER_FILES) | $(BUILD_DIR) # link target, the rest of the build is specified in ch32fun.mk $(TARGET).elf : $(FILES_TO_COMPILE) $(LINKER_SCRIPT) $(EXTRA_ELF_DEPENDENCIES) $(U8G2_OBJS) $(FRMATH_OBJS) $(TARGET_OBJS) ch32fun.o - $(PREFIX)-gcc -o $@ $(U8G2_OBJS) $(TARGET_OBJS) ch32fun.o $(CFLAGS) $(LDFLAGS) + $(PREFIX)-gcc -o $@ $(U8G2_OBJS) $(TARGET_OBJS) $(FRMATH_OBJS) ch32fun.o $(CFLAGS) $(LDFLAGS) flash : cv_flash diff --git a/fw/display.c b/fw/display.c index 04e5849..74d1c4e 100644 --- a/fw/display.c +++ b/fw/display.c @@ -249,3 +249,8 @@ const char* i16toa(int16_t value) return p; } + +static char str_buf[32]; +uint8_t str_len; +int buf_putc(char c) { str_buf[str_len++] = c; return 0; } +const char* buf_get(void) { str_buf[str_len] = '\0'; str_len = 0; return str_buf; } diff --git a/fw/display.h b/fw/display.h index 217806b..fcd77b3 100644 --- a/fw/display.h +++ b/fw/display.h @@ -10,6 +10,8 @@ u8g2_t* display_init(void); const char* i16toa(int16_t value); const char* u16toa(uint16_t value); +int buf_putc(char c); +const char* buf_get(void); #endif // _DISPLAY_H diff --git a/fw/main.c b/fw/main.c index a4f02d1..1c8c31e 100644 --- a/fw/main.c +++ b/fw/main.c @@ -1,3 +1,4 @@ +#include "fr_math/FR_math.h" #include #include #include @@ -533,7 +534,8 @@ __attribute__((noreturn)) int main(void) // 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, i16toa(delta)); + FR_printNumF(buf_putc, e, R, 0, 3); + u8g2_DrawStr(u8g2, x_off+0, y_off+15, buf_get()); // Display current u8g2_DrawStr(u8g2, x_off+45, y_off+15, "A:"); u8g2_DrawStr(u8g2, x_off+55, y_off+15, u16toa(current_ma)); @@ -565,23 +567,30 @@ __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; + static s32 err_p, err_i, err_d, prev_err; + uint16_t duty; + // absolute temperature error + s32 err = I2FR(delta, R); - err_p = delta; - err_i = MAX(-1000, MIN(1000, err_i+delta)); - err_d = delta - prev_delta; - prev_delta = delta; + err_p = err; + err_i = FR_CLAMP(FR_FixAddSat(err_i, err), I2FR(-1000, R), I2FR(1000, R)); + err_d = FR_SUB(err, R, prev_err, R); + prev_err = err; - const s32 kp = FR_NUM(0, 8, 1, R); - const s32 ki = FR_NUM(0, 15, 2, R); - const s32 kd = FR_NUM(0, 0, 1, R); - e = FR_FIxAddSat(e, FR_FixMulSat(I2FR(err_p, R), kp)); - e = FR_FIxAddSat(e, FR_FixMulSat(I2FR(err_i, R), ki)); - e = FR_FIxAddSat(e, FR_FixMulSat(I2FR(err_f, R), kd)); - uint16_t duty = MAX(0, MIN(FR2I(e, R), pd_profile.max_duty)); + const s32 kp = FR_numstr("0.00700", R); + const s32 ki = FR_numstr("0.00001", R); + const s32 kd = FR_numstr("0.00000", R); + e = FR_FixAddSat(e, FR_FixMulSat(err_p, kp)); + e = FR_FixAddSat(e, FR_FixMulSat(err_i, ki)); + e = FR_FixAddSat(e, FR_FixMulSat(err_d, kd)); - pwm_set(((u32)duty*tim_max)/100); - u8g2_DrawBox(u8g2, x_off+92, y_off+12, 4, 4); + e = FR_CLAMP(e, I2FR(0, R), I2FR(100, R)); + duty = FR2I(FR_FixMulSat(FR_DIV(e, R, I2FR(100, R), R), I2FR(pd_profile.max_duty, R)), R); + + if (duty <= 100) { + pwm_set(((u32)duty*tim_max)/100); + u8g2_DrawBox(u8g2, x_off+92, y_off+12, 4, 4); + } } else { pwm_set(0); } @@ -602,6 +611,7 @@ __attribute__((noreturn)) int main(void) if (enabled) { pwm_on(); } else { + e = 0; pwm_off(); } }