mirror of
https://github.com/paparazzi/paparazzi.git
synced 2026-05-22 20:36:06 +08:00
[ins] extended ins filter
This ins is using a vertical filter using the sonar in a better way, by estimating the baro offset It is probably not wise to use it without sonar for now, but using the gps should be a good idea The world is assumed to be flat here...
This commit is contained in:
Gautier Hattenberger
@@ -167,8 +167,10 @@ ap.CFLAGS += -DUSE_ADC
|
||||
ap.srcs += $(SRC_ARCH)/mcu_periph/adc_arch.c
|
||||
ap.srcs += subsystems/electrical.c
|
||||
# baro has variable offset amplifier on booz board
|
||||
ifeq ($(BOARD), booz)
|
||||
ap.CFLAGS += -DUSE_DAC
|
||||
ap.srcs += $(SRC_ARCH)/mcu_periph/dac_arch.c
|
||||
endif
|
||||
else ifeq ($(ARCH), stm32)
|
||||
ap.CFLAGS += -DUSE_ADC
|
||||
ap.CFLAGS += -DUSE_AD1 -DUSE_AD1_1 -DUSE_AD1_2 -DUSE_AD1_3 -DUSE_AD1_4
|
||||
@@ -208,19 +210,16 @@ ap.CFLAGS += -DUSE_NAVIGATION
|
||||
ap.srcs += $(SRC_FIRMWARE)/guidance/guidance_h.c
|
||||
ap.srcs += $(SRC_FIRMWARE)/guidance/guidance_v.c
|
||||
|
||||
ap.srcs += $(SRC_SUBSYSTEMS)/ins.c
|
||||
|
||||
#
|
||||
# INS choice
|
||||
#
|
||||
# include subsystems/rotorcraft/ins_hff.makefile
|
||||
# include subsystems/rotorcraft/ins.makefile
|
||||
# or
|
||||
# nothing
|
||||
# include subsystems/rotorcraft/ins_extended.makefile
|
||||
#
|
||||
# extra:
|
||||
# include subsystems/rotorcraft/ins_hff.makefile
|
||||
#
|
||||
|
||||
# vertical filter float version
|
||||
ap.srcs += $(SRC_SUBSYSTEMS)/ins/vf_float.c
|
||||
ap.CFLAGS += -DUSE_VFF -DDT_VFILTER='(1./$(PERIODIC_FREQUENCY).)'
|
||||
|
||||
ap.srcs += $(SRC_FIRMWARE)/navigation.c
|
||||
ap.srcs += subsystems/navigation/common_flight_plan.c
|
||||
|
||||
@@ -0,0 +1,10 @@
|
||||
#
|
||||
# simple INS with float vertical filter
|
||||
#
|
||||
|
||||
ap.srcs += $(SRC_SUBSYSTEMS)/ins.c
|
||||
|
||||
# vertical filter float version
|
||||
ap.srcs += $(SRC_SUBSYSTEMS)/ins/vf_float.c
|
||||
ap.CFLAGS += -DUSE_VFF -DDT_VFILTER='(1./$(PERIODIC_FREQUENCY).)'
|
||||
|
||||
@@ -0,0 +1,10 @@
|
||||
#
|
||||
# extended INS with vertical filter using sonar in a better way (flap ground)
|
||||
#
|
||||
|
||||
ap.srcs += $(SRC_SUBSYSTEMS)/ins_extended.c
|
||||
|
||||
# vertical filter float version
|
||||
ap.srcs += $(SRC_SUBSYSTEMS)/ins/vf_extended_float.c
|
||||
ap.CFLAGS += -DUSE_VFF_EXTENDED -DDT_VFILTER='(1./$(PERIODIC_FREQUENCY).)'
|
||||
|
||||
@@ -1334,6 +1334,16 @@
|
||||
</message>
|
||||
|
||||
<!-- 161 is free -->
|
||||
<message name="VFF_EXTENDED" id="161">
|
||||
<field name="measure1" type="float"/>
|
||||
<field name="measure2" type="float"/>
|
||||
<field name="z" type="float"/>
|
||||
<field name="zd" type="float"/>
|
||||
<field name="zdd" type="float"/>
|
||||
<field name="bias" type="float"/>
|
||||
<field name="offset" type="float"/>
|
||||
</message>
|
||||
|
||||
|
||||
<message name="VFF" id="162">
|
||||
<field name="measure" type="float"/>
|
||||
|
||||
@@ -0,0 +1,9 @@
|
||||
<!DOCTYPE settings SYSTEM "../settings.dtd">
|
||||
|
||||
<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_settings>
|
||||
</dl_settings>
|
||||
</settings>
|
||||
@@ -495,6 +495,21 @@
|
||||
#define PERIODIC_SEND_VFF(_trans, _dev) {}
|
||||
#endif
|
||||
|
||||
#if USE_VFF_EXTENDED
|
||||
#include "subsystems/ins/vf_extended_float.h"
|
||||
#define PERIODIC_SEND_VFF_EXTENDED(_trans, _dev) { \
|
||||
DOWNLINK_SEND_VFF_EXTENDED(_trans, _dev, \
|
||||
&vff_z_meas, \
|
||||
&vff_z_meas_baro, \
|
||||
&vff_z, \
|
||||
&vff_zdot, \
|
||||
&vff_bias, \
|
||||
&vff_offset); \
|
||||
}
|
||||
#else
|
||||
#define PERIODIC_SEND_VFF_EXTENDED(_trans, _dev) {}
|
||||
#endif
|
||||
|
||||
#if USE_HFF
|
||||
#include "subsystems/ins/hf_float.h"
|
||||
#define PERIODIC_SEND_HFF(_trans, _dev) { \
|
||||
|
||||
@@ -33,7 +33,7 @@ extern struct NedCoor_i ins_gps_pos_cm_ned;
|
||||
extern struct NedCoor_i ins_gps_speed_cm_s_ned;
|
||||
|
||||
/* barometer */
|
||||
#if USE_VFF
|
||||
#if USE_VFF || USE_VFF_EXTENDED
|
||||
extern int32_t ins_baro_alt;
|
||||
extern int32_t ins_qfe;
|
||||
extern bool_t ins_baro_initialised;
|
||||
|
||||
@@ -0,0 +1,313 @@
|
||||
/*
|
||||
* Copyright (C) 2008-2009 Antoine Drouin <poinix@gmail.com>
|
||||
* Copyright (C) 2012 Gautier Hattenberger
|
||||
*
|
||||
* This file is part of paparazzi.
|
||||
*
|
||||
* paparazzi is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation; either version 2, or (at your option)
|
||||
* any later version.
|
||||
*
|
||||
* paparazzi is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
* GNU General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License
|
||||
* along with paparazzi; see the file COPYING. If not, write to
|
||||
* the Free Software Foundation, 59 Temple Place - Suite 330,
|
||||
* Boston, MA 02111-1307, USA.
|
||||
*/
|
||||
|
||||
#include "subsystems/ins/vf_extended_float.h"
|
||||
|
||||
#define DEBUG_VFF_EXTENDED 1
|
||||
|
||||
#if DEBUG_VFF_EXTENDED
|
||||
#include "mcu_periph/uart.h"
|
||||
#include "messages.h"
|
||||
#include "subsystems/datalink/downlink.h"
|
||||
#endif
|
||||
|
||||
/*
|
||||
|
||||
X = [ z zdot accel_bias baro_offset ]
|
||||
|
||||
temps :
|
||||
propagate 86us
|
||||
update 46us
|
||||
|
||||
*/
|
||||
/* initial covariance diagonal */
|
||||
#define INIT_PXX 1.
|
||||
/* process noise */
|
||||
#define ACCEL_NOISE 0.5
|
||||
#define Qzz ACCEL_NOISE/512./512./2.
|
||||
#define Qzdotzdot ACCEL_NOISE/512.
|
||||
#define Qbiasbias 1e-7
|
||||
#define Qoffoff 1e-4
|
||||
#define R_BARO 1.
|
||||
#define R_ALT 0.1
|
||||
#define R_OFFSET 1.
|
||||
|
||||
float vff_z;
|
||||
float vff_zdot;
|
||||
float vff_bias;
|
||||
float vff_offset;
|
||||
float vff_zdotdot;
|
||||
|
||||
float vff_P[VFF_STATE_SIZE][VFF_STATE_SIZE];
|
||||
|
||||
float vff_z_meas;
|
||||
float vff_z_meas_baro;
|
||||
|
||||
void vff_init(float init_z, float init_zdot, float init_accel_bias, float init_baro_offset) {
|
||||
vff_z = init_z;
|
||||
vff_zdot = init_zdot;
|
||||
vff_bias = init_accel_bias;
|
||||
vff_offset = init_baro_offset;
|
||||
int i, j;
|
||||
for (i=0; i<VFF_STATE_SIZE; i++) {
|
||||
for (j=0; j<VFF_STATE_SIZE; j++)
|
||||
vff_P[i][j] = 0.;
|
||||
vff_P[i][i] = INIT_PXX;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
|
||||
F = [ 1 dt -dt^2/2 0
|
||||
0 1 -dt 0
|
||||
0 0 1 0
|
||||
0 0 0 1 ];
|
||||
|
||||
B = [ dt^2/2 dt 0 0]';
|
||||
|
||||
Q = [ Qzz 0 0 0
|
||||
0 Qzdotzdot 0 0
|
||||
0 0 Qbiasbias 0
|
||||
0 0 0 0 Qoffoff ];
|
||||
|
||||
Xk1 = F * Xk0 + B * accel;
|
||||
|
||||
Pk1 = F * Pk0 * F' + Q;
|
||||
|
||||
*/
|
||||
void vff_propagate(float accel) {
|
||||
/* update state */
|
||||
vff_zdotdot = accel + 9.81 - vff_bias;
|
||||
vff_z = vff_z + DT_VFILTER * vff_zdot;
|
||||
vff_zdot = vff_zdot + DT_VFILTER * vff_zdotdot;
|
||||
/* update covariance */
|
||||
const float FPF00 = vff_P[0][0] + DT_VFILTER * ( vff_P[1][0] + vff_P[0][1] + DT_VFILTER * vff_P[1][1] );
|
||||
const float FPF01 = vff_P[0][1] + DT_VFILTER * ( vff_P[1][1] - vff_P[0][2] - DT_VFILTER * vff_P[1][2] );
|
||||
const float FPF02 = vff_P[0][2] + DT_VFILTER * ( vff_P[1][2] );
|
||||
const float FPF10 = vff_P[1][0] + DT_VFILTER * (-vff_P[2][0] + vff_P[1][1] - DT_VFILTER * vff_P[2][1] );
|
||||
const float FPF11 = vff_P[1][1] + DT_VFILTER * (-vff_P[2][1] - vff_P[1][2] + DT_VFILTER * vff_P[2][2] );
|
||||
const float FPF12 = vff_P[1][2] + DT_VFILTER * (-vff_P[2][2] );
|
||||
const float FPF20 = vff_P[2][0] + DT_VFILTER * ( vff_P[2][1] );
|
||||
const float FPF21 = vff_P[2][1] + DT_VFILTER * (-vff_P[2][2] );
|
||||
const float FPF22 = vff_P[2][2];
|
||||
const float FPF33 = vff_P[3][3];
|
||||
|
||||
vff_P[0][0] = FPF00 + Qzz;
|
||||
vff_P[0][1] = FPF01;
|
||||
vff_P[0][2] = FPF02;
|
||||
vff_P[1][0] = FPF10;
|
||||
vff_P[1][1] = FPF11 + Qzdotzdot;
|
||||
vff_P[1][2] = FPF12;
|
||||
vff_P[2][0] = FPF20;
|
||||
vff_P[2][1] = FPF21;
|
||||
vff_P[2][2] = FPF22 + Qbiasbias;
|
||||
vff_P[3][3] = FPF33 + Qoffoff;
|
||||
|
||||
#if DEBUG_VFF_EXTENDED
|
||||
RunOnceEvery(10, DOWNLINK_SEND_VFF_EXTENDED(DefaultChannel, DefaultDevice, &vff_z_meas, &vff_z_meas_baro, &vff_z, &vff_zdot, &vff_zdotdot, &vff_bias, &vff_offset));
|
||||
#endif
|
||||
}
|
||||
|
||||
/*
|
||||
* Update sensor "with" offset (baro)
|
||||
H = [1 0 0 -1];
|
||||
// state residual
|
||||
y = rangemeter - H * Xm;
|
||||
// covariance residual
|
||||
S = H*Pm*H' + R;
|
||||
// kalman gain
|
||||
K = Pm*H'*inv(S);
|
||||
// update state
|
||||
Xp = Xm + K*y;
|
||||
// update covariance
|
||||
Pp = Pm - K*H*Pm;
|
||||
*/
|
||||
__attribute__ ((always_inline)) static inline void update_baro_conf(float z_meas, float conf) {
|
||||
vff_z_meas_baro = z_meas;
|
||||
|
||||
const float y = z_meas - vff_z + vff_offset;
|
||||
const float S = vff_P[0][0] - vff_P[0][3] - vff_P[3][0] + vff_P[3][3] + conf;
|
||||
const float K0 = (vff_P[0][0] - vff_P[0][3]) * 1/S;
|
||||
const float K1 = (vff_P[1][0] - vff_P[1][3]) * 1/S;
|
||||
const float K2 = (vff_P[2][0] - vff_P[2][3]) * 1/S;
|
||||
const float K3 = (vff_P[3][0] - vff_P[3][3]) * 1/S;
|
||||
|
||||
vff_z = vff_z + K0 * y;
|
||||
vff_zdot = vff_zdot + K1 * y;
|
||||
vff_bias = vff_bias + K2 * y;
|
||||
vff_offset = vff_offset + K3 * y;
|
||||
|
||||
const float P0 = vff_P[0][0] - vff_P[3][0];
|
||||
const float P1 = vff_P[0][1] - vff_P[3][1];
|
||||
const float P2 = vff_P[0][2] - vff_P[3][2];
|
||||
const float P3 = vff_P[0][3] - vff_P[3][3];
|
||||
|
||||
vff_P[0][0] -= K0 * P0;
|
||||
vff_P[0][1] -= K0 * P1;
|
||||
vff_P[0][2] -= K0 * P2;
|
||||
vff_P[0][3] -= K0 * P3;
|
||||
vff_P[1][0] -= K1 * P0;
|
||||
vff_P[1][1] -= K1 * P1;
|
||||
vff_P[1][2] -= K1 * P2;
|
||||
vff_P[1][3] -= K1 * P3;
|
||||
vff_P[2][0] -= K2 * P0;
|
||||
vff_P[2][1] -= K2 * P1;
|
||||
vff_P[2][2] -= K2 * P2;
|
||||
vff_P[2][3] -= K2 * P3;
|
||||
vff_P[3][0] -= K3 * P0;
|
||||
vff_P[3][1] -= K3 * P1;
|
||||
vff_P[3][2] -= K3 * P2;
|
||||
vff_P[3][3] -= K3 * P3;
|
||||
|
||||
}
|
||||
|
||||
void vff_update_baro(float z_meas) {
|
||||
update_baro_conf(z_meas, R_BARO);
|
||||
}
|
||||
|
||||
void vff_update_baro_conf(float z_meas, float conf) {
|
||||
update_baro_conf(z_meas, conf);
|
||||
}
|
||||
|
||||
/*
|
||||
* Update sensor "without" offset (gps, sonar)
|
||||
H = [1 0 0 0];
|
||||
// state residual
|
||||
y = rangemeter - H * Xm;
|
||||
// covariance residual
|
||||
S = H*Pm*H' + R;
|
||||
// kalman gain
|
||||
K = Pm*H'*inv(S);
|
||||
// update state
|
||||
Xp = Xm + K*y;
|
||||
// update covariance
|
||||
Pp = Pm - K*H*Pm;
|
||||
*/
|
||||
__attribute__ ((always_inline)) static inline void update_alt_conf(float z_meas, float conf) {
|
||||
vff_z_meas = z_meas;
|
||||
|
||||
const float y = z_meas - vff_z;
|
||||
const float S = vff_P[0][0] + conf;
|
||||
const float K0 = vff_P[0][0] * 1/S;
|
||||
const float K1 = vff_P[1][0] * 1/S;
|
||||
const float K2 = vff_P[2][0] * 1/S;
|
||||
const float K3 = vff_P[3][0] * 1/S;
|
||||
|
||||
vff_z = vff_z + K0 * y;
|
||||
vff_zdot = vff_zdot + K1 * y;
|
||||
vff_bias = vff_bias + K2 * y;
|
||||
vff_offset = vff_offset + K3 * y;
|
||||
|
||||
const float P0 = vff_P[0][0];
|
||||
const float P1 = vff_P[0][1];
|
||||
const float P2 = vff_P[0][2];
|
||||
const float P3 = vff_P[0][3];
|
||||
|
||||
vff_P[0][0] -= K0 * P0;
|
||||
vff_P[0][1] -= K0 * P1;
|
||||
vff_P[0][2] -= K0 * P2;
|
||||
vff_P[0][3] -= K0 * P3;
|
||||
vff_P[1][0] -= K1 * P0;
|
||||
vff_P[1][1] -= K1 * P1;
|
||||
vff_P[1][2] -= K1 * P2;
|
||||
vff_P[1][3] -= K1 * P3;
|
||||
vff_P[2][0] -= K2 * P0;
|
||||
vff_P[2][1] -= K2 * P1;
|
||||
vff_P[2][2] -= K2 * P2;
|
||||
vff_P[2][3] -= K2 * P3;
|
||||
vff_P[3][0] -= K3 * P0;
|
||||
vff_P[3][1] -= K3 * P1;
|
||||
vff_P[3][2] -= K3 * P2;
|
||||
vff_P[3][3] -= K3 * P3;
|
||||
}
|
||||
|
||||
void vff_update_alt(float z_meas) {
|
||||
update_alt_conf(z_meas, R_ALT);
|
||||
}
|
||||
|
||||
void vff_update_alt_conf(float z_meas, float conf) {
|
||||
update_alt_conf(z_meas, conf);
|
||||
}
|
||||
|
||||
/*
|
||||
* Update sensor offset (baro)
|
||||
H = [0 0 0 1];
|
||||
// state residual
|
||||
y = rangemeter - H * Xm;
|
||||
// covariance residual
|
||||
S = H*Pm*H' + R;
|
||||
// kalman gain
|
||||
K = Pm*H'*inv(S);
|
||||
// update state
|
||||
Xp = Xm + K*y;
|
||||
// update covariance
|
||||
Pp = Pm - K*H*Pm;
|
||||
*/
|
||||
__attribute__ ((always_inline)) static inline void update_offset_conf(float offset, float conf) {
|
||||
const float y = offset - vff_offset;
|
||||
const float S = vff_P[3][3] + conf;
|
||||
const float K0 = vff_P[0][3] * 1/S;
|
||||
const float K1 = vff_P[1][3] * 1/S;
|
||||
const float K2 = vff_P[2][3] * 1/S;
|
||||
const float K3 = vff_P[3][3] * 1/S;
|
||||
|
||||
vff_z = vff_z + K0 * y;
|
||||
vff_zdot = vff_zdot + K1 * y;
|
||||
vff_bias = vff_bias + K2 * y;
|
||||
vff_offset = vff_offset + K3 * y;
|
||||
|
||||
const float P0 = vff_P[3][0];
|
||||
const float P1 = vff_P[3][1];
|
||||
const float P2 = vff_P[3][2];
|
||||
const float P3 = vff_P[3][3];
|
||||
|
||||
vff_P[0][0] -= K0 * P0;
|
||||
vff_P[0][1] -= K0 * P1;
|
||||
vff_P[0][2] -= K0 * P2;
|
||||
vff_P[0][3] -= K0 * P3;
|
||||
vff_P[1][0] -= K1 * P0;
|
||||
vff_P[1][1] -= K1 * P1;
|
||||
vff_P[1][2] -= K1 * P2;
|
||||
vff_P[1][3] -= K1 * P3;
|
||||
vff_P[2][0] -= K2 * P0;
|
||||
vff_P[2][1] -= K2 * P1;
|
||||
vff_P[2][2] -= K2 * P2;
|
||||
vff_P[2][3] -= K2 * P3;
|
||||
vff_P[3][0] -= K3 * P0;
|
||||
vff_P[3][1] -= K3 * P1;
|
||||
vff_P[3][2] -= K3 * P2;
|
||||
vff_P[3][3] -= K3 * P3;
|
||||
}
|
||||
|
||||
void vff_update_offset(float offset) {
|
||||
update_offset_conf(offset, R_OFFSET);
|
||||
}
|
||||
|
||||
|
||||
void vff_realign(float z_meas) {
|
||||
//vff_z = z_meas;
|
||||
//vff_zdot = 0.;
|
||||
//vff_offset = 0.;
|
||||
vff_init(z_meas, 0., 0., 0.);
|
||||
}
|
||||
@@ -0,0 +1,48 @@
|
||||
/*
|
||||
* Copyright (C) 2008-2009 Antoine Drouin <poinix@gmail.com>
|
||||
* Copyright (C) 2012 Gautier Hattenberger
|
||||
*
|
||||
* This file is part of paparazzi.
|
||||
*
|
||||
* paparazzi is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation; either version 2, or (at your option)
|
||||
* any later version.
|
||||
*
|
||||
* paparazzi is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
* GNU General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License
|
||||
* along with paparazzi; see the file COPYING. If not, write to
|
||||
* the Free Software Foundation, 59 Temple Place - Suite 330,
|
||||
* Boston, MA 02111-1307, USA.
|
||||
*/
|
||||
|
||||
#ifndef VF_EXTENDED_FLOAT_H
|
||||
#define VF_EXTENDED_FLOAT_H
|
||||
|
||||
#define VFF_STATE_SIZE 4
|
||||
|
||||
extern float vff_z;
|
||||
extern float vff_zdot;
|
||||
extern float vff_bias;
|
||||
extern float vff_offset;
|
||||
extern float vff_P[VFF_STATE_SIZE][VFF_STATE_SIZE];
|
||||
extern float vff_zdotdot;
|
||||
|
||||
extern float vff_z_meas;
|
||||
extern float vff_z_meas_baro;
|
||||
|
||||
extern void vff_init(float z, float zdot, float accel_bias, float baro_offset);
|
||||
extern void vff_propagate(float accel);
|
||||
extern void vff_update_baro(float z_meas);
|
||||
extern void vff_update_alt(float z_meas);
|
||||
extern void vff_update_offset(float offset);
|
||||
extern void vff_update_baro_conf(float z_meas, float conf);
|
||||
extern void vff_update_alt_conf(float z_meas, float conf);
|
||||
//extern void vff_update_vz_conf(float vz_meas, float conf);
|
||||
extern void vff_realign(float z_meas);
|
||||
|
||||
#endif /* VF_EXTENDED_FLOAT_H */
|
||||
@@ -0,0 +1,299 @@
|
||||
/*
|
||||
* Copyright (C) 2008-2010 The Paparazzi Team
|
||||
* Copyright (C) 2012 Gautier Hattenberger
|
||||
*
|
||||
* This file is part of paparazzi.
|
||||
*
|
||||
* paparazzi is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation; either version 2, or (at your option)
|
||||
* any later version.
|
||||
*
|
||||
* paparazzi is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
* GNU General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License
|
||||
* along with paparazzi; see the file COPYING. If not, write to
|
||||
* the Free Software Foundation, 59 Temple Place - Suite 330,
|
||||
* Boston, MA 02111-1307, USA.
|
||||
*/
|
||||
|
||||
#include "subsystems/ins.h"
|
||||
|
||||
#include "subsystems/imu.h"
|
||||
#include "subsystems/sensors/baro.h"
|
||||
#include "subsystems/gps.h"
|
||||
|
||||
#include "generated/airframe.h"
|
||||
#include "math/pprz_algebra_int.h"
|
||||
#include "math/pprz_algebra_float.h"
|
||||
|
||||
#include "subsystems/ahrs.h"
|
||||
|
||||
#if USE_VFF_EXTENDED
|
||||
#include "subsystems/ins/vf_extended_float.h"
|
||||
#endif
|
||||
|
||||
#if USE_HFF
|
||||
#include "subsystems/ins/hf_float.h"
|
||||
#endif
|
||||
|
||||
#ifdef SITL
|
||||
#include "nps_fdm.h"
|
||||
#include <stdio.h>
|
||||
#endif
|
||||
|
||||
|
||||
#include "math/pprz_geodetic_int.h"
|
||||
|
||||
#include "generated/flight_plan.h"
|
||||
|
||||
/* gps transformed to LTP-NED */
|
||||
struct LtpDef_i ins_ltp_def;
|
||||
bool_t ins_ltp_initialised;
|
||||
struct NedCoor_i ins_gps_pos_cm_ned;
|
||||
struct NedCoor_i ins_gps_speed_cm_s_ned;
|
||||
#if USE_HFF
|
||||
/* horizontal gps transformed to NED in meters as float */
|
||||
struct FloatVect2 ins_gps_pos_m_ned;
|
||||
struct FloatVect2 ins_gps_speed_m_s_ned;
|
||||
#endif
|
||||
bool_t ins_hf_realign;
|
||||
|
||||
/* barometer */
|
||||
#if USE_VFF_EXTENDED
|
||||
int32_t ins_qfe;
|
||||
bool_t ins_baro_initialised;
|
||||
int32_t ins_baro_alt;
|
||||
#include "filters/median_filter.h"
|
||||
struct MedianFilterInt baro_median;
|
||||
#if USE_SONAR
|
||||
bool_t ins_update_on_agl;
|
||||
int32_t ins_sonar_offset;
|
||||
struct MedianFilterInt sonar_median;
|
||||
#define VFF_R_SONAR_0 0.1
|
||||
#define VFF_R_SONAR_OF_M 0.2
|
||||
#endif
|
||||
#endif
|
||||
bool_t ins_vf_realign;
|
||||
|
||||
/* output */
|
||||
struct NedCoor_i ins_ltp_pos;
|
||||
struct NedCoor_i ins_ltp_speed;
|
||||
struct NedCoor_i ins_ltp_accel;
|
||||
struct EnuCoor_i ins_enu_pos;
|
||||
struct EnuCoor_i ins_enu_speed;
|
||||
struct EnuCoor_i ins_enu_accel;
|
||||
|
||||
|
||||
void ins_init() {
|
||||
#if USE_INS_NAV_INIT
|
||||
ins_ltp_initialised = TRUE;
|
||||
|
||||
/** FIXME: should use the same code than MOVE_WP in firmwares/rotorcraft/datalink.c */
|
||||
struct LlaCoor_i llh_nav0; /* Height above the ellipsoid */
|
||||
llh_nav0.lat = INT32_RAD_OF_DEG(NAV_LAT0);
|
||||
llh_nav0.lon = INT32_RAD_OF_DEG(NAV_LON0);
|
||||
/* NAV_ALT0 = ground alt above msl, NAV_MSL0 = geoid-height (msl) over ellipsoid */
|
||||
llh_nav0.alt = NAV_ALT0 + NAV_MSL0;
|
||||
|
||||
struct EcefCoor_i ecef_nav0;
|
||||
ecef_of_lla_i(&ecef_nav0, &llh_nav0);
|
||||
|
||||
ltp_def_from_ecef_i(&ins_ltp_def, &ecef_nav0);
|
||||
ins_ltp_def.hmsl = NAV_ALT0;
|
||||
#else
|
||||
ins_ltp_initialised = FALSE;
|
||||
#endif
|
||||
#if USE_VFF_EXTENDED
|
||||
ins_baro_initialised = FALSE;
|
||||
init_median_filter(&baro_median);
|
||||
#if USE_SONAR
|
||||
ins_update_on_agl = FALSE;
|
||||
init_median_filter(&sonar_median);
|
||||
ins_sonar_offset = 0;
|
||||
#endif
|
||||
vff_init(0., 0., 0., 0.);
|
||||
#endif
|
||||
ins_vf_realign = FALSE;
|
||||
ins_hf_realign = FALSE;
|
||||
#if USE_HFF
|
||||
b2_hff_init(0., 0., 0., 0.);
|
||||
#endif
|
||||
INT32_VECT3_ZERO(ins_ltp_pos);
|
||||
INT32_VECT3_ZERO(ins_ltp_speed);
|
||||
INT32_VECT3_ZERO(ins_ltp_accel);
|
||||
INT32_VECT3_ZERO(ins_enu_pos);
|
||||
INT32_VECT3_ZERO(ins_enu_speed);
|
||||
INT32_VECT3_ZERO(ins_enu_accel);
|
||||
}
|
||||
|
||||
void ins_periodic( void ) {
|
||||
}
|
||||
|
||||
#if USE_HFF
|
||||
void ins_realign_h(struct FloatVect2 pos, struct FloatVect2 speed) {
|
||||
b2_hff_realign(pos, speed);
|
||||
}
|
||||
#else
|
||||
void ins_realign_h(struct FloatVect2 pos __attribute__ ((unused)), struct FloatVect2 speed __attribute__ ((unused))) {}
|
||||
#endif /* USE_HFF */
|
||||
|
||||
|
||||
void ins_realign_v(float z) {
|
||||
#if USE_VFF_EXTENDED
|
||||
vff_realign(z);
|
||||
#endif
|
||||
}
|
||||
|
||||
void ins_propagate() {
|
||||
/* untilt accels */
|
||||
struct Int32Vect3 accel_meas_body;
|
||||
INT32_RMAT_TRANSP_VMULT(accel_meas_body, imu.body_to_imu_rmat, imu.accel);
|
||||
struct Int32Vect3 accel_meas_ltp;
|
||||
INT32_RMAT_TRANSP_VMULT(accel_meas_ltp, ahrs.ltp_to_body_rmat, accel_meas_body);
|
||||
|
||||
#if USE_VFF_EXTENDED
|
||||
float z_accel_meas_float = ACCEL_FLOAT_OF_BFP(accel_meas_ltp.z);
|
||||
if (baro.status == BS_RUNNING && ins_baro_initialised) {
|
||||
vff_propagate(z_accel_meas_float);
|
||||
ins_ltp_accel.z = ACCEL_BFP_OF_REAL(vff_zdotdot);
|
||||
ins_ltp_speed.z = SPEED_BFP_OF_REAL(vff_zdot);
|
||||
ins_ltp_pos.z = POS_BFP_OF_REAL(vff_z);
|
||||
}
|
||||
else { // feed accel from the sensors
|
||||
// subtract -9.81m/s2 (acceleration measured due to gravity, but vehivle not accelerating in ltp)
|
||||
ins_ltp_accel.z = accel_meas_ltp.z + ACCEL_BFP_OF_REAL(9.81);
|
||||
}
|
||||
#if !USE_SONAR
|
||||
vff_update_offset(vff_offset);
|
||||
#endif
|
||||
#else
|
||||
ins_ltp_accel.z = accel_meas_ltp.z + ACCEL_BFP_OF_REAL(9.81);
|
||||
#endif /* USE_VFF_EXTENDED */
|
||||
|
||||
#if USE_HFF
|
||||
/* propagate horizontal filter */
|
||||
b2_hff_propagate();
|
||||
#else
|
||||
ins_ltp_accel.x = accel_meas_ltp.x;
|
||||
ins_ltp_accel.y = accel_meas_ltp.y;
|
||||
#endif /* USE_HFF */
|
||||
|
||||
INT32_VECT3_ENU_OF_NED(ins_enu_pos, ins_ltp_pos);
|
||||
INT32_VECT3_ENU_OF_NED(ins_enu_speed, ins_ltp_speed);
|
||||
INT32_VECT3_ENU_OF_NED(ins_enu_accel, ins_ltp_accel);
|
||||
}
|
||||
|
||||
void ins_update_baro() {
|
||||
#if USE_VFF_EXTENDED
|
||||
int32_t baro_pressure = update_median_filter(&baro_median, baro.absolute);
|
||||
if (baro.status == BS_RUNNING) {
|
||||
if (!ins_baro_initialised) {
|
||||
ins_qfe = baro_pressure;
|
||||
ins_baro_initialised = TRUE;
|
||||
}
|
||||
if (ins_vf_realign) {
|
||||
ins_vf_realign = FALSE;
|
||||
ins_qfe = baro.absolute;
|
||||
#if USE_SONAR
|
||||
// FIXME should use an averaged value
|
||||
//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);
|
||||
ins_ltp_pos.z = POS_BFP_OF_REAL(vff_z);
|
||||
ins_enu_pos.z = -ins_ltp_pos.z;
|
||||
ins_enu_speed.z = -ins_ltp_speed.z;
|
||||
ins_enu_accel.z = -ins_ltp_accel.z;
|
||||
}
|
||||
else { /* not realigning, so normal update with baro measurement */
|
||||
ins_baro_alt = ((baro_pressure - ins_qfe) * INS_BARO_SENS_NUM)/INS_BARO_SENS_DEN;
|
||||
float alt_float = POS_FLOAT_OF_BFP(ins_baro_alt);
|
||||
vff_update_baro(alt_float);
|
||||
}
|
||||
}
|
||||
#endif
|
||||
}
|
||||
|
||||
|
||||
void ins_update_gps(void) {
|
||||
#if USE_GPS
|
||||
if (gps.fix == GPS_FIX_3D) {
|
||||
if (!ins_ltp_initialised) {
|
||||
ltp_def_from_ecef_i(&ins_ltp_def, &gps.ecef_pos);
|
||||
ins_ltp_def.lla.alt = gps.lla_pos.alt;
|
||||
ins_ltp_def.hmsl = gps.hmsl;
|
||||
ins_ltp_initialised = TRUE;
|
||||
}
|
||||
ned_of_ecef_point_i(&ins_gps_pos_cm_ned, &ins_ltp_def, &gps.ecef_pos);
|
||||
ned_of_ecef_vect_i(&ins_gps_speed_cm_s_ned, &ins_ltp_def, &gps.ecef_vel);
|
||||
#if USE_HFF
|
||||
VECT2_ASSIGN(ins_gps_pos_m_ned, ins_gps_pos_cm_ned.x, ins_gps_pos_cm_ned.y);
|
||||
VECT2_SDIV(ins_gps_pos_m_ned, ins_gps_pos_m_ned, 100.);
|
||||
VECT2_ASSIGN(ins_gps_speed_m_s_ned, ins_gps_speed_cm_s_ned.x, ins_gps_speed_cm_s_ned.y);
|
||||
VECT2_SDIV(ins_gps_speed_m_s_ned, ins_gps_speed_m_s_ned, 100.);
|
||||
if (ins_hf_realign) {
|
||||
ins_hf_realign = FALSE;
|
||||
#ifdef SITL
|
||||
struct FloatVect2 true_pos, true_speed;
|
||||
VECT2_COPY(true_pos, fdm.ltpprz_pos);
|
||||
VECT2_COPY(true_speed, fdm.ltpprz_ecef_vel);
|
||||
b2_hff_realign(true_pos, true_speed);
|
||||
#else
|
||||
const struct FloatVect2 zero = {0.0, 0.0};
|
||||
b2_hff_realign(ins_gps_pos_m_ned, zero);
|
||||
#endif
|
||||
}
|
||||
b2_hff_update_gps();
|
||||
#if !USE_VFF_EXTENDED /* vff not used */
|
||||
ins_ltp_pos.z = (ins_gps_pos_cm_ned.z * INT32_POS_OF_CM_NUM) / INT32_POS_OF_CM_DEN;
|
||||
ins_ltp_speed.z = (ins_gps_speed_cm_s_ned.z * INT32_SPEED_OF_CM_S_NUM) INT32_SPEED_OF_CM_S_DEN;
|
||||
#endif /* vff not used */
|
||||
#endif /* hff used */
|
||||
|
||||
|
||||
#if !USE_HFF /* hff not used */
|
||||
#if !USE_VFF_EXTENDED /* neither hf nor vf used */
|
||||
INT32_VECT3_SCALE_3(ins_ltp_pos, ins_gps_pos_cm_ned, INT32_POS_OF_CM_NUM, INT32_POS_OF_CM_DEN);
|
||||
INT32_VECT3_SCALE_3(ins_ltp_speed, ins_gps_speed_cm_s_ned, INT32_SPEED_OF_CM_S_NUM, INT32_SPEED_OF_CM_S_DEN);
|
||||
#else /* only vff used */
|
||||
INT32_VECT2_SCALE_2(ins_ltp_pos, ins_gps_pos_cm_ned, INT32_POS_OF_CM_NUM, INT32_POS_OF_CM_DEN);
|
||||
INT32_VECT2_SCALE_2(ins_ltp_speed, ins_gps_speed_cm_s_ned, INT32_SPEED_OF_CM_S_NUM, INT32_SPEED_OF_CM_S_DEN);
|
||||
#endif
|
||||
|
||||
#if USE_GPS_LAG_HACK
|
||||
VECT2_COPY(d_pos, ins_ltp_speed);
|
||||
INT32_VECT2_RSHIFT(d_pos, d_pos, 11);
|
||||
VECT2_ADD(ins_ltp_pos, d_pos);
|
||||
#endif
|
||||
#endif /* hff not used */
|
||||
|
||||
INT32_VECT3_ENU_OF_NED(ins_enu_pos, ins_ltp_pos);
|
||||
INT32_VECT3_ENU_OF_NED(ins_enu_speed, ins_ltp_speed);
|
||||
INT32_VECT3_ENU_OF_NED(ins_enu_accel, ins_ltp_accel);
|
||||
}
|
||||
#endif /* USE_GPS */
|
||||
}
|
||||
|
||||
void ins_update_sonar() {
|
||||
#if USE_SONAR && USE_VFF_EXTENDED
|
||||
static int32_t last_offset = 0;
|
||||
//static int32_t sonar_filtered = 0;
|
||||
int32_t sonar_filtered = update_median_filter(&sonar_median, sonar_meas);
|
||||
//sonar_filtered = (sm + 2*sonar_filtered) / 3;
|
||||
float sonar = (sonar_filtered - ins_sonar_offset) * INS_SONAR_SENS;
|
||||
/* update filter assuming a flat ground */
|
||||
if (sonar < INS_SONAR_MAX_RANGE && ins_update_on_agl && baro.status == BS_RUNNING) {
|
||||
vff_update_alt_conf(-sonar, VFF_R_SONAR_0 + VFF_R_SONAR_OF_M * fabs(sonar));
|
||||
last_offset = vff_offset;
|
||||
}
|
||||
else {
|
||||
/* update offset with last value to avoid divergence */
|
||||
vff_update_offset(last_offset);
|
||||
}
|
||||
#endif
|
||||
}
|
||||
Reference in New Issue
Block a user