Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-05-23 13:24:03 +08:00
Code Issues Actions 26 Packages Projects Releases Wiki Activity

[ardrone] rewrite navdata_read

Browse Source
This commit is contained in:
Felix Ruess
2015-02-27 13:38:27 +01:00
parent 973372733d
commit a0afeb5d50
2 changed files with 60 additions and 40 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sw/airborne/boards/ardrone/navdata.c
+59 -38
View File
@@ -194,6 +194,7 @@ bool_t navdata_init()
}
// Reset available flags
navdata.packet_available = FALSE;
navdata.baro_calibrated = FALSE;
navdata.imu_available = FALSE;
navdata.baro_available = FALSE;
@@ -225,7 +226,7 @@ bool_t navdata_init()
// Start navdata reading 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");
return FALSE;
}
@@ -277,19 +278,64 @@ static inline bool_t navdata_baro_calib(void)
*/
static void *navdata_read(void *data __attribute__((unused)))
{
while(TRUE) {
/* Buffer insert index for reading/writing */
static uint8_t buffer_idx = 0;
while (TRUE) {
// Check if we need to read something or already have a packet
if((NAVDATA_PACKET_SIZE - navdata.buffer_idx) <= 0)
if (!navdata.packet_available) {
continue;
}
// Read new bytes
int newbytes = read(navdata.fd, navdata.buffer + navdata.buffer_idx, NAVDATA_PACKET_SIZE - navdata.buffer_idx);
int newbytes = read(navdata.fd, navdata.buffer + buffer_idx, NAVDATA_PACKET_SIZE - buffer_idx);
// When there was no signal interrupt
if (newbytes > 0) {
navdata.buffer_idx += newbytes;
buffer_idx += newbytes;
navdata.totalBytesRead += newbytes;
}
// If we got a full packet
if (buffer_idx >= NAVDATA_PACKET_SIZE) {
// 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
if (pint != NULL) {
memmove(navdata.buffer, pint, NAVDATA_PACKET_SIZE - (pint - navdata.buffer));
buffer_idx = pint - navdata.buffer;
} else {
buffer_idx = 0;
}
fprintf(stderr, "[navdata] sync error, startbyte not found, resetting...\n");
continue;
}
/* full packet read with startbyte at the beginning, reset insert index */
buffer_idx = 0;
// Calculating 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);
}
struct navdata_measure_t *new_measurement = (struct navdata_measure_t *)navdata.buffer;
// 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);
navdata.checksum_errors++;
continue;
}
// set flag that we have new valid navdata
navdata.packet_available = TRUE;
}
}
return NULL;
@@ -482,45 +528,20 @@ void navdata_update()
return;
}
// If we got a full packet
if (navdata.buffer_idx >= NAVDATA_PACKET_SIZE) {
// check if the start byte is correct
if (navdata.buffer[0] != NAVDATA_START_BYTE) {
uint8_t *pint = memchr(navdata.buffer, NAVDATA_START_BYTE, navdata.buffer_idx);
// Check if we found the start byte in the read data
if(pint != NULL) {
memmove(navdata.buffer, pint, NAVDATA_PACKET_SIZE - (pint - navdata.buffer));
navdata.buffer_idx = pint - navdata.buffer;
}
else {
navdata.buffer_idx = 0;
}
return;
}
// If we got a new navdata packet
if (navdata.packet_available) {
// Copy the navdata packet
memcpy(&navdata.measure, navdata.buffer, NAVDATA_PACKET_SIZE);
// Calculating 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);
}
// Check if the checksum is ok
if (navdata.measure.chksum != checksum) {
printf("[navdata] Checksum error [calculated: %d] [packet: %d] [diff: %d]\n", checksum , navdata.measure.chksum,
checksum - navdata.measure.chksum);
navdata.checksum_errors++;
navdata.buffer_idx = 0;
return;
}
// reset the flag
navdata.packet_available = FALSE;
// 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) {
printf("[navdata] Lost frame: %d should have been %d\n", navdata.measure.nu_trame, navdata.last_packet_number);
fprintf(stderr, "[navdata] Lost frame: %d should have been %d\n",
navdata.measure.nu_trame, navdata.last_packet_number);
navdata.lost_imu_frames++;
}
navdata.last_packet_number = navdata.measure.nu_trame;
@@ -549,7 +570,6 @@ void navdata_update()
navdata.imu_available = TRUE;
navdata.packetsRead++;
navdata.buffer_idx = 0;
}
}
@@ -557,6 +577,7 @@ void navdata_update()
/**
* Sends a one byte command
*/
static void navdata_cmd_send(uint8_t cmd) {
static void navdata_cmd_send(uint8_t cmd)
{
full_write(navdata.fd, &cmd, 1);
}
sw/airborne/boards/ardrone/navdata.h
+1 -2
View File
@@ -118,9 +118,8 @@ struct navdata_t {
uint32_t lost_imu_frames;
uint16_t last_packet_number;
volatile uint8_t buffer_idx; //< Buffer insert index for reading/writing
uint8_t buffer[NAVDATA_PACKET_SIZE]; //< Buffer filled in the thread (maximum one navdata packet)
volatile bool_t packet_available; //< new valid packet is available in buffer
struct navdata_measure_t measure; //< Main navdata packet receieved from navboard
struct bmp180_calib_t bmp180_calib; //< BMP180 calibration receieved from navboard