Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-05-25 23:46:04 +08:00
Code Issues Actions 29 Packages Projects Releases Wiki Activity

[ins] INS invariant filter can use GPS heading instead of magneto

Browse Source
This commit is contained in:
Gautier Hattenberger
2019-05-17 18:26:29 +02:00
committed by Freek van Tienen
parent bfb678dc01
commit 69df254c26
6 changed files with 97 additions and 56 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/airframes/ENAC/conf_enac.xml
+2 -2
View File
@@ -17,7 +17,7 @@
radio="radios/cockpitSX.xml"
telemetry="telemetry/default_rotorcraft.xml"
flight_plan="flight_plans/rotorcraft_basic_safety.xml"
settings="settings/rotorcraft_basic.xml settings/estimation/ins_float_invariant.xml"
settings="settings/rotorcraft_basic.xml"
settings_modules="modules/stabilization_int_quat.xml modules/nav_basic_rotorcraft.xml modules/guidance_rotorcraft.xml modules/gps_ubx_ucenter.xml modules/gps.xml modules/imu_common.xml"
gui_color="yellow"
/>
@@ -72,7 +72,7 @@
radio="radios/R617FS_5ch_neg.xml"
telemetry="telemetry/default_fixedwing_imu.xml"
flight_plan="flight_plans/basic.xml"
settings="settings/fixedwing_basic.xml settings/estimation/ins_float_invariant.xml [settings/nps.xml]"
settings="settings/fixedwing_basic.xml [settings/nps.xml]"
settings_modules="[modules/gps_ubx_ucenter.xml] modules/gps.xml modules/nav_basic_fw.xml modules/guidance_full_pid_fw.xml modules/stabilization_adaptive_fw.xml modules/airspeed_adc.xml modules/imu_common.xml modules/ahrs_float_dcm.xml"
gui_color="#ffff7d7d0000"
/>
conf/airframes/ENAC/fixed-wing/jp.xml
+3 -2
View File
@@ -3,7 +3,6 @@
<airframe name="JP">
<firmware name="fixedwing">
<define name="USE_I2C1"/>
<target name="ap" board="apogee_1.0_chibios">
<configure name="PERIODIC_FREQUENCY" value="125"/>
@@ -22,6 +21,8 @@
<configure name="PERIODIC_FREQUENCY" value="125"/>
<!-- Note NPS needs the ppm type radio_control module -->
<module name="fdm" type="jsbsim"/>
<!--module name="ahrs" type="float_dcm"/>
<module name="ins" type="alt_float"/-->
<module name="ins" type="float_invariant">
<configure name="AHRS_PROPAGATE_FREQUENCY" value="125"/>
<configure name="AHRS_CORRECT_FREQUENCY" value="125"/>
@@ -38,7 +39,7 @@
<!-- Actuators are automatically chosen according to board-->
<module name="imu" type="apogee">
<define name="USE_MAGNETOMETER"/>
<configure name="USE_MAGNETOMETER" value="FALSE"/>
<define name="APOGEE_LOWPASS_FILTER" value="MPU60X0_DLPF_20HZ"/>
<define name="APOGEE_SMPLRT_DIV" value="7"/>
</module>
conf/modules/ins_float_invariant.xml
+20
View File
@@ -9,6 +9,26 @@
<configure name="USE_MAGNETOMETER" value="TRUE|FALSE" description="use magnetometer"/>
<configure name="AHRS_ALIGNER_LED" value="2" description="LED number to indicate if AHRS/INS is aligned"/>
</doc>
<settings>
<dl_settings>
<dl_settings NAME="invariant">
<dl_setting MAX="1" MIN="1" STEP="1" VAR="ins_float_inv.reset" shortname="reset"/>
<dl_setting MAX="10" MIN="0." STEP="0.001" VAR="ins_float_inv.gains.lv" shortname="lv" module="subsystems/ins/ins_float_invariant"/>
<dl_setting MAX="10" MIN="0." STEP="0.001" VAR="ins_float_inv.gains.lb" shortname="lb"/>
<dl_setting MAX="10" MIN="0." STEP="0.001" VAR="ins_float_inv.gains.mv" shortname="mv"/>
<dl_setting MAX="10" MIN="0." STEP="0.01" VAR="ins_float_inv.gains.mh" shortname="mh"/>
<dl_setting MAX="20" MIN="0." STEP="0.01" VAR="ins_float_inv.gains.nx" shortname="nx"/>
<dl_setting MAX="10" MIN="0." STEP="0.01" VAR="ins_float_inv.gains.nxz" shortname="nxz"/>
<dl_setting MAX="30" MIN="0." STEP="0.01" VAR="ins_float_inv.gains.mvz" shortname="mvz"/>
<dl_setting MAX="5" MIN="0." STEP="0.01" VAR="ins_float_inv.gains.nh" shortname="nh"/>
<dl_setting MAX="5" MIN="0." STEP="0.01" VAR="ins_float_inv.gains.ov" shortname="ov"/>
<dl_setting MAX="3" MIN="0." STEP="0.01" VAR="ins_float_inv.gains.ob" shortname="ob"/>
<dl_setting MAX="10" MIN="0." STEP="0.01" VAR="ins_float_inv.gains.rv" shortname="rv"/>
<dl_setting MAX="10" MIN="0." STEP="0.01" VAR="ins_float_inv.gains.rh" shortname="rh"/>
<dl_setting MAX="1" MIN="0." STEP="0.001" VAR="ins_float_inv.gains.sh" shortname="sh"/>
</dl_settings>
</dl_settings>
</settings>
<header>
<file name="ins_float_invariant_wrapper.h" dir="subsystems/ins"/>
</header>
conf/settings/estimation/ins_float_invariant.xml
-22
View File
@@ -1,22 +0,0 @@
<!DOCTYPE settings SYSTEM "../settings.dtd">
<settings target="ap|nps">
<dl_settings>
<dl_settings NAME="invariant">
<dl_setting MAX="1" MIN="1" STEP="1" VAR="ins_float_inv.reset" shortname="reset"/>
<dl_setting MAX="10" MIN="0." STEP="0.001" VAR="ins_float_inv.gains.lv" shortname="lv" module="subsystems/ins/ins_float_invariant"/>
<dl_setting MAX="10" MIN="0." STEP="0.001" VAR="ins_float_inv.gains.lb" shortname="lb"/>
<dl_setting MAX="10" MIN="0." STEP="0.001" VAR="ins_float_inv.gains.mv" shortname="mv"/>
<dl_setting MAX="10" MIN="0." STEP="0.01" VAR="ins_float_inv.gains.mh" shortname="mh"/>
<dl_setting MAX="20" MIN="0." STEP="0.01" VAR="ins_float_inv.gains.nx" shortname="nx"/>
<dl_setting MAX="10" MIN="0." STEP="0.01" VAR="ins_float_inv.gains.nxz" shortname="nxz"/>
<dl_setting MAX="30" MIN="0." STEP="0.01" VAR="ins_float_inv.gains.mvz" shortname="mvz"/>
<dl_setting MAX="5" MIN="0." STEP="0.01" VAR="ins_float_inv.gains.nh" shortname="nh"/>
<dl_setting MAX="5" MIN="0." STEP="0.01" VAR="ins_float_inv.gains.ov" shortname="ov"/>
<dl_setting MAX="3" MIN="0." STEP="0.01" VAR="ins_float_inv.gains.ob" shortname="ob"/>
<dl_setting MAX="10" MIN="0." STEP="0.01" VAR="ins_float_inv.gains.rv" shortname="rv"/>
<dl_setting MAX="10" MIN="0." STEP="0.01" VAR="ins_float_inv.gains.rh" shortname="rh"/>
<dl_setting MAX="1" MIN="0." STEP="0.001" VAR="ins_float_inv.gains.sh" shortname="sh"/>
</dl_settings>
</dl_settings>
</settings>
sw/airborne/subsystems/ins/ins_float_invariant.c
+41 -17
View File
@@ -149,6 +149,11 @@ bool ins_baro_initialized;
/* gps */
bool ins_gps_fix_once;
/* min speed to update heading from GPS when mag are not used */
#ifndef INS_INV_HEADING_UPDATE_GPS_MIN_SPEED
#define INS_INV_HEADING_UPDATE_GPS_MIN_SPEED 5.f
#endif
/* error computation */
static inline void error_output(struct InsFloatInv *_ins);
@@ -237,9 +242,15 @@ void ins_float_invariant_init(void)
stateSetLocalOrigin_i(&ltp_def);
#endif
#if USE_MAGNETOMETER
B.x = INS_H_X;
B.y = INS_H_Y;
B.z = INS_H_Z;
#else
B.x = 1.f; // when using GPS as magnetometer, mag field is true north
B.y = 0.f;
B.z = 0.f;
#endif
// init state and measurements
init_invariant_state();
@@ -303,10 +314,15 @@ void ins_reset_altitude_ref(void)
void ins_float_invariant_align(struct FloatRates *lp_gyro,
struct FloatVect3 *lp_accel,
struct FloatVect3 *lp_mag)
struct FloatVect3 *lp_mag __attribute__((unused)))
{
#if USE_MAGNETOMETER
/* Compute an initial orientation from accel and mag directly as quaternion */
ahrs_float_get_quat_from_accel_mag(&ins_float_inv.state.quat, lp_accel, lp_mag);
#else
/* Compute an initial orientation from accel only directly as quaternion */
ahrs_float_get_quat_from_accel(&ins_float_inv.state.quat, lp_accel);
#endif
/* use average gyro as initial value for bias */
ins_float_inv.state.bias = *lp_gyro;
@@ -451,12 +467,30 @@ void ins_float_invariant_update_gps(struct GpsState *gps_s)
#endif
}
#if !USE_MAGNETOMETER
// Use pseudo-mag rebuilt from GPS horizontal velocity
struct FloatVect2 vel = { ins_float_inv.meas.speed_gps.x, ins_float_inv.meas.speed_gps.y };
float vel_norm = float_vect2_norm(&vel);
if (vel_norm > INS_INV_HEADING_UPDATE_GPS_MIN_SPEED) {
struct FloatVect3 pseudo_mag = {
vel.x / vel_norm,
-vel.y / vel_norm,
0.f
};
ins_float_invariant_update_mag(&pseudo_mag);
}
else {
// if speed is tool low, better set measurements to zero
FLOAT_VECT3_ZERO(ins_float_inv.meas.mag);
}
#endif
}
void ins_float_invariant_update_baro(float pressure)
{
static float ins_qfe = 101325.0f;
static float ins_qfe = PPRZ_ISA_SEA_LEVEL_PRESSURE;
static float alpha = 10.0f;
static int32_t i = 1;
static float baro_moy = 0.0f;
@@ -473,7 +507,7 @@ void ins_float_invariant_update_baro(float pressure)
alpha = (10.*alpha + (baro_moy - baro_prev)) / (11.0f);
baro_prev = baro_moy;
// test stop condition
if (fabs(alpha) < 0.005f) {
if (fabs(alpha) < 0.1f) {
ins_qfe = baro_moy;
ins_baro_initialized = true;
}
@@ -532,14 +566,6 @@ static inline void invariant_model(float *o, const float *x, const int n, const
struct FloatVect3 tmp_vect;
struct FloatQuat tmp_quat;
// test accel sensitivity
if (fabs(s->as) < 0.1) {
// too small, return x_dot = 0 to avoid division by 0
float_vect_zero(o, n);
// TODO set ins state to error
return;
}
/* dot_q = 0.5 * q * (x_rates - x_bias) + LE * q + (1 - ||q||^2) * q */
RATES_DIFF(rates_unbiased, c->rates, s->bias);
/* qd = 0.5 * q * rates_unbiased = -0.5 * rates_unbiased * q */
@@ -569,6 +595,10 @@ static inline void invariant_model(float *o, const float *x, const int n, const
/* as_dot = as * RE */
s_dot.as = (s->as) * (ins_float_inv.corr.RE);
// keep as in a reasonable range, so 50% around the nominal value
if (s->as < 0.5f || s->as > 1.5f) {
s_dot.as = 0.f;
}
/* hb_dot = SE */
s_dot.hb = ins_float_inv.corr.SE;
@@ -588,12 +618,6 @@ static inline void error_output(struct InsFloatInv *_ins)
float Eh;
float temp;
// test accel sensitivity
if (fabs(_ins->state.as) < 0.1) {
// too small, don't do anything to avoid division by 0
return;
}
/* YBt = q * yB * q-1 */
struct FloatQuat q_b2n;
float_quat_invert(&q_b2n, &(_ins->state.quat));
sw/airborne/subsystems/ins/ins_float_invariant_wrapper.c
+31 -13
View File
@@ -29,6 +29,9 @@
#include "subsystems/abi.h"
#include "mcu_periph/sys_time.h"
#include "message_pragmas.h"
#if USE_AHRS_ALIGNER
#include "subsystems/ahrs/ahrs_aligner.h"
#endif
#ifndef INS_FINV_FILTER_ID
#define INS_FINV_FILTER_ID 2
@@ -89,10 +92,14 @@ PRINT_CONFIG_VAR(INS_FINV_BARO_ID)
PRINT_CONFIG_VAR(INS_FINV_IMU_ID)
/** magnetometer */
#if USE_MAGNETOMETER
#ifndef INS_FINV_MAG_ID
#define INS_FINV_MAG_ID ABI_BROADCAST
#endif
PRINT_CONFIG_VAR(INS_FINV_MAG_ID)
#else
PRINT_CONFIG_MSG("INS invariant use GPS heading as magnetometer")
#endif
/** ABI binding for gps data.
* Used for GPS ABI messages.
@@ -103,12 +110,14 @@ PRINT_CONFIG_VAR(INS_FINV_MAG_ID)
PRINT_CONFIG_VAR(INS_FINV_GPS_ID)
static abi_event baro_ev;
static abi_event mag_ev;
static abi_event gyro_ev;
static abi_event accel_ev;
static abi_event aligner_ev;
static abi_event body_to_imu_ev;
#if USE_MAGNETOMETER
static abi_event mag_ev;
static abi_event geo_mag_ev;
#endif
static abi_event gps_ev;
static void baro_cb(uint8_t __attribute__((unused)) sender_id, __attribute__((unused)) uint32_t stamp, float pressure)
@@ -154,17 +163,6 @@ static void accel_cb(uint8_t sender_id __attribute__((unused)),
ACCELS_FLOAT_OF_BFP(ins_finv_accel, *accel);
}
static void mag_cb(uint8_t sender_id __attribute__((unused)),
uint32_t stamp __attribute__((unused)),
struct Int32Vect3 *mag)
{
if (ins_float_inv.is_aligned) {
struct FloatVect3 mag_f;
MAGS_FLOAT_OF_BFP(mag_f, *mag);
ins_float_invariant_update_mag(&mag_f);
}
}
static void aligner_cb(uint8_t __attribute__((unused)) sender_id,
uint32_t stamp __attribute__((unused)),
struct Int32Rates *lp_gyro, struct Int32Vect3 *lp_accel,
@@ -188,10 +186,23 @@ static void body_to_imu_cb(uint8_t sender_id __attribute__((unused)),
ins_float_inv_set_body_to_imu_quat(q_b2i_f);
}
#if USE_MAGNETOMETER
static void mag_cb(uint8_t sender_id __attribute__((unused)),
uint32_t stamp __attribute__((unused)),
struct Int32Vect3 *mag)
{
if (ins_float_inv.is_aligned) {
struct FloatVect3 mag_f;
MAGS_FLOAT_OF_BFP(mag_f, *mag);
ins_float_invariant_update_mag(&mag_f);
}
}
static void geo_mag_cb(uint8_t sender_id __attribute__((unused)), struct FloatVect3 *h)
{
ins_float_inv.mag_h = *h;
}
#endif
static void gps_cb(uint8_t sender_id __attribute__((unused)),
uint32_t stamp __attribute__((unused)),
@@ -203,16 +214,23 @@ static void gps_cb(uint8_t sender_id __attribute__((unused)),
void ins_float_invariant_wrapper_init(void)
{
// aligner
#if USE_AHRS_ALIGNER
ahrs_aligner_init();
#endif
ins_float_invariant_init();
// Bind to ABI messages
AbiBindMsgBARO_ABS(INS_FINV_BARO_ID, &baro_ev, baro_cb);
AbiBindMsgIMU_MAG_INT32(INS_FINV_MAG_ID, &mag_ev, mag_cb);
AbiBindMsgIMU_GYRO_INT32(INS_FINV_IMU_ID, &gyro_ev, gyro_cb);
AbiBindMsgIMU_ACCEL_INT32(INS_FINV_IMU_ID, &accel_ev, accel_cb);
AbiBindMsgIMU_LOWPASSED(INS_FINV_IMU_ID, &aligner_ev, aligner_cb);
AbiBindMsgBODY_TO_IMU_QUAT(INS_FINV_IMU_ID, &body_to_imu_ev, body_to_imu_cb);
#if USE_MAGNETOMETER
AbiBindMsgIMU_MAG_INT32(INS_FINV_MAG_ID, &mag_ev, mag_cb);
AbiBindMsgGEO_MAG(ABI_BROADCAST, &geo_mag_ev, geo_mag_cb);
#endif
AbiBindMsgGPS(INS_FINV_GPS_ID, &gps_ev, gps_cb);
#if PERIODIC_TELEMETRY && !INS_FINV_USE_UTM