fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-21 02:53:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

Minor improvements to ardrone interface, ready for prime time

Browse Source
This commit is contained in:
Lorenz Meier
2012-08-29 09:30:57 +02:00
parent 6fd7e12e13
commit cbf020de87
3 changed files with 32 additions and 41 deletions
Download Patch File Download Diff File Expand all files Collapse all files
apps/ardrone_interface/ardrone_interface.c
+30 -18
View File
@@ -206,25 +206,44 @@ int ardrone_interface_thread_main(int argc, char *argv[])
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));
while (!thread_should_exit) {
/* 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);
// if ..
ardrone_mixing_and_output(ardrone_write, &actuator_controls, motor_test_mode);
// } else {
// /* Silently lock down motor speeds to zero */
// ardrone_write_motor_commands(ardrone_write, 0, 0, 0, 0);
// }
if (motor_test_mode) {
/* set motors to idle speed */
ardrone_write_motor_commands(ardrone_write, 10, 0, 0, 0);
sleep(2);
ardrone_write_motor_commands(ardrone_write, 0, 10, 0, 0);
sleep(2);
ardrone_write_motor_commands(ardrone_write, 0, 0, 10, 0);
sleep(2);
ardrone_write_motor_commands(ardrone_write, 0, 0, 0, 10);
sleep(2);
} else {
/* MAIN OPERATION MODE */
if (counter % 30 == 0) {
/* 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);
if (armed.armed) {
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 % 22 == 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);
@@ -257,13 +276,6 @@ int ardrone_interface_thread_main(int argc, char *argv[])
/* run at approximately 50 Hz */
usleep(20000);
// This is a hardcore debug code piece to validate
// the motor interface
// uint8_t motorSpeedBuf[5] = {1, 2, 3, 4, 5};
// ar_get_motor_packet(motorSpeedBuf, 20, 20, 20, 20);
// write(ardrone_write, motorSpeedBuf, 5);
// usleep(15000);
counter++;
}