Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-05-09 22:49:53 +08:00
Code Issues Actions 27 Packages Projects Releases Wiki Activity

[vff_extended] Make process and measurement noise adjustable via settings (for inflight tuning)

Browse Source
This commit is contained in:
Michal Podhradsky
2016-11-15 15:30:15 -08:00
parent 7b4cfeb43a
commit ff265a57c1
3 changed files with 27 additions and 5 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/modules/ins_extended.xml
+12
View File
@@ -15,6 +15,18 @@
<define name="INS_SONAR_MAX_RANGE" value="4.0" description="max sonar range in meters"/>
<define name="INS_SONAR_UPDATE_ON_AGL" value="FALSE" description="assume flat ground and use sonar for height"/>
</doc>
<settings>
<dl_settings NAME="Ins Extended">
<dl_settings NAME="INS">
<dl_setting MAX="10" MIN="0" STEP="0.1" VAR="vff.accel_noise" shortname="accel_noise" module="subsystems/ins/vf_extended_float"/>
<dl_setting MAX="10" MIN="0" STEP="0.1" VAR="vff.r_baro" shortname="r_baro"/>
<dl_setting MAX="10" MIN="0" STEP="0.1" VAR="vff.r_alt" shortname="r_alt"/>
<dl_setting MAX="10" MIN="0" STEP="0.1" VAR="vff.r_offset" shortname="r_offset"/>
</dl_settings>
</dl_settings>
</settings>
<header>
<file name="ins_int.h" dir="subsystems/ins"/>
</header>
sw/airborne/subsystems/ins/vf_extended_float.c
+10 -5
View File
@@ -96,6 +96,11 @@ void vff_init(float init_z, float init_zdot, float init_accel_bias, float init_b
vff.P[i][i] = VFF_EXTENDED_INIT_PXX;
}
vff.accel_noise = VFF_EXTENDED_ACCEL_NOISE;
vff.r_baro = R_BARO;
vff.r_alt = R_ALT;
vff.r_offset = R_OFFSET;
#if PERIODIC_TELEMETRY
register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_VFF_EXTENDED, send_vffe);
#endif
@@ -142,11 +147,11 @@ void vff_propagate(float accel, float dt)
const float FPF22 = vff.P[2][2];
const float FPF33 = vff.P[3][3];
vff.P[0][0] = FPF00 + VFF_EXTENDED_ACCEL_NOISE * dt * dt / 2.;
vff.P[0][0] = FPF00 + vff.accel_noise * dt * dt / 2.;
vff.P[0][1] = FPF01;
vff.P[0][2] = FPF02;
vff.P[1][0] = FPF10;
vff.P[1][1] = FPF11 + VFF_EXTENDED_ACCEL_NOISE * dt;
vff.P[1][1] = FPF11 + vff.accel_noise * dt;
vff.P[1][2] = FPF12;
vff.P[2][0] = FPF20;
vff.P[2][1] = FPF21;
@@ -215,7 +220,7 @@ static void update_baro_conf(float z_meas, float conf)
void vff_update_baro(float z_meas)
{
update_baro_conf(z_meas, R_BARO);
update_baro_conf(z_meas, vff.r_baro);
}
void vff_update_baro_conf(float z_meas, float conf)
@@ -278,7 +283,7 @@ static void update_alt_conf(float z_meas, float conf)
void vff_update_z(float z_meas)
{
update_alt_conf(z_meas, R_ALT);
update_alt_conf(z_meas, vff.r_alt);
}
void vff_update_z_conf(float z_meas, float conf)
@@ -340,7 +345,7 @@ static void update_offset_conf(float offset, float conf)
void vff_update_offset(float offset)
{
update_offset_conf(offset, R_OFFSET);
update_offset_conf(offset, vff.r_offset);
}
sw/airborne/subsystems/ins/vf_extended_float.h
+5
View File
@@ -44,6 +44,11 @@ struct VffExtended {
float z_meas_baro; ///< last z measurement from baro in m
float P[VFF_STATE_SIZE][VFF_STATE_SIZE]; ///< covariance matrix
float accel_noise;
float r_baro;
float r_alt;
float r_offset;
};
extern struct VffExtended vff;