Untested, but it at least compiles :-)

diff --git a/ibus-mixer.c b/ibus-mixer.c
index c1b382b7a6e7a89e33183c72d834800804f4b17a..129923b0ee3c990adcd48ed75973ceb9ef2e243b 100644 (file)
--- 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();
                }
        }
 }