diff --git a/conf/abi.xml b/conf/abi.xml
index 1b4d345a29..ff6cd327c0 100644
--- a/conf/abi.xml
+++ b/conf/abi.xml
@@ -11,6 +11,11 @@
+
+
+
+
+
diff --git a/conf/airframes/ENAC/quadrotor/blender.xml b/conf/airframes/ENAC/quadrotor/blender.xml
index e1946c7a97..1cd73dd866 100644
--- a/conf/airframes/ENAC/quadrotor/blender.xml
+++ b/conf/airframes/ENAC/quadrotor/blender.xml
@@ -5,7 +5,7 @@
-
+
diff --git a/conf/airframes/ENAC/quadrotor/booz2_g1.xml b/conf/airframes/ENAC/quadrotor/booz2_g1.xml
index 632b993672..d3a1f27697 100644
--- a/conf/airframes/ENAC/quadrotor/booz2_g1.xml
+++ b/conf/airframes/ENAC/quadrotor/booz2_g1.xml
@@ -3,7 +3,7 @@
-
+
@@ -177,13 +177,17 @@
+
+
diff --git a/conf/messages.xml b/conf/messages.xml
index 0626f1ef4a..1cce017701 100644
--- a/conf/messages.xml
+++ b/conf/messages.xml
@@ -1498,8 +1498,7 @@
-
-
+
diff --git a/conf/modules/sonar_adc.xml b/conf/modules/sonar_adc.xml
index e0dd84eca3..83506640ff 100644
--- a/conf/modules/sonar_adc.xml
+++ b/conf/modules/sonar_adc.xml
@@ -7,8 +7,9 @@
Reads an anlog sonar sensor and outputs sonar distance in [m]
-
+
+
@@ -24,7 +25,6 @@
-
diff --git a/conf/modules/sonar_adc_ins.xml b/conf/modules/sonar_adc_ins.xml
deleted file mode 100644
index 7f0257f98e..0000000000
--- a/conf/modules/sonar_adc_ins.xml
+++ /dev/null
@@ -1,28 +0,0 @@
-
-
-
-
-
- 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.
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
diff --git a/sw/airborne/boards/ardrone/navdata.c b/sw/airborne/boards/ardrone/navdata.c
index 099034a73c..4373e6fa80 100644
--- a/sw/airborne/boards/ardrone/navdata.c
+++ b/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"
-uint16_t sonar_meas = 0;
+/** Sonar offset.
+ * 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);
- ins_update_sonar();
+ float sonar_meas = (navdata.ultrasound & 0x7FFF);
+ AbiSendMsgAGL(AGL_SONAR_ARDRONE2_ID, &sonar_meas);
}
#endif
diff --git a/sw/airborne/modules/sonar/sonar_adc.c b/sw/airborne/modules/sonar/sonar_adc.c
index 189cf079eb..14268aa27a 100644
--- a/sw/airborne/modules/sonar/sonar_adc.c
+++ b/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)
- * equals to the height when the ADC gives 0
+ * Offset value in ADC
+ * 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;
-bool_t sonar_data_available;
-float sonar_distance;
-float sonar_offset;
-float sonar_scale;
+struct SonarAdc sonar_adc;
#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_data_available = FALSE;
- sonar_distance = 0;
- sonar_offset = SONAR_OFFSET;
- sonar_scale = SONAR_SCALE;
+ sonar_adc.meas = 0;
+ sonar_adc.offset = SONAR_OFFSET;
#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_data_available = TRUE;
- sonar_distance = ((float)sonar_meas * sonar_scale) + sonar_offset;
-
+ sonar_adc.meas = sonar_adc_buf.sum / sonar_adc_buf.av_nb_sample;
+ sonar_distance = (float)(sonar_adc.meas - sonar_adc.offset) * SONAR_SCALE;
#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
}
diff --git a/sw/airborne/modules/sonar/sonar_adc.h b/sw/airborne/modules/sonar/sonar_adc.h
index ec5a8c06cb..1bccbe0ea6 100644
--- a/sw/airborne/modules/sonar/sonar_adc.h
+++ b/sw/airborne/modules/sonar/sonar_adc.h
@@ -29,26 +29,14 @@
#include "std.h"
-/** Raw ADC value.
- */
-extern uint16_t sonar_meas;
+struct SonarAdc {
+ uint16_t meas; ///< Raw ADC value
+ uint16_t offset; ///< Sonar offset in ADC
+};
-/** New data available.
- */
-extern bool_t sonar_data_available;
-
-/** Sonar distance in m.
- */
-extern float sonar_distance;
+extern struct SonarAdc sonar_adc;
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
diff --git a/sw/airborne/subsystems/abi_sender_ids.h b/sw/airborne/subsystems/abi_sender_ids.h
index f87b5298b8..b4f29738d5 100644
--- a/sw/airborne/subsystems/abi_sender_ids.h
+++ b/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 */
diff --git a/sw/airborne/subsystems/ins/ins_int.c b/sw/airborne/subsystems/ins/ins_int.c
index 8693feef2e..32143b5a4d 100644
--- a/sw/airborne/subsystems/ins/ins_int.c
+++ b/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);
- ins_impl.sonar_offset = INS_SONAR_OFFSET;
+ // Bind to AGL message
+ 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,&ins_impl.sonar_alt, &sonar, &var);
+ DOWNLINK_SEND_INS_SONAR(DefaultChannel,DefaultDevice, distance, &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 {
diff --git a/sw/airborne/subsystems/ins/ins_int.h b/sw/airborne/subsystems/ins/ins_int.h
index d04ef30c6e..0dfa18eb34 100644
--- a/sw/airborne/subsystems/ins/ins_int.h
+++ b/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 */
- int32_t sonar_alt;
- int32_t sonar_offset;
- struct MedianFilterInt sonar_median;
+ bool_t update_on_agl; ///< use sonar to update agl if available
#endif
};
diff --git a/sw/airborne/subsystems/sonar.h b/sw/airborne/subsystems/sonar.h
deleted file mode 100644
index daf310400d..0000000000
--- a/sw/airborne/subsystems/sonar.h
+++ /dev/null
@@ -1,4 +0,0 @@
-
-
-
-extern uint16_t sonar_meas;