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_motor_control.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_motor_control.c')
-rw-r--r-- | apps/ardrone_interface/ardrone_motor_control.c | 23 |
1 files changed, 1 insertions, 22 deletions
diff --git a/apps/ardrone_interface/ardrone_motor_control.c b/apps/ardrone_interface/ardrone_motor_control.c index ad5893963..81ea809f9 100644 --- a/apps/ardrone_interface/ardrone_motor_control.c +++ b/apps/ardrone_interface/ardrone_motor_control.c @@ -303,15 +303,13 @@ int ardrone_write_motor_commands(int ardrone_fd, uint16_t motor1, uint16_t motor } } -void ardrone_mixing_and_output(int ardrone_write, const struct actuator_controls_s *actuators, bool verbose) { +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]; float yaw_control = actuators->control[2]; float motor_thrust = actuators->control[3]; - unsigned int motor_skip_counter = 0; - 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 */ @@ -342,10 +340,6 @@ void ardrone_mixing_and_output(int ardrone_write, const struct actuator_controls output_band = band_factor * (max_thrust - motor_thrust); } - if (verbose && motor_skip_counter % 100 == 0) { - printf("1: mot1: %3.1f band: %3.1f r: %3.1f n: %3.1f y: %3.1f\n", (double)motor_thrust, (double)output_band, (double)roll_control, (double)pitch_control, (double)yaw_control); - } - //add the yaw, nick and roll components to the basic thrust //TODO:this should be done by the mixer // FRONT (MOTOR 1) @@ -380,10 +374,6 @@ 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 * yaw_factor); } - if (verbose && motor_skip_counter % 100 == 0) { - printf("2: m1: %3.1f m2: %3.1f m3: %3.1f m4: %3.1f\n", (double)motor_calc[0], (double)motor_calc[1], (double)motor_calc[2], (double)motor_calc[3]); - } - for (int i = 0; i < 4; i++) { //check for limits if (motor_calc[i] < motor_thrust - output_band) { @@ -395,10 +385,6 @@ void ardrone_mixing_and_output(int ardrone_write, const struct actuator_controls } } - if (verbose && motor_skip_counter % 100 == 0) { - printf("3: band lim: m1: %3.1f m2: %3.1f m3: %3.1f m4: %3.1f\n", (double)motor_calc[0], (double)motor_calc[1], (double)motor_calc[2], (double)motor_calc[3]); - } - /* set the motor values */ /* scale up from 0..1 to 10..512) */ @@ -407,10 +393,6 @@ void ardrone_mixing_and_output(int ardrone_write, const struct actuator_controls 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); - if (verbose && motor_skip_counter % 100 == 0) { - printf("4: scaled: m1: %d m2: %d m3: %d m4: %d\n", motor_pwm[0], motor_pwm[1], motor_pwm[2], motor_pwm[3]); - } - /* Keep motors spinning while armed and prevent overflows */ /* Failsafe logic - should never be necessary */ @@ -426,8 +408,5 @@ void ardrone_mixing_and_output(int ardrone_write, const struct actuator_controls motor_pwm[3] = (motor_pwm[3] <= 512) ? motor_pwm[3] : 512; /* send motors via UART */ - if (verbose && motor_skip_counter % 100 == 0) printf("5: mot: %3.1f-%i-%i-%i-%i\n\n", (double)motor_thrust, motor_pwm[0], motor_pwm[1], motor_pwm[2], motor_pwm[3]); ardrone_write_motor_commands(ardrone_write, motor_pwm[0], motor_pwm[1], motor_pwm[2], motor_pwm[3]); - - motor_skip_counter++; } |