From abbe998506e4ba49bbf6a9a9ae731b1eec521db6 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 25 Sep 2012 21:35:02 +0200 Subject: ardrone in the air again (workaround: rate controller disabled) --- apps/ardrone_interface/ardrone_interface.c | 11 +++++++++-- apps/ardrone_interface/ardrone_motor_control.c | 9 ++++++--- 2 files changed, 15 insertions(+), 5 deletions(-) (limited to 'apps/ardrone_interface') 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 -- cgit v1.2.3