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 IBUS_SERVO_FRAME_SIZE   32
  12 #define IBUS_SERVO_FRAME_ID     0x40    // first byte after the length
  13
  14 /* ---------- LEDs for debugging ---------- */
  15
  16 void led_init(void)
  17 {
  18         PORTB &= ~_BV(PB5);
  19         DDRB |= _BV(PB5);
  20 }
  21
  22 void led1_off(void)
  23 {
  24         PORTB &= ~_BV(PB5);
  25 }
  26
  27 void led1_on(void)
  28 {
  29         PORTB |= _BV(PB5);
  30 }
  31
  32 /* ----------------- Timer ----------------- */
  33
  34 typedef uint16_t time_t;
  35
  36 static void timer_init(void)
  37 {
  38         TCCR1A = 0; // no PWM or WGM output
  39         TCCR1B = _BV(CS11); // clk/8
  40
  41         DDRD |= _BV(PD2) | _BV(PD3) | _BV(PD4) | _BV(PD5);
  42         PORTD &= ~_BV(PD2);
  43 }
  44
  45 static time_t inline get_time(void)
  46 {
  47         time_t rv;
  48
  49         ATOMIC_BLOCK(ATOMIC_FORCEON) {
  50                 rv = TCNT1;
  51         };
  52
  53         return rv;
  54 }
  55
  56
  57 /* ----------------- USART ----------------- */
  58
  59 // I-Bus uses 115200n8
  60 #define UART_BAUD       115200
  61 #define UBRR_VAL        ((F_CPU + 8UL * UART_BAUD) / (16UL*UART_BAUD) - 1)
  62
  63 volatile uint8_t serial_frame[IBUS_SERVO_FRAME_SIZE];
  64 volatile uint8_t serial_frame_ready;
  65
  66 static volatile uint8_t serial_frame_pos;
  67
  68 void serial_init(void)
  69 {
  70         UBRR0 = UBRR_VAL;
  71
  72         UCSR0A = 0;
  73         UCSR0B = _BV(RXEN0) | _BV(RXCIE0);
  74         UCSR0C = _BV(UCSZ01)|_BV(UCSZ00);
  75
  76         serial_frame_ready = serial_frame_pos = 0;
  77 }
  78
  79 static void serial_enable_rx(void)
  80 {
  81         UCSR0B &= ~(_BV(TXEN0) | _BV(TXCIE0));
  82         UCSR0B |= _BV(RXEN0) | _BV(RXCIE0);
  83 }
  84
  85 #define serial_rx_vect USART_RX_vect
  86
  87 #define serial_data UDR0
  88
  89 /*
  90  * USART receive interrupt
  91  *
  92  * In order to save the latency, we do only the necessary things here,
  93  * and postpone parsing the frame to tne non-irq time in the main
  94  * loop. Also, we expect the main loop to finish before the second
  95  * byte of the next frame is read, so we do not do atomic reads on top
  96  * of serial_frame[].
  97  */
  98 ISR(serial_rx_vect)
  99 {
 100         uint8_t val = serial_data;
 101
 102         // a shorthand - for now, we accept fixed-size packets only
 103         if (serial_frame_pos == 0 && val != IBUS_SERVO_FRAME_SIZE)
 104                 goto restart;
 105
 106         if (serial_frame_pos == 1
 107                 && serial_frame[0] == IBUS_SERVO_FRAME_SIZE
 108                 && val != IBUS_SERVO_FRAME_ID)
 109                 goto restart;
 110
 111         serial_frame[serial_frame_pos++] = val;
 112
 113         if (serial_frame_pos == serial_frame[0])
 114                 serial_frame_ready = 1;
 115 restart:
 116         serial_frame_pos = 0;
 117 }
 118
 119 /* ----------------- Servos ----------------- */
 120
 121 #define SERVO_MASK      (_BV(PD2)|_BV(PD3)|_BV(PD4)|_BV(PD5)|_BV(PD6)|_BV(PD7))
 122
 123 static const PROGMEM uint8_t servo_bits[] = {
 124         _BV(PD2),
 125         _BV(PD3),
 126         _BV(PD4),
 127         _BV(PD5),
 128         _BV(PD6),
 129         _BV(PD7),
 130 };
 131
 132 static void xxx_init(void)
 133 {
 134         DDRD |= _BV(PD2) | _BV(PD3) | _BV(PD4) | _BV(PD5);
 135         PORTD &= ~_BV(PD2);
 136 }
 137
 138 // run this inside interrupt or in atomic context
 139 static void interrupt_after(uint16_t delay)
 140 {
 141         uint16_t now = TCNT1;
 142         now += delay;
 143         OCR1A = now;
 144         TIMSK1 |= _BV(OCIE1A);
 145 }
 146
 147 ISR(TIMER1_COMPA_vect) {
 148         static uint16_t led = 2000;
 149
 150         led++;
 151         if (led & 1) {
 152                 led1_off();
 153                 PORTD &= ~_BV(PD2);
 154                 interrupt_after(65000);
 155         } else {
 156                 led1_on();
 157                 PORTD |= _BV(PD2);
 158                 interrupt_after(led);
 159         }
 160         if (led >= 4000)
 161                 led = 2000;
 162 }
 163
 164 /* ----------------- iBus ------------------ */
 165
 166 static void ibus_servo_frame(void)
 167 {
 168         uint16_t csum = 0xFFFF, servo;
 169         uint8_t i;
 170
 171         for (i = 0; i < serial_frame[0]-2; i++)
 172                 csum -= (uint16_t)serial_frame[i];
 173
 174         if ((serial_frame[serial_frame[0]-2] != (csum & 0xFF))
 175                 || (serial_frame[serial_frame[0]-1] != (csum >> 8))) { // invalid csum
 176                 return;
 177         }
 178
 179         servo = serial_frame[12];
 180         servo |= ((uint16_t)serial_frame[13] & 0x000F) << 8;
 181
 182         /*
 183          * -120 % == 0x384 ==  900
 184          * -100 % == 0x3e8 == 1000
 185          *    0 % == 0x5dc == 1500
 186          */
 187         if (servo < 0x0834)
 188                 led1_on();
 189         else
 190                 led1_off();
 191 }
 192
 193 int main(void)
 194 {
 195         led_init();
 196         timer_init();
 197         serial_init();
 198 #if 0
 199         ibus_init();
 200
 201 #endif
 202         serial_enable_rx();
 203         sei();
 204
 205         while (1) {
 206                 if (serial_frame_ready) {
 207                         serial_frame_ready = 0;
 208                         ibus_servo_frame();
 209                 }
 210         }
 211 }
 212