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;