Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-06-04 13:55:40 +08:00
Code Issues Actions 28 Packages Projects Releases Wiki Activity

Merge pull request #1571 from OpenUAS/ardrone2_fix_ap_start_experience

Browse Source 
Improve experience of PPRZ on ARDrone2

Fix the dreaded 2x need to upload and trashed navdata and some more. e.g. from "Currently running: Paparazzi (ap.elf), Native (program.elf)", to "Currently running: Paparazzi (ap.elf)" first time powered on ARDrone2
This commit is contained in:
Felix Ruess
2016-03-22 22:33:22 +01:00
parent 382873ac24 5579ec650c
commit 83d4552059
8 changed files with 167 additions and 113 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/boards/ardrone2.makefile
+1 -1
View File
@@ -2,7 +2,7 @@
# #
# ardrone2.makefile # ardrone2.makefile
# #
# http://wiki.paparazziuav.org/wiki/AR.Drone_2_-_Specifications # http://wiki.paparazziuav.org/wiki/AR_Drone_2
# #
BOARD=ardrone BOARD=ardrone
sw/airborne/boards/ardrone/board.c
+47 -5
View File
@@ -1,5 +1,5 @@
/* /*
* Copyright (C) 2015 The Paparazzi Team * Copyright (C) 2016 The Paparazzi Team
* *
* This file is part of paparazzi. * This file is part of paparazzi.
* *
@@ -26,12 +26,15 @@
*/ */
#include <stdlib.h> #include <stdlib.h>
#include <stdio.h>
#include <string.h>
#include <unistd.h>
#include "mcu.h" #include "mcu.h"
/* check if the bat_voltage_ardrone2 module is loaded */ /* Check if the bat_voltage_ardrone2 module is loaded */
#include "generated/modules.h" #include "generated/modules.h"
#ifndef BAT_VOLTAGE_ARDRONE2_PERIODIC_FREQ #ifndef BAT_VOLTAGE_ARDRONE2_PERIODIC_FREQ
#warning No battery voltage measurement available! Please add <load name="bat_voltage_ardrone2.xml"/> to your modules. #warning No battery voltage measurement available! Please add <load name="bat_voltage_ardrone2.xml"/> to your modules section in aircraft file.
#endif #endif
#include "peripherals/video_device.h" #include "peripherals/video_device.h"
@@ -56,10 +59,49 @@ struct video_config_t bottom_camera = {
.filters = 0 .filters = 0
}; };
int KillGracefully(char *process_name);
int KillGracefully(char *process_name)
{
/* "pidof" always in /bin on ARdrone2, no need for "which" */
char pidof_commandline[200] = "/bin/pidof -s ";
strcat(pidof_commandline, process_name);
/* ARDrone2 Busybox a
$ cat /proc/sys/kernel/pid_max
Gives max 32768, makes sure it never kills existing other process
*/
char pid[7] = "";
int ret = 0; /* Return code of kill system call */
FILE *fp;
while (ret == 0) {
fp = popen(pidof_commandline, "r");
if (fp != NULL) { /* Could open the pidof with line */
if (fgets(pid, sizeof(pid) - 1, fp) != NULL) {
printf("Process ID deducted: \"%s\"\n", pid);
if (atoi(pid) > 0) { /* To make sure we end 0 > There is a real process id found */
char kill_command_and_process[200] = "kill -9 "; /* BTW there is no pkill on this Busybox */
strcat(kill_command_and_process, pid);
ret = system(kill_command_and_process);
/* No need to wait, since if it is not closed the next pidof scan still will still find it and try to kill it */
}
} else {
ret = 256; /* Could not get handle */
pclose(fp);
}
} else {
ret = 256; /* fp NULL, so no process, just return */
return 0;
}
} /* end while */
return 0;
}
void board_init(void) void board_init(void)
{ {
// First we try to kill the program.elf and its respawner if it is running /* Two process to kill, the respawner first to make sure program.elf does not get restarted */
int ret = system("killall -9 program.elf.respawner.sh; killall -9 program.elf"); int ret = system("killall -15 program.elf.respawner.sh");
usleep(50000); /* Give respawner 50ms time to end on a busy system */
KillGracefully("program.elf");
(void) ret; (void) ret;
} }
sw/airborne/boards/ardrone/gpio_ardrone.c
+10 -10
View File
@@ -53,14 +53,14 @@ struct gpio_direction {
void gpio_set(uint32_t port, uint16_t pin) void gpio_set(uint32_t port, uint16_t pin)
{ {
if (port != 0x32524) { return; } // protect ardrone board from unauthorized use if (port != 0x32524) { return; } /* protect ardrone board from unauthorized use */
struct gpio_data data; struct gpio_data data;
// Open the device if not open // Open the device if not open
if (gpiofp == 0) { if (gpiofp == 0) {
gpiofp = open("/dev/gpio", O_RDWR); gpiofp = open("/dev/gpio", O_RDWR);
} }
// Read the GPIO value /* Read the GPIO value */
data.pin = pin; data.pin = pin;
data.value = 1; data.value = 1;
ioctl(gpiofp, GPIO_WRITE, &data); ioctl(gpiofp, GPIO_WRITE, &data);
@@ -69,14 +69,14 @@ void gpio_set(uint32_t port, uint16_t pin)
void gpio_clear(uint32_t port, uint16_t pin) void gpio_clear(uint32_t port, uint16_t pin)
{ {
if (port != 0x32524) { return; } // protect ardrone board from unauthorized use if (port != 0x32524) { return; } /* protect ardrone board from unauthorized use */
struct gpio_data data; struct gpio_data data;
// Open the device if not open // Open the device if not open
if (gpiofp == 0) { if (gpiofp == 0) {
gpiofp = open("/dev/gpio", O_RDWR); gpiofp = open("/dev/gpio", O_RDWR);
} }
// Read the GPIO value /* Read the GPIO value */
data.pin = pin; data.pin = pin;
data.value = 0; data.value = 0;
ioctl(gpiofp, GPIO_WRITE, &data); ioctl(gpiofp, GPIO_WRITE, &data);
@@ -85,14 +85,14 @@ void gpio_clear(uint32_t port, uint16_t pin)
void gpio_setup_input(uint32_t port, uint16_t pin) void gpio_setup_input(uint32_t port, uint16_t pin)
{ {
if (port != 0x32524) { return; } // protect ardrone board from unauthorized use if (port != 0x32524) { return; } /* protect ardrone board from unauthorized use */
struct gpio_direction dir; struct gpio_direction dir;
// Open the device if not open /* Open the device if not yet opened*/
if (gpiofp == 0) { if (gpiofp == 0) {
gpiofp = open("/dev/gpio", O_RDWR); gpiofp = open("/dev/gpio", O_RDWR);
} }
// Read the GPIO value /* Read the GPIO value */
dir.pin = pin; dir.pin = pin;
dir.mode = GPIO_INPUT; dir.mode = GPIO_INPUT;
ioctl(gpiofp, GPIO_DIRECTION, &dir); ioctl(gpiofp, GPIO_DIRECTION, &dir);
@@ -119,14 +119,14 @@ void gpio_setup_output(uint32_t port, uint16_t pin)
uint16_t gpio_get(uint32_t port, uint16_t pin) uint16_t gpio_get(uint32_t port, uint16_t pin)
{ {
if (port != 0x32524) { return 0; } // protect ardrone board from unauthorized use if (port != 0x32524) { return 0; } /* protect ARDroneX board from unauthorized use */
struct gpio_data data; struct gpio_data data;
// Open the device if not open /* Open the device if not open */
if (gpiofp == 0) { if (gpiofp == 0) {
gpiofp = open("/dev/gpio", O_RDWR); gpiofp = open("/dev/gpio", O_RDWR);
} }
// Read the GPIO value /* Read the GPIO value */
data.pin = pin; data.pin = pin;
ioctl(gpiofp, GPIO_READ, &data); ioctl(gpiofp, GPIO_READ, &data);
return data.value; return data.value;
sw/airborne/boards/ardrone/navdata.c
+78 -76
View File
@@ -1,37 +1,39 @@
/* /*
* Copyright (C) 2013 Dino Hensen, Vincent van Hoek * Copyright (C) 2016 The Paparazzi Team
* 2015 Freek van Tienen <freek.v.tienen@gmail.com>
* *
* This file is part of Paparazzi. * This file is part of paparazzi.
* *
* Paparazzi is free software; you can redistribute it and/or modify * paparazzi is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by * it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2, or (at your option) * the Free Software Foundation; either version 2, or (at your option)
* any later version. * any later version.
* *
* Paparazzi is distributed in the hope that it will be useful, * paparazzi is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of * but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details. * GNU General Public License for more details.
* *
* You should have received a copy of the GNU General Public License * You should have received a copy of the GNU General Public License
* along with Paparazzi; see the file COPYING. If not, write to * along with paparazzi; see the file COPYING. If not, see
* the Free Software Foundation, 59 Temple Place - Suite 330, * <http://www.gnu.org/licenses/>.
* Boston, MA 02111-1307, USA.
*/ */
/* Thanks to TU Delft by assigning students Dino Hensen, Vincent van Hoek for
* their inital work and later improvements made in 2015 by Freek van Tienen
*/
/** /**
* @file boards/ardrone/navdata.c * @file boards/ardrone/navdata.c
* ardrone2 navdata aquisition driver. * ARDrone2 navdata aquisition driver.
* *
* The ardrone2 provides a navdata stream of packets * The ADRrone2 provides a navdata stream of packets
* containing info about all sensors at a rate of 200Hz. * containing info about all sensors at a rate of 200Hz.
*/ */
#include <stdio.h> #include <stdio.h>
#include <stdlib.h> #include <stdlib.h>
#include <fcntl.h> #include <fcntl.h>
#include <termios.h> // for baud rates and options #include <termios.h> /* for baud rates and options */
#include <unistd.h> #include <unistd.h>
#include <string.h> #include <string.h>
#include <math.h> #include <math.h>
@@ -170,28 +172,28 @@ bool_t navdata_init()
{ {
assert(sizeof(struct navdata_measure_t) == NAVDATA_PACKET_SIZE); assert(sizeof(struct navdata_measure_t) == NAVDATA_PACKET_SIZE);
// Check if the FD isn't already initialized /* Check if the FD isn't already initialized */
if (navdata.fd <= 0) { if (navdata.fd <= 0) {
navdata.fd = open("/dev/ttyO1", O_RDWR | O_NOCTTY); //O_NONBLOCK doesn't work navdata.fd = open("/dev/ttyO1", O_RDWR | O_NOCTTY); /* O_NONBLOCK doesn't work */
if (navdata.fd < 0) { if (navdata.fd < 0) {
printf("[navdata] Unable to open navdata board connection(/dev/ttyO1)\n"); printf("[navdata] Unable to open navdata board connection(/dev/ttyO1)\n");
return FALSE; return FALSE;
} }
// Update the settings of the UART connection /* Update the settings of the UART connection */
fcntl(navdata.fd, F_SETFL, 0); //read calls are non blocking fcntl(navdata.fd, F_SETFL, 0); /* read calls are non blocking */
//set port options /* set port options */
struct termios options; struct termios options;
//Get the current options for the port /* Get the current options for the port */
tcgetattr(navdata.fd, &options); tcgetattr(navdata.fd, &options);
//Set the baud rates to 460800 /* Set the baud rates to 460800 */
cfsetispeed(&options, B460800); cfsetispeed(&options, B460800);
cfsetospeed(&options, B460800); cfsetospeed(&options, B460800);
options.c_cflag |= (CLOCAL | CREAD); //Enable the receiver and set local mode options.c_cflag |= (CLOCAL | CREAD); /* Enable the receiver and set local mode */
options.c_iflag = 0; //clear input options options.c_iflag = 0; /* clear input options */
options.c_lflag = 0; //clear local options options.c_lflag = 0; /* clear local options */
options.c_oflag &= ~OPOST; //clear output options (raw output) options.c_oflag &= ~OPOST; //clear output options (raw output)
//Set the new options for the port //Set the new options for the port
@@ -211,24 +213,24 @@ bool_t navdata_init()
navdata.packetsRead = 0; navdata.packetsRead = 0;
navdata.last_packet_number = 0; navdata.last_packet_number = 0;
// Stop acquisition /* Stop acquisition */
navdata_cmd_send(NAVDATA_CMD_STOP); navdata_cmd_send(NAVDATA_CMD_STOP);
// Read the baro calibration(blocking) /* Read the baro calibration(blocking) */
if (!navdata_baro_calib()) { if (!navdata_baro_calib()) {
printf("[navdata] Could not acquire baro calibration!\n"); printf("[navdata] Could not acquire baro calibration!\n");
return FALSE; return FALSE;
} }
navdata.baro_calibrated = TRUE; navdata.baro_calibrated = TRUE;
// Start acquisition /* Start acquisition */
navdata_cmd_send(NAVDATA_CMD_START); navdata_cmd_send(NAVDATA_CMD_START);
// Set navboard gpio control /* Set navboard gpio control */
gpio_setup_output(ARDRONE_GPIO_PORT, ARDRONE_GPIO_PIN_NAVDATA); gpio_setup_output(ARDRONE_GPIO_PORT, ARDRONE_GPIO_PIN_NAVDATA);
gpio_set(ARDRONE_GPIO_PORT, ARDRONE_GPIO_PIN_NAVDATA); gpio_set(ARDRONE_GPIO_PORT, ARDRONE_GPIO_PIN_NAVDATA);
// Start navdata reading thread /* Start navdata reading thread */
pthread_t navdata_thread; pthread_t navdata_thread;
if (pthread_create(&navdata_thread, NULL, navdata_read, NULL) != 0) { if (pthread_create(&navdata_thread, NULL, navdata_read, NULL) != 0) {
printf("[navdata] Could not create navdata reading thread!\n"); printf("[navdata] Could not create navdata reading thread!\n");
@@ -239,7 +241,7 @@ bool_t navdata_init()
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_ARDRONE_NAVDATA, send_navdata); register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_ARDRONE_NAVDATA, send_navdata);
#endif #endif
// Set to initialized /* Set to initialized */
navdata.is_initialized = TRUE; navdata.is_initialized = TRUE;
return TRUE; return TRUE;
} }
@@ -256,30 +258,30 @@ static void *navdata_read(void *data __attribute__((unused)))
while (TRUE) { while (TRUE) {
// Wait until we are notified to read next data, /* Wait until we are notified to read next data,
// i.e. buffer has been copied in navdata_update i.e. buffer has been copied in navdata_update */
pthread_mutex_lock(&navdata_mutex); pthread_mutex_lock(&navdata_mutex);
while (navdata_available) { while (navdata_available) {
pthread_cond_wait(&navdata_cond, &navdata_mutex); pthread_cond_wait(&navdata_cond, &navdata_mutex);
} }
pthread_mutex_unlock(&navdata_mutex); pthread_mutex_unlock(&navdata_mutex);
// Read new bytes /* Read new bytes */
int newbytes = read(navdata.fd, navdata_buffer + buffer_idx, NAVDATA_PACKET_SIZE - buffer_idx); int newbytes = read(navdata.fd, navdata_buffer + buffer_idx, NAVDATA_PACKET_SIZE - buffer_idx);
// When there was no signal interrupt /* When there was no signal interrupt */
if (newbytes > 0) { if (newbytes > 0) {
buffer_idx += newbytes; buffer_idx += newbytes;
navdata.totalBytesRead += newbytes; navdata.totalBytesRead += newbytes;
} }
// If we got a full packet /* If we got a full packet */
if (buffer_idx >= NAVDATA_PACKET_SIZE) { if (buffer_idx >= NAVDATA_PACKET_SIZE) {
// check if the start byte is correct /* check if the start byte is correct */
if (navdata_buffer[0] != NAVDATA_START_BYTE) { if (navdata_buffer[0] != NAVDATA_START_BYTE) {
uint8_t *pint = memchr(navdata_buffer, NAVDATA_START_BYTE, buffer_idx); uint8_t *pint = memchr(navdata_buffer, NAVDATA_START_BYTE, buffer_idx);
// Check if we found the start byte in the read data /* Check if we found the start byte in the read data */
if (pint != NULL) { if (pint != NULL) {
memmove(navdata_buffer, pint, NAVDATA_PACKET_SIZE - (pint - navdata_buffer)); memmove(navdata_buffer, pint, NAVDATA_PACKET_SIZE - (pint - navdata_buffer));
buffer_idx = pint - navdata_buffer; buffer_idx = pint - navdata_buffer;
@@ -293,7 +295,7 @@ static void *navdata_read(void *data __attribute__((unused)))
/* full packet read with startbyte at the beginning, reset insert index */ /* full packet read with startbyte at the beginning, reset insert index */
buffer_idx = 0; buffer_idx = 0;
// Calculating the checksum /* Calculate the checksum */
uint16_t checksum = 0; uint16_t checksum = 0;
for (int i = 2; i < NAVDATA_PACKET_SIZE - 2; i += 2) { for (int i = 2; i < NAVDATA_PACKET_SIZE - 2; i += 2) {
checksum += navdata_buffer[i] + (navdata_buffer[i + 1] << 8); checksum += navdata_buffer[i] + (navdata_buffer[i + 1] << 8);
@@ -301,7 +303,7 @@ static void *navdata_read(void *data __attribute__((unused)))
struct navdata_measure_t *new_measurement = (struct navdata_measure_t *)navdata_buffer; struct navdata_measure_t *new_measurement = (struct navdata_measure_t *)navdata_buffer;
// Check if the checksum is ok /* Check if the checksum is OK */
if (new_measurement->chksum != checksum) { if (new_measurement->chksum != checksum) {
fprintf(stderr, "[navdata] Checksum error [calculated: %d] [packet: %d] [diff: %d]\n", fprintf(stderr, "[navdata] Checksum error [calculated: %d] [packet: %d] [diff: %d]\n",
checksum, new_measurement->chksum, checksum - new_measurement->chksum); checksum, new_measurement->chksum, checksum - new_measurement->chksum);
@@ -309,7 +311,7 @@ static void *navdata_read(void *data __attribute__((unused)))
continue; continue;
} }
// set flag that we have new valid navdata /* Set flag that we have new valid navdata */
pthread_mutex_lock(&navdata_mutex); pthread_mutex_lock(&navdata_mutex);
navdata_available = TRUE; navdata_available = TRUE;
pthread_mutex_unlock(&navdata_mutex); pthread_mutex_unlock(&navdata_mutex);
@@ -339,7 +341,7 @@ static void navdata_publish_imu(void)
*/ */
void navdata_update() void navdata_update()
{ {
// Check if initialized /* Check if initialized */
if (!navdata.is_initialized) { if (!navdata.is_initialized) {
navdata_init(); navdata_init();
mag_freeze_check(); mag_freeze_check();
@@ -347,19 +349,19 @@ void navdata_update()
} }
pthread_mutex_lock(&navdata_mutex); pthread_mutex_lock(&navdata_mutex);
// If we got a new navdata packet /* If we got a new navdata packet */
if (navdata_available) { if (navdata_available) {
// Copy the navdata packet /* Copy the navdata packet */
memcpy(&navdata.measure, navdata_buffer, NAVDATA_PACKET_SIZE); memcpy(&navdata.measure, navdata_buffer, NAVDATA_PACKET_SIZE);
// reset the flag /* reset the flag */
navdata_available = FALSE; navdata_available = FALSE;
// signal that we copied the buffer and new packet can be read /* signal that we copied the buffer and new packet can be read */
pthread_cond_signal(&navdata_cond); pthread_cond_signal(&navdata_cond);
pthread_mutex_unlock(&navdata_mutex); pthread_mutex_unlock(&navdata_mutex);
// Check if we missed a packet (our counter and the one from the navdata) /* Check if we missed a packet (our counter and the one from the navdata) */
navdata.last_packet_number++; navdata.last_packet_number++;
if (navdata.last_packet_number != navdata.measure.nu_trame) { if (navdata.last_packet_number != navdata.measure.nu_trame) {
fprintf(stderr, "[navdata] Lost frame: %d should have been %d\n", fprintf(stderr, "[navdata] Lost frame: %d should have been %d\n",
@@ -368,7 +370,7 @@ void navdata_update()
} }
navdata.last_packet_number = navdata.measure.nu_trame; navdata.last_packet_number = navdata.measure.nu_trame;
// Invert byte order so that TELEMETRY works better /* Invert byte order so that TELEMETRY works better */
uint8_t tmp; uint8_t tmp;
uint8_t *p = (uint8_t *) & (navdata.measure.pressure); uint8_t *p = (uint8_t *) & (navdata.measure.pressure);
tmp = p[0]; tmp = p[0];
@@ -383,7 +385,7 @@ void navdata_update()
mag_freeze_check(); mag_freeze_check();
#ifdef USE_SONAR #ifdef USE_SONAR
// Check if there is a new sonar measurement and update the sonar /* Check if there is a new sonar measurement and update the sonar */
if (navdata.measure.ultrasound >> 15) { if (navdata.measure.ultrasound >> 15) {
float sonar_meas = (float)((navdata.measure.ultrasound & 0x7FFF) - SONAR_OFFSET) * SONAR_SCALE; float sonar_meas = (float)((navdata.measure.ultrasound & 0x7FFF) - SONAR_OFFSET) * SONAR_SCALE;
AbiSendMsgAGL(AGL_SONAR_ARDRONE2_ID, sonar_meas); AbiSendMsgAGL(AGL_SONAR_ARDRONE2_ID, sonar_meas);
@@ -394,7 +396,7 @@ void navdata_update()
navdata.packetsRead++; navdata.packetsRead++;
} else { } else {
// no new packet available, still unlock mutex again /* no new packet available, still unlock mutex again */
pthread_mutex_unlock(&navdata_mutex); pthread_mutex_unlock(&navdata_mutex);
} }
} }
@@ -413,17 +415,17 @@ static void navdata_cmd_send(uint8_t cmd)
*/ */
static bool_t navdata_baro_calib(void) static bool_t navdata_baro_calib(void)
{ {
// Start baro calibration acquisition /* Start baro calibration acquisition */
navdata_cmd_send(NAVDATA_CMD_BARO_CALIB); navdata_cmd_send(NAVDATA_CMD_BARO_CALIB);
// Receive the calibration (blocking) /* Receive the calibration (blocking) */
uint8_t calibBuffer[22]; uint8_t calibBuffer[22];
if (full_read(navdata.fd, calibBuffer, sizeof calibBuffer) < 0) { if (full_read(navdata.fd, calibBuffer, sizeof calibBuffer) < 0) {
printf("[navdata] Could not read calibration data."); printf("[navdata] Could not read calibration data.");
return FALSE; return FALSE;
} }
//Convert the read bytes /* Convert the read bytes */
navdata.bmp180_calib.ac1 = calibBuffer[0] << 8 | calibBuffer[1]; navdata.bmp180_calib.ac1 = calibBuffer[0] << 8 | calibBuffer[1];
navdata.bmp180_calib.ac2 = calibBuffer[2] << 8 | calibBuffer[3]; navdata.bmp180_calib.ac2 = calibBuffer[2] << 8 | calibBuffer[3];
navdata.bmp180_calib.ac3 = calibBuffer[4] << 8 | calibBuffer[5]; navdata.bmp180_calib.ac3 = calibBuffer[4] << 8 | calibBuffer[5];
@@ -445,42 +447,42 @@ static bool_t navdata_baro_calib(void)
*/ */
static void mag_freeze_check(void) static void mag_freeze_check(void)
{ {
// Thanks to Daren.G.Lee for initial fix on 20140530 /* Thanks to Daren.G.Lee for initial fix on 20140530 */
static int16_t LastMagValue = 0; static int16_t LastMagValue = 0;
static int MagFreezeCounter = 0; static int MagFreezeCounter = 0;
if (LastMagValue == navdata.measure.mx) { if (LastMagValue == navdata.measure.mx) {
MagFreezeCounter++; MagFreezeCounter++;
// has to have at least 30 times the same value to consider it a frozen magnetometer value /* Has to have at least 30 times the exact same value to consider it a frozen magnetometer value */
if (MagFreezeCounter > 30) { if (MagFreezeCounter > 30) {
fprintf(stderr, "mag freeze detected, resetting!\n"); fprintf(stderr, "mag freeze detected, resetting!\n");
// set imu_lost flag /* set imu_lost flag */
navdata.imu_lost = TRUE; navdata.imu_lost = TRUE;
navdata.lost_imu_frames++; navdata.lost_imu_frames++;
// Stop acquisition /* Stop acquisition */
navdata_cmd_send(NAVDATA_CMD_STOP); navdata_cmd_send(NAVDATA_CMD_STOP);
// Reset the hardware of the navboard /* Reset the hardware of the navboard */
gpio_clear(ARDRONE_GPIO_PORT, ARDRONE_GPIO_PIN_NAVDATA); gpio_clear(ARDRONE_GPIO_PORT, ARDRONE_GPIO_PIN_NAVDATA);
usleep(20000); usleep(20000);
gpio_set(ARDRONE_GPIO_PORT, ARDRONE_GPIO_PIN_NAVDATA); gpio_set(ARDRONE_GPIO_PORT, ARDRONE_GPIO_PIN_NAVDATA);
// Wait for 40ms for it to boot /* Wait for 40ms for it to boot */
usleep(40000); usleep(40000);
// Start the navdata again and reset the counter /* Start the navdata again and reset the counter */
navdata_cmd_send(NAVDATA_CMD_START); navdata_cmd_send(NAVDATA_CMD_START);
MagFreezeCounter = 0; // reset counter back to zero MagFreezeCounter = 0; /* reset counter back to zero */
} }
} else { } else {
navdata.imu_lost = FALSE; navdata.imu_lost = FALSE;
// Reset counter if value _does_ change /* Reset counter if value _does_ change */
MagFreezeCounter = 0; MagFreezeCounter = 0;
} }
// set last value /* set last value */
LastMagValue = navdata.measure.mx; LastMagValue = navdata.measure.mx;
} }
@@ -496,52 +498,52 @@ static void baro_update_logic(void)
static int32_t lastpressval_nospike = 0; static int32_t lastpressval_nospike = 0;
static uint16_t lasttempval_nospike = 0; static uint16_t lasttempval_nospike = 0;
static uint8_t temp_or_press_was_updated_last = static uint8_t temp_or_press_was_updated_last =
0; // 0 = press, so we now wait for temp, 1 = temp so we now wait for press 0; /* 0 = press, so we now wait for temp, 1 = temp so we now wait for press */
static int sync_errors = 0; static int sync_errors = 0;
static int spike_detected = 0; static int spike_detected = 0;
if (temp_or_press_was_updated_last == 0) { // Last update was press so we are now waiting for temp if (temp_or_press_was_updated_last == 0) { /* Last update was press so we are now waiting for temp */
// temp was updated /* temp was updated */
temp_or_press_was_updated_last = TRUE; temp_or_press_was_updated_last = TRUE;
// This means that press must remain constant /* This means that press must remain constant */
if (lastpressval != 0) { if (lastpressval != 0) {
// If pressure was updated: this is a sync error /* If pressure was updated: this is a sync error */
if (lastpressval != navdata.measure.pressure) { if (lastpressval != navdata.measure.pressure) {
// wait for temp again /* wait for temp again */
temp_or_press_was_updated_last = FALSE; temp_or_press_was_updated_last = FALSE;
sync_errors++; sync_errors++;
//printf("Baro-Logic-Error (expected updated temp, got press)\n"); //printf("Baro-Logic-Error (expected updated temp, got press)\n");
} }
} }
} else { } else {
// press was updated /* press was updated */
temp_or_press_was_updated_last = FALSE; temp_or_press_was_updated_last = FALSE;
// This means that temp must remain constant /* This means that temp must remain constant */
if (lasttempval != 0) { if (lasttempval != 0) {
// If temp was updated: this is a sync error /* If temp was updated: this is a sync error */
if (lasttempval != navdata.measure.temperature_pressure) { if (lasttempval != navdata.measure.temperature_pressure) {
// wait for press again /* wait for press again */
temp_or_press_was_updated_last = TRUE; temp_or_press_was_updated_last = TRUE;
sync_errors++; sync_errors++;
//printf("Baro-Logic-Error (expected updated press, got temp)\n"); //printf("Baro-Logic-Error (expected updated press, got temp)\n");
} else { } else {
// We now got valid pressure and temperature /* We now got valid pressure and temperature */
navdata.baro_available = TRUE; navdata.baro_available = TRUE;
} }
} }
} }
// Detected a pressure switch /* Detected a pressure swap */
if (lastpressval != 0 && lasttempval != 0 if (lastpressval != 0 && lasttempval != 0
&& ABS(lastpressval - navdata.measure.pressure) > ABS(lasttempval - navdata.measure.pressure)) { && ABS(lastpressval - navdata.measure.pressure) > ABS(lasttempval - navdata.measure.pressure)) {
navdata.baro_available = FALSE; navdata.baro_available = FALSE;
} }
// Detected a temprature switch /* Detected a temprature swap */
if (lastpressval != 0 && lasttempval != 0 if (lastpressval != 0 && lasttempval != 0
&& ABS(lasttempval - navdata.measure.temperature_pressure) > ABS(lastpressval - navdata.measure.temperature_pressure)) { && ABS(lasttempval - navdata.measure.temperature_pressure) > ABS(lastpressval - navdata.measure.temperature_pressure)) {
navdata.baro_available = FALSE; navdata.baro_available = FALSE;
@@ -589,22 +591,22 @@ static void baro_update_logic(void)
*/ */
// if press and temp are same and temp has jump: neglect the next frame /* If press and temp are same and temp has jump: neglect the next frame */
if (navdata.measure.temperature_pressure == if (navdata.measure.temperature_pressure ==
navdata.measure.pressure) { // && (abs((int32_t)navdata.temperature_pressure - (int32_t)lasttempval) > 40)) navdata.measure.pressure) { // && (abs((int32_t)navdata.temperature_pressure - (int32_t)lasttempval) > 40))
// dont use next 3 packets /* dont use the next 3 packets */
spike_detected = 3; spike_detected = 3;
} }
if (spike_detected > 0) { if (spike_detected > 0) {
// disable kalman filter use /* disable kalman filter use */
navdata.baro_available = FALSE; navdata.baro_available = FALSE;
// override both to last good // override both to last good
navdata.measure.pressure = lastpressval_nospike; navdata.measure.pressure = lastpressval_nospike;
navdata.measure.temperature_pressure = lasttempval_nospike; navdata.measure.temperature_pressure = lasttempval_nospike;
// Countdown /* Countdown */
spike_detected--; spike_detected--;
} else { // both are good } else { // both are good
lastpressval_nospike = navdata.measure.pressure; lastpressval_nospike = navdata.measure.pressure;
sw/airborne/boards/ardrone/navdata.h
+11 -8
View File
@@ -1,24 +1,25 @@
/* /*
* Copyright (C) 2013 Dino Hensen, Vincent van Hoek * Copyright (C) 2016 The Paparazzi Team
* *
* This file is part of Paparazzi. * This file is part of paparazzi.
* *
* Paparazzi is free software; you can redistribute it and/or modify * paparazzi is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by * it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2, or (at your option) * the Free Software Foundation; either version 2, or (at your option)
* any later version. * any later version.
* *
* Paparazzi is distributed in the hope that it will be useful, * paparazzi is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of * but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details. * GNU General Public License for more details.
* *
* You should have received a copy of the GNU General Public License * You should have received a copy of the GNU General Public License
* along with Paparazzi; see the file COPYING. If not, write to * along with paparazzi; see the file COPYING. If not, see
* the Free Software Foundation, 59 Temple Place - Suite 330, * <http://www.gnu.org/licenses/>.
* Boston, MA 02111-1307, USA.
*/ */
/* Thanks to TU Delft by assigning students Dino Hensen, Vincent van Hoek */
/** /**
* @file boards/ardrone/navdata.h * @file boards/ardrone/navdata.h
* ardrone2 navdata aquisition driver. * ardrone2 navdata aquisition driver.
@@ -27,6 +28,8 @@
* containing info about all sensors at a rate of 200Hz. * containing info about all sensors at a rate of 200Hz.
*/ */
#ifndef NAVDATA_H_ #ifndef NAVDATA_H_
#define NAVDATA_H_ #define NAVDATA_H_
@@ -132,7 +135,7 @@ bool_t navdata_init(void);
void navdata_update(void); void navdata_update(void);
int16_t navdata_height(void); int16_t navdata_height(void);
/* This should be moved to the uart handling part! */ /* FIXME: This should be moved to the uart handling part! */
ssize_t full_write(int fd, const uint8_t *buf, size_t count); ssize_t full_write(int fd, const uint8_t *buf, size_t count);
ssize_t full_read(int fd, uint8_t *buf, size_t count); ssize_t full_read(int fd, uint8_t *buf, size_t count);
sw/airborne/mcu.c
+6 -3
View File
@@ -65,18 +65,21 @@
void WEAK board_init(void) void WEAK board_init(void)
{ {
// default board init function does nothing... /* default board init function does nothing... */
} }
void mcu_init(void) void mcu_init(void)
{ {
mcu_arch_init();
/* If we have a board specific init function, call it. /* If we have a board specific init function, call it.
* Otherwise it will simply call the empty weak function. * Otherwise it will simply call the empty weak function.
*
* For example the ARDrone2 has this implemented to prevent stray data of IMU
* from OEM program still running and also accessing AC sensors
*/ */
board_init(); board_init();
mcu_arch_init();
#ifdef PERIPHERALS_AUTO_INIT #ifdef PERIPHERALS_AUTO_INIT
sys_time_init(); sys_time_init();
#ifdef USE_LED #ifdef USE_LED
sw/tools/parrot/ardrone2.py
+11 -8
View File
@@ -163,7 +163,7 @@ def ardrone2_status():
print('Version:\t\t' + parrot_utils.check_version(tn, '/firmware')) print('Version:\t\t' + parrot_utils.check_version(tn, '/firmware'))
print('Host:\t\t\t' + args.host + ' (' + read_from_config('static_ip_address_base', config_ini) + print('Host:\t\t\t' + args.host + ' (' + read_from_config('static_ip_address_base', config_ini) +
read_from_config('static_ip_address_probe', config_ini) + ' after boot)') read_from_config('static_ip_address_probe', config_ini) + ' after boot)')
print('Currently running:\t' + parrot_utils.check_running(tn))
print('Serial number:\t\t' + read_from_config('drone_serial', config_ini)) print('Serial number:\t\t' + read_from_config('drone_serial', config_ini))
print('Network id:\t\t' + read_from_config('ssid_single_player', config_ini)) print('Network id:\t\t' + read_from_config('ssid_single_player', config_ini))
print('Motor software:\t\t' + print('Motor software:\t\t' +
@@ -172,7 +172,9 @@ def ardrone2_status():
print('Motor hardware:\t\t' + print('Motor hardware:\t\t' +
read_from_config('motor1_hard', config_ini) + '\t' + read_from_config('motor2_hard', config_ini) + '\t' + read_from_config('motor1_hard', config_ini) + '\t' + read_from_config('motor2_hard', config_ini) + '\t' +
read_from_config('motor3_hard', config_ini) + '\t' + read_from_config('motor4_hard', config_ini)) read_from_config('motor3_hard', config_ini) + '\t' + read_from_config('motor4_hard', config_ini))
sleep(2.0) #Wait running process reporting back lag
print('Currently running:\t' + parrot_utils.check_running(tn))
autorun = {'': 'Native', '0': 'Native', '1': 'Paparazzi'} autorun = {'': 'Native', '0': 'Native', '1': 'Paparazzi'}
if check_autoboot(): if check_autoboot():
print('Autorun at start:\tInstalled booting ' + autorun[read_from_config('start_paparazzi', config_ini)]) print('Autorun at start:\tInstalled booting ' + autorun[read_from_config('start_paparazzi', config_ini)])
@@ -260,7 +262,7 @@ elif args.command == 'reboot':
# Kill a program # Kill a program
elif args.command == 'kill': elif args.command == 'kill':
parrot_utils.execute_command(tn,'killall -9 ' + args.program) parrot_utils.execute_command(tn,'killall -9 ' + args.program + ' &')
print('Program "' + args.program + '" is now killed') print('Program "' + args.program + '" is now killed')
# Start a program # Start a program
@@ -400,15 +402,16 @@ elif args.command == 'upload_file_and_run':
f = parrot_utils.split_into_path_and_file(args.file) f = parrot_utils.split_into_path_and_file(args.file)
print("Kill running " + f[1] + " and make folder " + args.folder) print("Kill running " + f[1] + " and make folder " + args.folder)
parrot_utils.execute_command(tn,"killall -9 " + f[1]) parrot_utils.execute_command(tn,"killall -9 " + f[1] + ' &')
sleep(1) sleep(1)
parrot_utils.execute_command(tn, "mkdir -p /data/video/" + args.folder) parrot_utils.execute_command(tn, "mkdir -p /data/video/" + args.folder)
print('Uploading \'' + f[1] + "\' from " + f[0] + " to " + args.folder) print('Uploading \'' + f[1] + "\' from " + f[0] + " to " + args.folder)
print("#pragma message: Please wait, uploading can take some time...")
parrot_utils.uploadfile(ftp, args.folder + "/" + f[1], file(args.file, "rb")) parrot_utils.uploadfile(ftp, args.folder + "/" + f[1], file(args.file, "rb"))
sleep(0.5) sleep(0.5)
parrot_utils.execute_command(tn, "chmod 777 /data/video/" + args.folder + "/" + f[1]) parrot_utils.execute_command(tn, "chmod 777 /data/video/" + args.folder + "/" + f[1])
parrot_utils.execute_command(tn, "/data/video/" + args.folder + "/" + f[1] + " > /dev/null 2>&1 &") parrot_utils.execute_command(tn, "/data/video/" + args.folder + "/" + f[1] + " > /dev/null 2>&1 &")
print("#pragma message: Upload and Start of ap.elf to ARDrone2 Succes!") print("#pragma message: Upload to, and start of Autopilot on, ARDrone2 successful !")
elif args.command == 'upload_file': elif args.command == 'upload_file':
# Split filename and path # Split filename and path
@@ -417,7 +420,7 @@ elif args.command == 'upload_file':
parrot_utils.execute_command(tn,"mkdir -p /data/video/" + args.folder) parrot_utils.execute_command(tn,"mkdir -p /data/video/" + args.folder)
print('Uploading \'' + f[1] + "\' from " + f[0] + " to /data/video/" + args.folder) print('Uploading \'' + f[1] + "\' from " + f[0] + " to /data/video/" + args.folder)
parrot_utils.uploadfile(ftp, args.folder + "/" + f[1], file(args.file, "rb")) parrot_utils.uploadfile(ftp, args.folder + "/" + f[1], file(args.file, "rb"))
print("#pragma message: Upload of " + f[1] + " to ARDrone2 Succes!") print("#pragma message: Upload of " + f[1] + " to ARDrone2 successful !")
elif args.command == 'download_file': elif args.command == 'download_file':
# Split filename and path # Split filename and path
@@ -427,7 +430,7 @@ elif args.command == 'download_file':
file = open(args.file, 'wb') file = open(args.file, 'wb')
print('Downloading \'' + f[1] + "\' from " + args.folder + " to " + f[0]) print('Downloading \'' + f[1] + "\' from " + args.folder + " to " + f[0])
ftp.retrbinary("RETR " + args.folder + "/" + f[1], file.write) ftp.retrbinary("RETR " + args.folder + "/" + f[1], file.write)
print("#pragma message: Download of " + f[1] + " from ARDrone2 Succes!") print("#pragma message: Download of " + f[1] + " from ARDrone2 successful !")
except IOError: except IOError:
print("#pragma message: Fail to open file " + args.file) print("#pragma message: Fail to open file " + args.file)
except: except:
@@ -462,7 +465,7 @@ elif args.command == 'download_dir':
elif args.command == 'rm_dir': elif args.command == 'rm_dir':
# Split filename and path # Split filename and path
print("Deleting folder /data/video/" + args.folder + " from ARDrone2") print("Deleting folder /data/video/" + args.folder + " from ARDrone2...")
print(parrot_utils.execute_command(tn, 'rm -r /data/video/' + args.folder)) print(parrot_utils.execute_command(tn, 'rm -r /data/video/' + args.folder))
sw/tools/parrot/parrot_utils.py
+3 -2
View File
@@ -55,12 +55,13 @@ def check_running(tn):
ps_aux = execute_command(tn, 'ps') ps_aux = execute_command(tn, 'ps')
running = "" running = ""
if 'program.elf' in ps_aux:
running += ' Native (program.elf),'
if 'dragon-prog' in ps_aux: if 'dragon-prog' in ps_aux:
running += ' Native (dragon-prog),' running += ' Native (dragon-prog),'
if 'ap.elf' in ps_aux: if 'ap.elf' in ps_aux:
running += ' Paparazzi (ap.elf),' running += ' Paparazzi (ap.elf),'
if 'program.elf' in ps_aux:
running += ' Native (program.elf),'
if 'gst-launch' in ps_aux: if 'gst-launch' in ps_aux:
running += ' GStreamer (gst-launch)' running += ' GStreamer (gst-launch)'
return running[1:] return running[1:]