X-Git-Url: http://git.droids-corp.org/?p=protos%2Fxbee-avr.git;a=blobdiff_plain;f=spi_servo.c;h=917769c6250260ab57091fb7464ec3aa53931a7a;hp=c4fe3187dcd02e3084e70b4ba50bbdae279c166c;hb=1032fc9a27fbe5b24f3a837685467d87380b3e2c;hpb=87d9d882059ebe9e34adc088e0d6f4bea984bc91

diff --git a/spi_servo.c b/spi_servo.c
index c4fe318..917769c 100644
--- 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,9 +24,6 @@
  * callback is unloaded.
  */
 
-/* 1ms */
-#define SPI_EVT_PERIOD (10000UL/SCHEDULER_UNIT)
-
 #define PPM_BIT    0x01
 #define BYPASS_BIT 0x02
 
@@ -124,7 +121,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;
@@ -147,13 +144,13 @@ 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 */
@@ -170,6 +167,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)
@@ -185,8 +187,9 @@ void spi_servo_init(void)
 
 	SS_HIGH();
 
-	scheduler_add_periodical_event_priority(&spi_servo_cb, NULL,
-					    SPI_EVT_PERIOD, SPI_PRIO);
+	callout_init(&xbeeboard.spi_timer, spi_servo_cb, NULL, SPI_PRIO);
+	callout_schedule(&xbeeboard.intr_cm,
+		&xbeeboard.spi_timer, 0); /* immediate */
 	spi_servo_set_bypass(1);
 }
 
@@ -255,3 +258,14 @@ void spi_servo_set_ppm(uint8_t enable)
 	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());
+}