[sonar] sonar system using ABI interface

- unified interface for sonar (sonar_adc was broken with latest commit
  for ardrone)
- median filter removed from ins_int, implement in sonar drivers if
  needed
- compiles but not tested yet

conf/abi.xml

@@ -11,6 +11,11 @@
     <message name="BARO_DIFF" id="1">
       <field name="pressure" type="float" unit="Pa"/>
     </message>
+
+    <message name="AGL" id="2">
+      <field name="distance" type="float" unit="m"/>
+    </message>
+
   </class>
 
 

conf/airframes/ENAC/quadrotor/blender.xml

@@ -5,7 +5,7 @@
   <modules main_freq="512">
     <load name="servo_switch.xml"/>
     <load name="rotorcraft_cam.xml"/>
-    <!--load name="sonar_adc_ins.xml"/-->
+    <!--load name="sonar_adc.xml"/-->
     <!--load name="adc_generic.xml">
       <configure name="ADC_CHANNEL_GENERIC1" value="ADC_0"/>
     </load-->

conf/airframes/ENAC/quadrotor/booz2_g1.xml

@@ -3,7 +3,7 @@
   <modules main_freq="512">
     <load name="servo_switch.xml"/>
     <load name="rotorcraft_cam.xml"/>
-    <load name="sonar_adc_ins.xml">
+    <load name="sonar_adc.xml">
       <configure name="ADC_SONAR" value="ADC_0"/>
       <!--define name="SENSOR_SYNC_SEND_SONAR"/-->
     </load>
@@ -177,13 +177,17 @@
   </section>
 
   <section name="INS" prefix="INS_">
-    <!--define name="SONAR_SENS" value="0.00650498" integer="16"/--> <!-- XL-MaxSonar-EZ4 5V supply -->
-    <define name="SONAR_SENS" value="0.016775" integer="16"/> <!-- XL-MaxSonar-EZ4 5V supply scaled to 3.3V -->
     <define name="SONAR_MAX_RANGE" value="5.0"/>
     <define name="SONAR_MIN_RANGE" value="0.25"/>
     <!--define name="SONAR_VARIANCE_THRESHOLD" value="0.01"/-->
   </section>
 
+  <section name="SONAR">
+    <!--define name="SONAR_SENS" value="0.00650498" integer="16"/--> <!-- XL-MaxSonar-EZ4 5V supply -->
+    <define name="SONAR_SENS" value="0.016775" integer="16"/> <!-- XL-MaxSonar-EZ4 5V supply scaled to 3.3V -->
+    <define name="USE_SONAR"/>
+  </section>
+
   <section name="GUIDANCE_V" prefix="GUIDANCE_V_">
     <define name="MIN_ERR_Z"    value="POS_BFP_OF_REAL(-10.)"/>
     <define name="MAX_ERR_Z"    value="POS_BFP_OF_REAL( 10.)"/>

conf/messages.xml

@@ -1498,8 +1498,7 @@
   </message>
 
   <message name="INS_SONAR" id="167">
-    <field name="raw" type="int32" unit="adc" alt_unit="m" alt_unit_coef="0.0039063"/>
+    <field name="distance" type="float" unit="m"/>
-    <field name="scaled" type="float" unit="m"/>
     <field name="var" type="float"/>
   </message>
 

conf/modules/sonar_adc.xml

@@ -7,8 +7,9 @@
      Reads an anlog sonar sensor and outputs sonar distance in [m]
     </description>
     <configure name="ADC_SONAR" value="ADC_X" description="select ADC to use with the sonar"/>
-    <define name="SONAR_OFFSET" value="0.0"  description="sensor offset in [m] - default is 0.0"/>
+    <define name="SONAR_OFFSET" value="0"  description="sensor offset in [adc] - default is 0"/>
     <define name="SONAR_SCALE" value="0.0166" description="sensor scale factor [m/adc] - default is 0.0166"/>
+    <define name="USE_SONAR" value="" description="activate use of sonar in INS extended filter (only rotorcraft)"/>
   </doc>
 
   <header>
@@ -24,7 +25,6 @@
   <makefile target="ap">
     <define name="ADC_CHANNEL_SONAR" value="$(ADC_SONAR)"/>
     <define name="USE_$(ADC_SONAR)"/>
-    <define name="USE_SONAR"/>
   </makefile>
 
 </module>

conf/modules/sonar_adc_ins.xml

@@ -1,28 +0,0 @@
-<!DOCTYPE module SYSTEM "module.dtd">
-
-<module name="sonar">
-  <doc>
-    <description>
-     Sonar ADC (INS bindings).
-     Sonar ADC driver with INS binding, wich oes the same than sonar_adc module with an event function to feed INS subsystem.
-     Even if SONAR_OFFSET and _SCALE can be set, currently only the raw value and the INS_SONAR_SENS will be used in ins filters.
-    </description>
-    <configure name="ADC_SONAR" value="ADC_X" description="select ADC to use with the sonar"/>
-  </doc>
-  <header>
-    <file name="sonar_adc.h"/>
-    <file name="ins.h" dir="subsystems"/>
-  </header>
-  <init fun="sonar_adc_init()"/>
-  <periodic fun="sonar_adc_read()" freq="10"/>
-  <event fun="SonarEvent(ins_update_sonar)"/>
-  <makefile>
-    <file name="sonar_adc.c"/>
-  </makefile>
-  <makefile target="ap">
-    <define name="ADC_CHANNEL_SONAR" value="$(ADC_SONAR)"/>
-    <define name="USE_$(ADC_SONAR)"/>
-    <define name="USE_SONAR"/>
-  </makefile>
-
-</module>

sw/airborne/boards/ardrone/navdata.c

@@ -40,6 +40,7 @@
 #include "std.h"
 #include "navdata.h"
 #include "subsystems/ins.h"
+#include "subsystems/abi.h"
 
 #define NAVDATA_PACKET_SIZE 60
 #define NAVDATA_START_BYTE 0x3a
@@ -51,8 +52,20 @@ navdata_port nav_port;
 static int nav_fd = 0;
 measures_t navdata;
 
-#include "subsystems/sonar.h"
+/** Sonar offset.
-uint16_t sonar_meas = 0;
+ *  Offset value in ADC
+ *  equals to the ADC value so that height is zero
+ */
+#ifndef SONAR_OFFSET
+#define SONAR_OFFSET 880
+#endif
+
+/** Sonar scale.
+ *  Sensor sensitivity in m/adc (float)
+ */
+#ifndef SONAR_SCALE
+#define SONAR_SCALE 0.00034
+#endif
 
 
 // FIXME(ben): there must be a better home for these
@@ -469,8 +482,8 @@ void navdata_update()
         // Check if there is a new sonar measurement and update the sonar
         if (navdata.ultrasound >> 15)
         {
-          sonar_meas = (navdata.ultrasound & 0x7FFF);
+          float sonar_meas = (navdata.ultrasound & 0x7FFF);
-          ins_update_sonar();
+          AbiSendMsgAGL(AGL_SONAR_ARDRONE2_ID, &sonar_meas);
         }
 #endif
 

sw/airborne/modules/sonar/sonar_adc.c

@@ -23,6 +23,7 @@
 #include "modules/sonar/sonar_adc.h"
 #include "generated/airframe.h"
 #include "mcu_periph/adc.h"
+#include "subsystems/abi.h"
 #ifdef SITL
 #include "state.h"
 #endif
@@ -32,11 +33,11 @@
 #include "subsystems/datalink/downlink.h"
 
 /** Sonar offset.
- *  Offset value in m (float)
+ *  Offset value in ADC
- *  equals to the height when the ADC gives 0
+ *  equals to the ADC value so that height is zero
  */
 #ifndef SONAR_OFFSET
-#define SONAR_OFFSET 0.
+#define SONAR_OFFSET 0
 #endif
 
 /** Sonar scale.
@@ -46,43 +47,39 @@
 #define SONAR_SCALE 0.0166
 #endif
 
-uint16_t sonar_meas;
+struct SonarAdc sonar_adc;
-bool_t sonar_data_available;
-float sonar_distance;
-float sonar_offset;
-float sonar_scale;
 
 #ifndef SITL
-static struct adc_buf sonar_adc;
+static struct adc_buf sonar_adc_buf;
 #endif
 
 void sonar_adc_init(void) {
-  sonar_meas = 0;
+  sonar_adc.meas = 0;
-  sonar_data_available = FALSE;
+  sonar_adc.offset = SONAR_OFFSET;
-  sonar_distance = 0;
-  sonar_offset = SONAR_OFFSET;
-  sonar_scale = SONAR_SCALE;
 
 #ifndef SITL
-  adc_buf_channel(ADC_CHANNEL_SONAR, &sonar_adc, DEFAULT_AV_NB_SAMPLE);
+  adc_buf_channel(ADC_CHANNEL_SONAR, &sonar_adc_buf, DEFAULT_AV_NB_SAMPLE);
 #endif
 }
 
 /** Read ADC value to update sonar measurement
  */
 void sonar_adc_read(void) {
+  float sonar_distance;
 #ifndef SITL
-  sonar_meas = sonar_adc.sum / sonar_adc.av_nb_sample;
+  sonar_adc.meas = sonar_adc_buf.sum / sonar_adc_buf.av_nb_sample;
-  sonar_data_available = TRUE;
+  sonar_distance = (float)(sonar_adc.meas - sonar_adc.offset) * SONAR_SCALE;
-  sonar_distance = ((float)sonar_meas * sonar_scale) + sonar_offset;
-
 #else // SITL
   sonar_distance = stateGetPositionEnu_f()->z;
   Bound(sonar_distance, 0.1f, 7.0f);
 #endif // SITL
 
+  // Send ABI message
+  AbiSendMsgAGL(AGL_SONAR_ADC_ID, &sonar_distance);
+
 #ifdef SENSOR_SYNC_SEND_SONAR
-  DOWNLINK_SEND_SONAR(DefaultChannel, DefaultDevice, &sonar_meas, &sonar_distance);
+  // Send Telemetry report
+  DOWNLINK_SEND_SONAR(DefaultChannel, DefaultDevice, &sonar_adc.meas, &sonar_distance);
 #endif
 }
 

sw/airborne/modules/sonar/sonar_adc.h

@@ -29,26 +29,14 @@
 
 #include "std.h"
 
-/** Raw ADC value.
+struct SonarAdc {
- */
+  uint16_t meas;          ///< Raw ADC value
-extern uint16_t sonar_meas;
+  uint16_t offset;        ///< Sonar offset in ADC
+};
 
-/** New data available.
+extern struct SonarAdc sonar_adc;
- */
-extern bool_t sonar_data_available;
-
-/** Sonar distance in m.
- */
-extern float sonar_distance;
 
 extern void sonar_adc_init(void);
 extern void sonar_adc_read(void);
 
-#define SonarEvent(_handler) { \
-  if (sonar_data_available) { \
-    _handler(); \
-    sonar_data_available = FALSE; \
-  } \
-}
-
 #endif

sw/airborne/subsystems/abi_sender_ids.h

@@ -76,4 +76,16 @@
 #define BARO_SIM_SENDER_ID 19
 #endif
 
+/*
+ * IDs of AGL measurment modules that can be loaded (sonars,...)
+ */
+#ifndef AGL_SONAR_ADC_ID
+#define AGL_SONAR_ADC_ID 1
+#endif
+
+#ifndef AGL_SONAR_ARDRONE2_ID
+#define AGL_SONAR_ARDRONE2_ID 2
+#endif
+
+
 #endif /* ABI_SENDER_IDS_H */

sw/airborne/subsystems/ins/ins_int.c

@@ -58,18 +58,23 @@
 
 
 #if USE_SONAR
- #include "subsystems/sonar.h"
-
 #if !USE_VFF_EXTENDED
 #error USE_SONAR needs USE_VFF_EXTENDED
 #endif
 
+/** default sonar to use in INS */
+#ifndef INS_SONAR_ID
+#define INS_SONAR_ID ABI_BROADCAST
+#endif
+abi_event sonar_ev;
+static void sonar_cb(uint8_t sender_id, const float *distance);
+
 #ifdef INS_SONAR_THROTTLE_THRESHOLD
 #include "firmwares/rotorcraft/stabilization.h"
 #endif
 
 #ifndef INS_SONAR_OFFSET
-#define INS_SONAR_OFFSET 0
+#define INS_SONAR_OFFSET 0.
 #endif
 #define VFF_R_SONAR_0 0.1
 #define VFF_R_SONAR_OF_M 0.2
@@ -147,8 +152,8 @@ void ins_init(void) {
 
 #if USE_SONAR
   ins_impl.update_on_agl = INS_SONAR_UPDATE_ON_AGL;
-  init_median_filter(&ins_impl.sonar_median);
+  // Bind to AGL message
-  ins_impl.sonar_offset = INS_SONAR_OFFSET;
+  AbiBindMsgAGL(INS_SONAR_ID, &sonar_ev, sonar_cb);
 #endif
 
   ins_impl.vf_reset = FALSE;
@@ -313,38 +318,26 @@ uint8_t var_idx = 0;
 
 
 #if USE_SONAR
-void ins_update_sonar(void) {
+static void sonar_cb(uint8_t __attribute__((unused)) sender_id, const float *distance) {
   static float last_offset = 0.;
-  // new value filtered with median_filter
-  ins_impl.sonar_alt = update_median_filter(&ins_impl.sonar_median, sonar_meas);
-  float sonar = (ins_impl.sonar_alt - ins_impl.sonar_offset) * INS_SONAR_SENS;
 
 #ifdef INS_SONAR_VARIANCE_THRESHOLD
   /* compute variance of error between sonar and baro alt */
-  float err = sonar + ins_impl.baro_z; // sonar positive up, baro positive down !!!!
+  float err = distance + ins_impl.baro_z; // sonar positive up, baro positive down !!!!
   var_err[var_idx] = err;
   var_idx = (var_idx + 1) % VAR_ERR_MAX;
   float var = variance_float(var_err, VAR_ERR_MAX);
-  DOWNLINK_SEND_INS_SONAR(DefaultChannel,DefaultDevice,&err, &sonar, &var);
+  DOWNLINK_SEND_INS_SONAR(DefaultChannel,DefaultDevice, distance, &var);
-  //DOWNLINK_SEND_INS_SONAR(DefaultChannel,DefaultDevice,&ins_impl.sonar_alt, &sonar, &var);
 #endif
 
   /* update filter assuming a flat ground */
-  if (sonar < INS_SONAR_MAX_RANGE
+  if (*distance < INS_SONAR_MAX_RANGE
 #ifdef INS_SONAR_MIN_RANGE
-      && sonar > INS_SONAR_MIN_RANGE
+      && *distance > INS_SONAR_MIN_RANGE
 #endif
 #ifdef INS_SONAR_THROTTLE_THRESHOLD
       && stabilization_cmd[COMMAND_THRUST] < INS_SONAR_THROTTLE_THRESHOLD
 #endif
-#ifdef INS_SONAR_STAB_THRESHOLD
-      && stabilization_cmd[COMMAND_ROLL] < INS_SONAR_STAB_THRESHOLD
-      && stabilization_cmd[COMMAND_ROLL] > -INS_SONAR_STAB_THRESHOLD
-      && stabilization_cmd[COMMAND_PITCH] < INS_SONAR_STAB_THRESHOLD
-      && stabilization_cmd[COMMAND_PITCH] > -INS_SONAR_STAB_THRESHOLD
-      && stabilization_cmd[COMMAND_YAW] < INS_SONAR_STAB_THRESHOLD
-      && stabilization_cmd[COMMAND_YAW] > -INS_SONAR_STAB_THRESHOLD
-#endif
 #ifdef INS_SONAR_BARO_THRESHOLD
       && ins_impl.baro_z > -INS_SONAR_BARO_THRESHOLD /* z down */
 #endif
@@ -353,7 +346,7 @@ void ins_update_sonar(void) {
 #endif
       && ins_impl.update_on_agl
       && ins_impl.baro_initialized) {
-    vff_update_alt_conf(-sonar, VFF_R_SONAR_0 + VFF_R_SONAR_OF_M * fabs(sonar));
+    vff_update_alt_conf(-(*distance), VFF_R_SONAR_0 + VFF_R_SONAR_OF_M * fabsf(*distance));
     last_offset = vff.offset;
   }
   else {

sw/airborne/subsystems/ins/ins_int.h

@@ -34,10 +34,6 @@
 #include "math/pprz_geodetic_int.h"
 #include "math/pprz_algebra_float.h"
 
-#if USE_SONAR
-#include "filters/median_filter.h"
-#endif
-
 /** Ins implementation state (fixed point) */
 struct InsInt {
   struct LtpDef_i  ltp_def;
@@ -64,10 +60,7 @@ struct InsInt {
   bool_t baro_initialized;
 
 #if USE_SONAR
-  bool_t  update_on_agl; /* use sonar to update agl if available */
+  bool_t update_on_agl; ///< use sonar to update agl if available
-  int32_t sonar_alt;
-  int32_t sonar_offset;
-  struct MedianFilterInt sonar_median;
 #endif
 };
 

sw/airborne/subsystems/sonar.h

@@ -1,4 +0,0 @@
-
-
-
-extern uint16_t sonar_meas;