mirror of
https://github.com/paparazzi/paparazzi.git
synced 2026-06-04 13:55:40 +08:00
Fix upload and start experience of PPRZ on ARDrone2
This commit is contained in:
Open UAS
Developer
@@ -2,7 +2,7 @@
|
||||
#
|
||||
# ardrone2.makefile
|
||||
#
|
||||
# http://wiki.paparazziuav.org/wiki/AR.Drone_2_-_Specifications
|
||||
# http://wiki.paparazziuav.org/wiki/AR_Drone_2
|
||||
#
|
||||
|
||||
BOARD=ardrone
|
||||
|
||||
@@ -1,5 +1,5 @@
|
||||
/*
|
||||
* Copyright (C) 2015 The Paparazzi Team
|
||||
* Copyright (C) 2016 The Paparazzi Team
|
||||
*
|
||||
* This file is part of paparazzi.
|
||||
*
|
||||
@@ -26,12 +26,15 @@
|
||||
*/
|
||||
|
||||
#include <stdlib.h>
|
||||
#include <stdio.h>
|
||||
#include <string.h>
|
||||
#include <unistd.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"
|
||||
#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
|
||||
|
||||
#include "peripherals/video_device.h"
|
||||
@@ -56,10 +59,49 @@ struct video_config_t bottom_camera = {
|
||||
.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)
|
||||
{
|
||||
// First we try to kill the program.elf and its respawner if it is running
|
||||
int ret = system("killall -9 program.elf.respawner.sh; killall -9 program.elf");
|
||||
/* Two process to kill, the respawner first to make sure program.elf does not get restarted */
|
||||
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;
|
||||
}
|
||||
|
||||
@@ -53,14 +53,14 @@ struct gpio_direction {
|
||||
|
||||
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;
|
||||
// Open the device if not open
|
||||
if (gpiofp == 0) {
|
||||
gpiofp = open("/dev/gpio", O_RDWR);
|
||||
}
|
||||
|
||||
// Read the GPIO value
|
||||
/* Read the GPIO value */
|
||||
data.pin = pin;
|
||||
data.value = 1;
|
||||
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)
|
||||
{
|
||||
if (port != 0x32524) { return; } // protect ardrone board from unauthorized use
|
||||
if (port != 0x32524) { return; } /* protect ardrone board from unauthorized use */
|
||||
struct gpio_data data;
|
||||
// Open the device if not open
|
||||
if (gpiofp == 0) {
|
||||
gpiofp = open("/dev/gpio", O_RDWR);
|
||||
}
|
||||
|
||||
// Read the GPIO value
|
||||
/* Read the GPIO value */
|
||||
data.pin = pin;
|
||||
data.value = 0;
|
||||
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)
|
||||
{
|
||||
if (port != 0x32524) { return; } // protect ardrone board from unauthorized use
|
||||
if (port != 0x32524) { return; } /* protect ardrone board from unauthorized use */
|
||||
struct gpio_direction dir;
|
||||
// Open the device if not open
|
||||
/* Open the device if not yet opened*/
|
||||
if (gpiofp == 0) {
|
||||
gpiofp = open("/dev/gpio", O_RDWR);
|
||||
}
|
||||
|
||||
// Read the GPIO value
|
||||
/* Read the GPIO value */
|
||||
dir.pin = pin;
|
||||
dir.mode = GPIO_INPUT;
|
||||
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)
|
||||
{
|
||||
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;
|
||||
// Open the device if not open
|
||||
/* Open the device if not open */
|
||||
if (gpiofp == 0) {
|
||||
gpiofp = open("/dev/gpio", O_RDWR);
|
||||
}
|
||||
|
||||
// Read the GPIO value
|
||||
/* Read the GPIO value */
|
||||
data.pin = pin;
|
||||
ioctl(gpiofp, GPIO_READ, &data);
|
||||
return data.value;
|
||||
|
||||
@@ -1,37 +1,39 @@
|
||||
/*
|
||||
* Copyright (C) 2013 Dino Hensen, Vincent van Hoek
|
||||
* 2015 Freek van Tienen <freek.v.tienen@gmail.com>
|
||||
* 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
|
||||
* the Free Software Foundation; either version 2, or (at your option)
|
||||
* 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
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
* GNU General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License
|
||||
* along with Paparazzi; see the file COPYING. If not, write to
|
||||
* the Free Software Foundation, 59 Temple Place - Suite 330,
|
||||
* Boston, MA 02111-1307, USA.
|
||||
* along with paparazzi; see the file COPYING. If not, see
|
||||
* <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
/* 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
|
||||
* 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.
|
||||
*/
|
||||
|
||||
#include <stdio.h>
|
||||
#include <stdlib.h>
|
||||
#include <fcntl.h>
|
||||
#include <termios.h> // for baud rates and options
|
||||
#include <termios.h> /* for baud rates and options */
|
||||
#include <unistd.h>
|
||||
#include <string.h>
|
||||
#include <math.h>
|
||||
@@ -170,28 +172,28 @@ bool_t navdata_init()
|
||||
{
|
||||
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) {
|
||||
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) {
|
||||
printf("[navdata] Unable to open navdata board connection(/dev/ttyO1)\n");
|
||||
return FALSE;
|
||||
}
|
||||
|
||||
// Update the settings of the UART connection
|
||||
fcntl(navdata.fd, F_SETFL, 0); //read calls are non blocking
|
||||
//set port options
|
||||
/* Update the settings of the UART connection */
|
||||
fcntl(navdata.fd, F_SETFL, 0); /* read calls are non blocking */
|
||||
/* set port options */
|
||||
struct termios options;
|
||||
//Get the current options for the port
|
||||
/* Get the current options for the port */
|
||||
tcgetattr(navdata.fd, &options);
|
||||
//Set the baud rates to 460800
|
||||
/* Set the baud rates to 460800 */
|
||||
cfsetispeed(&options, B460800);
|
||||
cfsetospeed(&options, B460800);
|
||||
|
||||
options.c_cflag |= (CLOCAL | CREAD); //Enable the receiver and set local mode
|
||||
options.c_iflag = 0; //clear input options
|
||||
options.c_lflag = 0; //clear local options
|
||||
options.c_cflag |= (CLOCAL | CREAD); /* Enable the receiver and set local mode */
|
||||
options.c_iflag = 0; /* clear input options */
|
||||
options.c_lflag = 0; /* clear local options */
|
||||
options.c_oflag &= ~OPOST; //clear output options (raw output)
|
||||
|
||||
//Set the new options for the port
|
||||
@@ -211,24 +213,24 @@ bool_t navdata_init()
|
||||
navdata.packetsRead = 0;
|
||||
navdata.last_packet_number = 0;
|
||||
|
||||
// Stop acquisition
|
||||
/* Stop acquisition */
|
||||
navdata_cmd_send(NAVDATA_CMD_STOP);
|
||||
|
||||
// Read the baro calibration(blocking)
|
||||
/* Read the baro calibration(blocking) */
|
||||
if (!navdata_baro_calib()) {
|
||||
printf("[navdata] Could not acquire baro calibration!\n");
|
||||
return FALSE;
|
||||
}
|
||||
navdata.baro_calibrated = TRUE;
|
||||
|
||||
// Start acquisition
|
||||
/* Start acquisition */
|
||||
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_set(ARDRONE_GPIO_PORT, ARDRONE_GPIO_PIN_NAVDATA);
|
||||
|
||||
// Start navdata reading thread
|
||||
/* Start navdata reading thread */
|
||||
pthread_t navdata_thread;
|
||||
if (pthread_create(&navdata_thread, NULL, navdata_read, NULL) != 0) {
|
||||
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);
|
||||
#endif
|
||||
|
||||
// Set to initialized
|
||||
/* Set to initialized */
|
||||
navdata.is_initialized = TRUE;
|
||||
return TRUE;
|
||||
}
|
||||
@@ -256,30 +258,30 @@ static void *navdata_read(void *data __attribute__((unused)))
|
||||
|
||||
while (TRUE) {
|
||||
|
||||
// Wait until we are notified to read next data,
|
||||
// i.e. buffer has been copied in navdata_update
|
||||
/* Wait until we are notified to read next data,
|
||||
i.e. buffer has been copied in navdata_update */
|
||||
pthread_mutex_lock(&navdata_mutex);
|
||||
while (navdata_available) {
|
||||
pthread_cond_wait(&navdata_cond, &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);
|
||||
|
||||
// When there was no signal interrupt
|
||||
/* When there was no signal interrupt */
|
||||
if (newbytes > 0) {
|
||||
buffer_idx += newbytes;
|
||||
navdata.totalBytesRead += newbytes;
|
||||
}
|
||||
|
||||
// If we got a full packet
|
||||
/* If we got a full packet */
|
||||
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) {
|
||||
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) {
|
||||
memmove(navdata_buffer, pint, NAVDATA_PACKET_SIZE - (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 */
|
||||
buffer_idx = 0;
|
||||
|
||||
// Calculating the checksum
|
||||
/* Calculate the checksum */
|
||||
uint16_t checksum = 0;
|
||||
for (int i = 2; i < NAVDATA_PACKET_SIZE - 2; i += 2) {
|
||||
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;
|
||||
|
||||
// Check if the checksum is ok
|
||||
/* Check if the checksum is OK */
|
||||
if (new_measurement->chksum != checksum) {
|
||||
fprintf(stderr, "[navdata] Checksum error [calculated: %d] [packet: %d] [diff: %d]\n",
|
||||
checksum, new_measurement->chksum, checksum - new_measurement->chksum);
|
||||
@@ -309,7 +311,7 @@ static void *navdata_read(void *data __attribute__((unused)))
|
||||
continue;
|
||||
}
|
||||
|
||||
// set flag that we have new valid navdata
|
||||
/* Set flag that we have new valid navdata */
|
||||
pthread_mutex_lock(&navdata_mutex);
|
||||
navdata_available = TRUE;
|
||||
pthread_mutex_unlock(&navdata_mutex);
|
||||
@@ -339,7 +341,7 @@ static void navdata_publish_imu(void)
|
||||
*/
|
||||
void navdata_update()
|
||||
{
|
||||
// Check if initialized
|
||||
/* Check if initialized */
|
||||
if (!navdata.is_initialized) {
|
||||
navdata_init();
|
||||
mag_freeze_check();
|
||||
@@ -347,19 +349,19 @@ void navdata_update()
|
||||
}
|
||||
|
||||
pthread_mutex_lock(&navdata_mutex);
|
||||
// If we got a new navdata packet
|
||||
/* If we got a new navdata packet */
|
||||
if (navdata_available) {
|
||||
|
||||
// Copy the navdata packet
|
||||
/* Copy the navdata packet */
|
||||
memcpy(&navdata.measure, navdata_buffer, NAVDATA_PACKET_SIZE);
|
||||
|
||||
// reset the flag
|
||||
/* reset the flag */
|
||||
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_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++;
|
||||
if (navdata.last_packet_number != navdata.measure.nu_trame) {
|
||||
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;
|
||||
|
||||
// Invert byte order so that TELEMETRY works better
|
||||
/* Invert byte order so that TELEMETRY works better */
|
||||
uint8_t tmp;
|
||||
uint8_t *p = (uint8_t *) & (navdata.measure.pressure);
|
||||
tmp = p[0];
|
||||
@@ -383,7 +385,7 @@ void navdata_update()
|
||||
mag_freeze_check();
|
||||
|
||||
#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) {
|
||||
float sonar_meas = (float)((navdata.measure.ultrasound & 0x7FFF) - SONAR_OFFSET) * SONAR_SCALE;
|
||||
AbiSendMsgAGL(AGL_SONAR_ARDRONE2_ID, sonar_meas);
|
||||
@@ -394,7 +396,7 @@ void navdata_update()
|
||||
|
||||
navdata.packetsRead++;
|
||||
} else {
|
||||
// no new packet available, still unlock mutex again
|
||||
/* no new packet available, still unlock mutex again */
|
||||
pthread_mutex_unlock(&navdata_mutex);
|
||||
}
|
||||
}
|
||||
@@ -413,17 +415,17 @@ static void navdata_cmd_send(uint8_t cmd)
|
||||
*/
|
||||
static bool_t navdata_baro_calib(void)
|
||||
{
|
||||
// Start baro calibration acquisition
|
||||
/* Start baro calibration acquisition */
|
||||
navdata_cmd_send(NAVDATA_CMD_BARO_CALIB);
|
||||
|
||||
// Receive the calibration (blocking)
|
||||
/* Receive the calibration (blocking) */
|
||||
uint8_t calibBuffer[22];
|
||||
if (full_read(navdata.fd, calibBuffer, sizeof calibBuffer) < 0) {
|
||||
printf("[navdata] Could not read calibration data.");
|
||||
return FALSE;
|
||||
}
|
||||
|
||||
//Convert the read bytes
|
||||
/* Convert the read bytes */
|
||||
navdata.bmp180_calib.ac1 = calibBuffer[0] << 8 | calibBuffer[1];
|
||||
navdata.bmp180_calib.ac2 = calibBuffer[2] << 8 | calibBuffer[3];
|
||||
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)
|
||||
{
|
||||
// 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 int MagFreezeCounter = 0;
|
||||
|
||||
if (LastMagValue == navdata.measure.mx) {
|
||||
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) {
|
||||
fprintf(stderr, "mag freeze detected, resetting!\n");
|
||||
|
||||
// set imu_lost flag
|
||||
/* set imu_lost flag */
|
||||
navdata.imu_lost = TRUE;
|
||||
navdata.lost_imu_frames++;
|
||||
|
||||
// Stop acquisition
|
||||
/* Stop acquisition */
|
||||
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);
|
||||
usleep(20000);
|
||||
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);
|
||||
|
||||
// Start the navdata again and reset the counter
|
||||
/* Start the navdata again and reset the counter */
|
||||
navdata_cmd_send(NAVDATA_CMD_START);
|
||||
MagFreezeCounter = 0; // reset counter back to zero
|
||||
MagFreezeCounter = 0; /* reset counter back to zero */
|
||||
}
|
||||
} else {
|
||||
navdata.imu_lost = FALSE;
|
||||
// Reset counter if value _does_ change
|
||||
/* Reset counter if value _does_ change */
|
||||
MagFreezeCounter = 0;
|
||||
}
|
||||
// set last value
|
||||
/* set last value */
|
||||
LastMagValue = navdata.measure.mx;
|
||||
}
|
||||
|
||||
@@ -496,52 +498,52 @@ static void baro_update_logic(void)
|
||||
static int32_t lastpressval_nospike = 0;
|
||||
static uint16_t lasttempval_nospike = 0;
|
||||
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 spike_detected = 0;
|
||||
|
||||
if (temp_or_press_was_updated_last == 0) { // Last update was press so we are now waiting for temp
|
||||
// temp was updated
|
||||
if (temp_or_press_was_updated_last == 0) { /* Last update was press so we are now waiting for temp */
|
||||
/* temp was updated */
|
||||
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 pressure was updated: this is a sync error
|
||||
/* If pressure was updated: this is a sync error */
|
||||
if (lastpressval != navdata.measure.pressure) {
|
||||
// wait for temp again
|
||||
/* wait for temp again */
|
||||
temp_or_press_was_updated_last = FALSE;
|
||||
sync_errors++;
|
||||
//printf("Baro-Logic-Error (expected updated temp, got press)\n");
|
||||
}
|
||||
}
|
||||
} else {
|
||||
// press was updated
|
||||
/* press was updated */
|
||||
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 temp was updated: this is a sync error
|
||||
/* If temp was updated: this is a sync error */
|
||||
if (lasttempval != navdata.measure.temperature_pressure) {
|
||||
// wait for press again
|
||||
/* wait for press again */
|
||||
temp_or_press_was_updated_last = TRUE;
|
||||
sync_errors++;
|
||||
//printf("Baro-Logic-Error (expected updated press, got temp)\n");
|
||||
|
||||
} else {
|
||||
// We now got valid pressure and temperature
|
||||
/* We now got valid pressure and temperature */
|
||||
navdata.baro_available = TRUE;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// Detected a pressure switch
|
||||
/* Detected a pressure swap */
|
||||
if (lastpressval != 0 && lasttempval != 0
|
||||
&& ABS(lastpressval - navdata.measure.pressure) > ABS(lasttempval - navdata.measure.pressure)) {
|
||||
navdata.baro_available = FALSE;
|
||||
}
|
||||
|
||||
// Detected a temprature switch
|
||||
/* Detected a temprature swap */
|
||||
if (lastpressval != 0 && lasttempval != 0
|
||||
&& ABS(lasttempval - navdata.measure.temperature_pressure) > ABS(lastpressval - navdata.measure.temperature_pressure)) {
|
||||
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 ==
|
||||
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;
|
||||
}
|
||||
|
||||
if (spike_detected > 0) {
|
||||
// disable kalman filter use
|
||||
/* disable kalman filter use */
|
||||
navdata.baro_available = FALSE;
|
||||
|
||||
// override both to last good
|
||||
navdata.measure.pressure = lastpressval_nospike;
|
||||
navdata.measure.temperature_pressure = lasttempval_nospike;
|
||||
|
||||
// Countdown
|
||||
/* Countdown */
|
||||
spike_detected--;
|
||||
} else { // both are good
|
||||
lastpressval_nospike = navdata.measure.pressure;
|
||||
|
||||
@@ -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
|
||||
* the Free Software Foundation; either version 2, or (at your option)
|
||||
* 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
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
* GNU General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License
|
||||
* along with Paparazzi; see the file COPYING. If not, write to
|
||||
* the Free Software Foundation, 59 Temple Place - Suite 330,
|
||||
* Boston, MA 02111-1307, USA.
|
||||
* along with paparazzi; see the file COPYING. If not, see
|
||||
* <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
/* Thanks to TU Delft by assigning students Dino Hensen, Vincent van Hoek */
|
||||
|
||||
/**
|
||||
* @file boards/ardrone/navdata.h
|
||||
* ardrone2 navdata aquisition driver.
|
||||
@@ -27,6 +28,8 @@
|
||||
* containing info about all sensors at a rate of 200Hz.
|
||||
*/
|
||||
|
||||
|
||||
|
||||
#ifndef NAVDATA_H_
|
||||
#define NAVDATA_H_
|
||||
|
||||
@@ -132,7 +135,7 @@ bool_t navdata_init(void);
|
||||
void navdata_update(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_read(int fd, uint8_t *buf, size_t count);
|
||||
|
||||
|
||||
+6
-3
@@ -65,18 +65,21 @@
|
||||
|
||||
void WEAK board_init(void)
|
||||
{
|
||||
// default board init function does nothing...
|
||||
/* default board init function does nothing... */
|
||||
}
|
||||
|
||||
void mcu_init(void)
|
||||
{
|
||||
|
||||
mcu_arch_init();
|
||||
/* If we have a board specific init function, call it.
|
||||
* 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();
|
||||
|
||||
mcu_arch_init();
|
||||
|
||||
#ifdef PERIPHERALS_AUTO_INIT
|
||||
sys_time_init();
|
||||
#ifdef USE_LED
|
||||
|
||||
@@ -163,7 +163,7 @@ def ardrone2_status():
|
||||
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) +
|
||||
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('Network id:\t\t' + read_from_config('ssid_single_player', config_ini))
|
||||
print('Motor software:\t\t' +
|
||||
@@ -173,6 +173,8 @@ def ardrone2_status():
|
||||
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))
|
||||
|
||||
sleep(2.0) #Wait running process reporting back lag
|
||||
print('Currently running:\t' + parrot_utils.check_running(tn))
|
||||
autorun = {'': 'Native', '0': 'Native', '1': 'Paparazzi'}
|
||||
if check_autoboot():
|
||||
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
|
||||
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')
|
||||
|
||||
# Start a program
|
||||
@@ -400,15 +402,16 @@ elif args.command == 'upload_file_and_run':
|
||||
f = parrot_utils.split_into_path_and_file(args.file)
|
||||
|
||||
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)
|
||||
parrot_utils.execute_command(tn, "mkdir -p /data/video/" + 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"))
|
||||
sleep(0.5)
|
||||
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 &")
|
||||
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':
|
||||
# Split filename and path
|
||||
@@ -417,7 +420,7 @@ elif args.command == 'upload_file':
|
||||
parrot_utils.execute_command(tn,"mkdir -p /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"))
|
||||
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':
|
||||
# Split filename and path
|
||||
@@ -427,7 +430,7 @@ elif args.command == 'download_file':
|
||||
file = open(args.file, 'wb')
|
||||
print('Downloading \'' + f[1] + "\' from " + args.folder + " to " + f[0])
|
||||
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:
|
||||
print("#pragma message: Fail to open file " + args.file)
|
||||
except:
|
||||
@@ -462,7 +465,7 @@ elif args.command == 'download_dir':
|
||||
|
||||
elif args.command == 'rm_dir':
|
||||
# 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))
|
||||
|
||||
|
||||
|
||||
@@ -55,12 +55,13 @@ def check_running(tn):
|
||||
ps_aux = execute_command(tn, 'ps')
|
||||
running = ""
|
||||
|
||||
if 'program.elf' in ps_aux:
|
||||
running += ' Native (program.elf),'
|
||||
|
||||
if 'dragon-prog' in ps_aux:
|
||||
running += ' Native (dragon-prog),'
|
||||
if 'ap.elf' in ps_aux:
|
||||
running += ' Paparazzi (ap.elf),'
|
||||
if 'program.elf' in ps_aux:
|
||||
running += ' Native (program.elf),'
|
||||
if 'gst-launch' in ps_aux:
|
||||
running += ' GStreamer (gst-launch)'
|
||||
return running[1:]
|
||||
|
||||
Reference in New Issue
Block a user