From: Jan "Yenya" Kasprzak <kas@fi.muni.cz>
Date: Thu, 13 Feb 2020 16:52:23 +0000 (+0100)
Subject: Untested, but it at least compiles :-)
X-Git-Url: https://www.fi.muni.cz/~kas/git//home/kas/public_html/git/?p=ibus-mixer.git;a=commitdiff_plain

Untested, but it at least compiles :-)
---

diff --git a/ibus-mixer.c b/ibus-mixer.c
index c1b382b..129923b 100644
--- a/ibus-mixer.c
+++ b/ibus-mixer.c
@@ -7,9 +7,10 @@
 #include <util/atomic.h>
 #include <util/delay.h>
 
-#define N_SERVO_CHANNELS	18
+#define N_IBUS_CHANNELS		18
 #define IBUS_SERVO_FRAME_SIZE	32
 #define IBUS_SERVO_FRAME_ID	0x40	// first byte after the length
+#define N_PWM_CHANNELS		6
 
 /* ---------- LEDs for debugging ---------- */
 
@@ -32,6 +33,7 @@ void led1_on(void)
 /* ----------------- Timer ----------------- */
 
 typedef uint16_t time_t;
+#define TICKS_IN_US	(F_CPU/8000000UL)
 
 static void timer_init(void)
 {
@@ -116,11 +118,55 @@ restart:
 	serial_frame_pos = 0;
 }
 
-/* ----------------- Servos ----------------- */
+/* ----------------- iBus ------------------ */
+
+typedef int16_t servo_val_t;
+servo_val_t ibus_channel[N_IBUS_CHANNELS];
+#define N_IBUS_CHANNELS_DIRECT 14
+/*
+ * channel value      ibus frame value
+ *     -120 %               900
+ *     -100 %              1000
+ *        0 %              1500
+ *      100 %              2000
+ *      120 %              2100
+ */
+#define IBUS_SERVO_CENTER	1500
+
+static void ibus_servo_frame(void)
+{
+	uint16_t csum = 0xFFFF;
+	uint8_t i;
+
+	for (i = 0; i < serial_frame[0]-2; i++)
+		csum -= (uint16_t)serial_frame[i];
+
+	if ((serial_frame[serial_frame[0]-2] != (csum & 0xFF))
+		|| (serial_frame[serial_frame[0]-1] != (csum >> 8))) {
+		// invalid csum
+		return;
+	}
+
+	for (i = 0; i < N_IBUS_CHANNELS_DIRECT; i++)
+		ibus_channel[i] = (uint16_t)serial_frame[2 + 2*i]
+			+ (((uint16_t)serial_frame[3 + 2*i] & 0x000F) << 8)
+			- IBUS_SERVO_CENTER;
+	// TODO channels 15-18
+}
+
+/* -------------- PWM output --------------- */
+
+#define SERVO_PWM_CENTER	(1500 * TICKS_IN_US)
+
+time_t pwm_channels[N_PWM_CHANNELS];
+volatile uint8_t pwm_busy;
+uint8_t pwm_data_ready;
 
-#define SERVO_MASK	(_BV(PD2)|_BV(PD3)|_BV(PD4)|_BV(PD5)|_BV(PD6)|_BV(PD7))
+static uint8_t pwm_channel;
+static time_t pwm_frame_start;
 
-static const PROGMEM uint8_t servo_bits[] = {
+// TODO: move this into PROGMEM?
+static const uint8_t pwm_channel_bit[] = {
 	_BV(PD2),
 	_BV(PD3),
 	_BV(PD4),
@@ -129,65 +175,83 @@ static const PROGMEM uint8_t servo_bits[] = {
 	_BV(PD7),
 };
 
-static void xxx_init(void)
+#define PWM_CH_MASK  (_BV(PD2)|_BV(PD3)|_BV(PD4)|_BV(PD5)|_BV(PD6)|_BV(PD7))
+#define PWM_FRAME_LEN	(20000 * TICKS_IN_US)
+
+static void pwm_init(void)
 {
-	DDRD |= _BV(PD2) | _BV(PD3) | _BV(PD4) | _BV(PD5);
-	PORTD &= ~_BV(PD2);
+	uint8_t i;
+
+	for (i = 0; i < N_PWM_CHANNELS; i++)
+		pwm_channels[i] = 0;
+
+	pwm_channel = N_PWM_CHANNELS + 1;
+	pwm_data_ready = pwm_busy = 0;
+
+	TIMSK1 &= ~_BV(OCIE1A);
+	PORTD &= ~PWM_CH_MASK;
+	DDRD |= PWM_CH_MASK;
 }
 
-// run this inside interrupt or in atomic context
-static void interrupt_after(uint16_t delay)
+static void pwm_set(uint8_t channel, servo_val_t value)
 {
-	uint16_t now = TCNT1;
-	now += delay;
-	OCR1A = now;
-	TIMSK1 |= _BV(OCIE1A);
+	time_t tm = SERVO_PWM_CENTER + (value * TICKS_IN_US);
+
+	pwm_channels[channel] = tm;
 }
 
-ISR(TIMER1_COMPA_vect) {
-	static uint16_t led = 2000;
-
-	led++;
-	if (led & 1) {
-		led1_off();
-		PORTD &= ~_BV(PD2);
-		interrupt_after(65000);
-	} else {
-		led1_on();
-		PORTD |= _BV(PD2);
-		interrupt_after(led);
+static void pwm_send_pulse()
+{
+	uint8_t first_pulse = 0;
+
+	if (pwm_channel == N_PWM_CHANNELS + 1) {
+		if (!pwm_data_ready)
+			return;
+		pwm_channel = 0;
+	}
+
+	if (pwm_channel == 0) {
+		first_pulse = 1;
+		pwm_data_ready = 0;
 	}
-	if (led >= 4000)
-		led = 2000;
+
+	// find a non-empty channel
+	while (!pwm_channels[pwm_channel] && pwm_channel < N_PWM_CHANNELS)
+		pwm_channel++;
+
+	ATOMIC_BLOCK(ATOMIC_RESTORESTATE) {
+		time_t now = TCNT1;
+		if (pwm_channel < N_PWM_CHANNELS) {
+			PORTD |= pwm_channel_bit[pwm_channel];
+			OCR1A = now + pwm_channels[pwm_channel];
+			if (first_pulse)
+				pwm_frame_start = now;
+		} else {
+			OCR1A = pwm_frame_start + PWM_FRAME_LEN;
+		}
+
+		TIMSK1 |= _BV(OCIE1A);
+		pwm_busy = 1;
+	};
 }
 
-/* ----------------- iBus ------------------ */
+ISR(TIMER1_COMPA_vect) {
+	PORTD &= ~PWM_CH_MASK;
+	TIMSK1 &= ~_BV(OCIE1A);
+	pwm_channel++;
+	pwm_busy = 0;
+}
 
-static void ibus_servo_frame(void)
-{
-	uint16_t csum = 0xFFFF, servo;
-	uint8_t i;
 
-	for (i = 0; i < serial_frame[0]-2; i++)
-		csum -= (uint16_t)serial_frame[i];
+/* -------- Custom mixing done here -------- */
 
-	if ((serial_frame[serial_frame[0]-2] != (csum & 0xFF))
-		|| (serial_frame[serial_frame[0]-1] != (csum >> 8))) { // invalid csum
-		return;
-	}
+static void do_mixes()
+{
+	int i;
 
-	servo = serial_frame[12];
-	servo |= ((uint16_t)serial_frame[13] & 0x000F) << 8;
-
-	/*
-	 * -120 % == 0x384 ==  900
-	 * -100 % == 0x3e8 == 1000
-	 *    0 % == 0x5dc == 1500
-	 */
-	if (servo < 0x0834)
-		led1_on();
-	else
-		led1_off();
+	for (i = 0; i < N_PWM_CHANNELS; i++) {
+		pwm_set(i, ibus_channel[i]);
+	}
 }
 
 int main(void)
@@ -195,10 +259,8 @@ int main(void)
 	led_init();
 	timer_init();
 	serial_init();
-#if 0
-	ibus_init();
+	pwm_init();
 
-#endif
 	serial_enable_rx();
 	sei();
 
@@ -206,6 +268,11 @@ int main(void)
 		if (serial_frame_ready) {
 			serial_frame_ready = 0;
 			ibus_servo_frame();
+			do_mixes();
+			pwm_data_ready = 1;
+		}
+		if (!pwm_busy) {
+			pwm_send_pulse();
 		}
 	}
 }