X-Git-Url: http://git.droids-corp.org/?p=protos%2Fxbee-avr.git;a=blobdiff_plain;f=spi_servo.c;h=c180bc907c9933e954df5bcbb0b136c2d6398b12;hp=6e1da056a731aba4ea0024715825263bb9ae4ea6;hb=f74db5fd03bcd38af3ca533e7531dd70454dfaaf;hpb=cc6ac756becd992b51ddc7127846ff0fc3d4c662

diff --git a/spi_servo.c b/spi_servo.c
index 6e1da05..c180bc9 100644
--- a/spi_servo.c
+++ b/spi_servo.c
@@ -24,10 +24,8 @@
  * callback is unloaded.
  */
 
-/* 1ms */
-#define SPI_EVT_PERIOD (10000UL/SCHEDULER_UNIT)
-
-#define N_SERVO 6
+/* 1 scheduler unit (682us at 12Mhz) */
+#define SPI_EVT_PERIOD (1)
 
 #define PPM_BIT    0x01
 #define BYPASS_BIT 0x02
@@ -189,7 +187,7 @@ void spi_servo_init(void)
 
 	scheduler_add_periodical_event_priority(&spi_servo_cb, NULL,
 					    SPI_EVT_PERIOD, SPI_PRIO);
-	spi_servo_bypass(1);
+	spi_servo_set_bypass(1);
 }
 
 void spi_servo_set(uint8_t num, uint16_t val)
@@ -220,7 +218,17 @@ 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;
 
@@ -234,7 +242,7 @@ void spi_servo_bypass(uint8_t enable)
 	IRQ_UNLOCK(flags);
 }
 
-void spi_servo_ppm(uint8_t enable)
+void spi_servo_set_ppm(uint8_t enable)
 {
 	uint8_t flags;
 
@@ -247,3 +255,14 @@ void spi_servo_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());
+}