(void)data;
spi_servo_bypass(0);
+ spi_servo_ppm(0);
/* stress test: send many commands, no wait between each servo
* of a series, and a variable delay between series */
printf_P(PSTR("\r\n"));
}
+ printf_P(PSTR("PPM to servo\r\n"));
spi_servo_bypass(0);
+ spi_servo_ppm(0);
- printf_P(PSTR("PPM to servo\r\n"));
+ /* test PPM to servo (bypass) mode */
+ while (!cmdline_keypressed()) {
+ for (i = 0; i < 6; i++) {
+ val = spi_servo_get(i);
+ spi_servo_set(i, val);
+ }
+ }
+
+ printf_P(PSTR("PPM to (servo + PPM)\r\n"));
+ spi_servo_bypass(0);
+ spi_servo_ppm(1);
/* test PPM to servo (bypass) mode */
while (!cmdline_keypressed()) {
#define SPI_EVT_PERIOD (10000UL/SCHEDULER_UNIT)
#define N_SERVO 6
-#define BYPASS_ENABLE 14
-#define BYPASS_DISABLE 15
+
+#define PPM_BIT 0x01
+#define BYPASS_BIT 0x02
+
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;
* 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;
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;
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))
scheduler_add_periodical_event_priority(&spi_servo_cb, NULL,
SPI_EVT_PERIOD, SPI_PRIO);
- spi_servo_set(BYPASS_DISABLE, 0);
+ spi_servo_bypass(1);
}
void spi_servo_set(uint8_t num, uint16_t val)
{
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_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);
}