Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-05-10 06:59:54 +08:00
Code Issues Actions 25 Packages Projects Releases Wiki Activity

[ins] Fix EKF2 working with HMSL (#3053)

Browse Source
This commit is contained in:
Freek van Tienen
2023-08-30 13:46:11 +02:00
committed by GitHub
parent 3749664a1b
commit a267e39e82
1 changed files with 4 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sw/airborne/modules/ins/ins_ekf2.cpp
+4 -4
View File
@@ -522,13 +522,13 @@ void ins_ekf2_init(void)
/* Initialize the origin from flight plan */
#if USE_INS_NAV_INIT
if(ekf.setEkfGlobalOrigin(NAV_LAT0*1e-7, NAV_LON0*1e-7, (NAV_ALT0 + NAV_MSL0)*1e-3))
if(ekf.setEkfGlobalOrigin(NAV_LAT0*1e-7, NAV_LON0*1e-7, (NAV_ALT0)*1e-3)) // EKF2 works HMSL
{
struct LlaCoor_i llh_nav0; /* Height above the ellipsoid */
llh_nav0.lat = NAV_LAT0;
llh_nav0.lon = NAV_LON0;
/* NAV_ALT0 = ground alt above msl, NAV_MSL0 = geoid-height (msl) over ellipsoid */
llh_nav0.alt = NAV_ALT0 + NAV_MSL0;
llh_nav0.alt = NAV_ALT0 + NAV_MSL0; // in millimeters above WGS84 reference ellipsoid
ltp_def_from_lla_i(&ekf2.ltp_def, &llh_nav0);
ekf2.ltp_def.hmsl = NAV_ALT0;
@@ -621,7 +621,7 @@ void ins_ekf2_update(void)
if (ekf_origin_valid && (origin_time > ekf2.ltp_stamp)) {
lla_ref.lat = ekf_origin_lat * 1e7; // WGS-84 lat
lla_ref.lon = ekf_origin_lon * 1e7; // WGS-84 lon
lla_ref.alt = ref_alt * 1e3 + wgs84_ellipsoid_to_geoid_i(lla_ref.lat, lla_ref.lon); // WGS-84 height
lla_ref.alt = ref_alt * 1e3 + wgs84_ellipsoid_to_geoid_i(lla_ref.lat, lla_ref.lon); // in millimeters above WGS84 reference ellipsoid (ref_alt is in HMSL)
ltp_def_from_lla_i(&ekf2.ltp_def, &lla_ref);
ekf2.ltp_def.hmsl = ref_alt * 1e3;
stateSetLocalOrigin_i(&ekf2.ltp_def);
@@ -854,7 +854,7 @@ static void gps_cb(uint8_t sender_id __attribute__((unused)),
struct LlaCoor_i lla_pos = lla_int_from_gps(gps_s);
gps_msg.lat = lla_pos.lat;
gps_msg.lon = lla_pos.lon;
gps_msg.alt = lla_pos.alt;
gps_msg.alt = gps_s->hmsl; // EKF2 works with HMSL
#if INS_EKF2_GPS_COURSE_YAW
gps_msg.yaw = wrap_pi((float)gps_s->course / 1e7);
gps_msg.yaw_offset = 0;