added agl estimator when assuming non flat ground (#1910)

* added agl estimator when assuming non flat ground

* add documentation and expand message

* update pprzlink

conf/firmwares/subsystems/shared/baro_board.makefile

@@ -232,7 +232,6 @@ else ifeq ($(BOARD), apogee)
   BARO_BOARD_CFLAGS += -DUSE_I2C1
   BARO_BOARD_SRCS += peripherals/mpl3115.c
   BARO_BOARD_SRCS += $(SRC_BOARD)/baro_board.c
-  BARO_PERIODIC_FREQUENCY ?= 50
 
 # Umarim
 else ifeq ($(BOARD), umarim)
@@ -313,9 +312,8 @@ BARO_BOARD_CFLAGS += -DBARO_LED=$(BARO_LED)
 endif
 
 # make sure you can also use <configure name="BARO_PERIODIC_FREQUENCY" value="x"/> instead of define
-ifdef BARO_PERIODIC_FREQUENCY
+BARO_PERIODIC_FREQUENCY ?= 50
 BARO_BOARD_CFLAGS += -DBARO_PERIODIC_FREQUENCY=$(BARO_PERIODIC_FREQUENCY)
-endif
 
 ap.CFLAGS += $(BARO_BOARD_CFLAGS)
 ap.srcs += $(BARO_BOARD_SRCS)

conf/firmwares/subsystems/shared/nps_common.makefile

@@ -61,3 +61,6 @@ nps.srcs +=                                      \
 # for geo mag calculation
 nps.srcs += math/pprz_geodetic_wmm2015.c
 
+BARO_PERIODIC_FREQUENCY ?= 50
+BARO_BOARD_CFLAGS += -DBARO_PERIODIC_FREQUENCY=$(BARO_PERIODIC_FREQUENCY)
+

conf/modules/ins_extended.xml

@@ -6,7 +6,7 @@
       extended INS with vertical filter using sonar.
     </description>
     <define name="USE_INS_NAV_INIT" value="TRUE|FALSE" description="Initialize the origin of the local coordinate system from flight plan. (Default: TRUE)"/>
-    <define name="INS_INT_SONAR_ID" value="ABI_BROADCAST" description="The ABI sender id of the sonar to use"/>
+    <define name="INS_INT_AGL_ID" value="ABI_BROADCAST" description="The ABI sender id of the sonar to use"/>
     <define name="INS_INT_BARO_ID" value="BARO_BOARD_SENDER_ID" description="The ABI sender id of the baro to use"/>
     <define name="INS_INT_GPS_ID" value="GPS_MULTI_ID" description="The ABI sender id of the GPS to use"/>
     <define name="INS_INT_IMU_ID" value="ABI_BROADCAST" description="The ABI sender id of the IMU to use"/>
@@ -14,6 +14,12 @@
     <define name="INS_SONAR_MIN_RANGE" value="0.001" description="min sonar range in meters"/>
     <define name="INS_SONAR_MAX_RANGE" value="4.0" description="max sonar range in meters"/>
     <define name="INS_SONAR_UPDATE_ON_AGL" value="FALSE" description="assume flat ground and use sonar for height"/>
+    
+    <define name="DEBUG_VFF_EXTENDED" value="0|1|2" description="If set > 0, this will send the vff message. If > 1 then it will also print the P matrix"/>
+    <define name="VFF_EXTENDED_INIT_PXX" value="1." description="Initial value of the diagonal of the P matrix"/>
+    <define name="VFF_EXTENDED_ACCEL_NOISE" value="0.5" description="Accelerometer noise setting"/>
+    <define name="VFF_EXTENDED_R_BARO" value="2." description="Barometer noise setting"/>
+    <define name="VFF_EXTENDED_NON_FLAT_GROUND" value="FALSE" description="VFF_EXTENDED_NON_FLAT_GROUND removes the assumption of a flat ground and tries to estimate the height of the obstacles under the vehicle."/>
   </doc>
 
   <settings>
@@ -22,7 +28,7 @@
         <dl_setting MAX="10" MIN="0" STEP="0.1" VAR="vff.accel_noise" shortname="accel_noise" module="subsystems/ins/vf_extended_float"/>
         <dl_setting MAX="10" MIN="0" STEP="0.1" VAR="vff.r_baro" shortname="r_baro"/>
         <dl_setting MAX="10" MIN="0" STEP="0.1" VAR="vff.r_alt" shortname="r_alt"/>
-        <dl_setting MAX="10" MIN="0" STEP="0.1" VAR="vff.r_offset" shortname="r_offset"/>
+        <dl_setting MAX="10" MIN="0" STEP="0.1" VAR="vff.r_obs_height" shortname="r_obs_height"/>
       </dl_settings>
     </dl_settings>
   </settings>

conf/modules/ins_hff_extended.xml

@@ -6,7 +6,7 @@
       extended INS with vertical filter using sonar.
     </description>
     <define name="USE_INS_NAV_INIT" value="TRUE|FALSE" description="Initialize the origin of the local coordinate system from flight plan. (Default: TRUE)"/>
-    <define name="INS_INT_SONAR_ID" value="ABI_BROADCAST" description="The ABI sender id of the sonar to use"/>
+    <define name="INS_INT_AGL_ID" value="ABI_BROADCAST" description="The ABI sender id of the sonar to use"/>
     <define name="INS_INT_BARO_ID" value="BARO_BOARD_SENDER_ID" description="The ABI sender id of the baro to use"/>
     <define name="INS_INT_GPS_ID" value="GPS_MULTI_ID" description="The ABI sender id of the GPS to use"/>
     <define name="INS_INT_IMU_ID" value="ABI_BROADCAST" description="The ABI sender id of the IMU to use"/>
@@ -22,7 +22,7 @@
         <dl_setting MAX="10" MIN="0" STEP="0.1" VAR="vff.accel_noise" shortname="accel_noise" module="subsystems/ins/vf_extended_float"/>
         <dl_setting MAX="10" MIN="0" STEP="0.1" VAR="vff.r_baro" shortname="r_baro"/>
         <dl_setting MAX="10" MIN="0" STEP="0.1" VAR="vff.r_alt" shortname="r_alt"/>
-        <dl_setting MAX="10" MIN="0" STEP="0.1" VAR="vff.r_offset" shortname="r_offset"/>
+        <dl_setting MAX="10" MIN="0" STEP="0.1" VAR="vff.r_obs_height" shortname="r_obs_height"/>
       </dl_settings>
     </dl_settings>
   </settings>

conf/modules/sonar_bebop.xml

@@ -7,7 +7,6 @@
      Reads an anlog sonar sensor and outputs sonar distance in [m]
     </description>
     <define name="USE_SONAR" value="" description="activate use of sonar in INS extended filter (only rotorcraft)"/>
-    <configure name="BEBOP_SONAR_BARO_THRESHOLD" value="3" description="Barometric altitude above which the sonar is not used"/>
   </doc>
 
   <header>
@@ -22,8 +21,6 @@
   <makefile target="ap">
     <define name="USE_SPI0" value="1"/>
     <define name="USE_ADC0" value="1"/>
-    <configure name="BEBOP_SONAR_BARO_THRESHOLD" default="3"/>
-    <define name="INS_SONAR_BARO_THRESHOLD" value="$(BEBOP_SONAR_BARO_THRESHOLD)"/>
     <raw>
       include $(CFG_SHARED)/spi_master.makefile
     </raw>

sw/airborne/boards/baro_board_ms5611_i2c.c

@@ -28,9 +28,10 @@
  */
 
 #include "subsystems/sensors/baro.h"
-#include "boards/bebop/baro_board.h"
 #include "peripherals/ms5611_i2c.h"
 
+#include BOARD_CONFIG // include board specific settings
+
 #include "mcu_periph/sys_time.h"
 #include "led.h"
 #include "std.h"
@@ -40,10 +41,10 @@
 #include "pprzlink/messages.h"
 #include "subsystems/datalink/downlink.h"
 
-#ifdef BARO_PERIODIC_FREQUENCY
+#include "filters/median_filter.h"
+
 #if BARO_PERIODIC_FREQUENCY > 100
-#error "For MS5611 BARO_PERIODIC_FREQUENCY has to be < 100"
-#endif
+#error "For MS5611 BARO_PERIODIC_FREQUENCY has to be <= 100"
 #endif
 
 
@@ -64,7 +65,7 @@
 PRINT_CONFIG_VAR(BB_MS5611_TYPE_MS5607)
 
 struct Ms5611_I2c bb_ms5611;
-
+struct MedianFilterFloat bb_ms5611_filt;
 
 void baro_init(void)
 {
@@ -73,6 +74,8 @@ void baro_init(void)
 #ifdef BARO_LED
   LED_OFF(BARO_LED);
 #endif
+
+  init_median_filter_f(&bb_ms5611_filt, 5);
 }
 
 void baro_periodic(void)
@@ -103,7 +106,7 @@ void baro_event(void)
     ms5611_i2c_event(&bb_ms5611);
 
     if (bb_ms5611.data_available) {
-      float pressure = (float)bb_ms5611.data.pressure;
+      float pressure = update_median_filter_f(&bb_ms5611_filt, (float)bb_ms5611.data.pressure);
       AbiSendMsgBARO_ABS(BARO_BOARD_SENDER_ID, pressure);
       float temp = bb_ms5611.data.temperature / 100.0f;
       AbiSendMsgTEMPERATURE(BARO_BOARD_SENDER_ID, temp);

sw/airborne/boards/baro_board_ms5611_spi.c

@@ -39,10 +39,10 @@
 #include "pprzlink/messages.h"
 #include "subsystems/datalink/downlink.h"
 
-#ifdef BARO_PERIODIC_FREQUENCY
+#include "filters/median_filter.h"
+
 #if BARO_PERIODIC_FREQUENCY > 100
-#error "For MS5611 BARO_PERIODIC_FREQUENCY has to be < 100"
-#endif
+#error "For MS5611 BARO_PERIODIC_FREQUENCY has to be <= 100"
 #endif
 
 PRINT_CONFIG_VAR(BB_MS5611_SLAVE_IDX)
@@ -54,7 +54,7 @@ PRINT_CONFIG_VAR(BB_MS5611_SPI_DEV)
 #endif
 
 struct Ms5611_Spi bb_ms5611;
-
+struct MedianFilterFloat bb_ms5611_filt;
 
 void baro_init(void)
 {
@@ -63,6 +63,8 @@ void baro_init(void)
 #ifdef BARO_LED
   LED_OFF(BARO_LED);
 #endif
+
+  init_median_filter_f(&bb_ms5611_filt, 5);
 }
 
 void baro_periodic(void)
@@ -93,7 +95,7 @@ void baro_event(void)
     ms5611_spi_event(&bb_ms5611);
 
     if (bb_ms5611.data_available) {
-      float pressure = (float)bb_ms5611.data.pressure;
+      float pressure = update_median_filter_f(&bb_ms5611_filt, (float)bb_ms5611.data.pressure);
       AbiSendMsgBARO_ABS(BARO_BOARD_SENDER_ID, pressure);
       float temp = bb_ms5611.data.temperature / 100.0f;
       AbiSendMsgTEMPERATURE(BARO_BOARD_SENDER_ID, temp);

sw/airborne/firmwares/fixedwing/main_ap.c

@@ -115,10 +115,10 @@ PRINT_CONFIG_VAR(TELEMETRY_FREQUENCY)
  */
 PRINT_CONFIG_VAR(MODULES_FREQUENCY)
 
+/* BARO_PERIODIC_FREQUENCY is defined in baro_board.makefile
+ * defaults to 50Hz or set by BARO_PERIODIC_FREQUENCY configure option in airframe file
+ */
 #if USE_BARO_BOARD
-#ifndef BARO_PERIODIC_FREQUENCY
-#define BARO_PERIODIC_FREQUENCY 50
-#endif
 PRINT_CONFIG_VAR(BARO_PERIODIC_FREQUENCY)
 #endif
 

sw/airborne/firmwares/rotorcraft/main_ap.c

@@ -95,9 +95,7 @@ PRINT_CONFIG_VAR(TELEMETRY_FREQUENCY)
  */
 PRINT_CONFIG_VAR(MODULES_FREQUENCY)
 
-#ifndef BARO_PERIODIC_FREQUENCY
-#define BARO_PERIODIC_FREQUENCY 50
-#endif
+/* BARO_PERIODIC_FREQUENCY is defined in the shared/baro_board.makefile and defaults to 50Hz */
 PRINT_CONFIG_VAR(BARO_PERIODIC_FREQUENCY)
 
 #if USE_AHRS && USE_IMU && (defined AHRS_PROPAGATE_FREQUENCY)

sw/airborne/modules/sonar/sonar_bebop.c

@@ -77,7 +77,13 @@ static uint8_t mode;
 static uint8_t pulse_transition_counter;
 
 /** SONAR_BEBOP_PEAK_THRESHOLD minimum samples from broadcast stop */
-#define SONAR_BEBOP_PEAK_THRESHOLD 250
+#define SONAR_BEBOP_PEAK_THRESHOLD 100
+
+/** SONAR_BEBOP_MIN_PEAK_VAL minimum adc value of reflected peak that will be cosidered */
+#define SONAR_BEBOP_MIN_PEAK_VAL 1024 // max value is 4096
+
+/** SONAR_BEBOP_MAX_TRANS_TIME maximum time for a reflection to travel and return in the adc measurement window */
+#define SONAR_BEBOP_MAX_TRANS_TIME 270
 
 /** sonar_bebop_spi_d the waveforms emitted by the sonar
  * waveform 0 is long pulse used at high altitude
@@ -113,7 +119,7 @@ void sonar_bebop_init(void)
   pthread_setname_np(tid, "pprz_sonar_thread");
 #endif
 
-  init_median_filter_f(&sonar_filt, 3);
+  init_median_filter_f(&sonar_filt, 5);
 }
 
 uint16_t adc_buffer[SONAR_BEBOP_ADC_BUFFER_SIZE];
@@ -129,7 +135,7 @@ void *sonar_bebop_read(void *data __attribute__((unused)))
     /* Start ADC and send sonar output */
     adc_enable(&adc0, 1);
     sonar_bebop_spi_t.status = SPITransDone;
-    sonar_bebop_spi_t.output_buf    = sonar_bebop_spi_d[mode];
+    sonar_bebop_spi_t.output_buf = sonar_bebop_spi_d[mode];
     spi_submit(&spi0, &sonar_bebop_spi_t);
     while (sonar_bebop_spi_t.status != SPITransSuccess);
     adc_read(&adc0, adc_buffer, SONAR_BEBOP_ADC_BUFFER_SIZE);
@@ -144,9 +150,9 @@ void *sonar_bebop_read(void *data __attribute__((unused)))
 
     for (i = 0; i < SONAR_BEBOP_ADC_BUFFER_SIZE; i++) {
       uint16_t adc_val = adc_buffer[i] >> 4;
-      if (start_send == 0 && adc_val == SONAR_BEBOP_ADC_MAX_VALUE) {
+      if (start_send == 0 && adc_val >= SONAR_BEBOP_ADC_MAX_VALUE) {
         start_send = i;
-      } else if (start_send != 0 && stop_send == 0 && adc_val != SONAR_BEBOP_ADC_MAX_VALUE) {
+      } else if (start_send && stop_send == 0 && adc_val < SONAR_BEBOP_ADC_MAX_VALUE - SONAR_BEBOP_MIN_PEAK_VAL) {
         stop_send = i - 1;
         i += SONAR_BEBOP_PEAK_THRESHOLD;
         continue;
@@ -162,37 +168,32 @@ void *sonar_bebop_read(void *data __attribute__((unused)))
 
     /* Calculate the distance from the peeks */
     uint16_t diff = stop_send - start_send;
-    float peak_distance;
-    if (diff > 250) {
-      peak_distance = 0;
-    } else {
-      peak_distance = first_peak - (stop_send - diff / 2);
-    }
+    if (diff && diff < SONAR_BEBOP_MAX_TRANS_TIME
+        && peak_value > SONAR_BEBOP_MIN_PEAK_VAL) {
+      sonar_bebop.meas = first_peak - (stop_send - diff / 2);
+      sonar_bebop.distance = update_median_filter_f(&sonar_filt, (float)sonar_bebop.meas * SONAR_BEBOP_INX_DIFF_TO_DIST);
 
-    sonar_bebop.distance = update_median_filter_f(&sonar_filt, peak_distance * SONAR_BEBOP_INX_DIFF_TO_DIST);
-
-    // set sonar pulse mode for next pulse based on altitude
-    if (mode == 0 && sonar_bebop.distance > SONAR_BEBOP_TRANSITION_LOW_TO_HIGH) {
-      if (++pulse_transition_counter > SONAR_BEBOP_TRANSITION_COUNT) {
-        mode = 1;
+      // set sonar pulse mode for next pulse based on altitude
+      if (mode == 0 && sonar_bebop.distance > SONAR_BEBOP_TRANSITION_LOW_TO_HIGH) {
+        if (++pulse_transition_counter > SONAR_BEBOP_TRANSITION_COUNT) {
+          mode = 1;
+          pulse_transition_counter = 0;
+        }
+      } else if (mode == 1 && sonar_bebop.distance < SONAR_BEBOP_TRANSITION_HIGH_TO_LOW) {
+        if (++pulse_transition_counter > SONAR_BEBOP_TRANSITION_COUNT) {
+          mode = 0;
+          pulse_transition_counter = 0;
+        }
+      } else {
         pulse_transition_counter = 0;
       }
-    } else if (mode == 1 && sonar_bebop.distance < SONAR_BEBOP_TRANSITION_HIGH_TO_LOW) {
-      if (++pulse_transition_counter > SONAR_BEBOP_TRANSITION_COUNT) {
-        mode = 0;
-        pulse_transition_counter = 0;
-      }
-    } else {
-      pulse_transition_counter = 0;
-    }
 
 #else // SITL
-    sonar_bebop.distance = stateGetPositionEnu_f()->z;
-    Bound(sonar_bebop.distance, 0.1f, 7.0f);
-    uint16_t peak_distance = 1;
+      sonar_bebop.distance = stateGetPositionEnu_f()->z;
+      Bound(sonar_bebop.distance, 0.1f, 7.0f);
+      sonar_bebop.meas = sonar_bebop.distance / SONAR_BEBOP_INX_DIFF_TO_DIST;
 #endif // SITL
 
-    if (peak_distance > 0) {
       // Send ABI message
       AbiSendMsgAGL(AGL_SONAR_ADC_ID, sonar_bebop.distance);
 #ifdef SENSOR_SYNC_SEND_SONAR
@@ -200,7 +201,7 @@ void *sonar_bebop_read(void *data __attribute__((unused)))
       DOWNLINK_SEND_SONAR(DefaultChannel, DefaultDevice, &sonar_bebop.meas, &sonar_bebop.distance);
 #endif
     }
+    usleep(10000); //100Hz
   }
-  usleep(10000); //100Hz
   return NULL;
 }

sw/airborne/subsystems/ins/ins_int.c

@@ -53,20 +53,17 @@
 
 #include "math/pprz_geodetic_int.h"
 #include "math/pprz_isa.h"
+#include "math/pprz_stat.h"
 
+#ifndef VFF_R_AGL
+#define VFF_R_AGL 0.2
+#endif
 
 #if USE_SONAR
 #if !USE_VFF_EXTENDED
 #error USE_SONAR needs USE_VFF_EXTENDED
 #endif
 
-/** default sonar to use in INS */
-#ifndef INS_INT_SONAR_ID
-#define INS_INT_SONAR_ID ABI_BROADCAST
-#endif
-abi_event sonar_ev;
-static void sonar_cb(uint8_t sender_id, float distance);
-
 #ifdef INS_SONAR_THROTTLE_THRESHOLD
 #include "firmwares/rotorcraft/stabilization.h"
 #endif
@@ -77,7 +74,7 @@ static void sonar_cb(uint8_t sender_id, float distance);
 #ifndef INS_SONAR_MAX_RANGE
 #define INS_SONAR_MAX_RANGE 4.0
 #endif
-#define VFF_R_SONAR_0 0.1
+#define VFF_R_SONAR_0 0.2
 #ifndef VFF_R_SONAR_OF_M
 #define VFF_R_SONAR_OF_M 0.2
 #endif
@@ -110,6 +107,7 @@ PRINT_CONFIG_MSG("USE_INS_NAV_INIT defaulting to TRUE")
 #endif
 
 /** default barometer to use in INS */
+#define INS_BARO_MAX_INIT_VAR 1.f  // variance threshold to set initial baro measurement
 #ifndef INS_INT_BARO_ID
 #if USE_BARO_BOARD
 #define INS_INT_BARO_ID BARO_BOARD_SENDER_ID
@@ -156,6 +154,15 @@ static void pos_est_cb(uint8_t sender_id,
                        float x, float y, float z,
                        float noise_x, float noise_y, float noise_z);
 
+/** ABI binding for AGL.
+ * Usually this is comes from sonar or gps.
+ */
+#ifndef INS_INT_AGL_ID
+#define INS_INT_AGL_ID ABI_BROADCAST
+#endif
+static abi_event agl_ev;                 ///< The agl ABI event
+static void agl_cb(uint8_t sender_id, float distance);
+
 struct InsInt ins_int;
 
 #if PERIODIC_TELEMETRY
@@ -212,8 +219,6 @@ void ins_int_init(void)
 
 #if USE_SONAR
   ins_int.update_on_agl = INS_SONAR_UPDATE_ON_AGL;
-  // Bind to AGL message
-  AbiBindMsgAGL(INS_INT_SONAR_ID, &sonar_ev, sonar_cb);
 #endif
 
   ins_int.vf_reset = false;
@@ -242,6 +247,7 @@ void ins_int_init(void)
   AbiBindMsgGPS(INS_INT_GPS_ID, &gps_ev, gps_cb);
   AbiBindMsgVELOCITY_ESTIMATE(INS_INT_VEL_ID, &vel_est_ev, vel_est_cb);
   AbiBindMsgPOSITION_ESTIMATE(INS_INT_POS_ID, &pos_est_ev, pos_est_cb);
+  AbiBindMsgAGL(INS_INT_AGL_ID, &agl_ev, agl_cb); // ABI to the altitude above ground level
 }
 
 void ins_reset_local_origin(void)
@@ -330,39 +336,56 @@ void ins_int_propagate(struct Int32Vect3 *accel, float dt)
 
 static void baro_cb(uint8_t __attribute__((unused)) sender_id, float pressure)
 {
-  if (!ins_int.baro_initialized && pressure > 1e-7) {
-    // wait for a first positive value
-    ins_int.qfe = pressure;
-    ins_int.baro_initialized = true;
+  if (pressure < 1.f)
+  {
+    // bad baro pressure, don't use
+    return;
+  }
+
+  if (!ins_int.baro_initialized) {
+#define press_hist_len 10
+    static float press_hist[press_hist_len];
+    static uint8_t idx = 0;
+
+    press_hist[idx] = pressure;
+    idx = (idx + 1) % press_hist_len;
+    float var = variance_f(press_hist, press_hist_len);
+    if (var < INS_BARO_MAX_INIT_VAR){
+      // wait for a first positive value
+      ins_int.vf_reset = true;
+      ins_int.baro_initialized = true;
+    }
   }
 
   if (ins_int.baro_initialized) {
-    if (ins_int.vf_reset) {
-      ins_int.vf_reset = false;
-      ins_int.qfe = pressure;
-      vff_realign(0.);
-      ins_update_from_vff();
-    } else {
-      float baro_up = pprz_isa_height_of_pressure(pressure, ins_int.qfe);
-
+    float height_correction = 0.f;
+    if(ins_int.ltp_initialized){
       // Calculate the distance to the origin
       struct EnuCoor_f *enu = stateGetPositionEnu_f();
       double dist2_to_origin = enu->x * enu->x + enu->y * enu->y;
 
       // correction for the earth's curvature
       const double earth_radius = 6378137.0;
-      float height_correction = (float)(sqrt(earth_radius * earth_radius + dist2_to_origin) - earth_radius);
+      height_correction = (float)(sqrt(earth_radius * earth_radius + dist2_to_origin) - earth_radius);
+    }
 
-      // The VFF will update in the NED frame
-      ins_int.baro_z = -(baro_up - height_correction);
+    if (ins_int.vf_reset) {
+      ins_int.vf_reset = false;
+      ins_int.qfe = pressure;
+      vff_realign(height_correction);
+      ins_update_from_vff();
+    }
+
+    float baro_up = pprz_isa_height_of_pressure(pressure, ins_int.qfe);
+
+    // The VFF will update in the NED frame
+    ins_int.baro_z = -(baro_up - height_correction);
 
 #if USE_VFF_EXTENDED
-      vff_update_baro(ins_int.baro_z);
+    vff_update_baro(ins_int.baro_z);
 #else
-      vff_update(ins_int.baro_z);
+    vff_update(ins_int.baro_z);
 #endif
-    }
-    ins_ned_to_state();
 
     /* reset the counter to indicate we just had a measurement update */
     ins_int.propagation_cnt = 0;
@@ -448,33 +471,44 @@ void ins_int_update_gps(struct GpsState *gps_s)
 void ins_int_update_gps(struct GpsState *gps_s __attribute__((unused))) {}
 #endif /* USE_GPS */
 
-
-#if USE_SONAR
-static void sonar_cb(uint8_t __attribute__((unused)) sender_id, float distance)
-{
-  static float last_offset = 0.;
-
-  /* update filter assuming a flat ground */
-  if (distance < INS_SONAR_MAX_RANGE && distance > INS_SONAR_MIN_RANGE
-#ifdef INS_SONAR_THROTTLE_THRESHOLD
-      && stabilization_cmd[COMMAND_THRUST] < INS_SONAR_THROTTLE_THRESHOLD
-#endif
-#ifdef INS_SONAR_BARO_THRESHOLD
-      && ins_int.baro_z > -INS_SONAR_BARO_THRESHOLD /* z down */
-#endif
-      && ins_int.update_on_agl
-      && ins_int.baro_initialized) {
-    vff_update_z_conf(-(distance), VFF_R_SONAR_0 + VFF_R_SONAR_OF_M * fabsf(distance));
-    last_offset = vff.offset;
-  } else {
-    /* update offset with last value to avoid divergence */
-    vff_update_offset(last_offset);
+/** agl_cb
+ * This callback handles all estimates of the height of the vehicle above the ground under it
+ * This is only used with the extended version of the vertical float filter
+ */
+#if USE_VFF_EXTENDED
+static void agl_cb(uint8_t sender_id, float distance) {
+  if (distance <= 0 || !(ins_int.baro_initialized)) {
+    return;
   }
 
-  /* reset the counter to indicate we just had a measurement update */
-  ins_int.propagation_cnt = 0;
+#if USE_SONAR
+  if (distance > INS_SONAR_MAX_RANGE || distance < INS_SONAR_MIN_RANGE){
+    return;
+  }
+#endif
+#ifdef INS_AGL_THROTTLE_THRESHOLD
+   if(stabilization_cmd[COMMAND_THRUST] < INS_AGL_THROTTLE_THRESHOLD){
+     return;
+   }
+#endif
+#ifdef INS_AGL_BARO_THRESHOLD
+  if(ins_int.baro_z < -INS_SONAR_BARO_THRESHOLD){ /* z down */
+    return;
+  }
+#endif
+
+#if USE_SONAR
+  vff_update_agl(-distance, VFF_R_SONAR_0 + VFF_R_SONAR_OF_M * fabsf(distance));
+#else
+  // TODO: this assumes that you will either have sonar or other agl sensor never both
+  vff_update_agl(-distance, VFF_R_AGL);
+#endif
+    /* reset the counter to indicate we just had a measurement update */
+    ins_int.propagation_cnt = 0;
 }
-#endif // USE_SONAR
+#else
+static void agl_cb(uint8_t __attribute__((unused)) sender_id, __attribute__((unused)) float distance) {}
+#endif
 
 /** copy position and speed to state interface */
 static void ins_ned_to_state(void)

sw/airborne/subsystems/ins/vf_extended_float.h

@@ -30,7 +30,7 @@
 #ifndef VF_EXTENDED_FLOAT_H
 #define VF_EXTENDED_FLOAT_H
 
-#define VFF_STATE_SIZE 4
+#define VFF_STATE_SIZE 5
 
 struct VffExtended {
   /* state vector */
@@ -38,6 +38,7 @@ struct VffExtended {
   float zdot;        ///< z-velocity estimate in m/s (NED, z-down)
   float bias;        ///< accel bias estimate in m/s^2
   float offset;      ///< baro offset estimate
+  float obs_height;  ///< estimate of height of obstacles under the vehicle
 
   float zdotdot;     ///< z-acceleration in m/s^2 (NED, z-down)
   float z_meas;      ///< last z measurement in m
@@ -48,20 +49,20 @@ struct VffExtended {
   float accel_noise;
   float r_baro;
   float r_alt;
-  float r_offset;
+  float r_obs_height;
 };
 
 extern struct VffExtended vff;
 
 extern void vff_init_zero(void);
-extern void vff_init(float z, float zdot, float accel_bias, float baro_offset);
+extern void vff_init(float z, float zdot, float accel_bias, float baro_offset, float obstacle_hieght);
 extern void vff_propagate(float accel, float dt);
 extern void vff_update_baro(float z_meas);
 extern void vff_update_z(float z_meas);
-extern void vff_update_offset(float offset);
 extern void vff_update_baro_conf(float z_meas, float conf);
 extern void vff_update_z_conf(float z_meas, float conf);
 extern void vff_update_vz_conf(float vz_meas, float conf);
 extern void vff_realign(float z_meas);
+extern void vff_update_agl(float z_meas, float conf);
 
 #endif /* VF_EXTENDED_FLOAT_H */

sw/airborne/subsystems/ins/vf_float.c

@@ -255,5 +255,6 @@ void vff_update_vz_conf(float vz_meas, float conf)
 
 void vff_realign(float z_meas)
 {
-  vff_init(z_meas, 0., 0.);
+  vff.z    = z_meas;
+  vff.zdot = 0.;
 }

sw/airborne/test/test_baro_board.c

@@ -65,10 +65,8 @@ __attribute__((weak)) void test_baro_board_imu_event_task(void)
   /* Optionally, to be overriden by board specific code */
 }
 
+/* BARO_PERIODIC_FREQUENCY is defined in the shared/baro_board.makefile and defaults to 50Hz */
 
-#ifndef BARO_PERIODIC_FREQUENCY
-#define BARO_PERIODIC_FREQUENCY 50
-#endif
 PRINT_CONFIG_VAR(BARO_PERIODIC_FREQUENCY)
 
 #ifdef BARO_LED

sw/simulator/nps/nps_autopilot_fixedwing.c

@@ -158,7 +158,9 @@ void nps_autopilot_run_step(double time)
     AbiSendMsgAGL(AGL_SONAR_NPS_ID, dist);
 
     uint16_t foo = 0;
+#ifdef SENSOR_SYNC_SEND_SONAR
     DOWNLINK_SEND_SONAR(DefaultChannel, DefaultDevice, &foo, &dist);
+#endif
 
     Fbw(event_task);
     Ap(event_task);

sw/simulator/nps/nps_autopilot_rotorcraft.c

@@ -134,7 +134,9 @@ void nps_autopilot_run_step(double time)
     AbiSendMsgAGL(AGL_SONAR_NPS_ID, dist);
 
     uint16_t foo = 0;
+#ifdef SENSOR_SYNC_SEND_SONAR
     DOWNLINK_SEND_SONAR(DefaultChannel, DefaultDevice, &foo, &dist);
+#endif
 
     main_event();
   }