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); } }