[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

@@ -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

@@ -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
+ * Based on BMP180 implementation by C. de Wagter.
- * done by the Parrot Navigation board.
  */
 
 #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.absolute       = 0;
-  baro.differential = 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(baro.status == BS_RUNNING && navdata_baro_available == 1) {
-  if(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_read_temp(); // first read temperature because pressure calibration depends on temperature
-    baro.absolute = 0;
+    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

@@ -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

@@ -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

@@ -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_ */