fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-26 04:43:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

Minor but important fixes across system

Browse Source
This commit is contained in:
Lorenz Meier
2012-08-08 18:47:46 +02:00
parent 7a6a4b9352
commit 7a912a3fe4
8 changed files with 243 additions and 160 deletions
Download Patch File Download Diff File Expand all files Collapse all files
apps/ardrone_control/ardrone_control.c
+26 -18
View File
@@ -166,6 +166,9 @@ int ardrone_control_main(int argc, char *argv[])
ar_init_motors(ardrone_write, &gpios);
int counter = 0;
/* Led animation */
int led_counter = 0;
/* pthread for position control */
pthread_t position_control_thread;
position_control_thread_started = false;
@@ -223,41 +226,46 @@ int ardrone_control_main(int argc, char *argv[])
}
//fancy led animation...
static int blubb = 0;
if (counter % 30 == 0) {
if (led_counter == 0) ar_set_leds(ardrone_write, 0, 1, 0, 0, 0, 0, 0 , 0);
if (counter % 20 == 0) {
if (blubb == 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);
if (blubb == 1) ar_set_leds(ardrone_write, 1, 1, 0, 0, 0, 0, 0 , 0);
if (led_counter == 2) ar_set_leds(ardrone_write, 1, 0, 0, 0, 0, 0, 0 , 0);
if (blubb == 2) ar_set_leds(ardrone_write, 1, 0, 0, 0, 0, 0, 0 , 0);
if (led_counter == 3) ar_set_leds(ardrone_write, 0, 0, 0, 1, 0, 0, 0 , 0);
if (blubb == 3) ar_set_leds(ardrone_write, 0, 0, 0, 1, 0, 0, 0 , 0);
if (led_counter == 4) ar_set_leds(ardrone_write, 0, 0, 1, 1, 0, 0, 0 , 0);
if (blubb == 4) ar_set_leds(ardrone_write, 0, 0, 1, 1, 0, 0, 0 , 0);
if (led_counter == 5) ar_set_leds(ardrone_write, 0, 0, 1, 0, 0, 0, 0 , 0);
if (blubb == 5) ar_set_leds(ardrone_write, 0, 0, 1, 0, 0, 0, 0 , 0);
if (led_counter == 6) ar_set_leds(ardrone_write, 0, 0, 0, 0, 0, 1, 0 , 0);
if (blubb == 6) ar_set_leds(ardrone_write, 0, 0, 0, 0, 0, 1, 0 , 0);
if (led_counter == 7) ar_set_leds(ardrone_write, 0, 0, 0, 0, 1, 1, 0 , 0);
if (blubb == 7) ar_set_leds(ardrone_write, 0, 0, 0, 0, 1, 1, 0 , 0);
if (led_counter == 8) ar_set_leds(ardrone_write, 0, 0, 0, 0, 1, 0, 0 , 0);
if (blubb == 8) ar_set_leds(ardrone_write, 0, 0, 0, 0, 1, 0, 0 , 0);
if (led_counter == 9) ar_set_leds(ardrone_write, 0, 0, 0, 0, 0, 0, 0 , 1);
if (blubb == 9) ar_set_leds(ardrone_write, 0, 0, 0, 0, 0, 0, 0 , 1);
if (led_counter == 10) ar_set_leds(ardrone_write, 0, 0, 0, 0, 0, 0, 1 , 1);
if (blubb == 10) ar_set_leds(ardrone_write, 0, 0, 0, 0, 0, 0, 1 , 1);
if (led_counter == 11) ar_set_leds(ardrone_write, 0, 0, 0, 0, 0, 0, 1 , 0);
if (blubb == 11) ar_set_leds(ardrone_write, 0, 0, 0, 0, 0, 0, 1 , 0);
led_counter++;
blubb++;
if (blubb == 12) blubb = 0;
if (led_counter == 12) led_counter = 0;
}
/* run at approximately 200 Hz */
usleep(5000);
// 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++;
}
apps/ardrone_control/ardrone_motor_control.c
+12 -8
View File
@@ -33,7 +33,8 @@
****************************************************************************/
/*
* @file Implementation of AR.Drone 1.0 / 2.0 motor control interface
* @file ardrone_motor_control.c
* Implementation of AR.Drone 1.0 / 2.0 motor control interface
*/
@@ -97,23 +98,23 @@ int ar_multiplexing_init()
{
int fd;
fd = open("/dev/gpio", O_RDONLY | O_NONBLOCK);
fd = open(GPIO_DEVICE_PATH, 0);
if (fd < 0) {
printf("GPIO: open fail\n");
return fd;
}
/* deactivate all outputs */
int ret = 0;
ret += ioctl(fd, GPIO_SET, motor_gpios);
if (ioctl(fd, GPIO_SET_OUTPUT, motor_gpios) != 0) {
printf("GPIO: output set fail\n");
close(fd);
return -1;
}
/* deactivate all outputs */
int ret = 0;
ret += ioctl(fd, GPIO_SET, motor_gpios);
if (ret < 0) {
printf("GPIO: clearing pins fail\n");
close(fd);
@@ -167,12 +168,15 @@ int ar_select_motor(int fd, uint8_t motor)
ret += ioctl(fd, GPIO_CLEAR, motor_gpios);
} else {
/* deselect all */
ret += ioctl(fd, GPIO_SET, motor_gpios);
/* select reqested motor */
ret += ioctl(fd, GPIO_CLEAR, motor_gpio[motor - 1]);
/* deselect all others */
gpioset = motor_gpios ^ motor_gpio[motor - 1];
ret += ioctl(fd, GPIO_SET, gpioset);
// gpioset = motor_gpios ^ motor_gpio[motor - 1];
// ret += ioctl(fd, GPIO_SET, gpioset);
}
return ret;
apps/ardrone_control/attitude_control.c
+31 -14
View File
@@ -93,7 +93,7 @@ void control_attitude(const struct rc_channels_s *rc, const struct vehicle_attit
static PID_t nick_controller;
static PID_t roll_controller;
static const float min_gas = 1;
static const float min_gas = 10;
static const float max_gas = 512;
static uint16_t motor_pwm[4] = {0, 0, 0, 0};
static float motor_calc[4] = {0.0f, 0.0f, 0.0f, 0.0f};
@@ -201,11 +201,11 @@ void control_attitude(const struct rc_channels_s *rc, const struct vehicle_attit
// don't turn around the wrong side (only works if yaw angle is between +- 180 degree)
if (yaw_e > M_PI) {
yaw_e -= 2.0f * M_PI;
yaw_e -= 2.0f * M_PI_F;
}
if (yaw_e < -M_PI) {
yaw_e += 2.0f * M_PI;
yaw_e += 2.0f * M_PI_F;
}
attitude_setpoint_navigationframe_from_positioncontroller.z = pid_calculate(&yaw_pos_controller, 0, yaw_e, 0, CONTROL_PID_ATTITUDE_INTERVAL);
@@ -234,9 +234,9 @@ void control_attitude(const struct rc_channels_s *rc, const struct vehicle_attit
attitude_setpoint_bodyframe.z = attitude_setpoint_bodyframe_from_positioncontroller.z;
} else if (current_state == SYSTEM_STATE_MANUAL) {
attitude_setpoint_bodyframe.x = -((float)rc->chan[rc->function[ROLL]].scale / 10000.0f) * 3.14159265 / 8.0f;
attitude_setpoint_bodyframe.y = -((float)rc->chan[rc->function[PITCH]].scale / 10000.0f) * 3.14159265 / 8.0f;
attitude_setpoint_bodyframe.z = -((float)rc->chan[rc->function[YAW]].scale / 10000.0f) * 3.14159265;
attitude_setpoint_bodyframe.x = -((float)rc->chan[rc->function[ROLL]].scale / 10000.0f) * M_PI_F / 8.0f;
attitude_setpoint_bodyframe.y = -((float)rc->chan[rc->function[PITCH]].scale / 10000.0f) * M_PI_F / 8.0f;
attitude_setpoint_bodyframe.z = -((float)rc->chan[rc->function[YAW]].scale / 10000.0f) * M_PI_F;
}
/* add an attitude offset which needs to be estimated somewhere */
@@ -282,13 +282,13 @@ void control_attitude(const struct rc_channels_s *rc, const struct vehicle_attit
// FLYING MODES
if (current_state == SYSTEM_STATE_MANUAL) {
motor_thrust = (float)rc->chan[rc->function[THROTTLE]].scale;
motor_thrust = (float)rc->chan[rc->function[THROTTLE]].scale / 20000.0f;
} else if (current_state == SYSTEM_STATE_GROUND_READY || current_state == SYSTEM_STATE_STABILIZED || current_state == SYSTEM_STATE_AUTO || current_state == SYSTEM_STATE_MISSION_ABORT) {
motor_thrust = (float)rc->chan[rc->function[THROTTLE]].scale; //TODO
motor_thrust = (float)rc->chan[rc->function[THROTTLE]].scale / 20000.0f; //TODO
} else if (current_state == SYSTEM_STATE_EMCY_LANDING) {
motor_thrust = (float)rc->chan[rc->function[THROTTLE]].scale; //TODO
motor_thrust = (float)rc->chan[rc->function[THROTTLE]].scale / 20000.0f; //TODO
} else if (current_state == SYSTEM_STATE_EMCY_CUTOFF) {
motor_thrust = 0; //immediately cut off thrust!
@@ -297,10 +297,12 @@ void control_attitude(const struct rc_channels_s *rc, const struct vehicle_attit
motor_thrust = 0; // Motor thrust must be zero in any other mode!
}
if (status->rc_signal_lost) motor_thrust = 0;
// Convertion to motor-step units
motor_thrust *= zcompensation;
motor_thrust *= max_gas / 20000.0f; //TODO: check this
motor_thrust += (max_gas - min_gas) / 2.f;
/* scale up from 0..1 to 10..512) */
motor_thrust *= ((float)max_gas - min_gas);
//limit control output
//yawspeed
@@ -418,10 +420,25 @@ void control_attitude(const struct rc_channels_s *rc, const struct vehicle_attit
motor_pwm[2] = (uint16_t) motor_calc[2];
motor_pwm[3] = (uint16_t) motor_calc[3];
// Keep motors spinning while armed
motor_pwm[0] = (motor_pwm[0] > 0) ? motor_pwm[0] : 10;
motor_pwm[1] = (motor_pwm[1] > 0) ? motor_pwm[1] : 10;
motor_pwm[2] = (motor_pwm[2] > 0) ? motor_pwm[2] : 10;
motor_pwm[3] = (motor_pwm[3] > 0) ? motor_pwm[3] : 10;
/* Failsafe logic - should never be necessary */
motor_pwm[0] = (motor_pwm[0] <= 512) ? motor_pwm[0] : 512;
motor_pwm[1] = (motor_pwm[1] <= 512) ? motor_pwm[1] : 512;
motor_pwm[2] = (motor_pwm[2] <= 512) ? motor_pwm[2] : 512;
motor_pwm[3] = (motor_pwm[3] <= 512) ? motor_pwm[3] : 512;
//SEND MOTOR COMMANDS
uint8_t motorSpeedBuf[5] = {1, 2, 3, 4, 5};
ar_get_motor_packet(motorSpeedBuf, motor_pwm[0], motor_pwm[1], motor_pwm[2], motor_pwm[3]);
write(ardrone_write, motorSpeedBuf, 5);
if (motor_skip_counter % 5 == 0) {
uint8_t motorSpeedBuf[5] = {1, 2, 3, 4, 5};
ar_get_motor_packet(motorSpeedBuf, motor_pwm[0], motor_pwm[1], motor_pwm[2], motor_pwm[3]);
write(ardrone_write, motorSpeedBuf, 5);
}
motor_skip_counter++;