fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-04 13:40:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

AR Drone interface: Fixed code style

Browse Source
This commit is contained in:
Lorenz Meier
2015-10-19 13:15:49 +02:00
parent e2e83cd578
commit ab854f99e8
2 changed files with 62 additions and 43 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/ardrone_interface/ardrone_interface.c
+30 -21
View File
@@ -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 <UART>]\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 */
src/drivers/ardrone_interface/ardrone_motor_control.c
+32 -22
View File
@@ -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;