WIP: cleanups

diff --git a/ibus-mixer.c b/ibus-mixer.c
index 530385f4bc1bb71ae7fc70199c463c4f8a814ce0..c1b382b7a6e7a89e33183c72d834800804f4b17a 100644 (file)
--- a/ibus-mixer.c
+++ b/ibus-mixer.c
@@ -8,7 +8,51 @@
 #include <util/delay.h>
 
 #define N_SERVO_CHANNELS       18
-#define SERVO_FRAME_SIZE       32
+#define IBUS_SERVO_FRAME_SIZE  32
+#define IBUS_SERVO_FRAME_ID    0x40    // first byte after the length
+
+/* ---------- LEDs for debugging ---------- */
+
+void led_init(void)
+{
+       PORTB &= ~_BV(PB5);
+       DDRB |= _BV(PB5);
+}
+
+void led1_off(void)
+{
+       PORTB &= ~_BV(PB5);
+}
+
+void led1_on(void)
+{
+       PORTB |= _BV(PB5);
+}
+
+/* ----------------- Timer ----------------- */
+
+typedef uint16_t time_t;
+
+static void timer_init(void)
+{
+       TCCR1A = 0; // no PWM or WGM output
+       TCCR1B = _BV(CS11); // clk/8
+
+       DDRD |= _BV(PD2) | _BV(PD3) | _BV(PD4) | _BV(PD5);
+       PORTD &= ~_BV(PD2);
+}
+
+static time_t inline get_time(void)
+{
+       time_t rv;
+
+       ATOMIC_BLOCK(ATOMIC_FORCEON) {
+               rv = TCNT1;
+       };
+
+       return rv;
+}
+
 
 /* ----------------- USART ----------------- */
 
@@ -16,19 +60,20 @@
 #define UART_BAUD       115200
 #define UBRR_VAL        ((F_CPU + 8UL * UART_BAUD) / (16UL*UART_BAUD) - 1)
 
-#define BUFLEN SERVO_FRAME_SIZE
-static volatile uint8_t buffer[BUFLEN];
-static volatile uint8_t buf_offset;
+volatile uint8_t serial_frame[IBUS_SERVO_FRAME_SIZE];
+volatile uint8_t serial_frame_ready;
 
-static void handle_rx_packet(void);
+static volatile uint8_t serial_frame_pos;
 
-static void serial_init(void)
+void serial_init(void)
 {
        UBRR0 = UBRR_VAL;
 
        UCSR0A = 0;
        UCSR0B = _BV(RXEN0) | _BV(RXCIE0);
         UCSR0C = _BV(UCSZ01)|_BV(UCSZ00);
+
+       serial_frame_ready = serial_frame_pos = 0;
 }
 
 static void serial_enable_rx(void)
@@ -37,57 +82,45 @@ static void serial_enable_rx(void)
        UCSR0B |= _BV(RXEN0) | _BV(RXCIE0);
 }
 
-static void led_init(void)
-{
-       DDRB |= _BV(PB5);
-       PORTB &= ~_BV(PB5);
-}
-
-static void inline led1_off(void)
-{
-       PORTB &= ~_BV(PB5);
-}
-
-static void inline led1_on(void)
-{
-       PORTB |= _BV(PB5);
-}
-
 #define serial_rx_vect USART_RX_vect
 
 #define serial_data UDR0
 
-static void recv_restart(void)
-{
-       // led2_on();
-
-       buf_offset = 0;
-       serial_enable_rx();
-}
-
-// USART receive interrupt
+/*
+ * USART receive interrupt
+ *
+ * In order to save the latency, we do only the necessary things here,
+ * and postpone parsing the frame to tne non-irq time in the main
+ * loop. Also, we expect the main loop to finish before the second
+ * byte of the next frame is read, so we do not do atomic reads on top
+ * of serial_frame[].
+ */
 ISR(serial_rx_vect)
 {
        uint8_t val = serial_data;
 
-       // a shorthand - for now, we accept 4-byte packets only
-       if (buf_offset == 0 && val != SERVO_FRAME_SIZE)
-               return;
+       // a shorthand - for now, we accept fixed-size packets only
+       if (serial_frame_pos == 0 && val != IBUS_SERVO_FRAME_SIZE)
+               goto restart;
 
-       buffer[buf_offset++] = val;
+       if (serial_frame_pos == 1
+               && serial_frame[0] == IBUS_SERVO_FRAME_SIZE
+               && val != IBUS_SERVO_FRAME_ID)
+               goto restart;
 
-       if (buf_offset == buffer[0]) {
-               handle_rx_packet();
-               buf_offset = 0;
-       }
+       serial_frame[serial_frame_pos++] = val;
+
+       if (serial_frame_pos == serial_frame[0])
+               serial_frame_ready = 1;
+restart:
+       serial_frame_pos = 0;
 }
 
-/* ----------------- Timer ----------------- */
-typedef uint16_t time_t;
+/* ----------------- Servos ----------------- */
 
 #define SERVO_MASK     (_BV(PD2)|_BV(PD3)|_BV(PD4)|_BV(PD5)|_BV(PD6)|_BV(PD7))
 
-static const prog_uint8_t servo_bits[] = {
+static const PROGMEM uint8_t servo_bits[] = {
        _BV(PD2),
        _BV(PD3),
        _BV(PD4),
@@ -96,24 +129,10 @@ static const prog_uint8_t servo_bits[] = {
        _BV(PD7),
 };
 
-static void timer_init(void)
+static void xxx_init(void)
 {
-       TCCR1A = 0; // no PWM or WGM output
-       TCCR1B = _BV(CS11); // clk/8
-
-       DDRD |= _BV(PD2) | _BV(PD3) | _BV(PD4) | _BV(PD5)
-       PORTD &= ~_BV(PD2)
-}
-
-static time_t inline get_time(void)
-{
-       time_t rv;
-
-       ATOMIC_BLOCK(ATOMIC_FORCEON) {
-               rv = TCNT1;
-       };
-
-       return rv;
+       DDRD |= _BV(PD2) | _BV(PD3) | _BV(PD4) | _BV(PD5);
+       PORTD &= ~_BV(PD2);
 }
 
 // run this inside interrupt or in atomic context
@@ -143,27 +162,22 @@ ISR(TIMER1_COMPA_vect) {
 }
 
 /* ----------------- iBus ------------------ */
-static void ibus_init(void)
-{
-       recv_restart();
-}
 
-static void handle_rx_packet(void)
+static void ibus_servo_frame(void)
 {
        uint16_t csum = 0xFFFF, servo;
-       uint8_t i, cmd, dev;
+       uint8_t i;
 
-       for (i = 0; i < buf_offset-2; i++)
-               csum -= (uint16_t)buffer[i];
+       for (i = 0; i < serial_frame[0]-2; i++)
+               csum -= (uint16_t)serial_frame[i];
 
-       if ((buffer[buf_offset-2] != (csum & 0xFF))
-               || (buffer[buf_offset-1] != (csum >> 8))) { // invalid csum
-               buf_offset = 0; // start over
+       if ((serial_frame[serial_frame[0]-2] != (csum & 0xFF))
+               || (serial_frame[serial_frame[0]-1] != (csum >> 8))) { // invalid csum
                return;
        }
 
-       servo = buffer[12];
-       servo |= ((uint16_t)buffer[13] & 0x000F) << 8;
+       servo = serial_frame[12];
+       servo |= ((uint16_t)serial_frame[13] & 0x000F) << 8;
 
        /*
         * -120 % == 0x384 ==  900
@@ -179,19 +193,20 @@ static void handle_rx_packet(void)
 int main(void)
 {
        led_init();
-
-       // serial_init();
        timer_init();
+       serial_init();
 #if 0
        ibus_init();
 
 #endif
-       interrupt_after(1000);
-
+       serial_enable_rx();
        sei();
 
        while (1) {
+               if (serial_frame_ready) {
+                       serial_frame_ready = 0;
+                       ibus_servo_frame();
+               }
        }
-
 }