Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-06-06 07:53:43 +08:00
Code Issues Actions 26 Packages Projects Releases Wiki Activity

[ins] use sonar only with ins_int_extended

Browse Source
This commit is contained in:
Gautier Hattenberger
2013-08-22 15:04:59 +02:00
parent a18a74faf7
commit fd9130c88f
4 changed files with 6 additions and 28 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/firmwares/subsystems/rotorcraft/ins_extended.makefile
+1 -1
View File
@@ -2,7 +2,7 @@
# extended INS with vertical filter using sonar in a better way (flap ground)
#
$(TARGET).CFLAGS += -DINS_TYPE_H=\"subsystems/ins/ins_int.h\"
$(TARGET).CFLAGS += -DINS_TYPE_H=\"subsystems/ins/ins_int_extended.h\"
$(TARGET).srcs += $(SRC_SUBSYSTEMS)/ins.c
$(TARGET).srcs += $(SRC_SUBSYSTEMS)/ins/ins_int_extended.c
sw/airborne/subsystems/ins/ins_int.c
-19
View File
@@ -73,10 +73,6 @@ struct FloatVect2 ins_gps_speed_m_s_ned;
int32_t ins_qfe;
bool_t ins_baro_initialised;
int32_t ins_baro_alt;
#if USE_SONAR
bool_t ins_update_on_agl;
int32_t ins_sonar_offset;
#endif
#endif
/* output */
@@ -107,9 +103,6 @@ void ins_init() {
#endif
#if USE_VFF
ins_baro_initialised = FALSE;
#if USE_SONAR
ins_update_on_agl = FALSE;
#endif
vff_init(0., 0., 0.);
#endif
ins.vf_realign = FALSE;
@@ -185,9 +178,6 @@ void ins_update_baro() {
if (ins.vf_realign) {
ins.vf_realign = FALSE;
ins_qfe = baro.absolute;
#if USE_SONAR
ins_sonar_offset = sonar_meas;
#endif
vff_realign(0.);
ins_ltp_accel.z = ACCEL_BFP_OF_REAL(vff_zdotdot);
ins_ltp_speed.z = SPEED_BFP_OF_REAL(vff_zdot);
@@ -263,13 +253,4 @@ void ins_update_gps(void) {
}
void ins_update_sonar() {
#if USE_SONAR && USE_VFF
static int32_t sonar_filtered = 0;
sonar_filtered = (sonar_meas + 2*sonar_filtered) / 3;
/* update baro_qfe assuming a flat ground */
if (ins_update_on_agl && baro.status == BS_RUNNING) {
int32_t d_sonar = (((int32_t)sonar_filtered - ins_sonar_offset) * INS_SONAR_SENS_NUM) / INS_SONAR_SENS_DEN;
ins_qfe = baro.absolute + (d_sonar * (INS_BARO_SENS_DEN))/INS_BARO_SENS_NUM;
}
#endif
}
sw/airborne/subsystems/ins/ins_int.h
+1 -5
View File
@@ -49,14 +49,10 @@ extern struct NedCoor_i ins_gps_pos_cm_ned;
extern struct NedCoor_i ins_gps_speed_cm_s_ned;
/* barometer */
#if USE_VFF || USE_VFF_EXTENDED
#if USE_VFF
extern int32_t ins_baro_alt; ///< altitude calculated from baro in meters with #INT32_POS_FRAC
extern int32_t ins_qfe;
extern bool_t ins_baro_initialised;
#if USE_SONAR
extern bool_t ins_update_on_agl; /* use sonar to update agl if available */
extern int32_t ins_sonar_offset;
#endif
#endif
/* output LTP NED */
sw/airborne/subsystems/ins/ins_int_extended.c
+4 -3
View File
@@ -27,7 +27,7 @@
*
*/
#include "subsystems/ins/ins_int.h"
#include "subsystems/ins/ins_int_extended.h"
#include "subsystems/imu.h"
#include "subsystems/sensors/baro.h"
@@ -270,8 +270,8 @@ uint8_t var_idx = 0;
#endif
#if USE_SONAR
void ins_update_sonar() {
#if USE_SONAR
static float last_offset = 0.;
// new value filtered with median_filter
ins_sonar_alt = update_median_filter(&sonar_median, sonar_meas);
@@ -318,5 +318,6 @@ void ins_update_sonar() {
/* update offset with last value to avoid divergence */
vff_update_offset(last_offset);
}
}
#endif // USE_SONAR
}