diff --git a/src/drivers/ardrone_interface/ardrone_interface.c b/src/drivers/ardrone_interface/ardrone_interface.c index 1777e7f27c..8ad0ee7168 100644 --- a/src/drivers/ardrone_interface/ardrone_interface.c +++ b/src/drivers/ardrone_interface/ardrone_interface.c @@ -92,8 +92,10 @@ static void usage(const char *reason); static void usage(const char *reason) { - if (reason) + if (reason) { warnx("%s\n", reason); + } + warnx("usage: {start|stop|status} [-d ]\n\n"); exit(1); } @@ -122,11 +124,11 @@ int ardrone_interface_main(int argc, char *argv[]) thread_should_exit = false; ardrone_interface_task = px4_task_spawn_cmd("ardrone_interface", - SCHED_DEFAULT, - SCHED_PRIORITY_MAX - 15, - 1100, - ardrone_interface_thread_main, - (argv) ? (char * const *)&argv[2] : (char * const *)NULL); + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 15, + 1100, + ardrone_interface_thread_main, + (argv) ? (char *const *)&argv[2] : (char *const *)NULL); exit(0); } @@ -138,9 +140,11 @@ int ardrone_interface_main(int argc, char *argv[]) if (!strcmp(argv[1], "status")) { if (thread_running) { warnx("running"); + } else { warnx("not started"); } + exit(0); } @@ -212,19 +216,23 @@ int ardrone_interface_thread_main(int argc, char *argv[]) } if (strcmp(argv[i], "-m") == 0 || strcmp(argv[i], "--motor") == 0) { - if (i+1 < argc) { - int motor = atoi(argv[i+1]); + 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]; @@ -314,6 +322,7 @@ int ardrone_interface_thread_main(int argc, char *argv[]) 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); } @@ -338,33 +347,33 @@ int ardrone_interface_thread_main(int argc, char *argv[]) } if (counter % 24 == 0) { - if (led_counter == 0) ar_set_leds(ardrone_write, 0, 1, 0, 0, 0, 0, 0 , 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 == 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 == 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 == 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 == 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 == 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 == 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 == 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 == 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 == 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 == 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); + 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; + if (led_counter == 12) { led_counter = 0; } } /* run at approximately 200 Hz */ diff --git a/src/drivers/ardrone_interface/ardrone_motor_control.c b/src/drivers/ardrone_interface/ardrone_motor_control.c index 46e1f86511..4991389129 100644 --- a/src/drivers/ardrone_interface/ardrone_motor_control.c +++ b/src/drivers/ardrone_interface/ardrone_motor_control.c @@ -108,7 +108,7 @@ void ar_enable_broadcast(int fd) int ar_multiplexing_init() { int fd; - + fd = open(PX4FMU_DEVICE_PATH, 0); if (fd < 0) { @@ -176,7 +176,7 @@ int ar_select_motor(int fd, uint8_t motor) ret += ioctl(fd, GPIO_CLEAR, motor_gpios); } else { - /* select reqested motor */ + /* select reqested motor */ ret += ioctl(fd, GPIO_CLEAR, motor_gpio[motor - 1]); } @@ -199,7 +199,7 @@ int ar_deselect_motor(int fd, uint8_t motor) ret += ioctl(fd, GPIO_SET, motor_gpios); } else { - /* deselect reqested motor */ + /* deselect reqested motor */ ret = ioctl(fd, GPIO_SET, motor_gpio[motor - 1]); } @@ -235,7 +235,7 @@ int ar_init_motors(int ardrone_uart, int gpios) */ write(ardrone_uart, &(initbuf[0]), 1); fsync(ardrone_uart); - usleep(UART_TRANSFER_TIME_BYTE_US*1); + usleep(UART_TRANSFER_TIME_BYTE_US * 1); /* * write 0x91 - request checksum @@ -243,7 +243,7 @@ int ar_init_motors(int ardrone_uart, int gpios) */ write(ardrone_uart, &(initbuf[1]), 1); fsync(ardrone_uart); - usleep(UART_TRANSFER_TIME_BYTE_US*120); + usleep(UART_TRANSFER_TIME_BYTE_US * 120); /* * write 0xA1 - set status OK @@ -252,7 +252,7 @@ int ar_init_motors(int ardrone_uart, int gpios) */ write(ardrone_uart, &(initbuf[2]), 1); fsync(ardrone_uart); - usleep(UART_TRANSFER_TIME_BYTE_US*1); + usleep(UART_TRANSFER_TIME_BYTE_US * 1); /* * set as motor i, where i = 1..4 @@ -268,7 +268,7 @@ int ar_init_motors(int ardrone_uart, int gpios) */ write(ardrone_uart, &(initbuf[4]), 1); fsync(ardrone_uart); - usleep(UART_TRANSFER_TIME_BYTE_US*11); + usleep(UART_TRANSFER_TIME_BYTE_US * 11); ar_deselect_motor(gpios, i); /* sleep 200 ms */ @@ -304,13 +304,15 @@ int ar_init_motors(int ardrone_uart, int gpios) warnx("Failed %d times", -errcounter); fflush(stdout); } + return errcounter; } /** * Sets the leds on the motor controllers, 1 turns led on, 0 off. */ -void ar_set_leds(int ardrone_uart, uint8_t led1_red, uint8_t led1_green, uint8_t led2_red, uint8_t led2_green, uint8_t led3_red, uint8_t led3_green, uint8_t led4_red, uint8_t led4_green) +void ar_set_leds(int ardrone_uart, uint8_t led1_red, uint8_t led1_green, uint8_t led2_red, uint8_t led2_green, + uint8_t led3_red, uint8_t led3_green, uint8_t led4_red, uint8_t led4_green) { /* * 2 bytes are sent. The first 3 bits describe the command: 011 means led control @@ -322,12 +324,15 @@ void ar_set_leds(int ardrone_uart, uint8_t led1_red, uint8_t led1_green, uint8_t * 011 rrrr 0000 gggg 0 */ uint8_t leds[2]; - leds[0] = 0x60 | ((led4_red & 0x01) << 4) | ((led3_red & 0x01) << 3) | ((led2_red & 0x01) << 2) | ((led1_red & 0x01) << 1); - leds[1] = ((led4_green & 0x01) << 4) | ((led3_green & 0x01) << 3) | ((led2_green & 0x01) << 2) | ((led1_green & 0x01) << 1); + leds[0] = 0x60 | ((led4_red & 0x01) << 4) | ((led3_red & 0x01) << 3) | ((led2_red & 0x01) << 2) | (( + led1_red & 0x01) << 1); + leds[1] = ((led4_green & 0x01) << 4) | ((led3_green & 0x01) << 3) | ((led2_green & 0x01) << 2) | (( + led1_green & 0x01) << 1); write(ardrone_uart, leds, 2); } -int ardrone_write_motor_commands(int ardrone_fd, uint16_t motor1, uint16_t motor2, uint16_t motor3, uint16_t motor4) { +int ardrone_write_motor_commands(int ardrone_fd, uint16_t motor1, uint16_t motor2, uint16_t motor3, uint16_t motor4) +{ const unsigned int min_motor_interval = 4900; static uint64_t last_motor_time = 0; @@ -338,6 +343,7 @@ int ardrone_write_motor_commands(int ardrone_fd, uint16_t motor1, uint16_t motor outputs.output[2] = motor3; outputs.output[3] = motor4; static orb_advert_t pub = 0; + if (pub == 0) { /* advertise to channel 0 / primary */ pub = orb_advertise(ORB_ID(actuator_outputs), &outputs); @@ -355,15 +361,18 @@ int ardrone_write_motor_commands(int ardrone_fd, uint16_t motor1, uint16_t motor if (ret == sizeof(buf)) { return OK; + } else { return ret; } + } else { return -ERROR; } } -void ardrone_mixing_and_output(int ardrone_write, const struct actuator_controls_s *actuators) { +void ardrone_mixing_and_output(int ardrone_write, const struct actuator_controls_s *actuators) +{ float roll_control = actuators->control[0]; float pitch_control = actuators->control[1]; @@ -385,7 +394,8 @@ void ardrone_mixing_and_output(int ardrone_write, const struct actuator_controls /* linearly scale the control inputs from 0 to startpoint_full_control */ if (motor_thrust < startpoint_full_control) { - output_band = motor_thrust/startpoint_full_control; // linear from 0 to 1 + output_band = motor_thrust / startpoint_full_control; // linear from 0 to 1 + } else { output_band = 1.0f; } @@ -407,7 +417,7 @@ void ardrone_mixing_and_output(int ardrone_write, const struct actuator_controls motor_calc[3] = motor_thrust + (roll_control / 2 - pitch_control / 2 + yaw_control); /* if one motor is saturated, reduce throttle */ - float saturation = fmaxf(fmaxf(motor_calc[0], motor_calc[1]),fmaxf(motor_calc[2], motor_calc[3])) - max_thrust; + float saturation = fmaxf(fmaxf(motor_calc[0], motor_calc[1]), fmaxf(motor_calc[2], motor_calc[3])) - max_thrust; if (saturation > 0.0f) { @@ -428,16 +438,16 @@ void ardrone_mixing_and_output(int ardrone_write, const struct actuator_controls /* set the motor values */ /* scale up from 0..1 to 10..500) */ - motor_pwm[0] = (uint16_t) (motor_calc[0] * ((float)max_gas - min_gas) + min_gas); - motor_pwm[1] = (uint16_t) (motor_calc[1] * ((float)max_gas - min_gas) + min_gas); - motor_pwm[2] = (uint16_t) (motor_calc[2] * ((float)max_gas - min_gas) + min_gas); - motor_pwm[3] = (uint16_t) (motor_calc[3] * ((float)max_gas - min_gas) + min_gas); + motor_pwm[0] = (uint16_t)(motor_calc[0] * ((float)max_gas - min_gas) + min_gas); + motor_pwm[1] = (uint16_t)(motor_calc[1] * ((float)max_gas - min_gas) + min_gas); + motor_pwm[2] = (uint16_t)(motor_calc[2] * ((float)max_gas - min_gas) + min_gas); + motor_pwm[3] = (uint16_t)(motor_calc[3] * ((float)max_gas - min_gas) + min_gas); /* scale up from 0..1 to 10..500) */ - motor_pwm[0] = (uint16_t) (motor_calc[0] * (float)((max_gas - min_gas) + min_gas)); - motor_pwm[1] = (uint16_t) (motor_calc[1] * (float)((max_gas - min_gas) + min_gas)); - motor_pwm[2] = (uint16_t) (motor_calc[2] * (float)((max_gas - min_gas) + min_gas)); - motor_pwm[3] = (uint16_t) (motor_calc[3] * (float)((max_gas - min_gas) + min_gas)); + motor_pwm[0] = (uint16_t)(motor_calc[0] * (float)((max_gas - min_gas) + min_gas)); + motor_pwm[1] = (uint16_t)(motor_calc[1] * (float)((max_gas - min_gas) + min_gas)); + motor_pwm[2] = (uint16_t)(motor_calc[2] * (float)((max_gas - min_gas) + min_gas)); + motor_pwm[3] = (uint16_t)(motor_calc[3] * (float)((max_gas - min_gas) + min_gas)); /* Failsafe logic for min values - should never be necessary */ motor_pwm[0] = (motor_pwm[0] > 0) ? motor_pwm[0] : min_gas;