* 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
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)
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;
IRQ_UNLOCK(flags);
}
-void spi_servo_ppm(uint8_t enable)
+void spi_servo_set_ppm(uint8_t enable)
{
uint8_t flags;
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());
+}