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_IBUS_CHANNELS         18
  11 #define IBUS_SERVO_FRAME_SIZE   32
  12 #define IBUS_SERVO_FRAME_ID     0x40    // first byte after the length
  13 #define N_PWM_CHANNELS          6
  14
  15 /* ---------- LEDs for debugging ---------- */
  16
  17 void led_init(void)
  18 {
  19         PORTB &= ~_BV(PB5);
  20         DDRB |= _BV(PB5);
  21 }
  22
  23 void led1_off(void)
  24 {
  25         PORTB &= ~_BV(PB5);
  26 }
  27
  28 void led1_on(void)
  29 {
  30         PORTB |= _BV(PB5);
  31 }
  32
  33 /* ----------------- Timer ----------------- */
  34
  35 typedef uint16_t time_t;
  36 #define TICKS_IN_US     (F_CPU/8000000UL)
  37
  38 static void timer_init(void)
  39 {
  40         TCCR1A = 0; // no PWM or WGM output
  41         TCCR1B = _BV(CS11); // clk/8
  42
  43         DDRD |= _BV(PD2) | _BV(PD3) | _BV(PD4) | _BV(PD5);
  44         PORTD &= ~_BV(PD2);
  45 }
  46
  47 static time_t inline get_time(void)
  48 {
  49         time_t rv;
  50
  51         ATOMIC_BLOCK(ATOMIC_FORCEON) {
  52                 rv = TCNT1;
  53         };
  54
  55         return rv;
  56 }
  57
  58
  59 /* ----------------- USART ----------------- */
  60
  61 // I-Bus uses 115200n8
  62 #define UART_BAUD       115200
  63 #define UBRR_VAL        ((F_CPU + 8UL * UART_BAUD) / (16UL*UART_BAUD) - 1)
  64
  65 volatile uint8_t serial_frame[IBUS_SERVO_FRAME_SIZE];
  66 volatile uint8_t serial_frame_ready;
  67
  68 static volatile uint8_t serial_frame_pos;
  69
  70 void serial_init(void)
  71 {
  72         UBRR0 = UBRR_VAL;
  73
  74         UCSR0A = 0;
  75         UCSR0B = _BV(RXEN0) | _BV(RXCIE0);
  76         UCSR0C = _BV(UCSZ01)|_BV(UCSZ00);
  77
  78         serial_frame_ready = serial_frame_pos = 0;
  79 }
  80
  81 static void serial_enable_rx(void)
  82 {
  83         UCSR0B &= ~(_BV(TXEN0) | _BV(TXCIE0));
  84         UCSR0B |= _BV(RXEN0) | _BV(RXCIE0);
  85 }
  86
  87 #define serial_rx_vect USART_RX_vect
  88
  89 #define serial_data UDR0
  90
  91 /*
  92  * USART receive interrupt
  93  *
  94  * In order to save the latency, we do only the necessary things here,
  95  * and postpone parsing the frame to tne non-irq time in the main
  96  * loop. Also, we expect the main loop to finish before the second
  97  * byte of the next frame is read, so we do not do atomic reads on top
  98  * of serial_frame[].
  99  */
 100 ISR(serial_rx_vect)
 101 {
 102         uint8_t val = serial_data;
 103
 104         // a shorthand - for now, we accept fixed-size packets only
 105         if (serial_frame_pos == 0 && val != IBUS_SERVO_FRAME_SIZE)
 106                 goto restart;
 107
 108         if (serial_frame_pos == 1
 109                 && serial_frame[0] == IBUS_SERVO_FRAME_SIZE
 110                 && val != IBUS_SERVO_FRAME_ID)
 111                 goto restart;
 112
 113         serial_frame[serial_frame_pos++] = val;
 114
 115         if (serial_frame_pos == serial_frame[0])
 116                 serial_frame_ready = 1;
 117 restart:
 118         serial_frame_pos = 0;
 119 }
 120
 121 /* ----------------- iBus ------------------ */
 122
 123 typedef int16_t servo_val_t;
 124 servo_val_t ibus_channel[N_IBUS_CHANNELS];
 125 #define N_IBUS_CHANNELS_DIRECT 14
 126 /*
 127  * channel value      ibus frame value
 128  *     -120 %               900
 129  *     -100 %              1000
 130  *        0 %              1500
 131  *      100 %              2000
 132  *      120 %              2100
 133  */
 134 #define IBUS_SERVO_CENTER       1500
 135
 136 static void ibus_servo_frame(void)
 137 {
 138         uint16_t csum = 0xFFFF;
 139         uint8_t i;
 140
 141         for (i = 0; i < serial_frame[0]-2; i++)
 142                 csum -= (uint16_t)serial_frame[i];
 143
 144         if ((serial_frame[serial_frame[0]-2] != (csum & 0xFF))
 145                 || (serial_frame[serial_frame[0]-1] != (csum >> 8))) {
 146                 // invalid csum
 147                 return;
 148         }
 149
 150         for (i = 0; i < N_IBUS_CHANNELS_DIRECT; i++)
 151                 ibus_channel[i] = (uint16_t)serial_frame[2 + 2*i]
 152                         + (((uint16_t)serial_frame[3 + 2*i] & 0x000F) << 8)
 153                         - IBUS_SERVO_CENTER;
 154         // TODO channels 15-18
 155 }
 156
 157 /* -------------- PWM output --------------- */
 158
 159 #define SERVO_PWM_CENTER        (1500 * TICKS_IN_US)
 160
 161 time_t pwm_channels[N_PWM_CHANNELS];
 162 volatile uint8_t pwm_busy;
 163 uint8_t pwm_data_ready;
 164
 165 static uint8_t pwm_channel;
 166 static time_t pwm_frame_start;
 167
 168 // TODO: move this into PROGMEM?
 169 static const uint8_t pwm_channel_bit[] = {
 170         _BV(PD2),
 171         _BV(PD3),
 172         _BV(PD4),
 173         _BV(PD5),
 174         _BV(PD6),
 175         _BV(PD7),
 176 };
 177
 178 #define PWM_CH_MASK  (_BV(PD2)|_BV(PD3)|_BV(PD4)|_BV(PD5)|_BV(PD6)|_BV(PD7))
 179 #define PWM_FRAME_LEN   (20000 * TICKS_IN_US)
 180
 181 static void pwm_init(void)
 182 {
 183         uint8_t i;
 184
 185         for (i = 0; i < N_PWM_CHANNELS; i++)
 186                 pwm_channels[i] = 0;
 187
 188         pwm_channel = N_PWM_CHANNELS + 1;
 189         pwm_data_ready = pwm_busy = 0;
 190
 191         TIMSK1 &= ~_BV(OCIE1A);
 192         PORTD &= ~PWM_CH_MASK;
 193         DDRD |= PWM_CH_MASK;
 194 }
 195
 196 static void pwm_set(uint8_t channel, servo_val_t value)
 197 {
 198         time_t tm = SERVO_PWM_CENTER + (value * TICKS_IN_US);
 199
 200         pwm_channels[channel] = tm;
 201 }
 202
 203 static void pwm_send_pulse()
 204 {
 205         uint8_t first_pulse = 0;
 206
 207         if (pwm_channel == N_PWM_CHANNELS + 1) {
 208                 if (!pwm_data_ready)
 209                         return;
 210                 pwm_channel = 0;
 211         }
 212
 213         if (pwm_channel == 0) {
 214                 first_pulse = 1;
 215                 pwm_data_ready = 0;
 216         }
 217
 218         // find a non-empty channel
 219         while (!pwm_channels[pwm_channel] && pwm_channel < N_PWM_CHANNELS)
 220                 pwm_channel++;
 221
 222         ATOMIC_BLOCK(ATOMIC_RESTORESTATE) {
 223                 time_t now = TCNT1;
 224                 if (pwm_channel < N_PWM_CHANNELS) {
 225                         PORTD |= pwm_channel_bit[pwm_channel];
 226                         OCR1A = now + pwm_channels[pwm_channel];
 227                         if (first_pulse)
 228                                 pwm_frame_start = now;
 229                 } else {
 230                         OCR1A = pwm_frame_start + PWM_FRAME_LEN;
 231                 }
 232
 233                 TIMSK1 |= _BV(OCIE1A);
 234                 pwm_busy = 1;
 235         };
 236 }
 237
 238 ISR(TIMER1_COMPA_vect) {
 239         PORTD &= ~PWM_CH_MASK;
 240         TIMSK1 &= ~_BV(OCIE1A);
 241         pwm_channel++;
 242         pwm_busy = 0;
 243 }
 244
 245
 246 /* -------- Custom mixing done here -------- */
 247
 248 static void do_mixes()
 249 {
 250         int i;
 251
 252         for (i = 0; i < N_PWM_CHANNELS; i++) {
 253                 pwm_set(i, ibus_channel[i]);
 254         }
 255 }
 256
 257 int main(void)
 258 {
 259         led_init();
 260         timer_init();
 261         serial_init();
 262         pwm_init();
 263
 264         serial_enable_rx();
 265         sei();
 266
 267         while (1) {
 268                 if (serial_frame_ready) {
 269                         serial_frame_ready = 0;
 270                         ibus_servo_frame();
 271                         do_mixes();
 272                         pwm_data_ready = 1;
 273                 }
 274                 if (!pwm_busy) {
 275                         pwm_send_pulse();
 276                 }
 277         }
 278 }
 279