added agl estimator when assuming non flat ground

paparazzi · Jul 25, 2018 · f2e30d7 · f2e30d7
1 parent ae80024
commit f2e30d7
Show file tree

Hide file tree

Showing 17 changed files with 339 additions and 300 deletions.
diff --git a/conf/firmwares/subsystems/shared/baro_board.makefile b/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)

diff --git a/conf/firmwares/subsystems/shared/nps_common.makefile b/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)
+
diff --git a/conf/modules/ins_extended.xml b/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"/>
@@ -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>

diff --git a/conf/modules/ins_hff_extended.xml b/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>

diff --git a/conf/modules/sonar_bebop.xml b/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>

diff --git a/sw/airborne/boards/baro_board_ms5611_i2c.c b/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);

diff --git a/sw/airborne/boards/baro_board_ms5611_spi.c b/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);

diff --git a/sw/airborne/firmwares/fixedwing/main_ap.c b/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
 

diff --git a/sw/airborne/firmwares/rotorcraft/main_ap.c b/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)

diff --git a/sw/airborne/modules/sonar/sonar_bebop.c b/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,45 +168,40 @@ 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);
-    }
-
-    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;
-        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;
+    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);
+
+      // 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 {
-      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
       // Send Telemetry report
       DOWNLINK_SEND_SONAR(DefaultChannel, DefaultDevice, &sonar_bebop.meas, &sonar_bebop.distance);
 #endif
     }
+    usleep(10000); //100Hz
   }
-  usleep(10000); //100Hz
   return NULL;
 }