ibus-mixer.c

   1 /* I-Bus servo mixer for FlySky iA6B receiver */
   2
   3 #include <stdio.h>
   4 #include <avr/interrupt.h>
   5 #include <avr/io.h>
   6 #include <avr/pgmspace.h>
   7 #include <util/atomic.h>
   8 #include <util/delay.h>
   9
  10 #define N_SERVO_CHANNELS        18
  11 #define SERVO_FRAME_SIZE        32
  12
  13 /* ----------------- USART ----------------- */
  14
  15 // I-Bus uses 115200n8
  16 #define UART_BAUD       115200
  17 #define UBRR_VAL        ((F_CPU + 8UL * UART_BAUD) / (16UL*UART_BAUD) - 1)
  18
  19 #define BUFLEN SERVO_FRAME_SIZE
  20 static volatile uint8_t buffer[BUFLEN];
  21 static volatile uint8_t buf_offset;
  22
  23 static void handle_rx_packet(void);
  24
  25 static void serial_init(void)
  26 {
  27         UBRR0 = UBRR_VAL;
  28
  29         UCSR0A = 0;
  30         UCSR0B = _BV(RXEN0) | _BV(RXCIE0);
  31         UCSR0C = _BV(UCSZ01)|_BV(UCSZ00);
  32 }
  33
  34 static void serial_enable_rx(void)
  35 {
  36         UCSR0B &= ~(_BV(TXEN0) | _BV(TXCIE0));
  37         UCSR0B |= _BV(RXEN0) | _BV(RXCIE0);
  38 }
  39
  40 static void led_init(void)
  41 {
  42         DDRB |= _BV(PB5);
  43         PORTB &= ~_BV(PB5);
  44 }
  45
  46 static void inline led1_off(void)
  47 {
  48         PORTB &= ~_BV(PB5);
  49 }
  50
  51 static void inline led1_on(void)
  52 {
  53         PORTB |= _BV(PB5);
  54 }
  55
  56 #define serial_rx_vect USART_RX_vect
  57
  58 #define serial_data UDR0
  59
  60 static void recv_restart(void)
  61 {
  62         // led2_on();
  63
  64         buf_offset = 0;
  65         serial_enable_rx();
  66 }
  67
  68 // USART receive interrupt
  69 ISR(serial_rx_vect)
  70 {
  71         uint8_t val = serial_data;
  72
  73         // a shorthand - for now, we accept 4-byte packets only
  74         if (buf_offset == 0 && val != SERVO_FRAME_SIZE)
  75                 return;
  76
  77         buffer[buf_offset++] = val;
  78
  79         if (buf_offset == buffer[0]) {
  80                 handle_rx_packet();
  81                 buf_offset = 0;
  82         }
  83 }
  84
  85 /* ----------------- Timer ----------------- */
  86 typedef uint16_t time_t;
  87
  88 #define SERVO_MASK      (_BV(PD2)|_BV(PD3)|_BV(PD4)|_BV(PD5)|_BV(PD6)|_BV(PD7))
  89
  90 static const prog_uint8_t servo_bits[] = {
  91         _BV(PD2),
  92         _BV(PD3),
  93         _BV(PD4),
  94         _BV(PD5),
  95         _BV(PD6),
  96         _BV(PD7),
  97 };
  98
  99 static void timer_init(void)
 100 {
 101         TCCR1A = 0; // no PWM or WGM output
 102         TCCR1B = _BV(CS11); // clk/8
 103
 104         DDRD |= _BV(PD2) | _BV(PD3) | _BV(PD4) | _BV(PD5)
 105         PORTD &= ~_BV(PD2)
 106 }
 107
 108 static time_t inline get_time(void)
 109 {
 110         time_t rv;
 111
 112         ATOMIC_BLOCK(ATOMIC_FORCEON) {
 113                 rv = TCNT1;
 114         };
 115
 116         return rv;
 117 }
 118
 119 // run this inside interrupt or in atomic context
 120 static void interrupt_after(uint16_t delay)
 121 {
 122         uint16_t now = TCNT1;
 123         now += delay;
 124         OCR1A = now;
 125         TIMSK1 |= _BV(OCIE1A);
 126 }
 127
 128 ISR(TIMER1_COMPA_vect) {
 129         static uint16_t led = 2000;
 130
 131         led++;
 132         if (led & 1) {
 133                 led1_off();
 134                 PORTD &= ~_BV(PD2);
 135                 interrupt_after(65000);
 136         } else {
 137                 led1_on();
 138                 PORTD |= _BV(PD2);
 139                 interrupt_after(led);
 140         }
 141         if (led >= 4000)
 142                 led = 2000;
 143 }
 144
 145 /* ----------------- iBus ------------------ */
 146 static void ibus_init(void)
 147 {
 148         recv_restart();
 149 }
 150
 151 static void handle_rx_packet(void)
 152 {
 153         uint16_t csum = 0xFFFF, servo;
 154         uint8_t i, cmd, dev;
 155
 156         for (i = 0; i < buf_offset-2; i++)
 157                 csum -= (uint16_t)buffer[i];
 158
 159         if ((buffer[buf_offset-2] != (csum & 0xFF))
 160                 || (buffer[buf_offset-1] != (csum >> 8))) { // invalid csum
 161                 buf_offset = 0; // start over
 162                 return;
 163         }
 164
 165         servo = buffer[12];
 166         servo |= ((uint16_t)buffer[13] & 0x000F) << 8;
 167
 168         /*
 169          * -120 % == 0x384 ==  900
 170          * -100 % == 0x3e8 == 1000
 171          *    0 % == 0x5dc == 1500
 172          */
 173         if (servo < 0x0834)
 174                 led1_on();
 175         else
 176                 led1_off();
 177 }
 178
 179 int main(void)
 180 {
 181         led_init();
 182
 183         // serial_init();
 184         timer_init();
 185 #if 0
 186         ibus_init();
 187
 188 #endif
 189         interrupt_after(1000);
 190
 191         sei();
 192
 193         while (1) {
 194         }
 195
 196 }
 197