[airborne] added hack to use baro_bmp module to update the fw estimator

thx Eduardo
This should be only considered a temporary solution until we find time to write a proper new baro interface.

conf/airframes/examples/microjet_lisa_m.xml

@@ -12,9 +12,11 @@
     <target name="sim" 	board="pc"/>
     <target name="ap"   board="lisa_m_1.0"/>
 
-    <define name="AGR_CLIMB" />
+    <define name="AGR_CLIMB"/>
-    <define name="LOITER_TRIM" />
+    <define name="LOITER_TRIM"/>
-    <define name="ALT_KALMAN" />
+    <define name="ALT_KALMAN"/>
+    <define name="USE_BARO_BMP"/>
+
 
     <subsystem name="radio_control" type="ppm"/>
 
@@ -32,6 +34,10 @@
 
   <modules>
     <load name="sys_mon.xml"/>
+    <load name="baro_bmp.xml">
+      <define name="BMP_I2C_DEV" value="i2c2"/>
+      <define name="USE_I2C2"/>
+    </load>
   </modules>
 
   <servos>

conf/modules/baro_bmp.xml

@@ -18,7 +18,7 @@
   <init fun="baro_bmp_init()"/>
   <periodic fun="baro_bmp_periodic()" freq="8"/>
   <event fun="baro_bmp_event()"/>
-  <makefile target="ap">
+  <makefile>
     <file name="baro_bmp.c"/>
   </makefile>
 </module>

sw/airborne/estimator.c

@@ -113,7 +113,6 @@ bool_t alt_kalman_enabled;
 #endif
 
 #define GPS_SIGMA2 1.
-
 #define GPS_DT 0.25
 #define GPS_R 2.
 
@@ -150,6 +149,12 @@ void alt_kalman(float gps_z) {
     R = baro_ets_r;
     SIGMA2 = baro_ets_sigma2;
   } else
+#elif USE_BARO_BMP
+  if (baro_bmp_enabled) {
+    DT = BARO_BMP_DT;
+    R = baro_bmp_r;
+    SIGMA2 = baro_bmp_sigma2;
+  } else
 #endif
   {
     DT = GPS_DT;
@@ -205,7 +210,7 @@ void estimator_update_state_gps( void ) {
   gps_north -= nav_utm_north0;
 
   EstimatorSetPosXY(gps_east, gps_north);
-#ifndef USE_BARO_ETS
+#if !USE_BARO_BMP && !USE_BARO_ETS && !USE_BARO_MS5534A
   float falt = gps.hmsl / 1000.;
   EstimatorSetAlt(falt);
 #endif
@@ -217,4 +222,3 @@ void estimator_update_state_gps( void ) {
   // Heading estimation now in ahrs_infrared
 
 }
-

sw/airborne/estimator.h

@@ -40,6 +40,10 @@
 #include "modules/sensors/baro_ets.h"
 #endif
 
+#if USE_BARO_BMP
+#include "modules/sensors/baro_bmp.h"
+#endif
+
 /* position in meters, ENU frame, relative to reference */
 extern float estimator_x; ///< east position in meters
 extern float estimator_y; ///< north position in meters
@@ -92,7 +96,7 @@ extern void alt_kalman( float );
 #ifdef ALT_KALMAN
 #define EstimatorSetPosXY(x, y) { estimator_x = x; estimator_y = y; }
 
-#if USE_BARO_MS5534A || USE_BARO_ETS
+#if USE_BARO_MS5534A || USE_BARO_ETS || USE_BARO_BMP
 /* Kalman filter cannot be disabled in this mode (no z_dot) */
 #define EstimatorSetAlt(z) alt_kalman(z)
 #else /* USE_BARO_x */

sw/airborne/modules/sensors/baro_bmp.c

@@ -37,12 +37,18 @@
 #include "mcu_periph/uart.h"
 #include "messages.h"
 #include "subsystems/datalink/downlink.h"
+#include "estimator.h"
+#include "subsystems/nav.h"
+
+#ifdef SITL
+#include "subsystems/gps.h"
+#endif
 
 #ifndef DOWNLINK_DEVICE
 #define DOWNLINK_DEVICE DOWNLINK_AP_DEVICE
 #endif
 
-#ifndef SENSOR_SYNC_SEND
+#if !defined(SENSOR_SYNC_SEND) && !defined(USE_BARO_BMP)
 #warning set SENSOR_SYNC_SEND to use baro_bmp
 #endif
 
@@ -52,11 +58,25 @@
 
 #define BMP085_SLAVE_ADDR 0xEE
 
+#define BARO_BMP_OFFSET_MAX 30000
+#define BARO_BMP_OFFSET_MIN 10
+#define BARO_BMP_OFFSET_NBSAMPLES_INIT 2
+#define BARO_BMP_OFFSET_NBSAMPLES_AVRG 4
+#define BARO_BMP_R 0.5
+#define BARO_BMP_SIGMA2 0.1
+
 struct i2c_transaction bmp_trans;
 
+bool_t baro_bmp_enabled;
+float baro_bmp_r;
+float baro_bmp_sigma2;
+
+// Global variables
 uint8_t  baro_bmp_status;
 uint32_t baro_bmp_pressure;
 uint16_t baro_bmp_temperature;
+int32_t baro_bmp_altitude, baro_bmp,baro_bmp_temp,baro_bmp_offset;
+double   tmp_float;
 
 int16_t  bmp_ac1, bmp_ac2, bmp_ac3;
 uint16_t bmp_ac4, bmp_ac5, bmp_ac6;
@@ -64,14 +84,25 @@ int16_t  bmp_b1, bmp_b2;
 int16_t  bmp_mb, bmp_mc, bmp_md;
 int32_t  bmp_up, bmp_ut;
 
+// Local variables
+bool_t baro_bmp_offset_init;
+int32_t baro_bmp_offset_tmp;
+uint16_t baro_bmp_cnt;
+
 void baro_bmp_init( void ) {
   baro_bmp_status = BARO_BMP_UNINIT;
+  baro_bmp_r = BARO_BMP_R;
+  baro_bmp_sigma2 = BARO_BMP_SIGMA2;
+  baro_bmp_enabled = TRUE;
+  baro_bmp_offset_init = FALSE;
+  baro_bmp_cnt = BARO_BMP_OFFSET_NBSAMPLES_INIT + BARO_BMP_OFFSET_NBSAMPLES_AVRG;
   /* read calibration values */
   bmp_trans.buf[0] = BMP085_EEPROM_AC1;
   I2CTransceive(BMP_I2C_DEV, bmp_trans, BMP085_SLAVE_ADDR, 1, 22);
 }
 
 void baro_bmp_periodic( void ) {
+#ifndef SITL
   if (baro_bmp_status == BARO_BMP_IDLE) {
     /* start temp measurement (once) */
     bmp_trans.buf[0] = BMP085_CTRL_REG;
@@ -91,6 +122,11 @@ void baro_bmp_periodic( void ) {
     I2CTransceive(BMP_I2C_DEV, bmp_trans, BMP085_SLAVE_ADDR, 1, 3);
     baro_bmp_status = BARO_BMP_READ_PRESS;
   }
+#else // SITL
+  baro_bmp_altitude = gps.hmsl / 1000.0;
+  EstimatorSetAlt(baro_bmp_altitude);
+#endif
+
 }
 
 void baro_bmp_event( void ) {
@@ -129,8 +165,8 @@ void baro_bmp_event( void ) {
 
       /* get uncompensated pressure, oss=3 */
       bmp_up = (bmp_trans.buf[0] << 11) |
-               (bmp_trans.buf[1] << 3)  |
+        (bmp_trans.buf[1] << 3)  |
-               (bmp_trans.buf[2] >> 5);
+        (bmp_trans.buf[2] >> 5);
       /* start temp measurement */
       bmp_trans.buf[0] = BMP085_CTRL_REG;
       bmp_trans.buf[1] = BMP085_START_TEMP;
@@ -165,9 +201,51 @@ void baro_bmp_event( void ) {
 
       baro_bmp_temperature = bmp_t;
       baro_bmp_pressure = bmp_p;
-#ifdef SENSOR_SYNC_SEND
+
-      DOWNLINK_SEND_BMP_STATUS(DefaultChannel, DefaultDevice, &bmp_up, &bmp_ut, &bmp_p, &bmp_t);
+      tmp_float = bmp_p/101325.0; //pressao nivel mar
+      tmp_float = pow(tmp_float,0.190295); //eleva pressao ao expoente
+      baro_bmp = 44330*(1.0-tmp_float);
+
+      if (!baro_bmp_offset_init) {
+        baro_bmp_offset = baro_bmp;
+        baro_bmp_offset_init = TRUE;
+#if 0
+        --baro_bmp_cnt;
+        // Check if averaging completed
+        if (baro_bmp_cnt == 0) {
+          // Calculate average
+          baro_bmp_offset = (baro_bmp_offset_tmp / BARO_BMP_OFFSET_NBSAMPLES_AVRG);
+          // Limit offset
+          if (baro_bmp_offset < BARO_BMP_OFFSET_MIN)
+            baro_bmp_offset = BARO_BMP_OFFSET_MIN;
+          if (baro_bmp_offset > BARO_BMP_OFFSET_MAX)
+            baro_bmp_offset = BARO_BMP_OFFSET_MAX;
+          baro_bmp_offset_init = TRUE;
+        }
+        // Check if averaging needs to continue
+        else if (baro_bmp_cnt <= BARO_BMP_OFFSET_NBSAMPLES_AVRG)
+          baro_bmp_offset_tmp += baro_bmp;
 #endif
+      } //baro offset init
+
+      baro_bmp_temp = (baro_bmp - baro_bmp_offset);
+
+      if (baro_bmp_offset_init) {
+        baro_bmp_altitude = ground_alt + baro_bmp_temp;
+        // New value available
+#if USE_BARO_BMP
+#pragma message "USING BARO BMP"
+        EstimatorSetAlt(baro_bmp_altitude);
+#endif
+
+#ifdef SENSOR_SYNC_SEND
+        DOWNLINK_SEND_BMP_STATUS(DefaultChannel, DefaultDevice, &bmp_up, &bmp_ut, &bmp_p, &bmp_t);
+#else
+        RunOnceEvery(10, DOWNLINK_SEND_BMP_STATUS(DefaultChannel, DefaultDevice, &baro_bmp_temp, &bmp_ut, &bmp_p, &bmp_t));
+#endif
+      } else {
+        baro_bmp_altitude = 0.0;
+      }
     }
   }
 }

sw/airborne/modules/sensors/baro_bmp.h

@@ -34,9 +34,17 @@
 #define BARO_BMP_START_PRESS  4
 #define BARO_BMP_READ_PRESS   5
 
+#define BARO_BMP_DT 0.05
+extern bool_t baro_bmp_enabled;
+extern float baro_bmp_r;
+extern float baro_bmp_sigma2;
+
 extern uint8_t  baro_bmp_status;
 extern uint32_t baro_bmp_pressure;
 extern uint16_t baro_bmp_temperature;
+extern int32_t  baro_bmp_altitude;
+extern int32_t baro_bmp;
+extern int32_t baro_bmp_offset;
 
 void baro_bmp_init(void);
 void baro_bmp_periodic(void);