beep when GPS ready

[protos/xbee-avr.git] / spi_servo.c

diff --git a/spi_servo.c b/spi_servo.c
index 69afed2..7a85070 100644 (file)
--- a/spi_servo.c
+++ b/spi_servo.c
@@ -4,7 +4,7 @@
 #include <aversive.h>
 #include <aversive/wait.h>
 
-#include <scheduler.h>
+#include <callout.h>
 
 #include "spi_servo.h"
 #include "main.h"
@@ -24,14 +24,11 @@
  * callback is unloaded.
  */
 
-/* 1ms */
-#define SPI_EVT_PERIOD (10000UL/SCHEDULER_UNIT)
+#define PPM_BIT    0x01
+#define BYPASS_BIT 0x02
 
-#define N_SERVO 6
-#define BYPASS_ENABLE 14
-#define BYPASS_DISABLE 15
 struct spi_servo_tx {
-       uint16_t servo[N_SERVO];
+       uint16_t servo[N_SERVO+1]; /* one more for control channel */
        uint16_t cmd_mask;
        uint8_t next_byte; /* next byte to send, 0 if nothing in pipe */
        uint8_t cur_idx;
@@ -44,6 +41,8 @@ struct spi_servo_rx {
 };
 static struct spi_servo_rx spi_servo_rx;
 
+static struct callout spi_timer;
+
 /*
  * SPI protocol:
  *
@@ -51,9 +50,11 @@ static struct spi_servo_rx spi_servo_rx;
  * second one to 1. The first received byte contains the command number, and the
  * msb of the servo value. The second byte contains the lsb of the servo value.
  *
- * Commands 0 to NB_SERVO are to set the value of servo.
- * Command 14 is to enable bypass mode.
- * Command 15 is to disable bypass mode.
+ * Command 0 is only one byte long, it means "I have nothing to say".
+ * 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 last servo.
+ * - to enable/disable bypass mode
  */
 union spi_byte0 {
        uint8_t u8;
@@ -93,10 +94,7 @@ static void spi_send_one_servo(uint8_t num, uint16_t val)
        union spi_byte1 byte1;
 
        byte0.val_msb = val >> 7;
-       if (num < N_SERVO)
-               byte0.cmd_num = num + 1;
-       else
-               byte0.cmd_num = num;
+       byte0.cmd_num = num + 1;
        byte0.zero = 0;
        byte1.one = 1;
        byte1.val_lsb = val;
@@ -125,7 +123,7 @@ static void decode_rx_servo(union spi_byte0 byte0, union spi_byte1 byte1)
 }
 
 /* called by the scheduler */
-static void spi_servo_cb(void *arg)
+static void spi_servo_cb(struct callout_mgr *cm, struct callout *tim, void *arg)
 {
        uint8_t idx;
        union spi_byte0 byte0;
@@ -148,22 +146,20 @@ static void spi_servo_cb(void *arg)
        if (spi_servo_tx.next_byte != 0) {
                spi_send_byte(spi_servo_tx.next_byte);
                spi_servo_tx.next_byte = 0;
-               return;
+               goto reschedule;
        }
 
        /* if there is no updated servo, send 0 and return. */
        if (spi_servo_tx.cmd_mask == 0) {
                spi_send_byte(0);
-               return;
+               goto reschedule;
        }
 
        /* else find it and send it */
        idx = spi_servo_tx.cur_idx;
        while (1) {
                idx++;
-               if (idx == N_SERVO)
-                       idx = BYPASS_ENABLE;
-               else if (idx >= sizeof(uint16_t) * 8)
+               if (idx > N_SERVO + 1)
                        idx = 0;
 
                if (spi_servo_tx.cmd_mask & (1 << (uint16_t)idx))
@@ -173,6 +169,11 @@ static void spi_servo_cb(void *arg)
        spi_send_one_servo(idx, spi_servo_tx.servo[idx]);
        spi_servo_tx.cmd_mask &= (~(1 << idx));
        spi_servo_tx.cur_idx = idx;
+
+ reschedule:
+       /* don't use callout_reschedule() here, we want to schedule in one tick
+        * relative to current time: 1 tick is 682us at 12Mhz */
+       callout_schedule(cm, tim, 0);
 }
 
 void spi_servo_init(void)
@@ -183,14 +184,14 @@ void spi_servo_init(void)
        /* remove power reduction on spi */
        PRR0 &= ~(1 << PRSPI);
 
-       /* Enable SPI, Master, set clock rate fck/64 */
-       SPCR = (1<<SPE)|(1<<MSTR)|(1<<SPR1);
+       /* Enable SPI, Master, set clock rate fck/16 */
+       SPCR = (1<<SPE)|(1<<MSTR)|(1<<SPR0);
 
        SS_HIGH();
 
-       scheduler_add_periodical_event_priority(&spi_servo_cb, NULL,
-                                           SPI_EVT_PERIOD, SPI_PRIO);
-       spi_servo_set(BYPASS_DISABLE, 0);
+       callout_init(&spi_timer, spi_servo_cb, NULL, SPI_PRIO);
+       callout_schedule(&xbeeboard.intr_cm, &spi_timer, 0); /* immediate */
+       spi_servo_set_bypass(1);
 }
 
 void spi_servo_set(uint8_t num, uint16_t val)
@@ -221,18 +222,51 @@ uint16_t spi_servo_get(uint8_t num)
        return val;
 }
 
-void spi_servo_bypass(uint8_t enable)
+uint8_t spi_servo_get_bypass(void)
+{
+       return !!(spi_servo_tx.servo[N_SERVO] & BYPASS_BIT);
+}
+
+uint8_t spi_servo_get_ppm(void)
+{
+       return !!(spi_servo_tx.servo[N_SERVO] & PPM_BIT);
+}
+
+void spi_servo_set_bypass(uint8_t enable)
 {
        uint8_t flags;
 
-       if (enable) {
-               IRQ_LOCK(flags);
-               spi_servo_tx.cmd_mask |= (1 << BYPASS_ENABLE);
-               IRQ_UNLOCK(flags);
-       }
-       else {
-               IRQ_LOCK(flags);
-               spi_servo_tx.cmd_mask |= (1 << BYPASS_DISABLE);
-               IRQ_UNLOCK(flags);
-       }
+       IRQ_LOCK(flags);
+       spi_servo_tx.cmd_mask |= (1 << N_SERVO);
+       if (enable)
+               spi_servo_tx.servo[N_SERVO] |= BYPASS_BIT;
+       else
+               spi_servo_tx.servo[N_SERVO] &= (~BYPASS_BIT);
+       spi_servo_tx.cmd_mask |= (1 << N_SERVO);
+       IRQ_UNLOCK(flags);
+}
+
+void spi_servo_set_ppm(uint8_t enable)
+{
+       uint8_t flags;
+
+       IRQ_LOCK(flags);
+       spi_servo_tx.cmd_mask |= (1 << N_SERVO);
+       if (enable)
+               spi_servo_tx.servo[N_SERVO] |= PPM_BIT;
+       else
+               spi_servo_tx.servo[N_SERVO] &= (~PPM_BIT);
+       spi_servo_tx.cmd_mask |= (1 << N_SERVO);
+       IRQ_UNLOCK(flags);
+}
+
+void spi_servo_dump(void)
+{
+       uint8_t i;
+
+       for (i = 0; i < N_SERVO; i++)
+               printf_P(PSTR("%d: rx=%4.4d tx=%4.4d\r\n"), i,
+                        spi_servo_get(i), spi_servo_tx.servo[i]);
+       printf_P(PSTR("bypass=%d ppm=%d\n"),
+                spi_servo_get_bypass(), spi_servo_get_ppm());
 }