Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-05-21 20:04:09 +08:00
Code Issues Actions 27 Packages Projects Releases Wiki Activity

[sonar] sonar system using ABI interface

Browse Source 
- 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
This commit is contained in:
Gautier Hattenberger
2014-03-03 23:53:19 +01:00
parent 0a391da275
commit 03235ffae0
13 changed files with 84 additions and 112 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/abi.xml
+5
View File
@@ -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
+1 -1
View File
@@ -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
+7 -3
View File
@@ -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
+1 -2
View File
@@ -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="scaled" type="float" unit="m"/>
<field name="distance" type="float" unit="m"/>
<field name="var" type="float"/>
</message>
conf/modules/sonar_adc.xml
+2 -2
View File
@@ -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
-28
View File
@@ -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
+17 -4
View File
@@ -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
sw/airborne/modules/sonar/sonar_adc.c
+17 -20
View File
@@ -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
}
sw/airborne/modules/sonar/sonar_adc.h
+5 -17
View File
@@ -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
sw/airborne/subsystems/abi_sender_ids.h
+12
View File
@@ -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
+16 -23
View File
@@ -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 {
sw/airborne/subsystems/ins/ins_int.h
+1 -8
View File
@@ -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
};
sw/airborne/subsystems/sonar.h
-4
View File
@@ -1,4 +0,0 @@
extern uint16_t sonar_meas;