Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-06-04 05:42:49 +08:00
Code Issues Actions 25 Packages Projects Releases Wiki Activity

[ArDrone] Navdata decoder update (incl sonar)

Browse Source
This commit is contained in:
Christophe De Wagter
2013-09-22 15:34:43 +02:00
parent f652bae7e9
commit 70ab7ebfdf
6 changed files with 162 additions and 151 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/messages.xml
+6 -1
View File
@@ -48,6 +48,7 @@
<field name="my" type="int16" /> <field name="my" type="int16" />
<field name="mz" type="int16" /> <field name="mz" type="int16" />
<field name="chksum" type="uint16" /> <field name="chksum" type="uint16" />
<field name="checksum_errors" type="uint32" />
</message> </message>
<message name="ATTITUDE" id="6"> <message name="ATTITUDE" id="6">
@@ -626,7 +627,11 @@
<field name="GX3_chksm" type="uint16"/> <field name="GX3_chksm" type="uint16"/>
</message> </message>
<!-- 74 is free --> <message name="EXPLAIN" id="74">
<field name="type" type="uint8" values="NAME|SETTING|WAYPOINT|BLOCK|IMAV2013"/>
<field name="id" type="uint8"/>
<field name="string" type="uint8[]"/>
</message>
<message name="VIDEO_TELEMETRY" id="75"> <message name="VIDEO_TELEMETRY" id="75">
<field name="blob_x1" type="int32" unit="pixels"/> <field name="blob_x1" type="int32" unit="pixels"/>
sw/airborne/boards/ardrone/electrical_raw.c
+2 -1
View File
@@ -69,7 +69,8 @@ int fd;
void electrical_init(void) { void electrical_init(void) {
// First we try to kill the program.elf if it is running (done here because initializes first) // First we try to kill the program.elf if it is running (done here because initializes first)
system("killall -9 program.elf"); int ret = system("killall -9 program.elf");
(void) ret;
// Initialize 12c device for power // Initialize 12c device for power
fd = open( "/dev/i2c-1", O_RDWR ); fd = open( "/dev/i2c-1", O_RDWR );
sw/airborne/boards/ardrone/navdata.c
+107 -124
View File
@@ -36,26 +36,26 @@
#include <math.h> #include <math.h>
#include <errno.h> #include <errno.h>
#include <assert.h> #include <assert.h>
#include "std.h"
#include "navdata.h" #include "navdata.h"
#include "subsystems/ins.h"
#define NAVDATA_PACKET_SIZE 60 #define NAVDATA_PACKET_SIZE 60
#define NAVDATA_BUFFER_SIZE 80
#define NAVDATA_START_BYTE 0x3a #define NAVDATA_START_BYTE 0x3a
typedef struct { static inline bool_t acquire_baro_calibration(void);
uint8_t isInitialized; static void navdata_cropbuffer(int cropsize);
uint8_t isOpen;
uint16_t bytesRead;
uint32_t totalBytesRead;
uint32_t packetsRead;
uint8_t buffer[NAVDATA_BUFFER_SIZE];
} navdata_port;
static navdata_port port;
static int nav_fd;
navdata_port nav_port;
static int nav_fd = 0;
static int16_t previousUltrasoundHeight;
measures_t navdata; measures_t navdata;
#include "subsystems/sonar.h"
uint16_t sonar_meas = 0;
// FIXME(ben): there must be a better home for these // FIXME(ben): there must be a better home for these
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)
{ {
@@ -100,15 +100,15 @@ static void navdata_write(const uint8_t *buf, size_t count)
perror("navdata_write: Write failed"); perror("navdata_write: Write failed");
} }
int navdata_init() bool_t navdata_init()
{ {
if (nav_fd <= 0) {
nav_fd = open("/dev/ttyO1", O_RDWR | O_NOCTTY | O_NONBLOCK); nav_fd = open("/dev/ttyO1", O_RDWR | O_NOCTTY | O_NONBLOCK);
if (nav_fd == -1)
{ if (nav_fd == -1) {
perror("navdata_init: Unable to open /dev/ttyO1 - "); perror("navdata_init: Unable to open /dev/ttyO1 - ");
return 1; return FALSE;
} else { }
port.isOpen = 1;
} }
fcntl(nav_fd, F_SETFL, 0); //read calls are non blocking fcntl(nav_fd, F_SETFL, 0); //read calls are non blocking
@@ -132,31 +132,40 @@ int navdata_init()
uint8_t cmd=0x02; uint8_t cmd=0x02;
navdata_write(&cmd, 1); navdata_write(&cmd, 1);
baro_calibrated = 0; // read some potential dirt (retry alot of times)
acquire_baro_calibration(); char tmp[100];
for(int i = 0; i < 100; i++) {
uint16_t dirt = read(nav_fd, tmp, sizeof tmp);
(void) dirt;
cmd=0x02;
navdata_write(&cmd, 1);
usleep(200);
}
baro_calibrated = FALSE;
if(!acquire_baro_calibration())
return FALSE;
// start acquisition // start acquisition
cmd=0x01; cmd = 0x01;
navdata_write(&cmd, 1); navdata_write(&cmd, 1);
navdata_imu_available = 0; navdata_imu_available = FALSE;
navdata_baro_available = 0; navdata_baro_available = FALSE;
port.bytesRead = 0;
port.totalBytesRead = 0;
port.packetsRead = 0;
port.isInitialized = 1;
previousUltrasoundHeight = 0; previousUltrasoundHeight = 0;
nav_port.checksum_errors = 0;
nav_port.bytesRead = 0;
nav_port.totalBytesRead = 0;
nav_port.packetsRead = 0;
nav_port.isInitialized = TRUE;
return 0; return TRUE;
} }
void acquire_baro_calibration() static inline bool_t acquire_baro_calibration(void)
{ {
char tmp[100];
read(nav_fd, tmp, sizeof tmp); // read some potential dirt
// start baro calibration acquisition // start baro calibration acquisition
uint8_t cmd=0x17; // send cmd 23 uint8_t cmd=0x17; // send cmd 23
navdata_write(&cmd, 1); navdata_write(&cmd, 1);
@@ -166,7 +175,7 @@ void acquire_baro_calibration()
if (full_read(nav_fd, calibBuffer, sizeof calibBuffer) < 0) if (full_read(nav_fd, calibBuffer, sizeof calibBuffer) < 0)
{ {
perror("acquire_baro_calibration: read failed"); perror("acquire_baro_calibration: read failed");
return; return FALSE;
} }
baro_calibration.ac1 = calibBuffer[0] << 8 | calibBuffer[1]; baro_calibration.ac1 = calibBuffer[0] << 8 | calibBuffer[1];
@@ -195,32 +204,19 @@ void acquire_baro_calibration()
printf("Calibration MC: %d\n", baro_calibration.mc); printf("Calibration MC: %d\n", baro_calibration.mc);
printf("Calibration MD: %d\n", baro_calibration.md); printf("Calibration MD: %d\n", baro_calibration.md);
baro_calibrated = 1; baro_calibrated = TRUE;
} return TRUE;
void navdata_close()
{
port.isOpen = 0;
close(nav_fd);
} }
void navdata_read() void navdata_read()
{ {
int newbytes = 0; int newbytes = read(nav_fd, nav_port.buffer+nav_port.bytesRead, NAVDATA_BUFFER_SIZE-nav_port.bytesRead);
if (port.isInitialized != 1)
navdata_init();
if (port.isOpen != 1)
return;
newbytes = read(nav_fd, port.buffer+port.bytesRead, NAVDATA_BUFFER_SIZE-port.bytesRead);
// because non-blocking read returns -1 when no bytes available // because non-blocking read returns -1 when no bytes available
if (newbytes > 0) if (newbytes > 0)
{ {
port.bytesRead += newbytes; nav_port.bytesRead += newbytes;
port.totalBytesRead += newbytes; nav_port.totalBytesRead += newbytes;
} }
} }
@@ -235,7 +231,7 @@ static void baro_update_logic(void)
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 = 1; 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)
@@ -244,16 +240,16 @@ static void baro_update_logic(void)
if (lastpressval != navdata.pressure) if (lastpressval != navdata.pressure)
{ {
// wait for temp again // wait for temp again
temp_or_press_was_updated_last = 0; temp_or_press_was_updated_last = FALSE;
sync_errors++; sync_errors++;
navdata_baro_available = 1; navdata_baro_available = TRUE;
} }
} }
} }
else else
{ {
// press was updated // press was updated
temp_or_press_was_updated_last = 0; 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)
@@ -262,37 +258,52 @@ static void baro_update_logic(void)
if (lasttempval != navdata.temperature_pressure) if (lasttempval != navdata.temperature_pressure)
{ {
// wait for press again // wait for press again
temp_or_press_was_updated_last = 1; temp_or_press_was_updated_last = TRUE;
sync_errors++; sync_errors++;
} }
} }
navdata_baro_available = 1; navdata_baro_available = TRUE;
} }
lastpressval = navdata.pressure; lastpressval = navdata.pressure;
lasttempval = navdata.temperature_pressure; lasttempval = navdata.temperature_pressure;
// debug
// navdata->temperature_pressure = sync_errors;
} }
void navdata_update() void navdata_update()
{ {
static bool_t last_checksum_wrong = FALSE;
// Check if initialized
if (!nav_port.isInitialized) {
navdata_init();
return;
}
// Start reading
navdata_read(); navdata_read();
// while there is something interesting to do... // while there is something interesting to do...
while (port.bytesRead >= NAVDATA_PACKET_SIZE) while (nav_port.bytesRead >= NAVDATA_PACKET_SIZE)
{ {
if (port.buffer[0] == NAVDATA_START_BYTE) if (nav_port.buffer[0] == NAVDATA_START_BYTE)
{
// if checksum is OK
if ( 1 ) // we dont know how to calculate the checksum
// if ( navdata_checksum() == 0 )
{ {
assert(sizeof navdata == NAVDATA_PACKET_SIZE); assert(sizeof navdata == NAVDATA_PACKET_SIZE);
memcpy(&navdata, port.buffer, NAVDATA_PACKET_SIZE); memcpy(&navdata, nav_port.buffer, NAVDATA_PACKET_SIZE);
// Calculating the checksum
uint16_t checksum = 0;
for(int i = 2; i < NAVDATA_PACKET_SIZE-2; i += 2) {
checksum += nav_port.buffer[i] + (nav_port.buffer[i+1] << 8);
}
// When checksum is incorrect
if(navdata.chksum != checksum) {
printf("Checksum error [calculated: %d] [packet: %d] [diff: %d]\n",checksum , navdata.chksum, checksum-navdata.chksum);
nav_port.checksum_errors++;
}
// When we already dropped a packet or checksum is correct
if(last_checksum_wrong || navdata.chksum == checksum) {
// 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.pressure); uint8_t* p = (uint8_t*) &(navdata.pressure);
@@ -306,87 +317,59 @@ void navdata_update()
baro_update_logic(); baro_update_logic();
navdata_imu_available = 1; #ifdef USE_SONAR
if (navdata.ultrasound < 10000)
{
sonar_meas = navdata.ultrasound;
ins_update_sonar();
port.packetsRead++;
// printf("CCRC=%d, GCRC=%d, error=%d\n", crc, navdata->chksum, abs(crc-navdata->chksum));
//navdata_getHeight();
} }
navdata_CropBuffer(NAVDATA_PACKET_SIZE); #endif
navdata_imu_available = TRUE;
last_checksum_wrong = FALSE;
nav_port.packetsRead++;
}
// Crop the buffer
navdata_cropbuffer(NAVDATA_PACKET_SIZE);
} }
else else
{ {
// find start byte, copy all data from startbyte to buffer origin, update bytesread // find start byte, copy all data from startbyte to buffer origin, update bytesread
uint8_t * pint; uint8_t * pint;
pint = memchr(port.buffer, NAVDATA_START_BYTE, port.bytesRead); pint = memchr(nav_port.buffer, NAVDATA_START_BYTE, nav_port.bytesRead);
if (pint != NULL) { if (pint != NULL) {
navdata_CropBuffer(pint - port.buffer); navdata_cropbuffer(pint - nav_port.buffer);
} else { } else {
// if the start byte was not found, it means there is junk in the buffer // if the start byte was not found, it means there is junk in the buffer
port.bytesRead = 0; nav_port.bytesRead = 0;
} }
} }
} }
} }
void navdata_CropBuffer(int cropsize) int16_t navdata_height(void) {
{
if (port.bytesRead - cropsize < 0) {
// TODO think about why the amount of bytes read minus the cropsize gets below zero
printf("BytesRead(=%d) - Cropsize(=%d) may not be below zero. port->buffer=%p\n", port.bytesRead, cropsize, port.buffer);
return;
}
memmove(port.buffer, port.buffer+cropsize, NAVDATA_BUFFER_SIZE-cropsize);
port.bytesRead -= cropsize;
}
int16_t navdata_getHeight() {
if (navdata.ultrasound > 10000) { if (navdata.ultrasound > 10000) {
return previousUltrasoundHeight; return previousUltrasoundHeight;
} }
int16_t ultrasoundHeight = 0; int16_t ultrasoundHeight = 0;
ultrasoundHeight = (navdata.ultrasound - 880) / 26.553; ultrasoundHeight = (navdata.ultrasound - 880) / 26.553;
previousUltrasoundHeight = ultrasoundHeight; previousUltrasoundHeight = ultrasoundHeight;
return ultrasoundHeight; return ultrasoundHeight;
} }
// The checksum should be calculated here: we don't know the algorithm static void navdata_cropbuffer(int cropsize)
uint16_t navdata_checksum() { {
navdata_cks = 0; if (nav_port.bytesRead - cropsize < 0) {
navdata_cks += navdata.nu_trame; // TODO think about why the amount of bytes read minus the cropsize gets below zero
navdata_cks += navdata.ax; printf("BytesRead(=%d) - Cropsize(=%d) may not be below zero. port->buffer=%p\n", nav_port.bytesRead, cropsize, nav_port.buffer);
navdata_cks += navdata.ay; return;
navdata_cks += navdata.az; }
navdata_cks += navdata.vx;
navdata_cks += navdata.vy;
navdata_cks += navdata.vz;
navdata_cks += navdata.temperature_acc;
navdata_cks += navdata.temperature_gyro;
navdata_cks += navdata.ultrasound;
navdata_cks += navdata.us_debut_echo;
navdata_cks += navdata.us_fin_echo;
navdata_cks += navdata.us_association_echo;
navdata_cks += navdata.us_distance_echo;
navdata_cks += navdata.us_curve_time;
navdata_cks += navdata.us_curve_value;
navdata_cks += navdata.us_curve_ref;
navdata_cks += navdata.nb_echo;
navdata_cks += navdata.sum_echo;
navdata_cks += navdata.gradient;
navdata_cks += navdata.flag_echo_ini;
navdata_cks += navdata.pressure;
navdata_cks += navdata.temperature_pressure;
navdata_cks += navdata.mx;
navdata_cks += navdata.my;
navdata_cks += navdata.mz;
// navdata_cks += navdata->chksum;
return 0; // we dont know how to calculate the checksum memmove(nav_port.buffer, nav_port.buffer+cropsize, NAVDATA_BUFFER_SIZE-cropsize);
nav_port.bytesRead -= cropsize;
} }
sw/airborne/boards/ardrone/navdata.h
+14 -9
View File
@@ -95,25 +95,30 @@ struct bmp180_baro_calibration
int32_t b5; int32_t b5;
}; };
#define NAVDATA_BUFFER_SIZE 80
typedef struct {
uint8_t isInitialized;
uint16_t bytesRead;
uint32_t totalBytesRead;
uint32_t packetsRead;
uint32_t checksum_errors;
uint8_t buffer[NAVDATA_BUFFER_SIZE];
} navdata_port;
extern measures_t navdata; extern measures_t navdata;
extern navdata_port nav_port;
struct bmp180_baro_calibration baro_calibration; struct bmp180_baro_calibration baro_calibration;
navdata_port* port;
uint16_t navdata_cks; uint16_t navdata_cks;
uint8_t navdata_imu_available; uint8_t navdata_imu_available;
uint8_t navdata_baro_available; uint8_t navdata_baro_available;
int16_t previousUltrasoundHeight; int16_t previousUltrasoundHeight;
uint8_t baro_calibrated; uint8_t baro_calibrated;
int navdata_init(void); bool_t navdata_init(void);
void navdata_close(void);
void navdata_read(void); void navdata_read(void);
void navdata_update(void); void navdata_update(void);
void navdata_CropBuffer(int cropsize); int16_t navdata_height(void);
uint16_t navdata_checksum(void);
int16_t navdata_getHeight(void);
void acquire_baro_calibration(void);
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/firmwares/rotorcraft/telemetry.h
+26 -13
View File
@@ -991,7 +991,8 @@
&navdata.mx, \ &navdata.mx, \
&navdata.my, \ &navdata.my, \
&navdata.mz, \ &navdata.mz, \
&navdata.chksum \ &navdata.chksum, \
&nav_port.checksum_errors \
) )
#else #else
#define PERIODIC_SEND_ARDRONE_NAVDATA(_trans, _dev) {} #define PERIODIC_SEND_ARDRONE_NAVDATA(_trans, _dev) {}
@@ -1006,10 +1007,13 @@
#ifdef USE_UART1 #ifdef USE_UART1
#define PERIODIC_SEND_UART1_ERRORS(_trans, _dev) { \ #define PERIODIC_SEND_UART1_ERRORS(_trans, _dev) { \
const uint8_t _bus1 = 1; \ const uint8_t _bus1 = 1; \
uint16_t ore = uart1.ore; \
uint16_t ne_err = uart1.ne_err; \
uint16_t fe_err = uart1.fe_err; \
DOWNLINK_SEND_UART_ERRORS(_trans, _dev, \ DOWNLINK_SEND_UART_ERRORS(_trans, _dev, \
&uart1.ore, \ &ore, \
&uart1.ne_err, \ &ne_err, \
&uart1.fe_err, \ &fe_err, \
&_bus1); \ &_bus1); \
} }
#else #else
@@ -1019,10 +1023,13 @@
#ifdef USE_UART2 #ifdef USE_UART2
#define PERIODIC_SEND_UART2_ERRORS(_trans, _dev) { \ #define PERIODIC_SEND_UART2_ERRORS(_trans, _dev) { \
const uint8_t _bus2 = 2; \ const uint8_t _bus2 = 2; \
uint16_t ore = uart2.ore; \
uint16_t ne_err = uart2.ne_err; \
uint16_t fe_err = uart2.fe_err; \
DOWNLINK_SEND_UART_ERRORS(_trans, _dev, \ DOWNLINK_SEND_UART_ERRORS(_trans, _dev, \
&uart2.ore, \ &ore, \
&uart2.ne_err, \ &ne_err, \
&uart2.fe_err, \ &fe_err, \
&_bus2); \ &_bus2); \
} }
#else #else
@@ -1032,10 +1039,13 @@
#ifdef USE_UART3 #ifdef USE_UART3
#define PERIODIC_SEND_UART3_ERRORS(_trans, _dev) { \ #define PERIODIC_SEND_UART3_ERRORS(_trans, _dev) { \
const uint8_t _bus3 = 3; \ const uint8_t _bus3 = 3; \
uint16_t ore = uart3.ore; \
uint16_t ne_err = uart3.ne_err; \
uint16_t fe_err = uart3.fe_err; \
DOWNLINK_SEND_UART_ERRORS(_trans, _dev, \ DOWNLINK_SEND_UART_ERRORS(_trans, _dev, \
&uart3.ore, \ &ore, \
&uart3.ne_err, \ &ne_err, \
&uart3.fe_err, \ &fe_err, \
&_bus3); \ &_bus3); \
} }
#else #else
@@ -1045,10 +1055,13 @@
#ifdef USE_UART5 #ifdef USE_UART5
#define PERIODIC_SEND_UART5_ERRORS(_trans, _dev) { \ #define PERIODIC_SEND_UART5_ERRORS(_trans, _dev) { \
const uint8_t _bus5 = 5; \ const uint8_t _bus5 = 5; \
uint16_t ore = uart5.ore; \
uint16_t ne_err = uart5.ne_err; \
uint16_t fe_err = uart5.fe_err; \
DOWNLINK_SEND_UART_ERRORS(_trans, _dev, \ DOWNLINK_SEND_UART_ERRORS(_trans, _dev, \
&uart5.ore, \ &ore, \
&uart5.ne_err, \ &ne_err, \
&uart5.fe_err, \ &fe_err, \
&_bus5); \ &_bus5); \
} }
#else #else
sw/airborne/subsystems/sonar.h
+4
View File
@@ -0,0 +1,4 @@
extern uint16_t sonar_meas;