From addaecaf0677b0e6764f8b26b0230897345447a5 Mon Sep 17 00:00:00 2001 From: Olivier Matz Date: Tue, 15 Oct 2013 21:33:03 +0200 Subject: [PATCH] add PPM generation support and new spi protocol --- main.c | 105 +++++++++++++++++++++++++++++++++++++++------------------ 1 file changed, 72 insertions(+), 33 deletions(-) diff --git a/main.c b/main.c index 74f6629..81ac65b 100644 --- a/main.c +++ b/main.c @@ -32,21 +32,25 @@ static struct servo servo_table[] = { .command = 512, }, }; -#define NB_SERVO (sizeof(servo_table)/sizeof(*servo_table)) +#define N_SERVO (sizeof(servo_table)/sizeof(*servo_table)) + +/* we use the first servo for PPM output if enabled */ +#define PPM (0) static uint8_t bypass; +static uint8_t ppm_enabled; static volatile uint8_t done; static uint8_t portval; static uint8_t rxidx; -static uint8_t icp_idx = NB_SERVO; -static uint16_t icp_servos[NB_SERVO]; +static uint8_t icp_idx = N_SERVO; +static uint16_t icp_servos[N_SERVO]; static uint16_t icp_prev; static uint8_t spi_out_idx; /* current byte beeing sent */ -#define BYPASS_ENABLE 14 -#define BYPASS_DISABLE 15 +#define PPM_BIT 0x01 +#define BYPASS_BIT 0x02 #define LED_ON() do { PORTB |= 0x02; } while(0) #define LED_OFF() do { PORTB &= ~0x02; } while(0) @@ -60,9 +64,10 @@ static uint8_t spi_out_idx; /* current byte beeing sent */ * servo value. The second byte contains the lsb of the servo value. * * Command 0 is only one byte long, it means "I have nothing to say". - * Commands 1 to NB_SERVO+1 are to set the value of servo. - * Command 14 is to enable bypass mode. - * Command 15 is to disable bypass mode. + * Commands 1 to N_SERVO (included) are to set the value of servo. + * Command N_SERVO+1 is: + * - to enable/disable ppm generation in place of first servo. + * - to enable/disable bypass mode */ union byte0 { uint8_t u8; @@ -120,7 +125,7 @@ static void poll_spi(void) SPDR = byte1_tx.u8; } spi_out_idx ++; - if (spi_out_idx >= NB_SERVO * 2) + if (spi_out_idx >= N_SERVO * 2) spi_out_idx = 0; /* RX */ @@ -148,16 +153,21 @@ static void poll_spi(void) /* process command */ - if (byte0_rx.cmd_num < NB_SERVO+1) { - val = (uint16_t)byte0_rx.val_msb << 7; - val += byte1_rx.val_lsb; + val = (uint16_t)byte0_rx.val_msb << 7; + val += byte1_rx.val_lsb; + + if (byte0_rx.cmd_num < N_SERVO+1) { servo_table[byte0_rx.cmd_num-1].command = val; } - else if (byte0_rx.cmd_num == BYPASS_ENABLE) { - bypass = 1; - } - else if (byte0_rx.cmd_num == BYPASS_DISABLE) { - bypass = 0; + else if (byte0_rx.cmd_num == N_SERVO+1) { + if (val & PPM_BIT) + ppm_enabled = 1; + else + ppm_enabled = 0; + if (val & BYPASS_BIT) + bypass = 1; + else + bypass = 0; } } @@ -190,7 +200,7 @@ static void poll_input_capture(void) } /* get the value for the servo */ - if (icp_idx < NB_SERVO) { + if (icp_idx < N_SERVO) { if (diff < 1000) icp_servos[icp_idx] = 0; else if (diff > 2023) @@ -213,31 +223,62 @@ static void load_timer_at(uint16_t t) TIMSK1 |= _BV(OCIE1A); } -static void do_one_servo(struct servo *s) +static void do_servos(void) { - uint16_t t; + uint8_t i; + uint16_t t, start; + + /* skip first servo if ppm is enabled */ + if (ppm_enabled) + i = 1; + else + i = 0; - /* set bit */ - done = 0; - //portval = PORTC | (1 << s->bit); - portval = (1 << s->bit); t = TCNT1; - load_timer_at(t + 20); + start = t + 100; + + for (; i < N_SERVO; i++) { + + /* set servo and PPM bit */ + portval = 1 << servo_table[i].bit; + if (ppm_enabled) + portval |= (1 << servo_table[PPM].bit); + + done = 0; + load_timer_at(start); + while (done == 0) + poll(); + + /* reset PPM bit after 300us */ + portval = 1 << servo_table[i].bit; + done = 0; + load_timer_at(start + 300); + while (done == 0) + poll(); + + start = start + 1000 + servo_table[i].command; + } + + /* set PPM bit only for last servo */ + portval = 0; + if (ppm_enabled) + portval |= (1 << servo_table[PPM].bit); + + done = 0; + load_timer_at(start); while (done == 0) poll(); - /* reset bit */ - done = 0; + /* reset PPM bit after 300us */ portval = 0; - //portval = PORTC & (~(1 << s->bit)); - load_timer_at(t + 20 + 1000 + s->command); + done = 0; + load_timer_at(start + 300); while (done == 0) poll(); } int main(void) { - uint8_t i; uint8_t t, diff; uint8_t tmp; uint8_t cnt = 10; @@ -280,9 +321,7 @@ int main(void) bypass = 0; while (1) { t = TCNT0; - for (i = 0; i < NB_SERVO; i++) { - do_one_servo(&servo_table[i]); - } + do_servos(); /* wait 20 ms */ while (1) { diff = TCNT0 - t; -- 2.20.1