Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-06-04 22:17:01 +08:00
Code Issues Actions 30 Packages Projects Releases Wiki Activity

[ardrone] Barometer bmp180 implementation added.

Browse Source 
Also fixed a bug in the navdata driver, where cropping was done twice.
Baro still needs to be tested, because sometimes unexpected things happen.
This commit is contained in:
Dino Hensen
2013-08-16 00:33:17 +02:00
committed by Felix Ruess
parent 9500cdfae5
commit 8e84c8ecbd
5 changed files with 144 additions and 17 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/airframes/ardrone2_raw.xml
+1 -1
View File
@@ -117,7 +117,7 @@
</section>
<section name="INS" prefix="INS_">
<define name="BARO_SENS" value="22.3" integer="16" />
<define name="BARO_SENS" value="100." integer="16" />
</section>
<section name="STABILIZATION_RATE" prefix="STABILIZATION_RATE_">
sw/airborne/boards/ardrone/baro_board.c
+58 -8
View File
@@ -1,5 +1,5 @@
/*
* Copyright (C) 2012 TU Delft Quatrotor Team 1
* Copyright (C) 2013 TU Delft Quatrotor Team 1
*
* This file is part of Paparazzi.
*
@@ -23,15 +23,24 @@
* @file boards/ardrone/baro_board.c
* Paparazzi AR Drone 2 Baro Sensor implementation:.
*
* These functions are mostly empty because of the calibration and calculations
* done by the Parrot Navigation board.
* Based on BMP180 implementation by C. de Wagter.
*/
#include "subsystems/sensors/baro.h"
#include "baro_board.h"
#include "navdata.h"
struct Baro baro;
// Over sample setting (0-3)
#define BMP180_OSS 0
int32_t pressure;
int32_t temperature;
int32_t pres_raw = 0;
int32_t tmp_raw = 0;
int32_t pr0 = 0;
void baro_init(void) {
baro.status = BS_UNINITIALIZED;
baro.absolute = 0;
@@ -39,16 +48,57 @@ void baro_init(void) {
baro_data_available = 0;
}
static inline int32_t baro_apply_calibration(int32_t raw)
{
int32_t b6 = ((int32_t)baro_calibration.b5) - 4000L;
int32_t x1 = (((int32_t)baro_calibration.b2) * (b6 * b6 >> 12)) >> 11;
int32_t x2 = ((int32_t)baro_calibration.ac2) * b6 >> 11;
int32_t x3 = x1 + x2;
int32_t b3 = (((((int32_t)baro_calibration.ac1) * 4 + x3) << BMP180_OSS) + 2)/4;
x1 = ((int32_t)baro_calibration.ac3) * b6 >> 13;
x2 = (((int32_t)baro_calibration.b1) * (b6 * b6 >> 12)) >> 16;
x3 = ((x1 + x2) + 2) >> 2;
uint32_t b4 = (((int32_t)baro_calibration.ac4) * (uint32_t) (x3 + 32768L)) >> 15;
uint32_t b7 = (raw - b3) * (50000L >> BMP180_OSS);
int32_t p = b7 < 0x80000000L ? (b7 * 2) / b4 : (b7 / b4) * 2;
x1 = (p >> 8) * (p >> 8);
x1 = (x1 * 3038UL) >> 16;
x2 = (-7357L * p) >> 16;
return p + ((x1 + x2 + 3791L) >> 4);
}
static inline void baro_read_temp(void)
{
tmp_raw = (navdata->temperature_pressure & 0x00FF) << 8 | (navdata->temperature_pressure >> 8);
int32_t x1 = ((tmp_raw - ((int32_t)baro_calibration.ac6)) * ((int32_t)baro_calibration.ac5)) >> 15;
int32_t x2 = (((int32_t)baro_calibration.mc) << 11) / (x1 + ((int32_t)baro_calibration.md));
baro_calibration.b5 = x1 + x2;
temperature = (baro_calibration.b5 + 8) >> 4;
}
static inline void baro_read_pressure(void)
{
pres_raw = navdata->pressure >> 8;
pres_raw = pres_raw >> (8 - BMP180_OSS);
pressure = baro_apply_calibration(pres_raw);
if ((pr0 != 0) && (pressure != 0))
pr0 = pressure;
pressure -= pr0;
}
void baro_periodic(void) {
baro.status = BS_RUNNING;
if(navdata_baro_available == 1) {
if(baro.status == BS_RUNNING && navdata_baro_available == 1) {
navdata_baro_available = 0;
// baro.absolute = navdata->pressure; // When this is un-commented the ardrone gets a pressure
// TODO do the right calculations for the right absolute pressure
baro.absolute = 0;
baro_read_temp(); // first read temperature because pressure calibration depends on temperature
baro_read_pressure();
baro.absolute = pressure;
baro_data_available = TRUE;
}
else {
baro_data_available = FALSE;
if (baro_calibrated == TRUE) {
baro.status = BS_RUNNING;
}
}
}
sw/airborne/boards/ardrone/baro_board.h
+1 -1
View File
@@ -33,7 +33,7 @@
#if BOARD_HAS_BARO
#include "navdata.h"
int baro_data_available;
int8_t baro_data_available;
static inline void baro_event(void (*b_abs_handler)(void), void (*b_diff_handler)(void)){
if (baro_data_available) {
sw/airborne/boards/ardrone/navdata.c
+58 -3
View File
@@ -1,5 +1,5 @@
/*
* Copyright (C) 2012 Dino Hensen, Vincent van Hoek
* Copyright (C) 2013 Dino Hensen, Vincent van Hoek
*
* This file is part of Paparazzi.
*
@@ -72,6 +72,9 @@ int navdata_init()
uint8_t cmd=0x02;
write(nav_fd, &cmd, 1);
baro_calibrated = 0;
acquire_baro_calibration();
// start acquisition
cmd=0x01;
write(nav_fd, &cmd, 1);
@@ -90,6 +93,59 @@ int navdata_init()
return 0;
}
void acquire_baro_calibration()
{
read(nav_fd, NULL, 100); // read some potential dirt
// start baro calibration acquisition
uint8_t cmd=0x17; // send cmd 23
write(nav_fd, &cmd, 1);
uint8_t calibBuffer[22];
int calib_bytes_read, calib_bytes_left, readcount;
calib_bytes_read = 0;
calib_bytes_left = 22;
readcount = 0;
while(calib_bytes_read != 22) {
readcount = read(nav_fd, calibBuffer+(22-calib_bytes_left), calib_bytes_left);
if (readcount > 0) {
calib_bytes_read += readcount;
calib_bytes_left -= readcount;
}
}
baro_calibration.ac1 = calibBuffer[0] << 8 | calibBuffer[1];
baro_calibration.ac2 = calibBuffer[2] << 8 | calibBuffer[3];
baro_calibration.ac3 = calibBuffer[4] << 8 | calibBuffer[5];
baro_calibration.ac4 = calibBuffer[6] << 8 | calibBuffer[7];
baro_calibration.ac5 = calibBuffer[8] << 8 | calibBuffer[9];
baro_calibration.ac6 = calibBuffer[10] << 8 | calibBuffer[11];
baro_calibration.b1 = calibBuffer[12] << 8 | calibBuffer[13];
baro_calibration.b2 = calibBuffer[14] << 8 | calibBuffer[15];
baro_calibration.mb = calibBuffer[16] << 8 | calibBuffer[17];
baro_calibration.mc = calibBuffer[18] << 8 | calibBuffer[19];
baro_calibration.md = calibBuffer[20] << 8 | calibBuffer[21];
printf("Calibration bytes read: %d\n", calib_bytes_read);
printf("Calibration AC1: %d\n", baro_calibration.ac1);
printf("Calibration AC2: %d\n", baro_calibration.ac2);
printf("Calibration AC3: %d\n", baro_calibration.ac3);
printf("Calibration AC4: %d\n", baro_calibration.ac4);
printf("Calibration AC5: %d\n", baro_calibration.ac5);
printf("Calibration AC6: %d\n", baro_calibration.ac6);
printf("Calibration B1: %d\n", baro_calibration.b1);
printf("Calibration B2: %d\n", baro_calibration.b2);
printf("Calibration MB: %d\n", baro_calibration.mb);
printf("Calibration MC: %d\n", baro_calibration.mc);
printf("Calibration MD: %d\n", baro_calibration.md);
baro_calibrated = 1;
}
void navdata_close()
{
port->isOpen = 0;
@@ -146,7 +202,6 @@ void navdata_update()
pint = memchr(port->buffer, NAVDATA_START_BYTE, port->bytesRead);
if (pint != NULL) {
port->bytesRead -= pint - port->buffer;
navdata_CropBuffer(pint - port->buffer);
} else {
// if the start byte was not found, it means there is junk in the buffer
@@ -160,7 +215,7 @@ void navdata_CropBuffer(int cropsize)
{
if (port->bytesRead - cropsize < 0) {
// TODO think about why the amount of bytes read minus the cropsize gets below zero
printf("BytesRead - Cropsize may not be below zero...");
printf("BytesRead(=%d) - Cropsize(=%d) may not be below zero. port->buffer=%d\n", port->bytesRead, cropsize, port->buffer);
return;
}
sw/airborne/boards/ardrone/navdata.h
+24 -2
View File
@@ -1,5 +1,5 @@
/*
* Copyright (C) 2012 Dino Hensen, Vincent van Hoek
* Copyright (C) 2013 Dino Hensen, Vincent van Hoek
*
* This file is part of Paparazzi.
*
@@ -78,7 +78,7 @@ typedef struct
uint16_t flag_echo_ini;
int32_t pressure; //long
int32_t pressure;
int16_t temperature_pressure;
int16_t mx;
@@ -89,12 +89,32 @@ typedef struct
} __attribute__ ((packed)) measures_t;
struct bmp180_baro_calibration
{
int16_t ac1;
int16_t ac2;
int16_t ac3;
uint16_t ac4;
uint16_t ac5;
uint16_t ac6;
int16_t b1;
int16_t b2;
int16_t mb;
int16_t mc;
int16_t md;
// These values are calculated
int32_t b5;
};
measures_t* navdata;
struct bmp180_baro_calibration baro_calibration;
navdata_port* port;
uint16_t navdata_cks;
uint8_t navdata_imu_available;
uint8_t navdata_baro_available;
int16_t previousUltrasoundHeight;
uint8_t baro_calibrated;
int navdata_init(void);
void navdata_close(void);
@@ -106,4 +126,6 @@ void navdata_CropBuffer(int cropsize);
uint16_t navdata_checksum(void);
int16_t navdata_getHeight(void);
void acquire_baro_calibration(void);
#endif /* NAVDATA_H_ */