Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-06-04 13:55:40 +08:00
Code Issues Actions 30 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="sonar_adc.xml">
<configure name="ADC_SONAR" value="ADC_0"/>
<define name="USE_SONAR"/>
<!--define name="SENSOR_SYNC_SEND_SONAR"/-->
</load>
<!--load name="baro_mpl3115.xml">
@@ -24,6 +25,7 @@
</modules>
<firmware name="rotorcraft">
<define name="DEBUG_VFF_EXTENDED"/>
<define name="USE_INS_NAV_INIT"/>
<!--define name="GUIDANCE_H_USE_REF"/-->
@@ -185,7 +187,6 @@
<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_">
conf/settings/estimation/ins_sonar.xml
+1 -1
View File
@@ -3,7 +3,7 @@
<settings>
<dl_settings>
<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>
</settings>
sw/airborne/boards/ardrone/navdata.c
+2 -3
View File
@@ -64,10 +64,9 @@ measures_t navdata;
* Sensor sensitivity in m/adc (float)
*/
#ifndef SONAR_SCALE
#define SONAR_SCALE 0.00034
#define SONAR_SCALE 0.00047
#endif
// FIXME(ben): there must be a better home for these
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
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);
}
#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) {
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.baro_initialized = TRUE;
}
if (ins_impl.vf_reset) {
if (ins_impl.vf_reset && ins_impl.baro_initialized) {
ins_impl.vf_reset = FALSE;
ins_impl.qfe = *pressure;
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
/* 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_idx = (var_idx + 1) % VAR_ERR_MAX;
float var = variance_float(var_err, VAR_ERR_MAX);