[ardrone] Fixed Baro

conf/airframes/ardrone2_raw.xml

@@ -29,7 +29,11 @@
 
   </firmware>
 
-  <!-- <modules main_freq="512"> <load name="sys_mon.xml" /> </modules> -->
+<!--
+  <modules main_freq="512" >
+    <load name="gps_ubx_ucenter.xml" /> 
+  </modules>
+-->
 
   <commands>
     <axis name="PITCH" failsafe_value="0" />
@@ -117,7 +121,7 @@
   </section>
 
   <section name="INS" prefix="INS_">
-    <define name="BARO_SENS" value="100." integer="16" />
+    <define name="BARO_SENS" value="22.4" integer="16" />
   </section>
 
   <section name="STABILIZATION_RATE" prefix="STABILIZATION_RATE_">

conf/messages.xml

@@ -43,7 +43,7 @@
      <field name="gradient" type="int16" />
      <field name="flag_echo_ini" type="uint16" />
      <field name="pressure" type="int32" />
-     <field name="temperature_pressure" type="int16" />
+     <field name="temperature_pressure" type="uint16" />
      <field name="mx" type="int16" />
      <field name="my" type="int16" />
      <field name="mz" type="int16" />

sw/airborne/boards/ardrone/baro_board.c

@@ -32,20 +32,12 @@
 
 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;
+#define BMP180_OSS 0  // Parrot ARDrone uses no oversampling
 
 void baro_init(void) {
   baro.status = BS_UNINITIALIZED;
   baro.absolute       = 0;
   baro.differential   = 0;
-  baro_data_available = 0;
 }
 
 static inline int32_t baro_apply_calibration(int32_t raw)
@@ -67,36 +59,26 @@ static inline int32_t baro_apply_calibration(int32_t raw)
   return p + ((x1 + x2 + 3791L) >> 4);
 }
 
-static inline void baro_read_temp(void)
+static inline int32_t baro_apply_calibration_temp(int32_t tmp_raw)
 {
-  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;
+  return (baro_calibration.b5 + 8) >> 4;
 }
 
-static inline void baro_read_pressure(void)
+void baro_periodic(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) {
-  if(baro.status == BS_RUNNING && navdata_baro_available == 1) {
-    navdata_baro_available = 0;
-
-    baro_read_temp(); // first read temperature because pressure calibration depends on temperature
-    baro_read_pressure();
-    baro.absolute = pressure;
-    baro_data_available = TRUE;
+void process_ardrone_baro(void)
+{
+  if(baro.status == BS_RUNNING) {
+    // first read temperature because pressure calibration depends on temperature
+    baro.differential = baro_apply_calibration_temp(navdata->temperature_pressure);   // We store the temperature in Baro-Diff
+    baro.absolute = baro_apply_calibration(navdata->pressure);
   }
   else {
-    baro_data_available = FALSE;
     if (baro_calibrated == TRUE) {
       baro.status = BS_RUNNING;
     }

sw/airborne/boards/ardrone/baro_board.h

@@ -33,10 +33,14 @@
 #if BOARD_HAS_BARO
 #include "navdata.h"
 
-int8_t baro_data_available;
+void process_ardrone_baro(void);
+
 
 static inline void baro_event(void (*b_abs_handler)(void), void (*b_diff_handler)(void)){
-  if (baro_data_available) {
+  if (navdata_baro_available)
+  {
+    navdata_baro_available = 0;
+    process_ardrone_baro();
     b_abs_handler();
   }
 }

sw/airborne/boards/ardrone/navdata.c

@@ -170,7 +170,6 @@ void navdata_read()
     port->bytesRead += newbytes;
     port->totalBytesRead += newbytes;
   }
-
 }
 
 void navdata_update()
@@ -187,11 +186,24 @@ void navdata_update()
 //      if ( navdata_checksum() == 0 )
       {
         memcpy(navdata, port->buffer, NAVDATA_PACKET_SIZE);
+        
+        // Invert byte order so that TELEMETRY works better
+        uint8_t tmp;
+        uint8_t* p = (uint8_t*) &(navdata->pressure);
+        tmp = p[0];
+        p[0] = p[1];
+        p[1] = tmp;
+        p = (uint8_t*) &(navdata->temperature_pressure);
+        tmp = p[0];
+        p[0] = p[1];
+        p[1] = tmp;
+
         navdata_imu_available = 1;
         navdata_baro_available = 1;
+
         port->packetsRead++;
 //        printf("CCRC=%d, GCRC=%d, error=%d\n", crc, navdata->chksum, abs(crc-navdata->chksum));
-        navdata_getHeight();
+        //navdata_getHeight();
       }
       navdata_CropBuffer(60);
     }
@@ -215,7 +227,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(=%d) - Cropsize(=%d) may not be below zero. port->buffer=%d\n", port->bytesRead, cropsize, port->buffer);
+    printf("BytesRead(=%d) - Cropsize(=%d) may not be below zero. port->buffer=%p\n", port->bytesRead, cropsize, port->buffer);
     return;
   }
 

sw/airborne/boards/ardrone/navdata.h

@@ -79,7 +79,7 @@ typedef struct
   uint16_t flag_echo_ini;
 
   int32_t pressure;
-  int16_t temperature_pressure;
+  uint16_t temperature_pressure;
 
   int16_t mx;
   int16_t my;

sw/airborne/subsystems/imu/imu_ardrone2_raw.c

@@ -30,24 +30,14 @@
 #include "mcu_periph/uart.h"
 
 void imu_impl_init(void) {
-  imu_data_available = FALSE;
   navdata_init();
 }
 
 void imu_periodic(void) {
-  navdata_update();
-  //checks if the navboard has a new dataset ready
-  if (navdata_imu_available == TRUE) {
-    navdata_imu_available = FALSE;
-    RATES_ASSIGN(imu.gyro_unscaled, navdata->vx, navdata->vy, navdata->vz);
-    VECT3_ASSIGN(imu.accel_unscaled, navdata->ax, navdata->ay, navdata->az);
-    VECT3_ASSIGN(imu.mag_unscaled, navdata->mx, navdata->my, navdata->mz);
-    imu_data_available = TRUE;
-  }
-  else {
-    imu_data_available = FALSE;
 }
 
+void navdata_event(void) {
+
 #ifdef USE_UART1
   uart1_handler();
 #endif

sw/airborne/subsystems/imu/imu_ardrone2_raw.h

@@ -31,16 +31,23 @@
 #include "generated/airframe.h"
 #include "navdata.h"
 
-int imu_data_available;
+void navdata_event(void);
 
 static inline void imu_ardrone2_event ( void (* _gyro_handler)(void), void (* _accel_handler)(void), void (* _mag_handler)(void))
 {
-  if (imu_data_available) {
-    imu_data_available = FALSE;
+  navdata_update();
+  //checks if the navboard has a new dataset ready
+  if (navdata_imu_available == TRUE) {
+    navdata_imu_available = FALSE;
+    RATES_ASSIGN(imu.gyro_unscaled, navdata->vx, navdata->vy, navdata->vz);
+    VECT3_ASSIGN(imu.accel_unscaled, navdata->ax, navdata->ay, navdata->az);
+    VECT3_ASSIGN(imu.mag_unscaled, navdata->mx, navdata->my, navdata->mz);
+
     _gyro_handler();
     _accel_handler();
     _mag_handler();
   }
+  navdata_event();
 }
 
 #define ImuEvent(_gyro_handler, _accel_handler, _mag_handler) {  \