From 0c828161affa90307bb1ff4209e96acb10600241 Mon Sep 17 00:00:00 2001 From: "Jan \"Yenya\" Kasprzak" Date: Wed, 12 Feb 2020 21:30:18 +0100 Subject: [PATCH] WIP: cleanups --- ibus-mixer.c | 169 ++++++++++++++++++++++++++++----------------------- 1 file changed, 92 insertions(+), 77 deletions(-) diff --git a/ibus-mixer.c b/ibus-mixer.c index 530385f..c1b382b 100644 --- a/ibus-mixer.c +++ b/ibus-mixer.c @@ -8,7 +8,51 @@ #include #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(); + } } - } -- 2.43.0