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

update ins realignment stuff

Browse Source
This commit is contained in:
Felix Ruess
2010-03-09 22:56:44 +00:00
parent 4c7099f226
commit 96d1539c48
6 changed files with 57 additions and 34 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/settings/settings_booz2.xml
+2 -2
View File
@@ -48,7 +48,7 @@
<dl_setting var="booz2_guidance_v_kd" min="-600" step="1" max="0" module="guidance/booz2_guidance_v" shortname="kd"/>
<dl_setting var="booz2_guidance_v_ki" min="-300" step="1" max="0" module="guidance/booz2_guidance_v" shortname="ki" handler="SetKi" />
<dl_setting var="booz2_guidance_v_z_sp" min="-5" step="0.5" max="3" module="guidance/booz2_guidance_v" shortname="sp" unit="2e-8m" alt_unit="m" alt_unit_coef="0.00390625"/>
<dl_setting var="booz_ins_vff_realign" min="0" step="1" max="1" module="booz2_ins" shortname="vff_realign" values="OFF|ON"/>
<dl_setting var="booz_ins_vf_realign" min="0" step="1" max="1" module="booz2_ins" shortname="vf_realign" values="OFF|ON"/>
</dl_settings>
<dl_settings NAME="Horiz Loop">
@@ -60,7 +60,7 @@
<dl_setting var="booz2_guidance_h_igain" min="-400" step="1" max="0" module="guidance/booz2_guidance_h" shortname="ki" handler="SetKi"/>
<dl_setting var="booz2_guidance_h_ngain" min="-400" step="1" max="0" module="guidance/booz2_guidance_h" shortname="kn"/>
<dl_setting var="booz2_guidance_h_again" min="-400" step="1" max="0" module="guidance/booz2_guidance_h" shortname="ka"/>
<dl_setting var="booz_ins_hff_realign" min="0" step="1" max="1" module="booz2_ins" shortname="hff_realign" values="OFF|ON"/>
<dl_setting var="booz_ins_hf_realign" min="0" step="1" max="1" module="booz2_ins" shortname="hf_realign" values="OFF|ON"/>
</dl_settings>
<dl_settings NAME="NAV">
sw/airborne/booz/booz2_ins.c
+42 -18
View File
@@ -62,15 +62,15 @@ struct NedCoor_i booz_ins_gps_speed_cm_s_ned;
struct FloatVect2 booz_ins_gps_pos_m_ned;
struct FloatVect2 booz_ins_gps_speed_m_s_ned;
#endif
bool_t booz_ins_hff_realign;
bool_t booz_ins_hf_realign;
/* barometer */
#ifdef USE_VFF
int32_t booz_ins_qfe;
bool_t booz_ins_baro_initialised;
int32_t booz_ins_baro_alt;
bool_t booz_ins_vff_realign;
#endif
bool_t booz_ins_vf_realign;
/* output */
struct NedCoor_i booz_ins_ltp_pos;
@@ -100,14 +100,14 @@ void booz_ins_init() {
#else
booz_ins_ltp_initialised = FALSE;
#endif
booz_ins_vf_realign = FALSE;
#ifdef USE_VFF
booz_ins_baro_initialised = FALSE;
booz_ins_vff_realign = FALSE;
b2_vff_init(0., 0., 0.);
#endif
booz_ins_hf_realign = FALSE;
#ifdef USE_HFF
b2_hff_init(0., 0., 0., 0.);
booz_ins_hff_realign = FALSE;
#endif
INT32_VECT3_ZERO(booz_ins_ltp_pos);
INT32_VECT3_ZERO(booz_ins_ltp_speed);
@@ -126,6 +126,18 @@ void booz_ins_periodic( void ) {
#endif
}
void booz_ins_realign_h(struct FloatVect2 pos, struct FloatVect2 speed) {
#ifdef USE_HFF
b2_hff_realign(pos, speed);
#endif
}
void booz_ins_realign_v(float z) {
#ifdef USE_VFF
b2_vff_realign(z);
#endif
}
void booz_ins_propagate() {
/* untilt accels */
struct Int32Vect3 accel_body;
@@ -143,8 +155,9 @@ void booz_ins_propagate() {
}
else { // feed accel from the sensors
booz_ins_ltp_accel.z = ACCEL_BFP_OF_REAL(z_accel_float);
booz_ins_enu_accel.z = -booz_ins_ltp_accel.z;
}
#else
booz_ins_ltp_accel.z = ACCEL_BFP_OF_REAL(z_accel_float);
#endif /* USE_VFF */
#ifdef USE_HFF
@@ -160,6 +173,10 @@ void booz_ins_propagate() {
booz_ins_ltp_pos.x = POS_BFP_OF_REAL(b2_hff_state.x);
booz_ins_ltp_pos.y = POS_BFP_OF_REAL(b2_hff_state.y);
}
else {
booz_ins_ltp_accel.x = accel_ltp.x;
booz_ins_ltp_accel.y = accel_ltp.y;
}
#else
booz_ins_ltp_accel.x = accel_ltp.x;
booz_ins_ltp_accel.y = accel_ltp.y;
@@ -179,8 +196,8 @@ void booz_ins_update_baro() {
}
booz_ins_baro_alt = (((int32_t)booz2_analog_baro_value - booz_ins_qfe) * BOOZ_INS_BARO_SENS_NUM)/BOOZ_INS_BARO_SENS_DEN;
float alt_float = POS_FLOAT_OF_BFP(booz_ins_baro_alt);
if (booz_ins_vff_realign) {
booz_ins_vff_realign = FALSE;
if (booz_ins_vf_realign) {
booz_ins_vf_realign = FALSE;
booz_ins_qfe = booz2_analog_baro_value;
b2_vff_realign(0.);
booz_ins_ltp_accel.z = ACCEL_BFP_OF_REAL(b2_vff_zdotdot);
@@ -213,15 +230,16 @@ void booz_ins_update_gps(void) {
VECT2_SDIV(booz_ins_gps_pos_m_ned, booz_ins_gps_pos_m_ned, 100.);
VECT2_ASSIGN(booz_ins_gps_speed_m_s_ned, booz_ins_gps_speed_cm_s_ned.x, booz_ins_gps_speed_cm_s_ned.y);
VECT2_SDIV(booz_ins_gps_speed_m_s_ned, booz_ins_gps_speed_m_s_ned, 100.);
if (booz_ins_hff_realign) {
booz_ins_hff_realign = FALSE;
if (booz_ins_hf_realign) {
booz_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
b2_hff_realign(booz_ins_gps_pos_m_ned, booz_ins_gps_speed_m_s_ned);
const struct FloatVect2 zero = {0.0, 0.0};
b2_hff_realign(booz_ins_gps_pos_m_ned, zero);
#endif
}
b2_hff_update_gps();
@@ -231,23 +249,29 @@ void booz_ins_update_gps(void) {
booz_ins_ltp_speed.y = SPEED_BFP_OF_REAL(b2_hff_state.ydot);
booz_ins_ltp_pos.x = POS_BFP_OF_REAL(b2_hff_state.x);
booz_ins_ltp_pos.y = POS_BFP_OF_REAL(b2_hff_state.y);
#ifndef USE_VFF /* only hf */
#ifndef USE_VFF /* vff not used */
booz_ins_ltp_pos.z = (booz_ins_gps_pos_cm_ned.z * INT32_POS_OF_CM_NUM) / INT32_POS_OF_CM_DEN;
booz_ins_ltp_speed.z = (booz_ins_gps_speed_cm_s_ned.z * INT32_SPEED_OF_CM_S_NUM) INT32_SPEED_OF_CM_S_DEN;
#endif /* only hf */
#else /* hf not used */
#endif /* vff not used */
#endif /* hff used */
#ifndef USE_HFF /* hff not used */
#ifndef USE_VFF /* neither hf nor vf used */
INT32_VECT3_SCALE_3(booz_ins_ltp_pos, booz_ins_gps_pos_cm_ned, INT32_POS_OF_CM_NUM, INT32_POS_OF_CM_DEN);
INT32_VECT3_SCALE_3(booz_ins_ltp_speed, booz_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(booz_ins_ltp_pos, booz_ins_gps_pos_cm_ned, INT32_POS_OF_CM_NUM, INT32_POS_OF_CM_DEN);
INT32_VECT2_SCALE_2(booz_ins_ltp_speed, booz_ins_gps_speed_cm_s_ned, INT32_SPEED_OF_CM_S_NUM, INT32_SPEED_OF_CM_S_DEN);
#endif
#ifdef USE_GPS_LAG_HACK
VECT2_COPY(d_pos, booz_ins_ltp_speed);
INT32_VECT2_RSHIFT(d_pos, d_pos, 11);
VECT2_ADD(booz_ins_ltp_pos, d_pos);
#endif
#ifndef USE_VFF /* neither hf nor vf used */
INT32_VECT3_SCALE_2(booz_ins_ltp_pos, booz_ins_gps_pos_cm_ned, INT32_POS_OF_CM_NUM, INT32_POS_OF_CM_DEN);
INT32_VECT3_SCALE_2(booz_ins_ltp_speed, booz_ins_gps_speed_cm_s_ned, INT32_SPEED_OF_CM_S_NUM, INT32_SPEED_OF_CM_S_DEN);
#endif /* neither hf nor vf used */
#endif /* USE_HFF */
#endif /* hff not used */
INT32_VECT3_ENU_OF_NED(booz_ins_enu_pos, booz_ins_ltp_pos);
INT32_VECT3_ENU_OF_NED(booz_ins_enu_speed, booz_ins_ltp_speed);
sw/airborne/booz/booz2_ins.h
+5 -2
View File
@@ -39,7 +39,6 @@ extern struct NedCoor_i booz_ins_gps_speed_cm_s_ned;
extern int32_t booz_ins_baro_alt;
extern int32_t booz_ins_qfe;
extern bool_t booz_ins_baro_initialised;
extern bool_t booz_ins_vff_realign;
#endif
/* output LTP NED */
@@ -55,10 +54,14 @@ extern struct EnuCoor_i booz_ins_enu_accel;
extern struct FloatVect2 booz_ins_gps_pos_m_ned;
extern struct FloatVect2 booz_ins_gps_speed_m_s_ned;
#endif
extern bool_t booz_ins_hff_realign;
extern bool_t booz_ins_hf_realign;
extern bool_t booz_ins_vf_realign;
extern void booz_ins_init( void );
extern void booz_ins_periodic( void );
extern void booz_ins_realign_h(struct FloatVect2 pos, struct FloatVect2 speed);
extern void booz_ins_realign_v(float z);
extern void booz_ins_propagate( void );
extern void booz_ins_update_baro( void );
extern void booz_ins_update_gps( void );
sw/airborne/booz/booz2_navigation.c
+3 -5
View File
@@ -240,15 +240,13 @@ static inline void nav_set_altitude( void ) {
/** Reset the geographic reference to the current GPS fix */
unit_t nav_reset_reference( void ) {
booz_ins_ltp_initialised = FALSE;
booz_ins_vff_realign = TRUE;
#ifdef USE_HFF
booz_ins_hff_realign = TRUE;
#endif
booz_ins_hf_realign = TRUE;
booz_ins_vf_realign = TRUE;
return 0;
}
unit_t nav_reset_alt( void ) {
booz_ins_vff_realign = TRUE;
booz_ins_vf_realign = TRUE;
#ifdef USE_GPS
booz_ins_ltp_def.lla.alt = booz_gps_state.lla_pos.alt;
sw/airborne/booz/guidance/booz2_guidance_v.c
+1 -1
View File
@@ -152,7 +152,7 @@ void booz2_guidance_v_run(bool_t in_flight) {
}
else {
// reset vertical filter until takeoff
//booz_ins_vff_realign = TRUE;
//booz_ins_vf_realign = TRUE;
}
switch (booz2_guidance_v_mode) {
sw/airborne/modules/vision/cam_track.c
+4 -6
View File
@@ -122,18 +122,16 @@ void track_periodic_task(void) {
void track_event(void) {
if (!booz_ins_ltp_initialised) {
booz_ins_ltp_initialised = TRUE;
#ifdef USE_HFF
booz_ins_hff_realign = TRUE;
#endif
booz_ins_hf_realign = TRUE;
}
#ifdef USE_HFF
if (booz_ins_hff_realign) {
booz_ins_hff_realign = FALSE;
if (booz_ins_hf_realign) {
booz_ins_hf_realign = FALSE;
struct FloatVect2 pos, zero;
pos.x = -target_pos_ned.x;
pos.y = -target_pos_ned.y;
b2_hff_realign(pos, zero);
booz_ins_realign_h(pos, zero);
}
b2_hff_update_pos(-target_pos_ned.x, -target_pos_ned.y);
booz_ins_ltp_accel.x = ACCEL_BFP_OF_REAL(b2_hff_state.xdotdot);