add a command to read/write servo

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

diff --git a/spi_servo.c b/spi_servo.c
index 6e1da05..c4fe318 100644 (file)
--- a/spi_servo.c
+++ b/spi_servo.c
@@ -27,8 +27,6 @@
 /* 1ms */
 #define SPI_EVT_PERIOD (10000UL/SCHEDULER_UNIT)
 
-#define N_SERVO 6
-
 #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;