dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Minor improvements to ardrone interface, ready for prime time

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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

48
apps/ardrone_interface/ardrone_interface.c
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++;
}

23
apps/ardrone_interface/ardrone_motor_control.c
View File

@ -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++;
}

2
apps/ardrone_interface/ardrone_motor_control.h
View File

@ -76,4 +76,4 @@ int ar_init_motors(int ardrone_uart, int *gpios_pin);
*/
void ar_set_leds(int ardrone_uart, uint8_t led1_red, uint8_t led1_green, uint8_t led2_red, uint8_t led2_green, uint8_t led3_red, uint8_t led3_green, uint8_t led4_red, uint8_t led4_green);
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);