From 8e84c8ecbd3346b2876d78a08cc178a6b08a81ea Mon Sep 17 00:00:00 2001 From: Dino Hensen Date: Fri, 16 Aug 2013 00:33:17 +0200 Subject: [PATCH] [ardrone] Barometer bmp180 implementation added. Also fixed a bug in the navdata driver, where cropping was done twice. Baro still needs to be tested, because sometimes unexpected things happen. --- conf/airframes/ardrone2_raw.xml | 2 +- sw/airborne/boards/ardrone/baro_board.c | 70 +++++++++++++++++++++---- sw/airborne/boards/ardrone/baro_board.h | 2 +- sw/airborne/boards/ardrone/navdata.c | 61 +++++++++++++++++++-- sw/airborne/boards/ardrone/navdata.h | 26 ++++++++- 5 files changed, 144 insertions(+), 17 deletions(-) diff --git a/conf/airframes/ardrone2_raw.xml b/conf/airframes/ardrone2_raw.xml index 3e461ac70c..c2bf3cd45a 100644 --- a/conf/airframes/ardrone2_raw.xml +++ b/conf/airframes/ardrone2_raw.xml @@ -117,7 +117,7 @@
- +
diff --git a/sw/airborne/boards/ardrone/baro_board.c b/sw/airborne/boards/ardrone/baro_board.c index 7526ac485e..95d7b6bbf6 100644 --- a/sw/airborne/boards/ardrone/baro_board.c +++ b/sw/airborne/boards/ardrone/baro_board.c @@ -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,32 +23,82 @@ * @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; - baro.differential = 0; + baro.absolute = 0; + baro.differential = 0; 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; + } } } diff --git a/sw/airborne/boards/ardrone/baro_board.h b/sw/airborne/boards/ardrone/baro_board.h index 7233f8b437..cf76b5e273 100644 --- a/sw/airborne/boards/ardrone/baro_board.h +++ b/sw/airborne/boards/ardrone/baro_board.h @@ -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) { diff --git a/sw/airborne/boards/ardrone/navdata.c b/sw/airborne/boards/ardrone/navdata.c index 8231957d70..005bda75cc 100644 --- a/sw/airborne/boards/ardrone/navdata.c +++ b/sw/airborne/boards/ardrone/navdata.c @@ -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; } diff --git a/sw/airborne/boards/ardrone/navdata.h b/sw/airborne/boards/ardrone/navdata.h index cf149b2196..8c8149c622 100644 --- a/sw/airborne/boards/ardrone/navdata.h +++ b/sw/airborne/boards/ardrone/navdata.h @@ -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_ */