From 9a27eea7a7538af8583695618ec624ae2bbb8756 Mon Sep 17 00:00:00 2001 From: Gautier Hattenberger Date: Tue, 20 Sep 2022 17:09:20 +0200 Subject: [PATCH] [gps][ins] improve gps support for ins filters - use conveniance functions to access GPS position and speed - try to improve support of UBX fix status - gps.h compatible with cpp files --- conf/modules/ins_float_invariant.xml | 3 + conf/modules/ins_gps_passthrough.xml | 2 +- conf/modules/ins_mekf_wind.xml | 6 + sw/airborne/math/pprz_geodetic_int.h | 29 ++-- sw/airborne/modules/gps/gps.c | 128 ++++++++++++++++++ sw/airborne/modules/gps/gps.h | 71 ++++++++++ sw/airborne/modules/gps/gps_ubx.c | 23 +++- sw/airborne/modules/ins/ins.c | 2 + sw/airborne/modules/ins/ins.h | 6 + sw/airborne/modules/ins/ins_alt_float.c | 12 +- sw/airborne/modules/ins/ins_ekf2.cpp | 12 +- sw/airborne/modules/ins/ins_float_invariant.c | 17 ++- sw/airborne/modules/ins/ins_gps_passthrough.c | 12 +- .../modules/ins/ins_gps_passthrough_utm.c | 6 +- sw/airborne/modules/ins/ins_int.c | 20 ++- .../modules/ins/ins_mekf_wind_wrapper.c | 38 +++--- sw/airborne/modules/ins/ins_xsens.c | 6 +- sw/airborne/modules/ins/ins_xsens700.c | 6 +- 18 files changed, 317 insertions(+), 82 deletions(-) diff --git a/conf/modules/ins_float_invariant.xml b/conf/modules/ins_float_invariant.xml index f4abc97a1d..e53be554eb 100644 --- a/conf/modules/ins_float_invariant.xml +++ b/conf/modules/ins_float_invariant.xml @@ -50,6 +50,9 @@ + + + diff --git a/conf/modules/ins_gps_passthrough.xml b/conf/modules/ins_gps_passthrough.xml index dac908672f..3b23da0548 100644 --- a/conf/modules/ins_gps_passthrough.xml +++ b/conf/modules/ins_gps_passthrough.xml @@ -26,7 +26,7 @@ - + diff --git a/conf/modules/ins_mekf_wind.xml b/conf/modules/ins_mekf_wind.xml index e35e8cff06..65784915ed 100644 --- a/conf/modules/ins_mekf_wind.xml +++ b/conf/modules/ins_mekf_wind.xml @@ -82,6 +82,12 @@ + + + + + + diff --git a/sw/airborne/math/pprz_geodetic_int.h b/sw/airborne/math/pprz_geodetic_int.h index 3c235ce35c..64b6f67e9a 100644 --- a/sw/airborne/math/pprz_geodetic_int.h +++ b/sw/airborne/math/pprz_geodetic_int.h @@ -142,32 +142,37 @@ extern void ecef_of_ned_vect_i(struct EcefCoor_i *ecef, struct LtpDef_i *def, st #define HIGH_RES_TRIG_FRAC 20 #define VECT3_ENU_OF_NED(_o, _i) { \ - (_o).x = (_i).y; \ - (_o).y = (_i).x; \ - (_o).z = -(_i).z; \ + (_o).x = (_i).y; \ + (_o).y = (_i).x; \ + (_o).z = -(_i).z; \ } #define VECT3_NED_OF_ENU(_o, _i) VECT3_ENU_OF_NED(_o,_i) #define INT32_VECT3_NED_OF_ENU(_o, _i) VECT3_ENU_OF_NED(_o,_i) #define INT32_VECT3_ENU_OF_NED(_o, _i) VECT3_ENU_OF_NED(_o,_i) -#define ECEF_BFP_OF_REAL(_o, _i) { \ +#define VECT3_CM_OF_REAL(_o, _i) { \ (_o).x = (int32_t)CM_OF_M((_i).x); \ (_o).y = (int32_t)CM_OF_M((_i).y); \ (_o).z = (int32_t)CM_OF_M((_i).z); \ } -#define ECEF_FLOAT_OF_BFP(_o, _i) { \ - (_o).x = M_OF_CM((float)(_i).x); \ - (_o).y = M_OF_CM((float)(_i).y); \ - (_o).z = M_OF_CM((float)(_i).z); \ +#define VECT3_FLOAT_OF_CM(_o, _i) { \ + (_o).x = M_OF_CM((float)(_i).x); \ + (_o).y = M_OF_CM((float)(_i).y); \ + (_o).z = M_OF_CM((float)(_i).z); \ } -#define ECEF_DOUBLE_OF_BFP(_o, _i) { \ - (_o).x = M_OF_CM((double)(_i).x); \ - (_o).y = M_OF_CM((double)(_i).y); \ - (_o).z = M_OF_CM((double)(_i).z); \ +#define VECT3_DOUBLE_OF_CM(_o, _i) { \ + (_o).x = M_OF_CM((double)(_i).x); \ + (_o).y = M_OF_CM((double)(_i).y); \ + (_o).z = M_OF_CM((double)(_i).z); \ } + +#define ECEF_BFP_OF_REAL(_o, _i) VECT3_CM_OF_REAL(_o, _i) +#define ECEF_FLOAT_OF_BFP(_o, _i) VECT3_FLOAT_OF_CM(_o, _i) +#define ECEF_DOUBLE_OF_BFP(_o, _i) VECT3_DOUBLE_OF_CM(_o, _i) + #define LLA_BFP_OF_REAL(_o, _i) { \ (_o).lat = (int32_t)EM7DEG_OF_RAD((_i).lat); \ (_o).lon = (int32_t)EM7DEG_OF_RAD((_i).lon); \ diff --git a/sw/airborne/modules/gps/gps.c b/sw/airborne/modules/gps/gps.c index a49faae273..93906f9834 100644 --- a/sw/airborne/modules/gps/gps.c +++ b/sw/airborne/modules/gps/gps.c @@ -404,6 +404,134 @@ void gps_parse_RTCM_INJECT(uint8_t *buf) DL_RTCM_INJECT_data(buf)); } +/** + * Get GPS lla (float) + * Converted on the fly if not available + * @param[in] gps_s pointer to the gps structure + * @return lla position in float (rad), altitude ellipsoid (m) + */ +struct LlaCoor_f lla_float_from_gps(struct GpsState *gps_s) { + struct LlaCoor_i lla_i = lla_int_from_gps(gps_s); + struct LlaCoor_f lla_f; + LLA_FLOAT_OF_BFP(lla_f, lla_i); + return lla_f; +} + +/** + * Get GPS lla (integer) + * Converted on the fly if not available + * @param[in] gps_s pointer to the gps structure + * @return lla position (lat,lon: deg*1e7; alt: mm over ellipsoid) + */ +struct LlaCoor_i lla_int_from_gps(struct GpsState *gps_s) { + struct LlaCoor_i lla_i = { 0, 0, 0 }; + if (bit_is_set(gps_s->valid_fields, GPS_VALID_POS_LLA_BIT)) { + return gps_s->lla_pos; + } else if (bit_is_set(gps_s->valid_fields, GPS_VALID_POS_ECEF_BIT)) { + lla_of_ecef_i(&lla_i, &gps_s->ecef_pos); + } + return lla_i; +} + +/** + * Get GPS ecef pos (float) + * Converted on the fly if not available + * @param[in] gps_s pointer to the gps structure + * @return ecef position in float (m) + */ +struct EcefCoor_f ecef_float_from_gps(struct GpsState *gps_s) { + struct EcefCoor_i ecef_i = ecef_int_from_gps(gps_s); + struct EcefCoor_f ecef_f; + ECEF_FLOAT_OF_BFP(ecef_f, ecef_i); + return ecef_f; +} + +/** + * Get GPS ecef pos (integer) + * Converted on the fly if not available + * @param[in] gps_s pointer to the gps structure + * @return ecef position in cm + */ +struct EcefCoor_i ecef_int_from_gps(struct GpsState *gps_s) { + struct EcefCoor_i ecef_i = { 0, 0, 0 }; + if (bit_is_set(gps_s->valid_fields, GPS_VALID_POS_ECEF_BIT)) { + return gps_s->ecef_pos; + } else if (bit_is_set(gps_s->valid_fields, GPS_VALID_POS_LLA_BIT)) { + ecef_of_lla_i(&ecef_i, &gps_s->lla_pos); + } + return ecef_i; +} + +/** + * Get GPS ecef velocity (float) + * Converted on the fly if not available + * @param[in] gps_s pointer to the gps structure + * @return ecef velocity in float (m/s) + */ +struct EcefCoor_f ecef_vel_float_from_gps(struct GpsState *gps_s) { + struct EcefCoor_i ecef_vel_i = ecef_vel_int_from_gps(gps_s); + struct EcefCoor_f ecef_vel_f; + ECEF_FLOAT_OF_BFP(ecef_vel_f, ecef_vel_i); + return ecef_vel_f; +} + +/** + * Get GPS ecef velocity (integer) + * Converted on the fly if not available + * @param[in] gps_s pointer to the gps structure + * @return ecef velocity in cm/s + */ +struct EcefCoor_i ecef_vel_int_from_gps(struct GpsState *gps_s) { + struct EcefCoor_i ecef_vel_i = { 0, 0, 0 }; + if (bit_is_set(gps_s->valid_fields, GPS_VALID_VEL_ECEF_BIT)) { + return gps_s->ecef_vel; + } else if (bit_is_set(gps_s->valid_fields, GPS_VALID_VEL_NED_BIT)) { + struct LtpDef_i def; + if (bit_is_set(gps_s->valid_fields, GPS_VALID_POS_LLA_BIT)) { + ltp_def_from_lla_i(&def, &gps_s->lla_pos); + } else { // assume ECEF + ltp_def_from_ecef_i(&def, &gps_s->ecef_pos); + } + ecef_of_ned_vect_i(&ecef_vel_i, &def, &gps_s->ned_vel); + } + return ecef_vel_i; +} + +/** + * Get GPS ned velocity (float) + * Converted on the fly if not available + * @param[in] gps_s pointer to the gps structure + * @return ned velocity in float (m/s) + */ +struct NedCoor_f ned_vel_float_from_gps(struct GpsState *gps_s) { + struct NedCoor_i ned_vel_i = ned_vel_int_from_gps(gps_s); + struct NedCoor_f ned_vel_f; + VECT3_FLOAT_OF_CM(ned_vel_f, ned_vel_i); + return ned_vel_f; +} + +/** + * Get GPS ned velocity (integer) + * Converted on the fly if not available + * @param[in] gps_s pointer to the gps structure + * @return ned velocity in cm/s + */ +struct NedCoor_i ned_vel_int_from_gps(struct GpsState *gps_s) { + struct NedCoor_i ned_vel_i = { 0, 0, 0 }; + if (bit_is_set(gps_s->valid_fields, GPS_VALID_VEL_NED_BIT)) { + return gps_s->ned_vel; + } else if (bit_is_set(gps_s->valid_fields, GPS_VALID_VEL_ECEF_BIT)) { + struct LtpDef_i def; + if (bit_is_set(gps_s->valid_fields, GPS_VALID_POS_LLA_BIT)) { + ltp_def_from_lla_i(&def, &gps_s->lla_pos); + } else { // assume ECEF + ltp_def_from_ecef_i(&def, &gps_s->ecef_pos); + } + ned_of_ecef_vect_i(&ned_vel_i, &def, &gps_s->ecef_vel); + } + return ned_vel_i; +} + /** * Convenience functions to get utm position from GPS state */ diff --git a/sw/airborne/modules/gps/gps.h b/sw/airborne/modules/gps/gps.h index ccd0384b02..741ce76a19 100644 --- a/sw/airborne/modules/gps/gps.h +++ b/sw/airborne/modules/gps/gps.h @@ -27,6 +27,9 @@ #ifndef GPS_H #define GPS_H +#ifdef __cplusplus +extern "C" { +#endif #include "std.h" #include "math/pprz_geodetic_int.h" @@ -207,6 +210,70 @@ extern struct GpsTimeSync gps_time_sync; */ extern uint32_t gps_tow_from_sys_ticks(uint32_t sys_ticks); +/** + * Get GPS lla (float) + * Converted on the fly if not available + * @param[in] gps_s pointer to the gps structure + * @return lla position in float (rad), altitude ellipsoid (m) + */ +extern struct LlaCoor_f lla_float_from_gps(struct GpsState *gps_s); + +/** + * Get GPS lla (integer) + * Converted on the fly if not available + * @param[in] gps_s pointer to the gps structure + * @return lla position (lat,lon: deg*1e7; alt: mm over ellipsoid) + */ +extern struct LlaCoor_i lla_int_from_gps(struct GpsState *gps_s); + +/** + * Get GPS ecef pos (float) + * Converted on the fly if not available + * @param[in] gps_s pointer to the gps structure + * @return ecef position in float (m) + */ +extern struct EcefCoor_f ecef_float_from_gps(struct GpsState *gps_s); + +/** + * Get GPS ecef pos (integer) + * Converted on the fly if not available + * @param[in] gps_s pointer to the gps structure + * @return ecef position in cm + */ +extern struct EcefCoor_i ecef_int_from_gps(struct GpsState *gps_s); + +/** + * Get GPS ecef velocity (float) + * Converted on the fly if not available + * @param[in] gps_s pointer to the gps structure + * @return ecef velocity in float (m/s) + */ +extern struct EcefCoor_f ecef_vel_float_from_gps(struct GpsState *gps_s); + +/** + * Get GPS ecef velocity (integer) + * Converted on the fly if not available + * @param[in] gps_s pointer to the gps structure + * @return ecef velocity in cm/s + */ +extern struct EcefCoor_i ecef_vel_int_from_gps(struct GpsState *gps_s); + +/** + * Get GPS ned velocity (float) + * Converted on the fly if not available + * @param[in] gps_s pointer to the gps structure + * @return ned velocity in float (m/s) + */ +extern struct NedCoor_f ned_vel_float_from_gps(struct GpsState *gps_s); + +/** + * Get GPS ned velocity (integer) + * Converted on the fly if not available + * @param[in] gps_s pointer to the gps structure + * @return ned velocity in cm/s + */ +extern struct NedCoor_i ned_vel_int_from_gps(struct GpsState *gps_s); + /** * Convenience function to get utm position in float from GPS structure. * Beware that altitude is initialized to zero but not set to the correct value @@ -243,4 +310,8 @@ extern uint16_t gps_day_number(uint16_t year, uint8_t month, uint8_t day); */ extern uint16_t gps_week_number(uint16_t year, uint8_t month, uint8_t day); +#ifdef __cplusplus +} +#endif + #endif /* GPS_H */ diff --git a/sw/airborne/modules/gps/gps_ubx.c b/sw/airborne/modules/gps/gps_ubx.c index 5cd43e9913..28ec1b4975 100644 --- a/sw/airborne/modules/gps/gps_ubx.c +++ b/sw/airborne/modules/gps/gps_ubx.c @@ -194,14 +194,20 @@ static void gps_ubx_parse_nav_pvt(void) gps_ubx.state.hacc = UBX_NAV_PVT_hAcc(gps_ubx.msg_buf) / 10; gps_ubx.state.vacc = UBX_NAV_PVT_vAcc(gps_ubx.msg_buf) / 10; gps_ubx.state.sacc = UBX_NAV_PVT_sAcc(gps_ubx.msg_buf) / 10; + + //FIXME stupid workaround for PVT only + gps_ubx.state.pacc = gps_ubx.state.hacc; // report horizontal accuracy } static void gps_ubx_parse_nav_sol(void) { // Copy time and fix information -#if !USE_GPS_UBX_RTCM - gps_ubx.state.fix = UBX_NAV_SOL_gpsFix(gps_ubx.msg_buf); -#endif + uint8_t fix = UBX_NAV_SOL_gpsFix(gps_ubx.msg_buf); + if ((fix == GPS_FIX_3D && fix > gps_ubx.state.fix) || fix < GPS_FIX_3D) { + // update only if fix is better than current or fix not 3D + // leaving fix if in GNSS or RTK mode + gps_ubx.state.fix = fix; + } gps_ubx.state.tow = UBX_NAV_SOL_iTOW(gps_ubx.msg_buf); gps_ubx.state.week = UBX_NAV_SOL_week(gps_ubx.msg_buf); gps_ubx.state.num_sv = UBX_NAV_SOL_numSV(gps_ubx.msg_buf); @@ -351,9 +357,12 @@ static void gps_ubx_parse_nav_sat(void) static void gps_ubx_parse_nav_status(void) { -#if !USE_GPS_UBX_RTCM - gps_ubx.state.fix = UBX_NAV_STATUS_gpsFix(gps_ubx.msg_buf); -#endif + uint8_t fix = UBX_NAV_STATUS_gpsFix(gps_ubx.msg_buf); + if ((fix == GPS_FIX_3D && fix > gps_ubx.state.fix) || fix < GPS_FIX_3D) { + // update only if fix is better than current or fix not 3D + // leaving fix if in GNSS or RTK mode + gps_ubx.state.fix = fix; + } gps_ubx.state.tow = UBX_NAV_STATUS_iTOW(gps_ubx.msg_buf); gps_ubx.status_flags = UBX_NAV_STATUS_flags(gps_ubx.msg_buf); } @@ -370,7 +379,6 @@ static void gps_ubx_parse_nav_relposned(void) uint8_t gnssFixOK = RTCMgetbitu(&flags, 7, 1); /* Only save the latest valid relative position */ - if(relPosValid) { if (diffSoln && carrSoln == 2) { gps_ubx.state.fix = 5; // rtk } else if(diffSoln && carrSoln == 1) { @@ -380,6 +388,7 @@ static void gps_ubx_parse_nav_relposned(void) } else{ gps_ubx.state.fix = 0; } + if(relPosValid) { gps_relposned.iTOW = UBX_NAV_RELPOSNED_iTOW(gps_ubx.msg_buf); gps_relposned.refStationId = UBX_NAV_RELPOSNED_refStationId(gps_ubx.msg_buf); diff --git a/sw/airborne/modules/ins/ins.c b/sw/airborne/modules/ins/ins.c index f4f5695092..7e4aceff59 100644 --- a/sw/airborne/modules/ins/ins.c +++ b/sw/airborne/modules/ins/ins.c @@ -64,6 +64,8 @@ void WEAK ins_reset_local_origin(void) void WEAK ins_reset_altitude_ref(void) {} +void WEAK ins_reset_vertical_pos(void) {} + #if USE_GPS void WEAK ins_reset_utm_zone(struct UtmCoor_f *utm) { diff --git a/sw/airborne/modules/ins/ins.h b/sw/airborne/modules/ins/ins.h index 46846f12b8..e8556b5160 100644 --- a/sw/airborne/modules/ins/ins.h +++ b/sw/airborne/modules/ins/ins.h @@ -50,6 +50,12 @@ extern void ins_reset_local_origin(void); */ extern void ins_reset_altitude_ref(void); +/** INS vertical position reset. + * Reset only vertical position to zero. + * Does nothing if not implemented by specific INS algorithm. + */ +extern void ins_reset_vertical_pos(void); + /** INS utm zone reset. * Reset UTM zone according the the actual position. * Only used with fixedwing firmware. diff --git a/sw/airborne/modules/ins/ins_alt_float.c b/sw/airborne/modules/ins/ins_alt_float.c index 8683945b14..44cb51c809 100644 --- a/sw/airborne/modules/ins/ins_alt_float.c +++ b/sw/airborne/modules/ins/ins_alt_float.c @@ -210,6 +210,7 @@ void ins_alt_float_update_gps(struct GpsState *gps_s __attribute__((unused))) } struct UtmCoor_f utm = utm_float_from_gps(gps_s, nav_utm_zone0); + struct NedCoor_f ned_vel = ned_vel_float_from_gps(gps_s); #if !USE_BAROMETER #ifdef GPS_DT @@ -234,18 +235,15 @@ void ins_alt_float_update_gps(struct GpsState *gps_s __attribute__((unused))) alt_kalman_reset(); } else { alt_kalman(utm.alt, dt); - ins_altf.alt_dot = -gps_s->ned_vel.z / 100.0f; + ins_altf.alt_dot = -ned_vel.z; } -#endif +#endif // !USE_BAROMETER + utm.alt = ins_altf.alt; // set position stateSetPositionUtm_f(&utm); - struct NedCoor_f ned_vel = { - gps_s->ned_vel.x / 100.0f, - gps_s->ned_vel.y / 100.0f, - -ins_altf.alt_dot - }; + ned_vel.z = -ins_altf.alt_dot; // vz (down) from filter // set velocity stateSetSpeedNed_f(&ned_vel); diff --git a/sw/airborne/modules/ins/ins_ekf2.cpp b/sw/airborne/modules/ins/ins_ekf2.cpp index 86677c38e5..22f8271d71 100644 --- a/sw/airborne/modules/ins/ins_ekf2.cpp +++ b/sw/airborne/modules/ins/ins_ekf2.cpp @@ -854,8 +854,9 @@ static void gps_cb(uint8_t sender_id __attribute__((unused)), { gps_message gps_msg = {}; gps_msg.time_usec = stamp; - gps_msg.lat = gps_s->lla_pos.lat; - gps_msg.lon = gps_s->lla_pos.lon; + 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 = gps_s->hmsl; #if INS_EKF2_GPS_COURSE_YAW gps_msg.yaw = wrap_pi((float)gps_s->course / 1e7); @@ -869,9 +870,10 @@ static void gps_cb(uint8_t sender_id __attribute__((unused)), gps_msg.epv = gps_s->vacc / 100.0; gps_msg.sacc = gps_s->sacc / 100.0; gps_msg.vel_m_s = gps_s->gspeed / 100.0; - gps_msg.vel_ned(0) = (gps_s->ned_vel.x) / 100.0; - gps_msg.vel_ned(1) = (gps_s->ned_vel.y) / 100.0; - gps_msg.vel_ned(2) = (gps_s->ned_vel.z) / 100.0; + struct NedCoor_f ned_vel = ned_vel_float_from_gps(gps_s); + gps_msg.vel_ned(0) = ned_vel.x; + gps_msg.vel_ned(1) = ned_vel.y; + gps_msg.vel_ned(2) = ned_vel.z; gps_msg.vel_ned_valid = bit_is_set(gps_s->valid_fields, GPS_VALID_VEL_NED_BIT); gps_msg.nsats = gps_s->num_sv; gps_msg.pdop = gps_s->pdop; diff --git a/sw/airborne/modules/ins/ins_float_invariant.c b/sw/airborne/modules/ins/ins_float_invariant.c index 44ea15a56f..1c69365fae 100644 --- a/sw/airborne/modules/ins/ins_float_invariant.c +++ b/sw/airborne/modules/ins/ins_float_invariant.c @@ -286,8 +286,11 @@ void ins_reset_local_origin(void) // reset state UTM ref stateSetLocalUtmOrigin_f(&utm); #else + struct EcefCoor_i ecef_pos = ecef_int_from_gps(&gps); + struct LlaCoor_i lla_pos = lla_int_from_gps(&gps); struct LtpDef_i ltp_def; - ltp_def_from_ecef_i(<p_def, &gps.ecef_pos); + ltp_def_from_ecef_i(<p_def, &ecef_pos); + ltp_def.lla.alt = lla_pos.alt; ltp_def.hmsl = gps.hmsl; stateSetLocalOrigin_i(<p_def); #endif @@ -453,18 +456,18 @@ void ins_float_invariant_update_gps(struct GpsState *gps_s) ins_float_inv.meas.pos_gps.y = utm.east - state.utm_origin_f.east; ins_float_inv.meas.pos_gps.z = state.utm_origin_f.alt - utm.alt; // speed - ins_float_inv.meas.speed_gps.x = gps_s->ned_vel.x / 100.0f; - ins_float_inv.meas.speed_gps.y = gps_s->ned_vel.y / 100.0f; - ins_float_inv.meas.speed_gps.z = gps_s->ned_vel.z / 100.0f; + ins_float_inv.meas.speed_gps = ned_vel_float_from_gps(gps_s); } #else if (state.ned_initialized_f) { + // position struct NedCoor_i gps_pos_cm_ned, ned_pos; - ned_of_ecef_point_i(&gps_pos_cm_ned, &state.ned_origin_i, &gps_s->ecef_pos); + struct EcefCoor_i ecef_pos_i = ecef_int_from_gps(gps_s); + ned_of_ecef_point_i(&gps_pos_cm_ned, &state.ned_origin_i, &ecef_pos_i); INT32_VECT3_SCALE_2(ned_pos, gps_pos_cm_ned, INT32_POS_OF_CM_NUM, INT32_POS_OF_CM_DEN); NED_FLOAT_OF_BFP(ins_float_inv.meas.pos_gps, ned_pos); - struct EcefCoor_f ecef_vel; - ECEF_FLOAT_OF_BFP(ecef_vel, gps_s->ecef_vel); + // speed + struct EcefCoor_f ecef_vel = ecef_vel_float_from_gps(gps_s); ned_of_ecef_vect_f(&ins_float_inv.meas.speed_gps, &state.ned_origin_f, &ecef_vel); } #endif diff --git a/sw/airborne/modules/ins/ins_gps_passthrough.c b/sw/airborne/modules/ins/ins_gps_passthrough.c index bc8bccfa0b..70a61644c9 100644 --- a/sw/airborne/modules/ins/ins_gps_passthrough.c +++ b/sw/airborne/modules/ins/ins_gps_passthrough.c @@ -89,13 +89,15 @@ static void gps_cb(uint8_t sender_id __attribute__((unused)), /* simply scale and copy pos/speed from gps */ struct NedCoor_i gps_pos_cm_ned; - ned_of_ecef_point_i(&gps_pos_cm_ned, &ins_gp.ltp_def, &gps_s->ecef_pos); + struct EcefCoor_i ecef_pos = ecef_int_from_gps(gps_s); + ned_of_ecef_point_i(&gps_pos_cm_ned, &ins_gp.ltp_def, &ecef_pos); INT32_VECT3_SCALE_2(ins_gp.ltp_pos, gps_pos_cm_ned, INT32_POS_OF_CM_NUM, INT32_POS_OF_CM_DEN); stateSetPositionNed_i(&ins_gp.ltp_pos); struct NedCoor_i gps_speed_cm_s_ned; - ned_of_ecef_vect_i(&gps_speed_cm_s_ned, &ins_gp.ltp_def, &gps_s->ecef_vel); + struct EcefCoor_i ecef_vel = ecef_vel_int_from_gps(gps_s); + ned_of_ecef_vect_i(&gps_speed_cm_s_ned, &ins_gp.ltp_def, &ecef_vel); INT32_VECT3_SCALE_2(ins_gp.ltp_speed, gps_speed_cm_s_ned, INT32_SPEED_OF_CM_S_NUM, INT32_SPEED_OF_CM_S_DEN); stateSetSpeedNed_i(&ins_gp.ltp_speed); @@ -171,8 +173,10 @@ void ins_gps_passthrough_init(void) void ins_reset_local_origin(void) { - ltp_def_from_ecef_i(&ins_gp.ltp_def, &gps.ecef_pos); - ins_gp.ltp_def.lla.alt = gps.lla_pos.alt; + struct EcefCoor_i ecef_pos = ecef_int_from_gps(&gps); + struct LlaCoor_i lla_pos = lla_int_from_gps(&gps); + ltp_def_from_ecef_i(&ins_gp.ltp_def, &ecef_pos); + ins_gp.ltp_def.lla.alt = lla_pos.alt; ins_gp.ltp_def.hmsl = gps.hmsl; stateSetLocalOrigin_i(&ins_gp.ltp_def); ins_gp.ltp_initialized = true; diff --git a/sw/airborne/modules/ins/ins_gps_passthrough_utm.c b/sw/airborne/modules/ins/ins_gps_passthrough_utm.c index beb048c0d8..a5dda1bdf5 100644 --- a/sw/airborne/modules/ins/ins_gps_passthrough_utm.c +++ b/sw/airborne/modules/ins/ins_gps_passthrough_utm.c @@ -56,11 +56,7 @@ static void gps_cb(uint8_t sender_id __attribute__((unused)), // set position stateSetPositionUtm_f(&utm); - struct NedCoor_f ned_vel = { - gps_s->ned_vel.x / 100.0f, - gps_s->ned_vel.y / 100.0f, - gps_s->ned_vel.z / 100.0f - }; + struct NedCoor_f ned_vel = ned_vel_float_from_gps(gps_s); // set velocity stateSetSpeedNed_f(&ned_vel); } diff --git a/sw/airborne/modules/ins/ins_int.c b/sw/airborne/modules/ins/ins_int.c index 8016ebd35b..e86c3846c8 100644 --- a/sw/airborne/modules/ins/ins_int.c +++ b/sw/airborne/modules/ins/ins_int.c @@ -250,8 +250,10 @@ void ins_reset_local_origin(void) { #if USE_GPS if (GpsFixValid()) { - ltp_def_from_ecef_i(&ins_int.ltp_def, &gps.ecef_pos); - ins_int.ltp_def.lla.alt = gps.lla_pos.alt; + struct EcefCoor_i ecef_pos = ecef_int_from_gps(&gps); + struct LlaCoor_i lla_pos = lla_int_from_gps(&gps); + ltp_def_from_ecef_i(&ins_int.ltp_def, &ecef_pos); + ins_int.ltp_def.lla.alt = lla_pos.alt; ins_int.ltp_def.hmsl = gps.hmsl; ins_int.ltp_initialized = true; stateSetLocalOrigin_i(&ins_int.ltp_def); @@ -272,10 +274,11 @@ void ins_reset_altitude_ref(void) { #if USE_GPS if (GpsFixValid()) { + struct LlaCoor_i lla_pos = lla_int_from_gps(&gps); struct LlaCoor_i lla = { .lat = state.ned_origin_i.lla.lat, .lon = state.ned_origin_i.lla.lon, - .alt = gps.lla_pos.alt + .alt = lla_pos.alt }; ltp_def_from_lla_i(&ins_int.ltp_def, &lla); ins_int.ltp_def.hmsl = gps.hmsl; @@ -285,6 +288,11 @@ void ins_reset_altitude_ref(void) ins_int.vf_reset = true; } +void ins_reset_vertical_pos(void) +{ + ins_int.vf_reset = true; +} + void ins_int_propagate(struct Int32Vect3 *accel, float dt) { /* untilt accels */ @@ -396,7 +404,8 @@ void ins_int_update_gps(struct GpsState *gps_s) } struct NedCoor_i gps_pos_cm_ned; - ned_of_ecef_point_i(&gps_pos_cm_ned, &ins_int.ltp_def, &gps_s->ecef_pos); + struct EcefCoor_i ecef_pos_i = ecef_int_from_gps(gps_s); + ned_of_ecef_point_i(&gps_pos_cm_ned, &ins_int.ltp_def, &ecef_pos_i); /* calculate body frame position taking BODY_TO_GPS translation (in cm) into account */ #ifdef INS_BODY_TO_GPS_X @@ -417,7 +426,8 @@ void ins_int_update_gps(struct GpsState *gps_s) /// @todo maybe use gps_s->ned_vel directly?? struct NedCoor_i gps_speed_cm_s_ned; - ned_of_ecef_vect_i(&gps_speed_cm_s_ned, &ins_int.ltp_def, &gps_s->ecef_vel); + struct EcefCoor_i ecef_vel_i = ecef_vel_int_from_gps(gps_s); + ned_of_ecef_vect_i(&gps_speed_cm_s_ned, &ins_int.ltp_def, &ecef_vel_i); #if INS_USE_GPS_ALT vff_update_z_conf(((float)gps_pos_cm_ned.z) / 100.0, INS_VFF_R_GPS); diff --git a/sw/airborne/modules/ins/ins_mekf_wind_wrapper.c b/sw/airborne/modules/ins/ins_mekf_wind_wrapper.c index 2c277b5496..ec4c8980d6 100644 --- a/sw/airborne/modules/ins/ins_mekf_wind_wrapper.c +++ b/sw/airborne/modules/ins/ins_mekf_wind_wrapper.c @@ -39,8 +39,7 @@ #include "generated/airframe.h" #include "generated/flight_plan.h" -#define MEKF_WIND_USE_UTM TRUE -#if MEKF_WIND_USE_UTM +#if FIXEDWING_FIRMWARE #include "firmwares/fixedwing/nav.h" #endif @@ -441,24 +440,22 @@ static void gps_cb(uint8_t sender_id __attribute__((unused)), { if (ins_mekf_wind.is_aligned && gps_s->fix >= GPS_FIX_3D) { -#if MEKF_WIND_USE_UTM +#if FIXEDWING_FIRMWARE if (state.utm_initialized_f) { struct UtmCoor_f utm = utm_float_from_gps(gps_s, nav_utm_zone0); - struct FloatVect3 pos, speed; + struct NedCoor_f pos, speed; // position (local ned) pos.x = utm.north - state.utm_origin_f.north; pos.y = utm.east - state.utm_origin_f.east; pos.z = state.utm_origin_f.alt - utm.alt; // speed - speed.x = gps_s->ned_vel.x / 100.0f; - speed.y = gps_s->ned_vel.y / 100.0f; - speed.z = gps_s->ned_vel.z / 100.0f; + speed = ned_vel_float_from_gps(gps_s); if (!ins_mekf_wind.gps_initialized) { - ins_mekf_wind_set_pos_ned((struct NedCoor_f*)(&pos)); - ins_mekf_wind_set_speed_ned((struct NedCoor_f*)(&speed)); + ins_mekf_wind_set_pos_ned(&pos); + ins_mekf_wind_set_speed_ned(&speed); ins_mekf_wind.gps_initialized = true; } - ins_mekf_wind_update_pos_speed(&pos, &speed); + ins_mekf_wind_update_pos_speed((struct FloatVect3*)(&pos), (struct FloatVect3*)(&speed)); #if LOG_MEKFW_FILTER if (LogFileIsOpen()) { @@ -472,15 +469,15 @@ static void gps_cb(uint8_t sender_id __attribute__((unused)), #else if (state.ned_initialized_f) { - struct FloatVect3 pos, speed; + struct NedCoor_f pos, speed; struct NedCoor_i gps_pos_cm_ned, ned_pos; - ned_of_ecef_point_i(&gps_pos_cm_ned, &state.ned_origin_i, &gps_s->ecef_pos); + struct EcefCoor_i ecef_pos_i = ecef_int_from_gps(gps_s); + ned_of_ecef_point_i(&gps_pos_cm_ned, &state.ned_origin_i, &ecef_pos_i); INT32_VECT3_SCALE_2(ned_pos, gps_pos_cm_ned, INT32_POS_OF_CM_NUM, INT32_POS_OF_CM_DEN); NED_FLOAT_OF_BFP(pos, ned_pos); - struct EcefCoor_f ecef_vel; - ECEF_FLOAT_OF_BFP(ecef_vel, gps_s->ecef_vel); + struct EcefCoor_f ecef_vel = ecef_vel_float_from_gps(gps_s); ned_of_ecef_vect_f(&speed, &state.ned_origin_f, &ecef_vel); - ins_mekf_wind_update_pos_speed(&pos, &speed); + ins_mekf_wind_update_pos_speed((struct FloatVect3*)(&pos), (struct FloatVect3*)(&speed)); #if LOG_MEKFW_FILTER if (LogFileIsOpen()) { @@ -522,7 +519,7 @@ static void set_state_from_ins(void) void ins_mekf_wind_wrapper_init(void) { // init position -#if MEKF_WIND_USE_UTM +#if FIXEDWING_FIRMWARE struct UtmCoor_f utm0; utm0.north = (float)nav_utm_north0; utm0.east = (float)nav_utm_east0; @@ -606,13 +603,16 @@ void ins_mekf_wind_wrapper_init(void) void ins_reset_local_origin(void) { -#if MEKF_WIND_USE_UTM +#if FIXEDWING_FIRMWARE struct UtmCoor_f utm = utm_float_from_gps(&gps, 0); // reset state UTM ref stateSetLocalUtmOrigin_f(&utm); #else + struct EcefCoor_i ecef_pos = ecef_int_from_gps(&gps); + struct LlaCoor_i lla_pos = lla_int_from_gps(&gps); struct LtpDef_i ltp_def; - ltp_def_from_ecef_i(<p_def, &gps.ecef_pos); + ltp_def_from_ecef_i(<p_def, &ecef_pos); + ltp_def.lla.alt = lla_pos.alt; ltp_def.hmsl = gps.hmsl; stateSetLocalOrigin_i(<p_def); #endif @@ -622,7 +622,7 @@ void ins_reset_local_origin(void) void ins_reset_altitude_ref(void) { -#if MEKF_WIND_USE_UTM +#if FIXEDWING_FIRMWARE struct UtmCoor_f utm = state.utm_origin_f; utm.alt = gps.hmsl / 1000.0f; stateSetLocalUtmOrigin_f(&utm); diff --git a/sw/airborne/modules/ins/ins_xsens.c b/sw/airborne/modules/ins/ins_xsens.c index 55f7f668e6..3df25122f3 100644 --- a/sw/airborne/modules/ins/ins_xsens.c +++ b/sw/airborne/modules/ins/ins_xsens.c @@ -94,11 +94,7 @@ static void gps_cb(uint8_t sender_id __attribute__((unused)), // set position stateSetPositionUtm_f(&utm); - struct NedCoor_f ned_vel = { - gps_s->ned_vel.x / 100., - gps_s->ned_vel.y / 100., - gps_s->ned_vel.z / 100. - }; + struct NedCoor_f ned_vel = ned_vel_float_from_gps(gps_s); // set velocity stateSetSpeedNed_f(&ned_vel); } diff --git a/sw/airborne/modules/ins/ins_xsens700.c b/sw/airborne/modules/ins/ins_xsens700.c index d859e76ff6..8e6f8c6665 100644 --- a/sw/airborne/modules/ins/ins_xsens700.c +++ b/sw/airborne/modules/ins/ins_xsens700.c @@ -96,11 +96,7 @@ static void gps_cb(uint8_t sender_id __attribute__((unused)), // set position stateSetPositionUtm_f(&utm); - struct NedCoor_f ned_vel = { - gps_s->ned_vel.x / 100., - gps_s->ned_vel.y / 100., - gps_s->ned_vel.z / 100. - }; + struct NedCoor_f ned_vel = ned_vel_float_from_gps(gps_s); // set velocity stateSetSpeedNed_f(&ned_vel); }