From b7a902c7fd1b604cbabcd88b0be460ad2a8064d0 Mon Sep 17 00:00:00 2001 From: neoxic Date: Mon, 17 Jul 2023 22:35:15 -0700 Subject: [PATCH] Firmware revision 5 + DSHOT 1200 + Servo PWM up to 500Hz + Oneshot125 up to 4kHz New targets: FLYCOLOR2, NEUTRONRC2 16kHz main loop (was 10kHz) Set DSHOT telemetry delay at 30us (was 25us) Force arming after reset Minor desync detection tuning ANALOG never return rather than reset Fix broken pause modifier Add MUSIC (startup music) build option Cosmetics --- CMakeLists.txt | 2 + README.md | 8 ++-- mcu/AT32F421/config.c | 2 +- mcu/AT32F421/config.h | 8 ++-- mcu/GD32E230/config.c | 2 +- mcu/STM32F051/config.c | 2 +- mcu/STM32F051/config.h | 10 ++--- mcu/STM32G071/config.c | 2 +- mcu/STM32G071/config.h | 11 +++-- mcu/STSPIN32F0/config.c | 2 +- src/common.h | 2 +- src/defs.h | 7 ++++ src/io.c | 59 +++++++++++++++----------- src/main.c | 91 ++++++++++++++++++++++------------------- src/telem.c | 2 +- src/util.c | 4 +- 16 files changed, 121 insertions(+), 93 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 548bd18..8b9e89c 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -51,6 +51,7 @@ add_target(AIRBOT2 STM32F051 DEAD_TIME=26 COMP_MAP=321 SENS_MAP=0xA3 VOLT_MUL=74 add_target(EMAX1 STM32F051 DEAD_TIME=26 COMP_MAP=123 IO_PA2) add_target(ESCAPE1 STM32G071 DEAD_TIME=35 COMP_MAP=123 SENS_MAP=0xA5A4 VOLT_MUL=110 CURR_MUL=40 LED_WS2812 IO_PA2) add_target(FLYCOLOR1 STM32F051 DEAD_TIME=26 COMP_MAP=123 SENS_MAP=0xA6 VOLT_MUL=110 LED_MAP=0xB5B4B3 IO_PA2) +add_target(FLYCOLOR2 STM32G071 DEAD_TIME=35 COMP_MAP=123 SENS_MAP=0xA6 VOLT_MUL=110 LED_MAP=0xB8) add_target(HAKRC1 STM32F051 DEAD_TIME=26 COMP_MAP=213 SENS_MAP=0xA3 VOLT_MUL=110 LED_MAP=0xA15B5B3 LED_INV) add_target(HAKRC2 AT32F421 DEAD_TIME=66 COMP_MAP=213 SENS_MAP=0xA3 VOLT_MUL=110 LED_MAP=0xA15B5B3 LED_INV) add_target(HGLRC1 STM32F051 DEAD_TIME=26 COMP_MAP=123 SENS_MAP=0xA6 VOLT_MUL=210 IO_PA2) @@ -58,6 +59,7 @@ add_target(IFLIGHT1 STM32F051 DEAD_TIME=26 COMP_MAP=321 SENS_MAP=0xA3A6 VOLT_MUL add_target(IFLIGHT2 STM32G071 DEAD_TIME=35 COMP_MAP=213 SENS_MAP=0xA5A4 VOLT_MUL=110 CURR_MUL=20 LED_WS2812 IO_PA6) add_target(IFLIGHT3 STM32G071 DEAD_TIME=35 COMP_MAP=132 SENS_MAP=0xA0 VOLT_MUL=110 LED_WS2812) add_target(NEUTRONRC1 STM32G071 DEAD_TIME=35 COMP_MAP=123 SENS_MAP=0xA6A4 VOLT_MUL=210 CURR_MUL=50 LED_WS2812) +add_target(NEUTRONRC2 AT32F421 DEAD_TIME=66 COMP_MAP=321) add_target(SKYSTARS1 GD32E230 DEAD_TIME=40 COMP_MAP=321 SENS_MAP=0xA3 VOLT_MUL=110 LED_MAP=0xB5B3A15) add_target(TMOTOR1 STM32F051 DEAD_TIME=26 COMP_MAP=132 IO_PA2) add_target(TMOTOR2 STM32F051 DEAD_TIME=26 COMP_MAP=321) diff --git a/README.md b/README.md index 53e74e1..0b606fc 100644 --- a/README.md +++ b/README.md @@ -7,16 +7,16 @@ Firmware for 32-bit BLDC motor electronic speed controllers that aims at simplic Features -------- -+ Servo PWM up to 400Hz, automatic throttle calibration -+ DSHOT 300/600, bidirectional DSHOT, extended telemetry ++ Servo PWM, Oneshot125, automatic throttle calibration ++ DSHOT 300/600/1200, bidirectional DSHOT, extended telemetry + Analog/serial/iBUS/SBUS input mode + KISS/iBUS/S.Port telemetry + DSHOT 3D mode, turtle mode, beacon, LED, programming -+ Sine startup mode ++ Sine startup mode (crawler mode) + Proportional brake, drag brake + Temperature/voltage/current protection + Variable PWM frequency, active freewheeling -+ Customizable sounds ++ Customizable startup music + Configuration via CLI using a USB-TTL adapter or Betaflight passthrough diff --git a/mcu/AT32F421/config.c b/mcu/AT32F421/config.c index c3a7629..6e5c83f 100644 --- a/mcu/AT32F421/config.c +++ b/mcu/AT32F421/config.c @@ -22,8 +22,8 @@ #define ADC1_BASE ADC_BASE #define COMP_CSR MMIO32(SYSCFG_COMP_BASE + 0x1c) -static uint16_t buf[10]; static char len, ain; +static uint16_t buf[10]; void init(void) { RCC_CFGR &= ~RCC_CFGR_SW_PLL; diff --git a/mcu/AT32F421/config.h b/mcu/AT32F421/config.h index 9aa9d58..63e1f4f 100644 --- a/mcu/AT32F421/config.h +++ b/mcu/AT32F421/config.h @@ -17,12 +17,12 @@ #pragma once -#if SENS_MAP == 0xA3A6 // A3 (volt), A6 (curr) -#define SENS_CNT 2 -#define SENS_CHAN 0x66 -#elif SENS_MAP == 0xA3 // A3 (volt) +#if SENS_MAP == 0xA3 // A3 (volt) #define SENS_CNT 1 #define SENS_CHAN 0x3 +#elif SENS_MAP == 0xA3A6 // A3 (volt), A6 (curr) +#define SENS_CNT 2 +#define SENS_CHAN 0x66 #endif #ifndef THROT_CHAN diff --git a/mcu/GD32E230/config.c b/mcu/GD32E230/config.c index 5600c2b..377fc1b 100644 --- a/mcu/GD32E230/config.c +++ b/mcu/GD32E230/config.c @@ -22,8 +22,8 @@ #define ADC1_BASE ADC_BASE #define COMP_CSR MMIO32(SYSCFG_COMP_BASE + 0x1c) -static uint16_t buf[5]; static char len, ain; +static uint16_t buf[5]; void init(void) { RCC_CFGR &= ~RCC_CFGR_SW_PLL; diff --git a/mcu/STM32F051/config.c b/mcu/STM32F051/config.c index 3e0e2bd..4ab6055 100644 --- a/mcu/STM32F051/config.c +++ b/mcu/STM32F051/config.c @@ -20,8 +20,8 @@ #define COMP_CSR MMIO32(SYSCFG_COMP_BASE + 0x1c) -static uint16_t buf[5]; static char len, ain; +static uint16_t buf[5]; void init(void) { RCC_APB2RSTR = -1; diff --git a/mcu/STM32F051/config.h b/mcu/STM32F051/config.h index f1f2cd9..51343ea 100644 --- a/mcu/STM32F051/config.h +++ b/mcu/STM32F051/config.h @@ -17,16 +17,16 @@ #pragma once -#if SENS_MAP == 0xA3A6 // A3 (volt), A6 (curr) -#define SENS_CNT 2 -#define SENS_CHAN 0x48 -#define SENS_SWAP -#elif SENS_MAP == 0xA3 // A3 (volt) +#if SENS_MAP == 0xA3 // A3 (volt) #define SENS_CNT 1 #define SENS_CHAN 0x8 #elif SENS_MAP == 0xA6 // A6 (volt) #define SENS_CNT 1 #define SENS_CHAN 0x40 +#elif SENS_MAP == 0xA3A6 // A3 (volt), A6 (curr) +#define SENS_CNT 2 +#define SENS_CHAN 0x48 +#define SENS_SWAP #endif #ifndef THROT_CHAN diff --git a/mcu/STM32G071/config.c b/mcu/STM32G071/config.c index 7f849de..7a605ea 100644 --- a/mcu/STM32G071/config.c +++ b/mcu/STM32G071/config.c @@ -30,8 +30,8 @@ #define ADC1_DR ADC_DR(ADC1) #define ADC1_CCR ADC_CCR(ADC1) -static uint16_t buf[10]; static char len, ain; +static uint16_t buf[10]; void init(void) { RCC_APBRSTR2 = -1; diff --git a/mcu/STM32G071/config.h b/mcu/STM32G071/config.h index 11e575a..73e0a08 100644 --- a/mcu/STM32G071/config.h +++ b/mcu/STM32G071/config.h @@ -17,15 +17,18 @@ #pragma once -#if SENS_MAP == 0xA5A4 // A5 (volt), A4 (curr) +#if SENS_MAP == 0xA0 // A0 (volt) +#define SENS_CNT 1 +#define SENS_CHAN 0x0 +#elif SENS_MAP == 0xA6 // A6 (volt) +#define SENS_CNT 1 +#define SENS_CHAN 0x6 +#elif SENS_MAP == 0xA5A4 // A5 (volt), A4 (curr) #define SENS_CNT 2 #define SENS_CHAN 0x54 #elif SENS_MAP == 0xA6A4 // A6 (volt), A4 (curr) #define SENS_CNT 2 #define SENS_CHAN 0x64 -#elif SENS_MAP == 0xA0 // A0 (volt) -#define SENS_CNT 1 -#define SENS_CHAN 0x0 #endif #ifndef THROT_CHAN diff --git a/mcu/STSPIN32F0/config.c b/mcu/STSPIN32F0/config.c index a8f634b..58fe165 100644 --- a/mcu/STSPIN32F0/config.c +++ b/mcu/STSPIN32F0/config.c @@ -18,8 +18,8 @@ #include #include "common.h" -static uint16_t buf[3]; static char len, ain; +static uint16_t buf[3]; void init(void) { RCC_APB2RSTR = -1; diff --git a/src/common.h b/src/common.h index 90d0cd9..fee9dbc 100644 --- a/src/common.h +++ b/src/common.h @@ -98,7 +98,7 @@ extern const Cfg cfgdata; extern Cfg cfg; extern int throt, ertm, erpm, temp, volt, curr, csum, dshotval, beepval; extern char analog, telreq, flipdir, beacon, dshotext; -extern volatile uint32_t tick; +extern volatile uint32_t tickms; void init(void); void initio(void); diff --git a/src/defs.h b/src/defs.h index 9efa0d0..9a8fffe 100644 --- a/src/defs.h +++ b/src/defs.h @@ -64,6 +64,10 @@ #define LED_CNT 3 #elif !defined LED_MAP #define LED_CNT 0 +#elif LED_MAP == 0xB8 +#define LED_CNT 1 +#define LED1_PORT B +#define LED1_PIN 8 #elif LED_MAP == 0xA15B3B4 #define LED_CNT 3 #define LED1_PORT A @@ -212,6 +216,9 @@ #ifndef PROT_CURR #define PROT_CURR 0 #endif +#ifndef MUSIC +#define MUSIC "dfa#" +#endif #ifndef VOLUME #define VOLUME 25 #endif diff --git a/src/io.c b/src/io.c index 53e0ed3..b366864 100644 --- a/src/io.c +++ b/src/io.c @@ -44,7 +44,8 @@ static char rxlen; static void (*ioirq)(void); static void (*iodma)(void); -static char iobuf[1024], dshotinv; +static char dshotinv, iobuf[1024]; +static uint16_t dshotarr1, dshotarr2, dshotbuf[23]; void initio(void) { ioirq = entryirq; @@ -61,7 +62,7 @@ void initio(void) { } static void entryirq(void) { - static int c, n, u = 5; + static int n, c, d; if (TIM_SR(IOTIM) & TIM_SR_UIF) { // Timeout ~66ms TIM_SR(IOTIM) = ~TIM_SR_UIF; if (!IOTIM_IDR) { // Low level @@ -129,34 +130,43 @@ static void entryirq(void) { #endif int t = TIM_CCR1(IOTIM); // Time between two rising edges if (!n++) return; // First capture is always invalid - if (t >= 2400) { // Servo PWM - ioirq = calibirq; - calibirq(); + if (TIM_PSC(IOTIM)) { + if (t > 2000) { // Servo/Oneshot125 + ioirq = calibirq; + calibirq(); + return; + } + TIM_PSC(IOTIM) = TIM_PSC(IOTIM) == CLK_MHZ - 1 ? CLK_MHZ / 8 - 1 : 0; + TIM_EGR(IOTIM) = TIM_EGR_UG; + n = 0; return; } - if (t >= 5 || n <= 4) return; - if (u != t) { - u = t; + int m = 3; + while (t >= CLK_CNT(800000)) t >>= 1, --m; + if (d != m) { + d = m; n = 1; return; } + if (m < 1 || n < 4) return; ioirq = dshotirq; iodma = dshotdma; + dshotarr1 = CLK_CNT(m * 150000) - 1; + dshotarr2 = CLK_CNT(m * 375000) - 1; TIM_SMCR(IOTIM) = TIM_SMCR_SMS_RM | TIM_SMCR_TS_TI1F_ED; // Reset on any edge on TI1 TIM_CCER(IOTIM) = 0; TIM_DIER(IOTIM) = TIM_DIER_UIE; - TIM_PSC(IOTIM) = u >= 2; // 0 - DSHOT 600, 1 - DSHOT 300 - TIM_ARR(IOTIM) = CLK_CNT(300000) - 1; // Minimum idle time + TIM_ARR(IOTIM) = dshotarr1; // Frame reset time (two bits) TIM_EGR(IOTIM) = TIM_EGR_UG; DMA1_CPAR(IOTIM_DMA) = (uint32_t)&TIM_CCR1(IOTIM); - DMA1_CMAR(IOTIM_DMA) = (uint32_t)iobuf; + DMA1_CMAR(IOTIM_DMA) = (uint32_t)dshotbuf; } static void calibirq(void) { // Align pulse period to the nearest millisecond via HSI trimming within 6.25% margin static int n, q, x, y; if (!cfg.throt_cal) goto done; int p = TIM_CCR1(IOTIM); // Pulse period - if (p < 2400) return; // Invalid signal + if (p < 2000) return; // Invalid signal IWDG_KR = IWDG_KR_RESET; q += p - ((p + 500) / 1000) * 1000; // Cumulative error if (++n & 3) return; @@ -189,7 +199,7 @@ static void servoval(int x) { static void servoirq(void) { int p = TIM_CCR1(IOTIM); // Pulse period int w = TIM_CCR2(IOTIM); // Pulse width - if (p < 2400) return; // Invalid signal + if (p < 2000) return; // Invalid signal if (w >= 28 && w <= 32) { // Telemetry request telreq = 1; IWDG_KR = IWDG_KR_RESET; @@ -213,7 +223,7 @@ static void dshotirq(void) { } } DMA1_CNDTR(IOTIM_DMA) = 16; - DMA1_CCR(IOTIM_DMA) = DMA_CCR_EN | DMA_CCR_TCIE | DMA_CCR_CIRC | DMA_CCR_MINC | DMA_CCR_PSIZE_16BIT | DMA_CCR_MSIZE_8BIT; + DMA1_CCR(IOTIM_DMA) = DMA_CCR_EN | DMA_CCR_TCIE | DMA_CCR_CIRC | DMA_CCR_MINC | DMA_CCR_PSIZE_16BIT | DMA_CCR_MSIZE_16BIT; TIM_ARR(IOTIM) = -1; TIM_EGR(IOTIM) = TIM_EGR_UG; TIM_CR1(IOTIM) = TIM_CR1_CEN | TIM_CR1_ARPE; @@ -245,7 +255,7 @@ static void dshotdma(void) { #endif DMA1_CCR(IOTIM_DMA) = 0; DMA1_CNDTR(IOTIM_DMA) = 16; - DMA1_CCR(IOTIM_DMA) = DMA_CCR_EN | DMA_CCR_TCIE | DMA_CCR_CIRC | DMA_CCR_MINC | DMA_CCR_PSIZE_16BIT | DMA_CCR_MSIZE_8BIT; + DMA1_CCR(IOTIM_DMA) = DMA_CCR_EN | DMA_CCR_TCIE | DMA_CCR_CIRC | DMA_CCR_MINC | DMA_CCR_PSIZE_16BIT | DMA_CCR_MSIZE_16BIT; TIM_ARR(IOTIM) = -1; TIM_EGR(IOTIM) = TIM_EGR_UG; TIM_SMCR(IOTIM) = TIM_SMCR_SMS_RM | TIM_SMCR_TS_TI1F_ED; // Reset on any edge on TI1 @@ -255,14 +265,15 @@ static void dshotdma(void) { return; } int x = 0; + int y = (dshotarr1 + 1) >> 2; // Half-bit time for (int i = 0; i < 16; ++i) { x <<= 1; - if (iobuf[i] >= CLK_CNT(1200000)) x |= 1; + if (dshotbuf[i] >= y) x |= 1; } if (dshotcrc(x, dshotinv)) { // Invalid checksum DMA1_CCR(IOTIM_DMA) = 0; TIM_CR1(IOTIM) = TIM_CR1_CEN | TIM_CR1_ARPE | TIM_CR1_URS; - TIM_ARR(IOTIM) = CLK_CNT(300000) - 1; // Minimum idle time + TIM_ARR(IOTIM) = dshotarr1; // Frame reset time (two bits) TIM_EGR(IOTIM) = TIM_EGR_UG; TIM_SR(IOTIM) = ~TIM_SR_UIF; TIM_DIER(IOTIM) = TIM_DIER_UIE; @@ -277,7 +288,7 @@ static void dshotdma(void) { TIM_CR2(IOTIM) = TIM_CR2_CCDS; // CC1 DMA request on UEV using the same DMA channel DMA1_CCR(IOTIM_DMA) = 0; DMA1_CNDTR(IOTIM_DMA) = 23; - DMA1_CCR(IOTIM_DMA) = DMA_CCR_EN | DMA_CCR_TCIE | DMA_CCR_DIR | DMA_CCR_MINC | DMA_CCR_PSIZE_16BIT | DMA_CCR_MSIZE_8BIT; + DMA1_CCR(IOTIM_DMA) = DMA_CCR_EN | DMA_CCR_TCIE | DMA_CCR_DIR | DMA_CCR_MINC | DMA_CCR_PSIZE_16BIT | DMA_CCR_MSIZE_16BIT; if (!dshotval) { int a = ertm ? ertm : 65408; int b = 0; @@ -290,18 +301,18 @@ static void dshotdma(void) { for (int i = 0, j = 0; i < 16; i += 4, j += 5) b |= gcr[a >> i & 0xf] << j; for (int p = -1, i = 19; i >= 0; --i) { if (b >> i & 1) p = ~p; - iobuf[20 - i] = p; + dshotbuf[20 - i] = p; } - iobuf[0] = -1; - iobuf[21] = 0; - iobuf[22] = 0; + dshotbuf[0] = -1; + dshotbuf[21] = 0; + dshotbuf[22] = 0; __disable_irq(); - int arr = (CLK_CNT(40000) >> TIM_PSC(IOTIM)) - TIM_CNT(IOTIM) - 1; // Calculate 25us output delay + int arr = CLK_CNT(33333) - TIM_CNT(IOTIM) - 1; // Calculate 30us output delay if (arr < 99) arr = 99; // Sanity check TIM_ARR(IOTIM) = arr; // Preload output delay TIM_CCR1(IOTIM) = 0; // Preload high level TIM_EGR(IOTIM) = TIM_EGR_UG; // Update registers and trigger DMA to preload the first bit - TIM_ARR(IOTIM) = CLK_CNT(750000) - 1; // Preload bit time + TIM_ARR(IOTIM) = dshotarr2; // Preload bit time TIM_CCER(IOTIM) = TIM_CCER_CC1E; // Enable output __enable_irq(); if (!rep || !--rep) dshotval = 0; diff --git a/src/main.c b/src/main.c index d5e7e5f..ad45cc5 100644 --- a/src/main.c +++ b/src/main.c @@ -17,7 +17,7 @@ #include "common.h" -#define REVISION 4 +#define REVISION 5 const Cfg cfgdata = { .id = 0x32ea, @@ -42,7 +42,7 @@ const Cfg cfgdata = { .throt_min = THROT_MIN, // Minimum throttle (us) .throt_mid = THROT_MID, // Middle throttle (us) .throt_max = THROT_MAX, // Maximum throttle (us) - .input_mode = INPUT_MODE, // Input mode (0 - servo/DSHOT, 1 - analog, 2 - serial, 3 - iBUS, 4 - SBUS) + .input_mode = INPUT_MODE, // Input mode (0 - servo/Oneshot125/DSHOT, 1 - analog, 2 - serial, 3 - iBUS, 4 - SBUS) .input_chid = INPUT_CHID, // iBUS/SBUS channel ID [0 - off, 1..14 - iBUS, 1..16 - SBUS] .telem_mode = TELEM_MODE, // Telemetry mode (0 - KISS, 1 - KISS auto, 2 - iBUS, 3 - S.Port) .telem_phid = TELEM_PHID, // S.Port physical ID [0 - off, 1..28] @@ -51,7 +51,7 @@ const Cfg cfgdata = { .prot_volt = PROT_VOLT, // Low voltage cutoff per battery cell (V/10) [0 - off, 28..38] .prot_cells = PROT_CELLS, // Number of battery cells [0 - auto, 1..12] .prot_curr = PROT_CURR, // Maximum current (A) [0..255] - .music = "dfa#", // Startup music + .music = MUSIC, // Startup music .volume = VOLUME, // Sound volume (%) [0..100] .beacon = BEACON, // Beacon volume (%) [0..100] .led = LED, // LED bits @@ -62,10 +62,20 @@ Cfg cfg = cfgdata; int throt, ertm, erpm, temp, volt, curr, csum, dshotval, beepval = -1; char analog, telreq, flipdir, beacon, dshotext; -volatile uint32_t tick; +volatile uint32_t tickms; static int step, sine, sync, ival; -static char prep, accl, reverse, ready; +static char prep, accl, tick, reverse, ready; + +#ifdef ANALOG +#define reset() { \ + __disable_irq(); \ + TIM1_CCMR1 = TIM_CCMR1_OC1M_FORCE_LOW | TIM_CCMR1_OC2M_FORCE_LOW; \ + TIM1_CCMR2 = TIM_CCMR2_OC3M_FORCE_LOW; \ + TIM1_EGR = TIM_EGR_COMG; \ + for (;;) WWDG_CR = 0xff; \ +} +#endif /* 6-step commutation sequence: @@ -141,7 +151,6 @@ static void nextstep(void) { #ifndef SENSORED int cc = z ? 4 : 0; #endif - // Phase A if (p & 1) { m1 |= TIM_CCMR1_OC1M_PWM1; er |= cfg.damp ? TIM_CCER_CC1NE | TIM_CCER_CC1E : TIM_CCER_CC1E; @@ -157,7 +166,6 @@ static void nextstep(void) { cc |= 1; #endif } - // Phase B if (p & 2) { m1 |= TIM_CCMR1_OC2M_PWM1; er |= cfg.damp ? TIM_CCER_CC2NE | TIM_CCER_CC2E : TIM_CCER_CC2E; @@ -173,7 +181,6 @@ static void nextstep(void) { cc |= 2; #endif } - // Phase C if (p & 4) { m2 |= TIM_CCMR2_OC3M_PWM1; er |= cfg.damp ? TIM_CCER_CC3NE | TIM_CCER_CC3E : TIM_CCER_CC3E; @@ -198,11 +205,11 @@ static void nextstep(void) { static int pcc, a, b; compctl(pcc); pcc = cc; - if (ival > 800) { - a = 800; + if (ival > 1000) { + a = 1000; b = 0; } else if (++b == 6) { - if (a > ival * 3 >> 1) { // Desync + if (a - ival > ival >> 1) { // Desync TIM_DIER(IFTIM) = TIM_DIER_UIE; return; } @@ -287,24 +294,22 @@ void adc_data(int t, int v, int c, int x) { } void sys_tick_handler(void) { - static int n; SCB_ICSR = SCB_ICSR_PENDSVSET; // Continue with low priority - if (++n < 10) return; // 10kHz -> 1kHz - ++tick; - n = 0; + SCB_SCR = 0; // Resume main loop + if (++tick & 15) return; // 16kHz -> 1kHz + ++tickms; } void pend_sv_handler(void) { - static int n, d, i, m, q; - static char led = -1; - SCB_SCR = 0; // Resume main loop + static char d, led = -1; + static int i, n, q; if (telreq && !cfg.telem_mode) { // Telemetry request sendtelem(); telreq = 0; } - if (++n < 10) return; // 10kHz -> 1kHz + if (tick & 15) return; // 16kHz -> 1kHz adc_trig(); - if (!(tick & 31)) { // Telemetry every 32ms + if (!(tickms & 31)) { // Telemetry every 32ms if (dshotext) { int v = 0; switch (++d) { @@ -327,11 +332,10 @@ void pend_sv_handler(void) { } if (led != cfg.led) ledctl(led = cfg.led); // Update LEDs i += curr; - n = 0; - if (++m < 1000) return; // 1ms -> 1s + if (++n < 1000) return; // 1ms -> 1s csum = (q += i / 1000) / 360; // mAh i = 0; - m = 0; + n = 0; } static void beep(void) { @@ -387,7 +391,7 @@ void main(void) { #endif nvic_set_priority(NVIC_PENDSV_IRQ, 0x80); - STK_RVR = CLK_KHZ / 10 - 1; // 10kHz + STK_RVR = CLK_KHZ / 16 - 1; // 16kHz STK_CVR = 0; STK_CSR = STK_CSR_ENABLE | STK_CSR_TICKINT | STK_CSR_CLKSOURCE_AHB; @@ -400,11 +404,13 @@ void main(void) { if (!cells) cells = (volt + 439) / 440; // Assume maximum 4.4V per battery cell #endif #ifndef ANALOG - if (!(RCC_CSR & (RCC_CSR_IWDGRSTF | RCC_CSR_WWDGRSTF))) { // Power-on - playmusic(cfg.music, cfg.volume); // Play startup music + int csr = RCC_CSR; + RCC_CSR = RCC_CSR_RMVF; // Clear reset flags + if (!(csr & (RCC_CSR_IWDGRSTF | RCC_CSR_WWDGRSTF))) { // Power-on + playmusic(cfg.music, cfg.volume); if (cfg.prot_volt) for (int i = 0; i < cells; ++i) playmusic("_2D", cfg.volume); // Number of battery cells } - if (cfg.arm) { + if (cfg.arm || (csr & RCC_CSR_WWDGRSTF)) { // Arming required TIM14_PSC = CLK_KHZ / 10 - 1; // 0.1ms resolution TIM14_ARR = 2499; // 250ms TIM14_CR1 = TIM_CR1_URS; @@ -420,39 +426,39 @@ void main(void) { } throt = 0; TIM14_CR1 = 0; - playmusic("GC", cfg.volume); // Arming beep + playmusic("GC", cfg.volume); } #endif laststep(); TIM1_EGR = TIM_EGR_COMG; PID curpid = {.Kp = 400, .Ki = 0, .Kd = 600}; - for (int curduty = 0, running = 0, braking = 2, choke = 0, r = 0, n = 0, v = 0;;) { + for (int curduty = 0, running = 0, braking = 2, choke = 0, r = 0, v = 0;;) { SCB_SCR = SCB_SCR_SLEEPONEXIT; // Suspend main loop WWDG_CR = 0xff; __WFI(); - int throtval = throt; + int input = throt; int range = cfg.sine_range * 20; int margin = range && sine ? 20 : 0; int newduty = 0; if (!running) curduty = 0; - if (throtval > 0) { // Forward throttle - if (range + margin < throtval) newduty = scale(throtval, range, 2000, cfg.duty_min * 20, cfg.duty_max * 20); - else sine = scale(throtval, 0, range, 1000, 145); + if (input > 0) { // Forward + if (range + margin < input) newduty = scale(input, range, 2000, cfg.duty_min * 20, cfg.duty_max * 20); + else sine = scale(input, 0, range, 1000, 145); reverse = cfg.revdir ^ flipdir; running = 1; braking = 0; - } else if (throtval < 0) { // Reverse throttle + } else if (input < 0) { // Reverse if (cfg.throt_mode == 2 && braking != 2) { // Proportional brake - curduty = scale(-throtval, 0, 2000, cfg.duty_drag * 20, 2000); + curduty = scale(-input, 0, 2000, cfg.duty_drag * 20, 2000); running = 0; braking = 1; } else { - if (range + margin < -throtval) newduty = scale(-throtval, range, 2000, cfg.duty_min * 20, cfg.duty_max * 20); - else sine = scale(-throtval, 0, range, 1000, 145); + if (range + margin < -input) newduty = scale(-input, range, 2000, cfg.duty_min * 20, cfg.duty_max * 20); + else sine = scale(-input, 0, range, 1000, 145); reverse = !cfg.revdir ^ flipdir; running = 1; } - } else { // Neutral throttle + } else { // Neutral curduty = cfg.duty_drag * 20; running = 0; if (braking == 1) braking = 2; // Reverse after braking @@ -494,9 +500,9 @@ void main(void) { if (newduty > maxduty) newduty = maxduty; } int a = accl ? 0 : cfg.duty_ramp; - int b = a / 5; - if (r < a % 5) ++b; - if (++r == 5) r = 0; + int b = a >> 3; + if (r < (a & 7)) ++b; + if (++r == 8) r = 0; if (curduty >= newduty || (curduty += b) > newduty) curduty = newduty; // Acceleration ramping } int ccr = scale(curduty, 0, 2000, running && cfg.damp ? DEAD_TIME : 0, arr); @@ -545,12 +551,11 @@ void main(void) { __enable_irq(); } beep(); - if (++n < 10) continue; // 10kHz -> 1kHz + if (tick & 15) continue; // 16kHz -> 1kHz if (volt >= cfg.prot_volt * cells * 10) v = 0; else if (++v == 3000) reset(); // Low voltage cutoff after 3s int t = cfg.prot_temp ? clamp((temp - cfg.prot_temp) * 100, 0, 1500) : 0; // 25% power cap @ 15C above threshold int u = cfg.prot_curr ? calcpid(&curpid, curr, cfg.prot_curr * 100) >> 10 : 0; // Current PID control choke = clamp(choke + u, t, 2000); - n = 0; } } diff --git a/src/telem.c b/src/telem.c index deae235..ebc004a 100644 --- a/src/telem.c +++ b/src/telem.c @@ -29,7 +29,7 @@ static int sportdma(void); static int (*iodma)(void); -static char iobuf[16], rxlen; +static char rxlen, iobuf[16]; void inittelem(void) { USART1_BRR = CLK_CNT(115200); diff --git a/src/util.c b/src/util.c index 3116236..3865e9e 100644 --- a/src/util.c +++ b/src/util.c @@ -197,12 +197,12 @@ int playmusic(const char *str, int vol) { a = *str; if (a >= '1' && a <= '8') a -= '0', ++str; else a = 1; - for (uint32_t t = tick + a * 125; t != tick;) { // Duration 125*X ms + for (uint32_t t = tickms + a * 125; t != tickms;) { // Duration 125*X ms if (TIM14_CR1 & TIM_CR1_CEN) TIM14_EGR = TIM_EGR_UG; // Reset arming timeout IWDG_KR = IWDG_KR_RESET; WWDG_CR = 0xff; } - TIM1_CCR1 = 0; // Preload silence + TIM1_CCR3 = 0; // Preload silence } TIM1_CCMR1 = TIM_CCMR1_OC1M_FORCE_LOW | TIM_CCMR1_OC2M_FORCE_LOW; TIM1_CCMR2 = TIM_CCMR2_OC3M_FORCE_LOW;