diff options
author | Julian Oes <joes@student.ethz.ch> | 2012-09-25 21:35:02 +0200 |
---|---|---|
committer | Julian Oes <joes@student.ethz.ch> | 2012-09-25 21:35:02 +0200 |
commit | abbe998506e4ba49bbf6a9a9ae731b1eec521db6 (patch) | |
tree | 181fb20a6d091e2c516bb0b8409571093bdac7d1 /apps/ardrone_interface | |
parent | 0eae48d480edae2f22fc1f486f26609a49c9d69e (diff) | |
download | px4-firmware-abbe998506e4ba49bbf6a9a9ae731b1eec521db6.tar.gz px4-firmware-abbe998506e4ba49bbf6a9a9ae731b1eec521db6.tar.bz2 px4-firmware-abbe998506e4ba49bbf6a9a9ae731b1eec521db6.zip |
ardrone in the air again (workaround: rate controller disabled)
Diffstat (limited to 'apps/ardrone_interface')
-rw-r--r-- | apps/ardrone_interface/ardrone_interface.c | 11 | ||||
-rw-r--r-- | apps/ardrone_interface/ardrone_motor_control.c | 9 |
2 files changed, 15 insertions, 5 deletions
diff --git a/apps/ardrone_interface/ardrone_interface.c b/apps/ardrone_interface/ardrone_interface.c index 8d77e7502..f12f9cb47 100644 --- a/apps/ardrone_interface/ardrone_interface.c +++ b/apps/ardrone_interface/ardrone_interface.c @@ -239,14 +239,15 @@ int ardrone_interface_thread_main(int argc, char *argv[]) /* declare and safely initialize all structs */ struct vehicle_status_s state; - memset(&state, 0, sizeof(state)); + //memset(&state, 0, sizeof(state)); struct actuator_controls_s actuator_controls; - memset(&actuator_controls, 0, sizeof(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)); @@ -328,7 +329,13 @@ int ardrone_interface_thread_main(int argc, char *argv[]) * if in failsafe */ if (armed.armed && !armed.lockdown) { + + + + //printf("AMO_BEF: Roll: %4.4f, Pitch: %4.4f, Yaw: %4.4f, Thrust: %4.4f\n",actuator_controls.control[0], actuator_controls.control[1], actuator_controls.control[2], actuator_controls.control[3]); + 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); diff --git a/apps/ardrone_interface/ardrone_motor_control.c b/apps/ardrone_interface/ardrone_motor_control.c index 787db1877..cbf9600a5 100644 --- a/apps/ardrone_interface/ardrone_motor_control.c +++ b/apps/ardrone_interface/ardrone_motor_control.c @@ -368,6 +368,8 @@ void ardrone_mixing_and_output(int ardrone_write, const struct actuator_controls float yaw_control = actuators->control[2]; float motor_thrust = actuators->control[3]; + //printf("AMO: Roll: %4.4f, Pitch: %4.4f, Yaw: %4.4f, Thrust: %4.4f\n",roll_control, pitch_control, yaw_control, motor_thrust); + const float min_thrust = 0.02f; /**< 2% minimum thrust */ const float max_thrust = 1.0f; /**< 100% max thrust */ const float scaling = 512.0f; /**< 100% thrust equals a value of 512 */ @@ -387,15 +389,16 @@ void ardrone_mixing_and_output(int ardrone_write, const struct actuator_controls if (motor_thrust <= min_thrust) { motor_thrust = min_thrust; output_band = 0.0f; - + //printf("0 silent\n"); } else if (motor_thrust < startpoint_full_control && motor_thrust > min_thrust) { output_band = band_factor * (motor_thrust - min_thrust); - + //printf("1 starting\n"); } else if (motor_thrust >= startpoint_full_control && motor_thrust < max_thrust - band_factor * startpoint_full_control) { output_band = band_factor * startpoint_full_control; - + //printf("2 working\n"); } else if (motor_thrust >= max_thrust - band_factor * startpoint_full_control) { output_band = band_factor * (max_thrust - motor_thrust); + //printf("3 full\n"); } //add the yaw, nick and roll components to the basic thrust //TODO:this should be done by the mixer |