#include <util/atomic.h>
#include <util/delay.h>
-#define N_SERVO_CHANNELS 18
-#define SERVO_FRAME_SIZE 32
+#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 ---------- */
+
+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;
+#define TICKS_IN_US (F_CPU/8000000UL)
+
+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);
-}
+#define serial_rx_vect USART_RX_vect
-static void inline led1_off(void)
-{
- PORTB &= ~_BV(PB5);
-}
+#define serial_data UDR0
-static void inline led1_on(void)
+/*
+ * 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)
{
- PORTB |= _BV(PB5);
-}
+ uint8_t val = serial_data;
-#define serial_rx_vect USART_RX_vect
+ // a shorthand - for now, we accept fixed-size packets only
+ if (serial_frame_pos == 0 && val != IBUS_SERVO_FRAME_SIZE)
+ goto restart;
-#define serial_data UDR0
+ if (serial_frame_pos == 1
+ && serial_frame[0] == IBUS_SERVO_FRAME_SIZE
+ && val != IBUS_SERVO_FRAME_ID)
+ goto restart;
-static void recv_restart(void)
-{
- // led2_on();
+ serial_frame[serial_frame_pos++] = val;
- buf_offset = 0;
- serial_enable_rx();
+ if (serial_frame_pos == serial_frame[0])
+ serial_frame_ready = 1;
+restart:
+ serial_frame_pos = 0;
}
-// USART receive interrupt
-ISR(serial_rx_vect)
-{
- uint8_t val = serial_data;
+/* ----------------- iBus ------------------ */
- // a shorthand - for now, we accept 4-byte packets only
- if (buf_offset == 0 && val != SERVO_FRAME_SIZE)
- return;
+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;
- buffer[buf_offset++] = val;
+ for (i = 0; i < serial_frame[0]-2; i++)
+ csum -= (uint16_t)serial_frame[i];
- if (buf_offset == buffer[0]) {
- handle_rx_packet();
- buf_offset = 0;
+ 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
}
-/* ----------------- Timer ----------------- */
-typedef uint16_t time_t;
+/* -------------- PWM output --------------- */
-#define SERVO_MASK (_BV(PD2)|_BV(PD3)|_BV(PD4)|_BV(PD5)|_BV(PD6)|_BV(PD7))
+#define SERVO_PWM_CENTER (1500 * TICKS_IN_US)
-static const prog_uint8_t servo_bits[] = {
+time_t pwm_channels[N_PWM_CHANNELS];
+volatile uint8_t pwm_busy;
+uint8_t pwm_data_ready;
+
+static uint8_t pwm_channel;
+static time_t pwm_frame_start;
+
+// TODO: move this into PROGMEM?
+static const uint8_t pwm_channel_bit[] = {
_BV(PD2),
_BV(PD3),
_BV(PD4),
_BV(PD7),
};
-static void timer_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)
{
- TCCR1A = 0; // no PWM or WGM output
- TCCR1B = _BV(CS11); // clk/8
+ uint8_t i;
- DDRD |= _BV(PD2) | _BV(PD3) | _BV(PD4) | _BV(PD5)
- PORTD &= ~_BV(PD2)
+ 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;
}
-static time_t inline get_time(void)
+static void pwm_set(uint8_t channel, servo_val_t value)
{
- time_t rv;
-
- ATOMIC_BLOCK(ATOMIC_FORCEON) {
- rv = TCNT1;
- };
+ time_t tm = SERVO_PWM_CENTER + (value * TICKS_IN_US);
- return rv;
+ pwm_channels[channel] = tm;
}
-// run this inside interrupt or in atomic context
-static void interrupt_after(uint16_t delay)
+static void pwm_send_pulse()
{
- uint16_t now = TCNT1;
- now += delay;
- OCR1A = now;
- TIMSK1 |= _BV(OCIE1A);
-}
+ uint8_t first_pulse = 0;
-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);
+ 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 ------------------ */
-static void ibus_init(void)
-{
- recv_restart();
+ISR(TIMER1_COMPA_vect) {
+ PORTD &= ~PWM_CH_MASK;
+ TIMSK1 &= ~_BV(OCIE1A);
+ pwm_channel++;
+ pwm_busy = 0;
}
-static void handle_rx_packet(void)
-{
- uint16_t csum = 0xFFFF, servo;
- uint8_t i, cmd, dev;
- for (i = 0; i < buf_offset-2; i++)
- csum -= (uint16_t)buffer[i];
+/* -------- Custom mixing done here -------- */
- if ((buffer[buf_offset-2] != (csum & 0xFF))
- || (buffer[buf_offset-1] != (csum >> 8))) { // invalid csum
- buf_offset = 0; // start over
- return;
- }
+static void do_mixes()
+{
+ int i;
- servo = buffer[12];
- servo |= ((uint16_t)buffer[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)
{
led_init();
-
- // serial_init();
timer_init();
-#if 0
- ibus_init();
-
-#endif
- interrupt_after(1000);
+ serial_init();
+ pwm_init();
+ serial_enable_rx();
sei();
while (1) {
+ if (serial_frame_ready) {
+ serial_frame_ready = 0;
+ ibus_servo_frame();
+ do_mixes();
+ pwm_data_ready = 1;
+ }
+ if (!pwm_busy) {
+ pwm_send_pulse();
+ }
}
-
}
https://www.fi.muni.cz/~kas/git / ibus-mixer.git / blobdiff