#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 ----------------- */
#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)
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),
_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
}
/* ----------------- 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
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();
+ }
}
-
}