diff --git a/conf/airframes/examples/microjet_lisa_m.xml b/conf/airframes/examples/microjet_lisa_m.xml
index 641542cc1e..edf2a0d156 100644
--- a/conf/airframes/examples/microjet_lisa_m.xml
+++ b/conf/airframes/examples/microjet_lisa_m.xml
@@ -12,9 +12,11 @@
-
-
-
+
+
+
+
+
@@ -32,6 +34,10 @@
+
+
+
+
diff --git a/conf/modules/baro_bmp.xml b/conf/modules/baro_bmp.xml
index cb132fffb5..050e49d515 100644
--- a/conf/modules/baro_bmp.xml
+++ b/conf/modules/baro_bmp.xml
@@ -18,7 +18,7 @@
-
+
diff --git a/sw/airborne/estimator.c b/sw/airborne/estimator.c
index 4ea97a3696..30c3b76e23 100644
--- a/sw/airborne/estimator.c
+++ b/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
}
-
diff --git a/sw/airborne/estimator.h b/sw/airborne/estimator.h
index ce6dff074d..2d01172f43 100644
--- a/sw/airborne/estimator.h
+++ b/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 */
diff --git a/sw/airborne/modules/sensors/baro_bmp.c b/sw/airborne/modules/sensors/baro_bmp.c
index c8909fe2fa..4bb12a8b34 100644
--- a/sw/airborne/modules/sensors/baro_bmp.c
+++ b/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[2] >> 5);
+ (bmp_trans.buf[1] << 3) |
+ (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;
+ }
}
}
}
diff --git a/sw/airborne/modules/sensors/baro_bmp.h b/sw/airborne/modules/sensors/baro_bmp.h
index ff967980b3..0d82a37b3c 100644
--- a/sw/airborne/modules/sensors/baro_bmp.h
+++ b/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);