Firmware revision 12 (patch 6)

Skip servo PWM calibration for odd frequencies
Fix DSHOT1200 reference timings
Prevent saving config while playing music
Improve sound output

mcu/AT32F421/config.c

@@ -160,7 +160,7 @@ void ledctl(int x) {
 void hsictl(int x) {
 	int cr = RCC_CR;
 	int tv = (cr & 0xfc) >> 2; // 6 bits
-	RCC_CR = (cr & ~0xfc) | ((tv + x) & 0x3f) << 2;
+	RCC_CR = (cr & ~0xfc) | clamp(tv + x, 0, 0x3f) << 2;
 }
 
 void compctl(int x) {

mcu/GD32E230/config.c

@@ -84,7 +84,7 @@ void init(void) {
 	nvic_enable_irq(NVIC_DMA1_CHANNEL2_3_DMA2_CHANNEL1_2_IRQ);
 	nvic_enable_irq(NVIC_DMA1_CHANNEL4_7_DMA2_CHANNEL3_5_IRQ);
 
-	TIM1_DIER = TIM_DIER_UIE | TIM_DIER_CC1IE | TIM_DIER_CC4IE; // Software comparator blanking, ADC trigger
+	TIM1_DIER = TIM_DIER_CC1IE; // ADC trigger
 	TIM1_SMCR = TIM_SMCR_TS_ITR2; // TRGI=TIM3
 	TIM3_CR2 = TIM_CR2_MMS_COMPARE_OC3REF; // TRGO=OC3REF
 	TIM3_CCMR1 = TIM_CCMR1_CC1S_IN_TI1;

mcu/GD32E230/config.h

@@ -18,6 +18,7 @@
 #pragma once
 
 #define CLK 72000000
+#define SW_BLANKING
 #define IO_PA2
 #define XTIM TIM6
 

mcu/GD32F350/config.c

@@ -97,7 +97,7 @@ void init(void) {
 	nvic_enable_irq(NVIC_DMA1_CHANNEL2_3_DMA2_CHANNEL1_2_IRQ);
 	nvic_enable_irq(NVIC_DMA1_CHANNEL4_7_DMA2_CHANNEL3_5_IRQ);
 
-	TIM1_DIER = TIM_DIER_UIE | TIM_DIER_CC1IE | TIM_DIER_CC4IE; // Software comparator blanking, ADC trigger
+	TIM1_DIER = TIM_DIER_CC1IE; // ADC trigger
 	TIM1_SMCR = TIM_SMCR_TS_ITR1; // TRGI=TIM2
 	TIM2_CR2 = TIM_CR2_MMS_COMPARE_OC1REF; // TRGO=OC1REF
 	TIM2_CCMR1 = TIM_CCMR1_OC1PE | TIM_CCMR1_OC1M_PWM2; // Inverted PWM on OC1

mcu/GD32F350/config.h

@@ -18,6 +18,7 @@
 #pragma once
 
 #define CLK 104000000
+#define SW_BLANKING
 #define XTIM TIM6
 
 #define IFTIM TIM2

mcu/STM32F051/config.c

@@ -91,7 +91,6 @@ void init(void) {
 	nvic_enable_irq(NVIC_DMA1_CHANNEL2_3_DMA2_CHANNEL1_2_IRQ);
 	nvic_enable_irq(NVIC_DMA1_CHANNEL4_7_DMA2_CHANNEL3_5_IRQ);
 
-	TIM1_DIER = TIM_DIER_UIE | TIM_DIER_CC4IE; // Software comparator blanking
 	TIM1_SMCR = TIM_SMCR_TS_ITR1; // TRGI=TIM2
 	TIM2_CR2 = TIM_CR2_MMS_COMPARE_OC1REF; // TRGO=OC1REF
 	TIM2_CCMR1 = TIM_CCMR1_OC1PE | TIM_CCMR1_OC1M_PWM2; // Inverted PWM on OC1

mcu/STM32F051/config.h

@@ -18,6 +18,7 @@
 #pragma once
 
 #define CLK 48000000
+#define SW_BLANKING
 #define XTIM TIM6
 
 #define IFTIM TIM2

mcu/STM32G071/config.c

@@ -199,7 +199,7 @@ void ledctl(int x) {
 void hsictl(int x) {
 	int cr = RCC_ICSCR;
 	int tv = (cr & 0x7f00) >> 8; // 7 bits
-	RCC_ICSCR = (cr & ~0x7f00) | ((tv + x) & 0x7f) << 8;
+	RCC_ICSCR = (cr & ~0x7f00) | clamp(tv + x, 0, 0x7f) << 8;
 }
 
 void compctl(int x) {

mcu/STM32G431/config.c

@@ -219,7 +219,7 @@ void ledctl(int x) {
 void hsictl(int x) {
 	int cr = RCC_ICSCR;
 	int tv = (cr & 0x7f000000) >> 24; // 7 bits
-	RCC_ICSCR = (cr & ~0x7f000000) | ((tv + x) & 0x7f) << 24;
+	RCC_ICSCR = (cr & ~0x7f000000) | clamp(tv + x, 0, 0x7f) << 24;
 }
 
 void compctl(int x) {

src/common.h

@@ -114,6 +114,8 @@ typedef struct {
 	int Kp, Ki, Kd, Li, i, x;
 } PID;
 
+typedef void (*Func)(void);
+
 extern char _cfg[], _cfg_start[], _cfg_end[], _rom[], _ram[], _boot[], _vec[]; // Linker exports
 extern const uint16_t sinedata[];
 extern const Cfg cfgdata;
@@ -134,7 +136,7 @@ void io_serial(void);
 void io_analog(void);
 void adctrig(void);
 void adcdata(int t, int u, int v, int c, int a);
-void delay(int ms);
+void delay(int ms, Func f);
 void kisstelem(void);
 void autotelem(void);
 int execcmd(char *buf);

src/io.c

@@ -41,9 +41,7 @@ static void crsfirq(void);
 static char rxlen;
 #endif
 
-static void (*ioirq)(void);
-static void (*iodma)(void);
-
+static Func ioirq, iodma;
 static char dshotinv, iobuf[1024];
 static uint16_t dshotarr1, dshotarr2, dshotbuf1[32], dshotbuf2[23] = {-1, -1, 0, -1, 0, -1, -1, 0, -1, 0, -1, -1, 0, -1, 0, -1, 0, -1, -1, -1};
 
@@ -173,18 +171,18 @@ static void entryirq(void) {
 		n = 0;
 		return;
 	}
-	int m = 3;
+	int m = 2;
 	while (t >= CLK_CNT(800000)) t >>= 1, --m;
 	if (d != m) {
 		d = m;
 		n = 1;
 		return;
 	}
-	if (m < 1 || n < 4) return;
+	if (m < 0 || n < 4) return;
 	ioirq = dshotirq;
 	iodma = dshotdma;
-	dshotarr1 = CLK_CNT(m * 150000) - 1;
-	dshotarr2 = CLK_CNT(m * 375000) - 1;
+	dshotarr1 = CLK_CNT(150000 << m) - 1;
+	dshotarr2 = CLK_CNT(375000 << m) - 1;
 	TIM_CCER(IOTIM) = 0;
 	TIM_SMCR(IOTIM) = TIM_SMCR_SMS_RM | TIM_SMCR_TS_TI1F_ED; // Reset on any edge on TI1
 	TIM_CCMR1(IOTIM) = TIM_CCMR1_CC1S_IN_TRC | TIM_CCMR1_IC1F_CK_INT_N_8;
@@ -195,26 +193,28 @@ static void entryirq(void) {
 	DMA1_CMAR(IOTIM_DMA) = (uint32_t)dshotbuf1;
 }
 
-static void calibirq(void) { // Align pulse period to the nearest millisecond via HSI trimming within 6.25% margin
+static void calibirq(void) {
 	static int n, q, x, y;
-	if (!cfg.throt_cal) goto done;
 	int p = TIM_CCR1(IOTIM); // Pulse period
-	if (p < 2000) return; // Invalid signal
-	IWDG_KR = IWDG_KR_RESET;
-	q += p - ((p + 500) / 1000) * 1000; // Cumulative error
-	if (++n & 3) return;
-	if (q > x) { // Slow down
-		if ((q << 3) > p) goto done;
-		y = -q;
-		hsictl(-1);
-	} else if (q < y) { // Speed up
-		if ((q << 3) < -p) goto done;
-		x = -q;
-		hsictl(1);
-	} else goto done;
-	q = 0;
-	return;
-done:
+	if (cfg.throt_cal && // 50/100/125/200/250/333/500Hz servo PWM within 8% margin
+		((p < 20800 && p > 19200) || (p < 10400 && p > 9600) || (p < 8320 && p > 7680) ||
+		(p < 5200 && p > 4800) || (p < 4160 && p > 3840) || (p < 3120 && p > 2880) || (p < 2080 && p > 1920))) {
+		IWDG_KR = IWDG_KR_RESET;
+		q += p - ((p + 500) / 1000) * 1000; // Cumulative error
+		if (++n & 3) return;
+		if (q > x) { // Slow down
+			y = -q;
+			q = 0;
+			hsictl(-1);
+			return;
+		}
+		if (q < y) { // Speed up
+			x = -q;
+			q = 0;
+			hsictl(1);
+			return;
+		}
+	}
 	ioirq = servoirq;
 	TIM_CCMR1(IOTIM) = TIM_CCMR1_CC1S_IN_TI1 | TIM_CCMR1_IC1F_DTF_DIV_8_N_8 | TIM_CCMR1_CC2S_IN_TI1 | TIM_CCMR1_IC2F_DTF_DIV_8_N_8;
 	TIM_CCER(IOTIM) = TIM_CCER_CC1E | TIM_CCER_CC2E | TIM_CCER_CC2P; // IC1 on rising edge on TI1, IC2 on falling edge on TI1

src/main.c

@@ -18,7 +18,7 @@
 #include "common.h"
 
 #define REVISION 12
-#define REVPATCH 5
+#define REVPATCH 6
 
 const Cfg cfgdata = {
 	.id = 0x32ea,
@@ -467,12 +467,12 @@ void sys_tick_handler(void) {
 	if (++tickms == tickmsv) tickmsf = 0;
 }
 
-void delay(int ms) {
+void delay(int ms, Func f) {
 	__disable_irq();
 	tickmsv = tickms + ms;
 	tickmsf = 1;
 	__enable_irq();
-	while (tickmsf) TIM_EGR(XTIM) = TIM_EGR_UG; // Reset arming timeout
+	while (tickmsf) f();
 }
 
 void pend_sv_handler(void) {
@@ -493,6 +493,10 @@ void pend_sv_handler(void) {
 	n = 0;
 }
 
+static void delayf(void) {
+	TIM_EGR(XTIM) = TIM_EGR_UG; // Reset arming timeout
+}
+
 static void beep(void) {
 	static const char *const beacons[] = {"EG", "FA", "GB", "AB#", "aDGE"};
 	static const char *const values[] = {"c6", "C2", "D2C2", "E2D2C2", "F#2E2D2C2", "G#A#G#A#G#2", "G#A#G#A#F#2G#2", "G#A#G#A#E2F#2G#2", "G#A#G#A#D2E2F#2G#2", "G#A#G#A#C2D2E2F#2G#2", 0};
@@ -504,7 +508,7 @@ static void beep(void) {
 	int i[10], n = 0, x = beepval, vol = max(cfg.volume, 25);
 	while (i[n++] = x % 10, x /= 10);
 	while (n--) {
-		if (x++) playmusic("_4", vol);
+		if (x++) delay(500, delayf);
 		playmusic(values[i[n]], vol);
 	}
 	beepval = -1;
@@ -571,7 +575,7 @@ void main(void) {
 		playmusic(cfg.music, cfg.volume);
 		if (cfg.prot_volt) { // Report the number of battery cells
 			beepval = cells;
-			delay(250);
+			delay(250, delayf);
 			beep();
 		}
 	}
@@ -740,15 +744,23 @@ void main(void) {
 			ertm = 100000000;
 			nextstep();
 			TIM1_EGR = TIM_EGR_UG | TIM_EGR_COMG;
+#ifdef SW_BLANKING
+			TIM1_DIER |= TIM_DIER_COMIE | TIM_DIER_UIE | TIM_DIER_CC4IE;
+#else
 			TIM1_DIER |= TIM_DIER_COMIE;
+#endif
 			TIM_ARR(IFTIM) = IFTIM_OCR = (1 << (IFTIM_XRES + 16)) - 1;
 			TIM_EGR(IFTIM) = TIM_EGR_UG;
 			__enable_irq();
 			initpid(&bpid, 10000 << IFTIM_XRES);
 			boost = 0;
 		} else if (!running && step) { // Stop motor
 			__disable_irq();
+#ifdef SW_BLANKING
+			TIM1_DIER &= ~(TIM_DIER_COMIE | TIM_DIER_UIE | TIM_DIER_CC4IE);
+#else
 			TIM1_DIER &= ~TIM_DIER_COMIE;
+#endif
 			TIM_DIER(IFTIM) = 0;
 			TIM_ARR(IFTIM) = 0;
 			TIM_EGR(IFTIM) = TIM_EGR_UG;

src/util.c

@@ -95,6 +95,8 @@
 #define FLASH_CR_STRT FLASH_CR_START
 #endif
 
+static char lock;
+
 void initgpio(void) {
 #ifdef HALL_MAP
 	GPIO(HALL_PORT, PUPDR) |= (1 << HALL_PIN1 * 2) | (1 << HALL_PIN2 * 2) | (1 << HALL_PIN3 * 2);
@@ -202,7 +204,7 @@ __attribute__((__weak__))
 void hsictl(int x) {
 	int cr = RCC_CR;
 	int tv = (cr & 0xf8) >> 3; // 5 bits
-	RCC_CR = (cr & ~0xf8) | ((tv + x) & 0x1f) << 3;
+	RCC_CR = (cr & ~0xf8) | clamp(tv + x, 0, 0x1f) << 3;
 }
 
 char crc8(const char *buf, int len) {
@@ -369,7 +371,7 @@ void checkcfg(void) {
 }
 
 int savecfg(void) {
-	if (ertm) return 0;
+	if (ertm || lock) return 0;
 	__disable_irq();
 	FLASH_KEYR = FLASH_KEYR_KEY1;
 	FLASH_KEYR = FLASH_KEYR_KEY2;
@@ -435,29 +437,37 @@ void resetcom(void) {
 	TIM1_EGR = TIM_EGR_UG | TIM_EGR_COMG;
 }
 
+static void delayf(void) {
+	TIM_EGR(XTIM) = TIM_EGR_UG; // Reset arming timeout
+	if (!(TIM1_SR & TIM_SR_UIF)) return;
+	TIM1_SR = ~TIM_SR_UIF;
+	int a = TIM1_CCR1;
+	int b = TIM1_CCR3;
+	TIM1_CCR1 = b;
+	TIM1_CCR3 = a;
+}
+
 int playmusic(const char *str, int vol) {
-	static const uint16_t arr[] = {30575, 28859, 27240, 25713, 24268, 22906, 21621, 20407, 19261, 18181, 17160, 16196, 15287};
-	static char flag;
+	static const uint16_t arr[] = {15287, 14429, 13619, 12856, 12133, 11452, 10810, 10203, 9630, 9090, 8579, 8097, 7643};
 	char *end;
 	int tmp = strtol(str, &end, 10); // Tempo
-	if (str == end) tmp = 125; // 120BPM by default
+	if (str == end) tmp = 125; // 120 BPM by default
 	else {
 		if (tmp < 10 || tmp > 999) return 0; // Sanity check
 		tmp = 15000 / tmp;
 		str = end;
 	}
-	if (!vol || ertm || flag) return 0;
-	flag = 1;
-	vol <<= 1;
+	if (!vol || ertm || lock) return 0;
+	lock = 1;
 	resetcom();
 #ifdef PWM_ENABLE
-	TIM1_CCMR1 = TIM_CCMR1_OC1M_FORCE_LOW | TIM_CCMR1_OC2PE | TIM_CCMR1_OC2M_PWM1;
-	TIM1_CCMR2 = TIM_CCMR2_OC3M_FORCE_LOW;
-	int er = TIM_CCER_CC1NE | TIM_CCER_CC2E | TIM_CCER_CC3NE;
+	TIM1_CCMR1 = TIM_CCMR1_OC1PE | TIM_CCMR1_OC1M_PWM1 | TIM_CCMR1_OC2M_FORCE_LOW;
+	TIM1_CCMR2 = TIM_CCMR2_OC3PE | TIM_CCMR2_OC3M_PWM1;
+	int er = TIM_CCER_CC1E | TIM_CCER_CC2NE | TIM_CCER_CC3E;
 #else
-	TIM1_CCMR1 = TIM_CCMR1_OC1M_FORCE_HIGH | TIM_CCMR1_OC2PE | TIM_CCMR1_OC2M_PWM1;
-	TIM1_CCMR2 = TIM_CCMR2_OC3M_FORCE_HIGH;
-	int er = TIM_CCER_CC1NE | TIM_CCER_CC2E | TIM_CCER_CC2NE | TIM_CCER_CC3NE;
+	TIM1_CCMR1 = TIM_CCMR1_OC1PE | TIM_CCMR1_OC1M_PWM1 | TIM_CCMR1_OC2M_FORCE_HIGH;
+	TIM1_CCMR2 = TIM_CCMR2_OC3PE | TIM_CCMR2_OC3M_PWM1;
+	int er = TIM_CCER_CC1E | TIM_CCER_CC1NE | TIM_CCER_CC2NE | TIM_CCER_CC3E | TIM_CCER_CC3NE;
 #endif
 #ifdef INVERTED_HIGH
 	er |= TIM_CCER_CC1P | TIM_CCER_CC2P | TIM_CCER_CC3P;
@@ -471,7 +481,8 @@ int playmusic(const char *str, int vol) {
 		if (a >= 'a' && a <= 'g') a -= 'c', b = 0; // Low note
 		else if (a >= 'A' && a <= 'G') a -= 'C', b = 1; // High note
 		else if (a == '_') { // Pause
-			TIM1_CCR2 = 0;
+			TIM1_CCR1 = 0;
+			TIM1_CCR3 = 0;
 			goto update;
 		} else {
 			if (a == '+' && !c++) continue; // Octave up
@@ -481,10 +492,9 @@ int playmusic(const char *str, int vol) {
 		a = (a + 7) % 7 << 1;
 		if (a > 4) --a;
 		if (*str == '#') ++a, ++str;
-		TIM1_CR1 = TIM_CR1_CEN | TIM_CR1_ARPE | TIM_CR1_UDIS;
 		TIM1_ARR = arr[a] >> (b + c); // Frequency
-		TIM1_CCR2 = vol; // Volume
-		TIM1_CR1 = TIM_CR1_CEN | TIM_CR1_ARPE;
+		TIM1_CCR1 = vol; // Volume
+		TIM1_CCR3 = 0;
 	update:
 		TIM1_EGR = TIM_EGR_UG | TIM_EGR_COMG;
 		a = strtol(str, &end, 10); // Duration
@@ -493,12 +503,11 @@ int playmusic(const char *str, int vol) {
 			if (a < 1 || a > 99) break; // Sanity check
 			str = end;
 		}
-		delay(tmp * a);
+		delay(tmp * a, delayf);
 	}
-	resetcom();
 	TIM1_PSC = 0;
 	TIM1_ARR = CLK_KHZ / 24 - 1;
-	TIM1_EGR = TIM_EGR_UG | TIM_EGR_COMG;
-	flag = 0;
+	resetcom();
+	lock = 0;
 	return !str[-1];
 }