Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-06-04 22:17:01 +08:00
Code Issues Actions 29 Packages Projects Releases Wiki Activity

[sonar] fix some sonar related issues

Browse Source
This commit is contained in:
Gautier Hattenberger
2014-03-13 17:46:21 +01:00
parent 7ca6ca53e3
commit e138f43b44
4 changed files with 9 additions and 8 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/airframes/ENAC/quadrotor/booz2_g1.xml
+2 -1
View File
@@ -5,6 +5,7 @@
<load name="rotorcraft_cam.xml"/> <load name="rotorcraft_cam.xml"/>
<load name="sonar_adc.xml"> <load name="sonar_adc.xml">
<configure name="ADC_SONAR" value="ADC_0"/> <configure name="ADC_SONAR" value="ADC_0"/>
<define name="USE_SONAR"/>
<!--define name="SENSOR_SYNC_SEND_SONAR"/--> <!--define name="SENSOR_SYNC_SEND_SONAR"/-->
</load> </load>
<!--load name="baro_mpl3115.xml"> <!--load name="baro_mpl3115.xml">
@@ -24,6 +25,7 @@
</modules> </modules>
<firmware name="rotorcraft"> <firmware name="rotorcraft">
<define name="DEBUG_VFF_EXTENDED"/>
<define name="USE_INS_NAV_INIT"/> <define name="USE_INS_NAV_INIT"/>
<!--define name="GUIDANCE_H_USE_REF"/--> <!--define name="GUIDANCE_H_USE_REF"/-->
@@ -185,7 +187,6 @@
<section name="SONAR"> <section name="SONAR">
<!--define name="SONAR_SENS" value="0.00650498" integer="16"/--> <!-- XL-MaxSonar-EZ4 5V supply --> <!--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_SENS" value="0.016775" integer="16"/> <!-- XL-MaxSonar-EZ4 5V supply scaled to 3.3V -->
<define name="USE_SONAR"/>
</section> </section>
<section name="GUIDANCE_V" prefix="GUIDANCE_V_"> <section name="GUIDANCE_V" prefix="GUIDANCE_V_">
conf/settings/estimation/ins_sonar.xml
+1 -1
View File
@@ -3,7 +3,7 @@
<settings> <settings>
<dl_settings> <dl_settings>
<dl_settings NAME="sonar"> <dl_settings NAME="sonar">
<dl_setting MAX="1" MIN="0" STEP="1" VAR="ins_update_on_agl" shortname="use_sonar" values="FALSE|TRUE" module="subsystems/ins/vf_extended_float"/> <dl_setting MAX="1" MIN="0" STEP="1" VAR="ins_impl.update_on_agl" shortname="use_sonar" values="FALSE|TRUE" module="subsystems/ins/vf_extended_float"/>
</dl_settings> </dl_settings>
</dl_settings> </dl_settings>
</settings> </settings>
sw/airborne/boards/ardrone/navdata.c
+2 -3
View File
@@ -64,10 +64,9 @@ measures_t navdata;
* Sensor sensitivity in m/adc (float) * Sensor sensitivity in m/adc (float)
*/ */
#ifndef SONAR_SCALE #ifndef SONAR_SCALE
#define SONAR_SCALE 0.00034 #define SONAR_SCALE 0.00047
#endif #endif
// FIXME(ben): there must be a better home for these // FIXME(ben): there must be a better home for these
ssize_t full_write(int fd, const uint8_t *buf, size_t count) ssize_t full_write(int fd, const uint8_t *buf, size_t count)
{ {
@@ -482,7 +481,7 @@ void navdata_update()
// Check if there is a new sonar measurement and update the sonar // Check if there is a new sonar measurement and update the sonar
if (navdata.ultrasound >> 15) if (navdata.ultrasound >> 15)
{ {
float sonar_meas = (navdata.ultrasound & 0x7FFF); float sonar_meas = (float)((navdata.ultrasound & 0x7FFF) - SONAR_OFFSET) * SONAR_SCALE;
AbiSendMsgAGL(AGL_SONAR_ARDRONE2_ID, &sonar_meas); AbiSendMsgAGL(AGL_SONAR_ARDRONE2_ID, &sonar_meas);
} }
#endif #endif
sw/airborne/subsystems/ins/ins_int.c
+4 -3
View File
@@ -230,11 +230,12 @@ void ins_propagate(void) {
} }
static void baro_cb(uint8_t __attribute__((unused)) sender_id, const float *pressure) { static void baro_cb(uint8_t __attribute__((unused)) sender_id, const float *pressure) {
if (!ins_impl.baro_initialized) { if (!ins_impl.baro_initialized && *pressure > 1e-7) {
// wait for a first positive value
ins_impl.qfe = *pressure; ins_impl.qfe = *pressure;
ins_impl.baro_initialized = TRUE; ins_impl.baro_initialized = TRUE;
} }
if (ins_impl.vf_reset) { if (ins_impl.vf_reset && ins_impl.baro_initialized) {
ins_impl.vf_reset = FALSE; ins_impl.vf_reset = FALSE;
ins_impl.qfe = *pressure; ins_impl.qfe = *pressure;
vff_realign(0.); vff_realign(0.);
@@ -323,7 +324,7 @@ static void sonar_cb(uint8_t __attribute__((unused)) sender_id, const float *dis
#ifdef INS_SONAR_VARIANCE_THRESHOLD #ifdef INS_SONAR_VARIANCE_THRESHOLD
/* compute variance of error between sonar and baro alt */ /* compute variance of error between sonar and baro alt */
float err = distance + 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_err[var_idx] = err;
var_idx = (var_idx + 1) % VAR_ERR_MAX; var_idx = (var_idx + 1) % VAR_ERR_MAX;
float var = variance_float(var_err, VAR_ERR_MAX); float var = variance_float(var_err, VAR_ERR_MAX);