int ardrone_control_main(int argc, char *argv[])
{
/* welcome user */
printf("[ardrone_control] Control started, taking over motors\n");
/* default values for arguments */
char *ardrone_uart_name = "/dev/ttyS1";
control_mode = CONTROL_MODE_RATES;
char *commandline_usage = "\tusage: ardrone_control -d ardrone-devicename -m mode\n\tmodes are:\n\t\trates\n\t\tattitude\n";
/* read commandline arguments */
int i;
for (i = 1; i < argc; i++) {
if (strcmp(argv[i], "-d") == 0 || strcmp(argv[i], "--device") == 0) { //ardrone set
if (argc > i + 1) {
ardrone_uart_name = argv[i + 1];
} else {
printf(commandline_usage);
return 0;
}
} else if (strcmp(argv[i], "-m") == 0 || strcmp(argv[i], "--mode") == 0) {
if (argc > i + 1) {
if (strcmp(argv[i + 1], "rates") == 0) {
control_mode = CONTROL_MODE_RATES;
} else if (strcmp(argv[i + 1], "attitude") == 0) {
control_mode = CONTROL_MODE_ATTITUDE;
} else {
printf(commandline_usage);
return 0;
}
} else {
printf(commandline_usage);
return 0;
}
}
}
/* open uarts */
printf("[ardrone_control] AR.Drone UART is %s\n", ardrone_uart_name);
ardrone_write = open(ardrone_uart_name, O_RDWR | O_NOCTTY | O_NDELAY);
/* initialize motors */
ar_init_motors(ardrone_write, &gpios);
int counter = 0;
/* pthread for position control */
pthread_t position_control_thread;
position_control_thread_started = false;
/* structures */
struct vehicle_status_s state;
struct vehicle_attitude_s att;
struct ardrone_control_s ar_control;
struct rc_channels_s rc;
struct sensor_combined_s raw;
struct ardrone_motors_setpoint_s setpoint;
/* subscribe to attitude, motor setpoints and system state */
int att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
int state_sub = orb_subscribe(ORB_ID(vehicle_status));
int rc_sub = orb_subscribe(ORB_ID(rc_channels));
int sensor_sub = orb_subscribe(ORB_ID(sensor_combined));
int setpoint_sub = orb_subscribe(ORB_ID(ardrone_motors_setpoint));
/* publish AR.Drone motor control state */
int ardrone_pub = orb_advertise(ORB_ID(ardrone_control), &ar_control);
while (1) {
/* get a local copy of the vehicle state */
orb_copy(ORB_ID(vehicle_status), state_sub, &state);
/* get a local copy of rc */
orb_copy(ORB_ID(rc_channels), rc_sub, &rc);
/* get a local copy of attitude */
orb_copy(ORB_ID(vehicle_attitude), att_sub, &att);
if (state.state_machine == SYSTEM_STATE_AUTO) {
if (false == position_control_thread_started) {
pthread_attr_t position_control_thread_attr;
pthread_attr_init(&position_control_thread_attr);
pthread_attr_setstacksize(&position_control_thread_attr, 2048);
pthread_create(&position_control_thread, &position_control_thread_attr, position_control_loop, &state);
position_control_thread_started = true;
}
control_attitude(&rc, &att, &state, ardrone_pub, &ar_control);
//No check for remote sticks to disarm in auto mode, land/disarm with ground station
} else if (state.state_machine == SYSTEM_STATE_MANUAL) {
if (control_mode == CONTROL_MODE_RATES) {
orb_copy(ORB_ID(sensor_combined), sensor_sub, &raw);
orb_copy(ORB_ID(ardrone_motors_setpoint), setpoint_sub, &setpoint);
control_rates(&raw, &setpoint);
} else if (control_mode == CONTROL_MODE_ATTITUDE) {
control_attitude(&rc, &att, &state, ardrone_pub, &ar_control);
}
} else {
}
//fancy led animation...
static int blubb = 0;
if (counter % 20 == 0) {
if (blubb == 0) ar_set_leds(ardrone_write, 0, 1, 0, 0, 0, 0, 0 , 0);
if (blubb == 1) ar_set_leds(ardrone_write, 1, 1, 0, 0, 0, 0, 0 , 0);
if (blubb == 2) ar_set_leds(ardrone_write, 1, 0, 0, 0, 0, 0, 0 , 0);
if (blubb == 3) ar_set_leds(ardrone_write, 0, 0, 0, 1, 0, 0, 0 , 0);
if (blubb == 4) ar_set_leds(ardrone_write, 0, 0, 1, 1, 0, 0, 0 , 0);
if (blubb == 5) ar_set_leds(ardrone_write, 0, 0, 1, 0, 0, 0, 0 , 0);
if (blubb == 6) ar_set_leds(ardrone_write, 0, 0, 0, 0, 0, 1, 0 , 0);
if (blubb == 7) ar_set_leds(ardrone_write, 0, 0, 0, 0, 1, 1, 0 , 0);
if (blubb == 8) ar_set_leds(ardrone_write, 0, 0, 0, 0, 1, 0, 0 , 0);
if (blubb == 9) ar_set_leds(ardrone_write, 0, 0, 0, 0, 0, 0, 0 , 1);
if (blubb == 10) ar_set_leds(ardrone_write, 0, 0, 0, 0, 0, 0, 1 , 1);
if (blubb == 11) ar_set_leds(ardrone_write, 0, 0, 0, 0, 0, 0, 1 , 0);
blubb++;
if (blubb == 12) blubb = 0;
}
/* run at approximately 200 Hz */
usleep(5000);
counter++;
}
/* close uarts */
close(ardrone_write);
ar_multiplexing_deinit(gpios);
printf("[ardrone_control] ending now...\r\n");
fflush(stdout);
return 0;
}
int ardrone_interface_thread_main(int argc, char *argv[])
{
thread_running = true;
char *device = "/dev/ttyS1";
/* welcome user */
printf("[ardrone_interface] Control started, taking over motors\n");
/* File descriptors */
int gpios;
char *commandline_usage = "\tusage: ardrone_interface start|status|stop [-t for motor test (10%% thrust)]\n";
bool motor_test_mode = false;
int test_motor = -1;
/* read commandline arguments */
for (int i = 0; i < argc && argv[i]; i++) {
if (strcmp(argv[i], "-t") == 0 || strcmp(argv[i], "--test") == 0) {
motor_test_mode = true;
}
if (strcmp(argv[i], "-m") == 0 || strcmp(argv[i], "--motor") == 0) {
if (i+1 < argc) {
int motor = atoi(argv[i+1]);
if (motor > 0 && motor < 5) {
test_motor = motor;
} else {
thread_running = false;
errx(1, "supply a motor # between 1 and 4. Example: -m 1\n %s", commandline_usage);
}
} else {
thread_running = false;
errx(1, "missing parameter to -m 1..4\n %s", commandline_usage);
}
}
if (strcmp(argv[i], "-d") == 0 || strcmp(argv[i], "--device") == 0) { //device set
if (argc > i + 1) {
device = argv[i + 1];
} else {
thread_running = false;
errx(1, "missing parameter to -m 1..4\n %s", commandline_usage);
}
}
}
struct termios uart_config_original;
if (motor_test_mode) {
printf("[ardrone_interface] Motor test mode enabled, setting 10 %% thrust.\n");
}
/* Led animation */
int counter = 0;
int led_counter = 0;
/* declare and safely initialize all structs */
struct vehicle_status_s state;
memset(&state, 0, sizeof(state));
struct actuator_controls_s actuator_controls;
memset(&actuator_controls, 0, sizeof(actuator_controls));
struct actuator_armed_s armed;
armed.armed = false;
/* subscribe to attitude, motor setpoints and system state */
int actuator_controls_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS);
int state_sub = orb_subscribe(ORB_ID(vehicle_status));
int armed_sub = orb_subscribe(ORB_ID(actuator_armed));
printf("[ardrone_interface] Motors initialized - ready.\n");
fflush(stdout);
/* enable UART, writes potentially an empty buffer, but multiplexing is disabled */
ardrone_write = ardrone_open_uart(device, &uart_config_original);
/* initialize multiplexing, deactivate all outputs - must happen after UART open to claim GPIOs on PX4FMU */
gpios = ar_multiplexing_init();
if (ardrone_write < 0) {
fprintf(stderr, "[ardrone_interface] Failed opening AR.Drone UART, exiting.\n");
thread_running = false;
exit(ERROR);
}
/* initialize motors */
if (OK != ar_init_motors(ardrone_write, gpios)) {
close(ardrone_write);
fprintf(stderr, "[ardrone_interface] Failed initializing AR.Drone motors, exiting.\n");
thread_running = false;
exit(ERROR);
}
ardrone_write_motor_commands(ardrone_write, 0, 0, 0, 0);
// XXX Re-done initialization to make sure it is accepted by the motors
// XXX should be removed after more testing, but no harm
/* close uarts */
close(ardrone_write);
/* enable UART, writes potentially an empty buffer, but multiplexing is disabled */
ardrone_write = ardrone_open_uart(device, &uart_config_original);
/* initialize multiplexing, deactivate all outputs - must happen after UART open to claim GPIOs on PX4FMU */
gpios = ar_multiplexing_init();
if (ardrone_write < 0) {
fprintf(stderr, "[ardrone_interface] Failed opening AR.Drone UART, exiting.\n");
thread_running = false;
exit(ERROR);
}
/* initialize motors */
if (OK != ar_init_motors(ardrone_write, gpios)) {
close(ardrone_write);
fprintf(stderr, "[ardrone_interface] Failed initializing AR.Drone motors, exiting.\n");
thread_running = false;
exit(ERROR);
}
while (!thread_should_exit) {
if (motor_test_mode) {
/* set motors to idle speed */
if (test_motor > 0 && test_motor < 5) {
int motors[4] = {0, 0, 0, 0};
motors[test_motor - 1] = 10;
ardrone_write_motor_commands(ardrone_write, motors[0], motors[1], motors[2], motors[3]);
} else {
ardrone_write_motor_commands(ardrone_write, 10, 10, 10, 10);
}
} else {
/* MAIN OPERATION MODE */
/* get a local copy of the vehicle state */
orb_copy(ORB_ID(vehicle_status), state_sub, &state);
/* get a local copy of the actuator controls */
orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_controls_sub, &actuator_controls);
orb_copy(ORB_ID(actuator_armed), armed_sub, &armed);
/* for now only spin if armed and immediately shut down
* if in failsafe
*/
if (armed.armed && !armed.lockdown) {
ardrone_mixing_and_output(ardrone_write, &actuator_controls);
} else {
/* Silently lock down motor speeds to zero */
ardrone_write_motor_commands(ardrone_write, 0, 0, 0, 0);
}
}
if (counter % 24 == 0) {
if (led_counter == 0) ar_set_leds(ardrone_write, 0, 1, 0, 0, 0, 0, 0 , 0);
if (led_counter == 1) ar_set_leds(ardrone_write, 1, 1, 0, 0, 0, 0, 0 , 0);
if (led_counter == 2) ar_set_leds(ardrone_write, 1, 0, 0, 0, 0, 0, 0 , 0);
if (led_counter == 3) ar_set_leds(ardrone_write, 0, 0, 0, 1, 0, 0, 0 , 0);
if (led_counter == 4) ar_set_leds(ardrone_write, 0, 0, 1, 1, 0, 0, 0 , 0);
if (led_counter == 5) ar_set_leds(ardrone_write, 0, 0, 1, 0, 0, 0, 0 , 0);
if (led_counter == 6) ar_set_leds(ardrone_write, 0, 0, 0, 0, 0, 1, 0 , 0);
if (led_counter == 7) ar_set_leds(ardrone_write, 0, 0, 0, 0, 1, 1, 0 , 0);
if (led_counter == 8) ar_set_leds(ardrone_write, 0, 0, 0, 0, 1, 0, 0 , 0);
if (led_counter == 9) ar_set_leds(ardrone_write, 0, 0, 0, 0, 0, 0, 0 , 1);
if (led_counter == 10) ar_set_leds(ardrone_write, 0, 0, 0, 0, 0, 0, 1 , 1);
if (led_counter == 11) ar_set_leds(ardrone_write, 0, 0, 0, 0, 0, 0, 1 , 0);
led_counter++;
if (led_counter == 12) led_counter = 0;
}
/* run at approximately 200 Hz */
usleep(4500);
counter++;
}
/* restore old UART config */
int termios_state;
if ((termios_state = tcsetattr(ardrone_write, TCSANOW, &uart_config_original)) < 0) {
fprintf(stderr, "[ardrone_interface] ERROR setting baudrate / termios config for (tcsetattr)\n");
}
printf("[ardrone_interface] Restored original UART config, exiting..\n");
/* close uarts */
close(ardrone_write);
ar_multiplexing_deinit(gpios);
fflush(stdout);
thread_running = false;
return OK;
}
