void ar_enable_broadcast(int fd)
{
ar_select_motor(fd, 0);
}
int ar_init_motors(int ardrone_uart, int gpios)
{
/* Write ARDrone commands on UART2 */
uint8_t initbuf[] = {0xE0, 0x91, 0xA1, 0x00, 0x40};
uint8_t multicastbuf[] = {0xA0, 0xA0, 0xA0, 0xA0, 0xA0, 0xA0};
/* deselect all motors */
ar_deselect_motor(gpios, 0);
/* initialize all motors
* - select one motor at a time
* - configure motor
*/
int i;
int errcounter = 0;
/* initial setup run */
for (i = 1; i < 5; ++i) {
/* Initialize motors 1-4 */
errcounter += ar_select_motor(gpios, i);
usleep(200);
/*
* write 0xE0 - request status
* receive one status byte
*/
write(ardrone_uart, &(initbuf[0]), 1);
fsync(ardrone_uart);
usleep(UART_TRANSFER_TIME_BYTE_US*1);
/*
* write 0x91 - request checksum
* receive 120 status bytes
*/
write(ardrone_uart, &(initbuf[1]), 1);
fsync(ardrone_uart);
usleep(UART_TRANSFER_TIME_BYTE_US*120);
/*
* write 0xA1 - set status OK
* receive one status byte - should be A0
* to confirm status is OK
*/
write(ardrone_uart, &(initbuf[2]), 1);
fsync(ardrone_uart);
usleep(UART_TRANSFER_TIME_BYTE_US*1);
/*
* set as motor i, where i = 1..4
* receive nothing
*/
initbuf[3] = (uint8_t)i;
write(ardrone_uart, &(initbuf[3]), 1);
fsync(ardrone_uart);
/*
* write 0x40 - check version
* receive 11 bytes encoding the version
*/
write(ardrone_uart, &(initbuf[4]), 1);
fsync(ardrone_uart);
usleep(UART_TRANSFER_TIME_BYTE_US*11);
ar_deselect_motor(gpios, i);
/* sleep 200 ms */
usleep(200000);
}
/* start the multicast part */
errcounter += ar_select_motor(gpios, 0);
usleep(200);
/*
* first round
* write six times A0 - enable broadcast
* receive nothing
*/
write(ardrone_uart, multicastbuf, sizeof(multicastbuf));
fsync(ardrone_uart);
usleep(UART_TRANSFER_TIME_BYTE_US * sizeof(multicastbuf));
/*
* second round
* write six times A0 - enable broadcast
* receive nothing
*/
write(ardrone_uart, multicastbuf, sizeof(multicastbuf));
fsync(ardrone_uart);
usleep(UART_TRANSFER_TIME_BYTE_US * sizeof(multicastbuf));
/* set motors to zero speed (fsync is part of the write command */
ardrone_write_motor_commands(ardrone_uart, 0, 0, 0, 0);
if (errcounter != 0) {
fprintf(stderr, "[ardrone_interface] init sequence incomplete, failed %d times", -errcounter);
fflush(stdout);
}
return errcounter;
}
void ar_init_motors(int ardrone_uart, int *gpios_pin)
{
/* Initialize multiplexing */
*gpios_pin = ar_multiplexing_init();
/* Write ARDrone commands on UART2 */
uint8_t initbuf[] = {0xE0, 0x91, 0xA1, 0x00, 0x40};
uint8_t multicastbuf[] = {0xA0, 0xA0, 0xA0, 0xA0, 0xA0, 0xA0};
/* initialize all motors
* - select one motor at a time
* - configure motor
*/
int i;
int errcounter = 0;
for (i = 1; i < 5; ++i) {
/* Initialize motors 1-4 */
initbuf[3] = i;
errcounter += ar_select_motor(*gpios_pin, i);
write(ardrone_uart, initbuf + 0, 1);
/* sleep 400 ms */
usleep(200000);
usleep(200000);
write(ardrone_uart, initbuf + 1, 1);
/* wait 50 ms */
usleep(50000);
write(ardrone_uart, initbuf + 2, 1);
/* wait 50 ms */
usleep(50000);
write(ardrone_uart, initbuf + 3, 1);
/* wait 50 ms */
usleep(50000);
write(ardrone_uart, initbuf + 4, 1);
/* wait 50 ms */
usleep(50000);
/* enable multicast */
write(ardrone_uart, multicastbuf + 0, 1);
/* wait 1 ms */
usleep(1000);
write(ardrone_uart, multicastbuf + 1, 1);
/* wait 1 ms */
usleep(1000);
write(ardrone_uart, multicastbuf + 2, 1);
/* wait 1 ms */
usleep(1000);
write(ardrone_uart, multicastbuf + 3, 1);
/* wait 1 ms */
usleep(1000);
write(ardrone_uart, multicastbuf + 4, 1);
/* wait 1 ms */
usleep(1000);
write(ardrone_uart, multicastbuf + 5, 1);
/* wait 5 ms */
usleep(50000);
}
/* start the multicast part */
errcounter += ar_select_motor(*gpios_pin, 0);
if (errcounter != 0) {
printf("AR: init sequence incomplete, failed %d times", -errcounter);
fflush(stdout);
}
}
