diff options
author | Lorenz Meier <lm@inf.ethz.ch> | 2012-08-29 09:30:57 +0200 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2012-08-29 09:30:57 +0200 |
commit | cbf020de87a6a4b492c60ff918632369cf4ec887 (patch) | |
tree | 1e0d80e399a72342d20c3a51de5b8eedc674ae9a /apps/ardrone_interface/ardrone_interface.c | |
parent | 6fd7e12e1349dd4805e851624db3a2e50b48ffa3 (diff) | |
download | px4-firmware-cbf020de87a6a4b492c60ff918632369cf4ec887.tar.gz px4-firmware-cbf020de87a6a4b492c60ff918632369cf4ec887.tar.bz2 px4-firmware-cbf020de87a6a4b492c60ff918632369cf4ec887.zip |
Minor improvements to ardrone interface, ready for prime time
Diffstat (limited to 'apps/ardrone_interface/ardrone_interface.c')
-rw-r--r-- | apps/ardrone_interface/ardrone_interface.c | 50 |
1 files changed, 31 insertions, 19 deletions
diff --git a/apps/ardrone_interface/ardrone_interface.c b/apps/ardrone_interface/ardrone_interface.c index e781d4613..fdbe3cb62 100644 --- a/apps/ardrone_interface/ardrone_interface.c +++ b/apps/ardrone_interface/ardrone_interface.c @@ -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 (counter % 30 == 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 */ + + /* 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++; } |