Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-06-07 01:04:19 +08:00
Code Issues Actions 12 Packages Projects Releases Wiki Activity

Checkpoint - this is worth an AR.Drone flight test. Fixed thrust scaling in sensors for manual input, kind of fixed AR.Drone motor interface, very reliable now

Browse Source
This commit is contained in:
Lorenz Meier
2012-09-04 21:16:39 +02:00
parent 62682d805e
commit e503c15361
5 changed files with 115 additions and 71 deletions
Download Patch File Download Diff File Expand all files Collapse all files
apps/ardrone_interface/ardrone_interface.c
+41 -11
View File
@@ -217,14 +217,6 @@ int ardrone_interface_thread_main(int argc, char *argv[])
exit(ERROR);
}
/* initialize motors */
if (OK != ar_init_motors(ardrone_write, gpios)) {
close(ardrone_write);
fprintf(stderr, "[ardrone_interface] Failed initializing AR.Drone motors, exiting.\n");
thread_running = false;
exit(ERROR);
}
/* Led animation */
int counter = 0;
int led_counter = 0;
@@ -245,6 +237,44 @@ int ardrone_interface_thread_main(int argc, char *argv[])
printf("[ardrone_interface] Motors initialized - ready.\n");
fflush(stdout);
/* initialize motors */
if (OK != ar_init_motors(ardrone_write, gpios)) {
close(ardrone_write);
fprintf(stderr, "[ardrone_interface] Failed initializing AR.Drone motors, exiting.\n");
thread_running = false;
exit(ERROR);
}
ardrone_write_motor_commands(ardrone_write, 0, 0, 0, 0);
// XXX Re-done initialization to make sure it is accepted by the motors
// XXX should be removed after more testing, but no harm
/* close uarts */
close(ardrone_write);
ar_multiplexing_deinit(gpios);
/* enable UART, writes potentially an empty buffer, but multiplexing is disabled */
ardrone_write = ardrone_open_uart(&uart_config_original);
/* initialize multiplexing, deactivate all outputs - must happen after UART open to claim GPIOs on PX4FMU */
gpios = ar_multiplexing_init();
if (ardrone_write < 0) {
fprintf(stderr, "[ardrone_interface] Failed opening AR.Drone UART, exiting.\n");
thread_running = false;
exit(ERROR);
}
/* initialize motors */
if (OK != ar_init_motors(ardrone_write, gpios)) {
close(ardrone_write);
fprintf(stderr, "[ardrone_interface] Failed initializing AR.Drone motors, exiting.\n");
thread_running = false;
exit(ERROR);
}
while (!thread_should_exit) {
if (motor_test_mode) {
@@ -267,7 +297,7 @@ int ardrone_interface_thread_main(int argc, char *argv[])
}
}
if (counter % 22 == 0) {
if (counter % 16 == 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);
@@ -297,8 +327,8 @@ int ardrone_interface_thread_main(int argc, char *argv[])
if (led_counter == 12) led_counter = 0;
}
/* run at approximately 100 Hz */
usleep(1000);
/* run at approximately 200 Hz */
usleep(5000);
counter++;
}
apps/ardrone_interface/ardrone_motor_control.c
+61 -47
View File
@@ -55,6 +55,8 @@ typedef union {
uint8_t bytes[2];
} motor_union_t;
#define UART_TRANSFER_TIME_BYTE_US (9+50) /**< 9 us per byte at 115200k plus overhead */
/**
* @brief Generate the 8-byte motor set packet
*
@@ -217,69 +219,80 @@ int ar_init_motors(int ardrone_uart, int gpios)
int i;
int errcounter = 0;
/* initial setup run */
for (i = 1; i < 5; ++i) {
/* Initialize motors 1-4 */
initbuf[3] = (uint8_t)i;
errcounter += ar_select_motor(gpios, i);
write(ardrone_uart, &(initbuf[0]), 1);
/* sleep 5 ms */
usleep(5000);
write(ardrone_uart, &(initbuf[1]), 1);
/* wait 50 ms */
usleep(5000);
write(ardrone_uart, &(initbuf[2]), 1);
/* wait 50 ms */
usleep(50000);
write(ardrone_uart, &(initbuf[3]), 1);
/* wait 50 ms */
usleep(50000);
write(ardrone_uart, &(initbuf[4]), 1);
/* wait 50 ms */
usleep(50000);
/* wait 5 ms */
usleep(50000);
/* enforce immediate write */
fsync(ardrone_uart);
ar_deselect_motor(gpios, i);
/*
* write 0xE0 - request status
* receive one status byte
*/
write(ardrone_uart, &(initbuf[0]), 1);
fsync(ardrone_uart);
usleep(UART_TRANSFER_TIME_BYTE_US*1);
usleep(500000);
/* sleep 500 ms */
/*
* write 0x91 - request checksum
* receive 120 status bytes
*/
write(ardrone_uart, &(initbuf[1]), 1);
fsync(ardrone_uart);
usleep(UART_TRANSFER_TIME_BYTE_US*120);
/*
* write 0xA1 - set status OK
* receive one status byte - should be A0
* to confirm status is OK
*/
write(ardrone_uart, &(initbuf[2]), 1);
fsync(ardrone_uart);
usleep(UART_TRANSFER_TIME_BYTE_US*1);
/*
* set as motor i, where i = 1..4
* receive nothing
*/
initbuf[3] = (uint8_t)i;
write(ardrone_uart, &(initbuf[3]), 1);
fsync(ardrone_uart);
/*
* write 0x40 - check version
* receive 11 bytes encoding the version
*/
write(ardrone_uart, &(initbuf[4]), 1);
fsync(ardrone_uart);
usleep(UART_TRANSFER_TIME_BYTE_US*11);
ar_deselect_motor(gpios, i);
/* sleep 400 ms */
usleep(400000);
}
/* start the multicast part */
errcounter += ar_select_motor(gpios, 0);
/* enable multicast */
write(ardrone_uart, &(multicastbuf[0]), 1);
/* wait 1 ms */
usleep(1000);
/*
* write six times A0 - enable broadcast
* receive nothing
*/
// for (int i = 0; i < sizeof(multicastbuf); i++) {
write(ardrone_uart, multicastbuf, 6);
fsync(ardrone_uart);
write(ardrone_uart, &(multicastbuf[1]), 1);
/* wait 1 ms */
usleep(1000);
usleep(200000);
write(ardrone_uart, &(multicastbuf[2]), 1);
/* wait 1 ms */
usleep(1000);
write(ardrone_uart, multicastbuf, 6);
fsync(ardrone_uart);
write(ardrone_uart, &(multicastbuf[3]), 1);
/* wait 1 ms */
usleep(1000);
write(ardrone_uart, &(multicastbuf[4]), 1);
/* wait 1 ms */
usleep(1000);
write(ardrone_uart, &(multicastbuf[5]), 1);
/* set motors to zero speed */
ardrone_write_motor_commands(ardrone_uart, 0, 0, 0, 0);
fsync(ardrone_uart);
if (errcounter != 0) {
fprintf(stderr, "[ardrone_interface] init sequence incomplete, failed %d times", -errcounter);
@@ -316,6 +329,7 @@ int ardrone_write_motor_commands(int ardrone_fd, uint16_t motor1, uint16_t motor
ar_get_motor_packet(buf, motor1, motor2, motor3, motor4);
int ret;
ret = write(ardrone_fd, buf, sizeof(buf));
fsync(ardrone_fd);
if (ret == sizeof(buf)) {
return OK;
} else {
apps/px4/attitude_estimator_bm/attitude_bm.c
+9 -9
View File
@@ -147,18 +147,18 @@ void attitude_blackmagic_init(void)
// };
static m_elem kal_gain[12 * 9] = {
0.001f , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0,
0 , 0.001f , 0 , 0 , 0 , 0 , 0 , 0 , 0,
0 , 0 , 0.001f , 0 , 0 , 0 , 0 , 0 , 0,
0 , 0 , 0 , 0.015f, 0 , 0 , 0 , 0 , 0,
0 , 0 , 0 , 0 , 0.015f, 0 , 0 , 0 , 0,
0 , 0 , 0 , 0 , 0 , 0.015f, 0 , 0 , 0,
0.0007f , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0,
0 , 0.0007f , 0 , 0 , 0 , 0 , 0 , 0 , 0,
0 , 0 , 0.0007f , 0 , 0 , 0 , 0 , 0 , 0,
0 , 0 , 0 , 0.01f, 0 , 0 , 0 , 0 , 0,
0 , 0 , 0 , 0 , 0.01f, 0 , 0 , 0 , 0,
0 , 0 , 0 , 0 , 0 , 0.01f, 0 , 0 , 0,
0.0000f , +0.00002f, 0 , 0 , 0, 0, 0, 0 , 0,
-0.00002f, 0 , 0 , 0 , 0, 0, 0, 0, 0,
0, 0 , 0 , 0, 0, 0, 0, 0, 0,
0 , 0 , 0 , 0 , 0 , 0 , 0.6f , 0 , 0,
0 , 0 , 0 , 0 , 0 , 0 , 0 , 0.6f , 0,
0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0.6f
0 , 0 , 0 , 0 , 0 , 0 , 0.4f , 0 , 0,
0 , 0 , 0 , 0 , 0 , 0 , 0 , 0.4f , 0,
0 , 0 , 0 , 0 , 0 , 0 , 0 , 0 , 0.4f
};
//offset update only correct if not upside down.
apps/px4/attitude_estimator_bm/attitude_estimator_bm.c
+3 -3
View File
@@ -87,9 +87,9 @@ int attitude_estimator_bm_update(struct sensor_combined_s *raw, float_vect3 *eul
gyro_values.z = raw->gyro_rad_s[2];
float_vect3 accel_values;
accel_values.x = (raw->accelerometer_m_s2[0] / 9.81f) * 120;
accel_values.y = (raw->accelerometer_m_s2[1] / 9.81f) * 120;
accel_values.z = (raw->accelerometer_m_s2[2] / 9.81f) * 120;
accel_values.x = (raw->accelerometer_m_s2[0] / 9.81f) * 100;
accel_values.y = (raw->accelerometer_m_s2[1] / 9.81f) * 100;
accel_values.z = (raw->accelerometer_m_s2[2] / 9.81f) * 100;
float_vect3 mag_values;
mag_values.x = raw->magnetometer_ga[0]*1500.0f;
apps/sensors/sensors.cpp
+1 -1
View File
@@ -914,7 +914,7 @@ Sensors::ppm_poll()
if (manual_control.yaw > 1.0f) manual_control.yaw = 1.0f;
/* throttle input */
manual_control.throttle = (_rc.chan[_rc.function[THROTTLE]].scaled+1.0f)/2.0f;
manual_control.throttle = _rc.chan[_rc.function[THROTTLE]].scaled/2.0f;
if (manual_control.throttle < 0.0f) manual_control.throttle = 0.0f;
if (manual_control.throttle > 1.0f) manual_control.throttle = 1.0f;