From 29c4174be73ef51024cfa0291cdb80241285a034 Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Sun, 13 Dec 2015 00:33:21 +0100 Subject: [PATCH] [gps] add fields_valid and remove GPS_USE_LATLONG add fields_valid bitfiled to gps struct and use that to decide whether a valid UTM pos is availabe instead of always computing UTM if GPS_USE_LATLONG is defined should close #641 --- conf/conf_tests.xml | 2 +- .../fixedwing/gps_mediatek_diy.makefile | 7 ++- .../subsystems/fixedwing/gps_piksi.makefile | 6 +-- .../fixedwing/gps_ublox_hitl.makefile | 2 +- .../fixedwing/gps_ublox_utm.makefile | 2 +- .../fixedwing/ins_vectornav.makefile | 6 +-- .../subsystems/fixedwing/ins_xsens.makefile | 4 +- .../subsystems/rotorcraft/gps_furuno.makefile | 25 ----------- .../subsystems/rotorcraft/gps_nmea.makefile | 24 ---------- .../rotorcraft/gps_skytraq.makefile | 21 --------- .../subsystems/rotorcraft/gps_ublox.makefile | 20 --------- .../{fixedwing => shared}/gps_furuno.makefile | 6 +-- .../{fixedwing => shared}/gps_nmea.makefile | 7 ++- .../gps_skytraq.makefile | 6 +-- .../{fixedwing => shared}/gps_ublox.makefile | 7 ++- .../ins_xsens700.makefile | 2 +- conf/modules/gps_ublox.xml | 3 -- sw/airborne/arch/sim/sim_gps.c | 23 +++------- sw/airborne/modules/cam_control/point.c | 8 ++-- sw/airborne/modules/ins/ins_xsens.c | 8 ++++ sw/airborne/modules/ins/ins_xsens700.c | 3 ++ sw/airborne/subsystems/gps.c | 1 + sw/airborne/subsystems/gps.h | 10 +++++ sw/airborne/subsystems/gps/gps_datalink.c | 43 +++++------------- sw/airborne/subsystems/gps/gps_mtk.c | 33 +++----------- sw/airborne/subsystems/gps/gps_nmea.c | 21 ++------- sw/airborne/subsystems/gps/gps_piksi.c | 45 +++++++------------ sw/airborne/subsystems/gps/gps_sim_nps.c | 29 ++++-------- sw/airborne/subsystems/gps/gps_sirf.c | 24 +++------- sw/airborne/subsystems/gps/gps_skytraq.c | 31 +++++-------- sw/airborne/subsystems/gps/gps_ubx.c | 41 +++++++---------- sw/airborne/subsystems/gps/gps_ubx.h | 2 - sw/airborne/subsystems/gps/gps_udp.c | 25 +++-------- sw/airborne/subsystems/ins.c | 36 ++++++++------- sw/airborne/subsystems/ins/ins_alt_float.c | 25 ++++++----- .../subsystems/ins/ins_float_invariant.c | 26 ++++++----- .../subsystems/ins/ins_gps_passthrough_utm.c | 25 ++++++----- sw/airborne/subsystems/ins/ins_vectornav.c | 20 ++------- sw/airborne/subsystems/ins/ins_vectornav.h | 6 --- sw/tools/generators/gen_flight_plan.ml | 2 +- 40 files changed, 205 insertions(+), 432 deletions(-) delete mode 100644 conf/firmwares/subsystems/rotorcraft/gps_furuno.makefile delete mode 100644 conf/firmwares/subsystems/rotorcraft/gps_nmea.makefile delete mode 100644 conf/firmwares/subsystems/rotorcraft/gps_skytraq.makefile delete mode 100644 conf/firmwares/subsystems/rotorcraft/gps_ublox.makefile rename conf/firmwares/subsystems/{fixedwing => shared}/gps_furuno.makefile (86%) rename conf/firmwares/subsystems/{fixedwing => shared}/gps_nmea.makefile (80%) rename conf/firmwares/subsystems/{fixedwing => shared}/gps_skytraq.makefile (84%) rename conf/firmwares/subsystems/{fixedwing => shared}/gps_ublox.makefile (80%) rename conf/firmwares/subsystems/{fixedwing => shared}/ins_xsens700.makefile (97%) diff --git a/conf/conf_tests.xml b/conf/conf_tests.xml index 0e6ac57388..31b1ebc521 100644 --- a/conf/conf_tests.xml +++ b/conf/conf_tests.xml @@ -161,7 +161,7 @@ telemetry="telemetry/default_fixedwing.xml" flight_plan="flight_plans/basic.xml" settings="settings/fixedwing_basic.xml settings/control/ctl_basic.xml" - settings_modules="modules/nav_smooth.xml modules/nav_survey_poly_osam.xml modules/infrared_adc.xml modules/tune_airspeed.xml" + settings_modules="modules/tune_airspeed.xml modules/infrared_adc.xml modules/nav_smooth.xml modules/nav_survey_poly_osam.xml" gui_color="#6293ba" /> - - ap.CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_ubx.h\" ap.CFLAGS += -DUSE_$(GPS_PORT) -D$(GPS_PORT)_BAUD=$(GPS_BAUD) @@ -44,7 +42,6 @@ - sim.CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_sim.h\" jsbsim.CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_sim.h\" diff --git a/sw/airborne/arch/sim/sim_gps.c b/sw/airborne/arch/sim/sim_gps.c index f60fc99b7f..39199eb6e6 100644 --- a/sw/airborne/arch/sim/sim_gps.c +++ b/sw/airborne/arch/sim/sim_gps.c @@ -22,38 +22,29 @@ value sim_use_gps_pos(value x, value y, value z, value c, value a, value s, valu { gps.fix = (Bool_val(m) ? 3 : 0); gps.course = Double_val(c) * 1e7; + SetBit(gps.valid_fields, GPS_VALID_COURSE_BIT); gps.hmsl = Double_val(a) * 1000.; + SetBit(gps.valid_fields, GPS_VALID_HMSL_BIT); gps.gspeed = Double_val(s) * 100.; gps.ned_vel.x = gps.gspeed * cos(Double_val(c)); gps.ned_vel.y = gps.gspeed * sin(Double_val(c)); gps.ned_vel.z = -Double_val(cl) * 100.; + SetBit(gps.valid_fields, GPS_VALID_VEL_NED_BIT); gps.week = 0; // FIXME gps.tow = Double_val(t) * 1000.; - //TODO set alt above ellipsoid and hmsl - -#ifdef GPS_USE_LATLONG struct LlaCoor_f lla_f; - struct UtmCoor_f utm_f; lla_f.lat = Double_val(lat); lla_f.lon = Double_val(lon); + //TODO set alt above ellipsoid, use hmsl for now lla_f.alt = Double_val(a); - utm_f.zone = nav_utm_zone0; - utm_of_lla_f(&utm_f, &lla_f); LLA_BFP_OF_REAL(gps.lla_pos, lla_f); - gps.utm_pos.east = utm_f.east * 100; - gps.utm_pos.north = utm_f.north * 100; - gps.utm_pos.zone = nav_utm_zone0; - x = y = z; /* Just to get rid of the "unused arg" warning */ - y = x; /* Just to get rid of the "unused arg" warning */ -#else // GPS_USE_LATLONG + SetBit(gps.valid_fields, GPS_VALID_POS_UTM_BIT); + gps.utm_pos.east = Int_val(x); gps.utm_pos.north = Int_val(y); gps.utm_pos.zone = Int_val(z); - lat = lon; /* Just to get rid of the "unused arg" warning */ - lon = lat; /* Just to get rid of the "unused arg" warning */ -#endif // GPS_USE_LATLONG - + SetBit(gps.valid_fields, GPS_VALID_POS_UTM_BIT); /** Space vehicle info simulation */ gps.nb_channels = 7; diff --git a/sw/airborne/modules/cam_control/point.c b/sw/airborne/modules/cam_control/point.c index ebb5f42707..2be9eae0fb 100644 --- a/sw/airborne/modules/cam_control/point.c +++ b/sw/airborne/modules/cam_control/point.c @@ -79,6 +79,7 @@ #include "subsystems/navigation/common_nav.h" #include "subsystems/gps.h" #include "math/pprz_geodetic_float.h" +#include "state.h" typedef struct { float fx; @@ -363,10 +364,9 @@ void vPoint(float fPlaneEast, float fPlaneNorth, float fPlaneAltitude, cam_point_distance_from_home = distance_correction * (uint16_t)(sqrt((cam_point_x * cam_point_x) + (cam_point_y * cam_point_y))); - struct UtmCoor_f utm; - utm.east = gps.utm_pos.east / 100. + sv_cam_projection.fx; - utm.north = gps.utm_pos.north / 100. + sv_cam_projection.fy; - utm.zone = gps.utm_pos.zone; + struct UtmCoor_f utm = *stateGetPositionUtm_f(); + utm.east += sv_cam_projection.fx; + utm.north += sv_cam_projection.fy; struct LlaCoor_f lla; lla_of_utm_f(&lla, &utm); cam_point_lon = lla.lon * (180 / M_PI); diff --git a/sw/airborne/modules/ins/ins_xsens.c b/sw/airborne/modules/ins/ins_xsens.c index 25a6ecc35f..8a98e60c8c 100644 --- a/sw/airborne/modules/ins/ins_xsens.c +++ b/sw/airborne/modules/ins/ins_xsens.c @@ -469,6 +469,7 @@ void handle_ins_msg(void) float fcourse = atan2f((float)ins_vy, (float)ins_vx); gps.course = fcourse * 1e7; + SetBit(gps.valid_fields, GPS_VALID_COURSE_BIT); #endif // USE_GPS_XSENS } @@ -534,6 +535,7 @@ void parse_ins_msg(void) gps.lla_pos.lat = XSENS_DATA_RAWGPS_lat(xsens_msg_buf, offset); gps.lla_pos.lon = XSENS_DATA_RAWGPS_lon(xsens_msg_buf, offset); gps.lla_pos.alt = XSENS_DATA_RAWGPS_alt(xsens_msg_buf, offset); + SetBit(gps.valid_fields, GPS_VALID_POS_LLA_BIT); /* Set the real UTM zone */ gps.utm_pos.zone = (gps.lla_pos.lon / 1e7 + 180) / 6 + 1; @@ -545,6 +547,7 @@ void parse_ins_msg(void) gps.utm_pos.east = utm_f.east * 100; gps.utm_pos.north = utm_f.north * 100; gps.utm_pos.alt = gps.lla_pos.alt; + SetBit(gps.valid_fields, GPS_VALID_POS_UTM_BIT); ins_x = utm_f.east; ins_y = utm_f.north; @@ -554,6 +557,7 @@ void parse_ins_msg(void) // Compute geoid (MSL) height float hmsl = wgs84_ellipsoid_to_geoid(lla_f.lat, lla_f.lon); gps.hmsl = XSENS_DATA_RAWGPS_alt(xsens_msg_buf, offset) - (hmsl * 1000.0f); + SetBit(gps.valid_fields, GPS_VALID_HMSL_BIT); ins_vx = ((INS_FORMAT)XSENS_DATA_RAWGPS_vel_n(xsens_msg_buf, offset)) / 100.; ins_vy = ((INS_FORMAT)XSENS_DATA_RAWGPS_vel_e(xsens_msg_buf, offset)) / 100.; @@ -561,6 +565,7 @@ void parse_ins_msg(void) gps.ned_vel.x = XSENS_DATA_RAWGPS_vel_n(xsens_msg_buf, offset); gps.ned_vel.y = XSENS_DATA_RAWGPS_vel_e(xsens_msg_buf, offset); gps.ned_vel.z = XSENS_DATA_RAWGPS_vel_d(xsens_msg_buf, offset); + SetBit(gps.valid_fields, GPS_VALID_VEL_NED_BIT); gps.pacc = XSENS_DATA_RAWGPS_hacc(xsens_msg_buf, offset) / 100; gps.sacc = XSENS_DATA_RAWGPS_sacc(xsens_msg_buf, offset) / 100; gps.pdop = 5; // FIXME Not output by XSens @@ -647,16 +652,19 @@ void parse_ins_msg(void) lla_f.lon = RadOfDeg(XSENS_DATA_Position_lon(xsens_msg_buf, offset)); gps.lla_pos.lat = (int32_t)(DegOfRad(lla_f.lat) * 1e7); gps.lla_pos.lon = (int32_t)(DegOfRad(lla_f.lon) * 1e7); + SetBit(gps.valid_fields, GPS_VALID_POS_LLA_BIT); gps.utm_pos.zone = (DegOfRad(lla_f.lon) + 180) / 6 + 1; /* convert to utm */ utm_of_lla_f(&utm_f, &lla_f); /* copy results of utm conversion */ gps.utm_pos.east = utm_f.east * 100; gps.utm_pos.north = utm_f.north * 100; + SetBit(gps.valid_fields, GPS_VALID_POS_UTM_BIT); ins_x = utm_f.east; ins_y = utm_f.north; ins_z = XSENS_DATA_Position_alt(xsens_msg_buf, offset); //TODO is this hms or above ellipsoid? gps.hmsl = ins_z * 1000; + SetBit(gps.valid_fields, GPS_VALID_HMSL_BIT); // what about gps.lla_pos.alt and gps.utm_pos.alt ? gps_xsens_publish(); diff --git a/sw/airborne/modules/ins/ins_xsens700.c b/sw/airborne/modules/ins/ins_xsens700.c index 51930ab1e0..727ad4943b 100644 --- a/sw/airborne/modules/ins/ins_xsens700.c +++ b/sw/airborne/modules/ins/ins_xsens700.c @@ -490,6 +490,7 @@ void parse_ins_msg(void) // Compute geoid (MSL) height float geoid_h = wgs84_ellipsoid_to_geoid(lla_f.lat, lla_f.lon); gps.hmsl = gps.utm_pos.alt - (geoid_h * 1000.0f); + SetBit(gps.valid_fields, GPS_VALID_HMSL_BIT); //gps.tow = geoid_h * 1000.0f; //gps.utm_pos.alt; } else if (code2 == 0x40) { @@ -512,6 +513,7 @@ void parse_ins_msg(void) // copy results of utm conversion gps.utm_pos.east = utm_f.east * 100; gps.utm_pos.north = utm_f.north * 100; + SetBit(gps.valid_fields, GPS_VALID_POS_UTM_BIT); gps_xsens_publish(); } @@ -524,6 +526,7 @@ void parse_ins_msg(void) gps.ned_vel.x = ins_vx; gps.ned_vel.y = ins_vy; gps.ned_vel.z = ins_vx; + SetBit(gps.valid_fields, GPS_VALID_VEL_NED_BIT); } } diff --git a/sw/airborne/subsystems/gps.c b/sw/airborne/subsystems/gps.c index f5e811874c..a15ae97b2a 100644 --- a/sw/airborne/subsystems/gps.c +++ b/sw/airborne/subsystems/gps.c @@ -134,6 +134,7 @@ static void send_gps_sol(struct transport_tx *trans, struct link_device *dev) void gps_init(void) { + gps.valid_fields = 0; gps.fix = GPS_FIX_NONE; gps.week = 0; gps.tow = 0; diff --git a/sw/airborne/subsystems/gps.h b/sw/airborne/subsystems/gps.h index 33970ec3d5..7e2679d88e 100644 --- a/sw/airborne/subsystems/gps.h +++ b/sw/airborne/subsystems/gps.h @@ -53,6 +53,14 @@ #define GPS_NB_CHANNELS 1 #endif +#define GPS_VALID_POS_ECEF_BIT 0 +#define GPS_VALID_POS_LLA_BIT 1 +#define GPS_VALID_POS_UTM_BIT 2 +#define GPS_VALID_VEL_ECEF_BIT 3 +#define GPS_VALID_VEL_NED_BIT 4 +#define GPS_VALID_HMSL_BIT 5 +#define GPS_VALID_COURSE_BIT 6 + /** data structure for Space Vehicle Information of a single satellite */ struct SVinfo { uint8_t svid; ///< Satellite ID @@ -65,6 +73,8 @@ struct SVinfo { /** data structure for GPS information */ struct GpsState { + uint8_t valid_fields; ///< bitfield indicating valid fields (GPS_VALID_x_BIT) + struct EcefCoor_i ecef_pos; ///< position in ECEF in cm struct LlaCoor_i lla_pos; ///< position in LLA (lat,lon: deg*1e7; alt: mm over ellipsoid) struct UtmCoor_i utm_pos; ///< position in UTM (north,east: cm; alt: mm over ellipsoid) diff --git a/sw/airborne/subsystems/gps/gps_datalink.c b/sw/airborne/subsystems/gps/gps_datalink.c index 1c7b3914f2..1b6f4c3545 100644 --- a/sw/airborne/subsystems/gps/gps_datalink.c +++ b/sw/airborne/subsystems/gps/gps_datalink.c @@ -100,6 +100,7 @@ void parse_gps_datalink_small(uint8_t num_sv, uint32_t pos_xyz, uint32_t speed_x lla_of_ecef_i(&lla_pos, &ecef_pos); gps.lla_pos = lla_pos; + SetBit(gps.valid_fields, GPS_VALID_POS_LLA_BIT); enu_speed.x = (int32_t)((speed_xy >> 22) & 0x3FF); // bits 31-22 speed x in cm/s if (enu_speed.x & 0x200) { @@ -114,37 +115,24 @@ void parse_gps_datalink_small(uint8_t num_sv, uint32_t pos_xyz, uint32_t speed_x // printf("ENU Speed: %u (%d, %d, %d)\n", speed_xy, enu_speed.x, enu_speed.y, enu_speed.z); ecef_of_enu_vect_i(&gps.ecef_vel , &tracking_ltp , &enu_speed); + SetBit(gps.valid_fields, GPS_VALID_VEL_ECEF_BIT); gps.hmsl = tracking_ltp.hmsl + enu_pos.z * 10; // TODO: try to compensate for the loss in accuracy + SetBit(gps.valid_fields, GPS_VALID_HMSL_BIT); gps.course = (int32_t)((speed_xy >> 2) & 0x3FF); // bits 11-2 heading in rad*1e2 if (gps.course & 0x200) { gps.course |= 0xFFFFFC00; // fix for twos complements } - // printf("Heading: %d\n", gps.course); - gps.course *= 1e5; + SetBit(gps.valid_fields, GPS_VALID_COURSE_BIT); + gps.num_sv = num_sv; gps.tow = 0; // set time-of-weak to 0 gps.fix = GPS_FIX_3D; // set 3D fix to true gps_available = TRUE; // set GPS available to true -#if GPS_USE_LATLONG - // Computes from (lat, long) in the referenced UTM zone - struct LlaCoor_f lla_f; - LLA_FLOAT_OF_BFP(lla_f, gps.lla_pos); - struct UtmCoor_f utm_f; - utm_f.zone = nav_utm_zone0; - // convert to utm - utm_of_lla_f(&utm_f, &lla_f); - // copy results of utm conversion - gps.utm_pos.east = utm_f.east * 100; - gps.utm_pos.north = utm_f.north * 100; - gps.utm_pos.alt = gps.lla_pos.alt; - gps.utm_pos.zone = nav_utm_zone0; -#endif - // publish new GPS data uint32_t now_ts = get_sys_time_usec(); gps.last_msg_ticks = sys_time.nb_sec_rem; @@ -165,37 +153,28 @@ void parse_gps_datalink(uint8_t numsv, int32_t ecef_x, int32_t ecef_y, int32_t e gps.lla_pos.lat = lat; gps.lla_pos.lon = lon; gps.lla_pos.alt = alt; + SetBit(gps.valid_fields, GPS_VALID_POS_LLA_BIT); + gps.hmsl = hmsl; + SetBit(gps.valid_fields, GPS_VALID_HMSL_BIT); gps.ecef_pos.x = ecef_x; gps.ecef_pos.y = ecef_y; gps.ecef_pos.z = ecef_z; + SetBit(gps.valid_fields, GPS_VALID_POS_ECEF_BIT); gps.ecef_vel.x = ecef_xd; gps.ecef_vel.y = ecef_yd; gps.ecef_vel.z = ecef_zd; + SetBit(gps.valid_fields, GPS_VALID_VEL_ECEF_BIT); gps.course = course; + SetBit(gps.valid_fields, GPS_VALID_COURSE_BIT); gps.num_sv = numsv; gps.tow = tow; gps.fix = GPS_FIX_3D; gps_available = TRUE; -#if GPS_USE_LATLONG - // Computes from (lat, long) in the referenced UTM zone - struct LlaCoor_f lla_f; - LLA_FLOAT_OF_BFP(lla_f, gps.lla_pos); - struct UtmCoor_f utm_f; - utm_f.zone = nav_utm_zone0; - // convert to utm - utm_of_lla_f(&utm_f, &lla_f); - // copy results of utm conversion - gps.utm_pos.east = utm_f.east * 100; - gps.utm_pos.north = utm_f.north * 100; - gps.utm_pos.alt = gps.lla_pos.alt; - gps.utm_pos.zone = nav_utm_zone0; -#endif - // publish new GPS data uint32_t now_ts = get_sys_time_usec(); gps.last_msg_ticks = sys_time.nb_sec_rem; diff --git a/sw/airborne/subsystems/gps/gps_mtk.c b/sw/airborne/subsystems/gps/gps_mtk.c index 48f0f8a351..1ff03545d6 100644 --- a/sw/airborne/subsystems/gps/gps_mtk.c +++ b/sw/airborne/subsystems/gps/gps_mtk.c @@ -35,10 +35,6 @@ #include "subsystems/abi.h" #include "led.h" -/* currently needed to get nav_utm_zone0 */ -#include "subsystems/navigation/common_nav.h" -#include "math/pprz_geodetic_float.h" - #include "mcu_periph/sys_time.h" #define MTK_DIY_OUTPUT_RATE MTK_DIY_OUTPUT_4HZ @@ -196,18 +192,21 @@ void gps_mtk_read_message(void) gps_time_sync.t0_tow_frac = 0; gps.lla_pos.lat = MTK_DIY14_NAV_LAT(gps_mtk.msg_buf) * 10; gps.lla_pos.lon = MTK_DIY14_NAV_LON(gps_mtk.msg_buf) * 10; + SetBit(gps.valid_fields, GPS_VALID_POS_LLA_BIT); // FIXME: with MTK you do not receive vertical speed if (sys_time.nb_sec - gps.last_3dfix_time < 2) { gps.ned_vel.z = ((gps.hmsl - MTK_DIY14_NAV_HEIGHT(gps_mtk.msg_buf) * 10) * OUTPUT_RATE) / 10; } else { gps.ned_vel.z = 0; } gps.hmsl = MTK_DIY14_NAV_HEIGHT(gps_mtk.msg_buf) * 10; + SetBit(gps.valid_fields, GPS_VALID_HMSL_BIT); // FIXME: with MTK you do not receive ellipsoid altitude gps.lla_pos.alt = gps.hmsl; gps.gspeed = MTK_DIY14_NAV_GSpeed(gps_mtk.msg_buf); // FIXME: with MTK you do not receive speed 3D gps.speed_3d = gps.gspeed; gps.course = (RadOfDeg(MTK_DIY14_NAV_Heading(gps_mtk.msg_buf))) * 10; + SetBit(gps.valid_fields, GPS_VALID_COURSE_BIT); gps.num_sv = MTK_DIY14_NAV_numSV(gps_mtk.msg_buf); switch (MTK_DIY14_NAV_GPSfix(gps_mtk.msg_buf)) { case MTK_DIY_FIX_3D: @@ -222,18 +221,6 @@ void gps_mtk_read_message(void) gps.tow = MTK_DIY14_NAV_ITOW(gps_mtk.msg_buf);; // FIXME: with MTK DIY 1.4 you do not receive GPS week gps.week = 0; - /* Computes from (lat, long) in the referenced UTM zone */ - struct LlaCoor_f lla_f; - LLA_FLOAT_OF_BFP(lla_f, gps.lla_pos); - struct UtmCoor_f utm_f; - utm_f.zone = nav_utm_zone0; - /* convert to utm */ - utm_of_lla_f(&utm_f, &lla_f); - /* copy results of utm conversion */ - gps.utm_pos.east = utm_f.east * 100; - gps.utm_pos.north = utm_f.north * 100; - gps.utm_pos.alt = gps.lla_pos.alt; - gps.utm_pos.zone = nav_utm_zone0; #ifdef GPS_LED if (gps.fix == GPS_FIX_3D) { LED_ON(GPS_LED); @@ -264,12 +251,14 @@ void gps_mtk_read_message(void) MTK_DIY16_NAV_HEIGHT(gps_mtk.msg_buf) * 10) * OUTPUT_RATE) / 10; } else { gps.ned_vel.z = 0; } gps.hmsl = MTK_DIY16_NAV_HEIGHT(gps_mtk.msg_buf) * 10; + SetBit(gps.valid_fields, GPS_VALID_HMSL_BIT); // FIXME: with MTK you do not receive ellipsoid altitude gps.lla_pos.alt = gps.hmsl; gps.gspeed = MTK_DIY16_NAV_GSpeed(gps_mtk.msg_buf); // FIXME: with MTK you do not receive speed 3D gps.speed_3d = gps.gspeed; gps.course = (RadOfDeg(MTK_DIY16_NAV_Heading(gps_mtk.msg_buf) * 10000)) * 10; + SetBit(gps.valid_fields, GPS_VALID_COURSE_BIT); gps.num_sv = MTK_DIY16_NAV_numSV(gps_mtk.msg_buf); switch (MTK_DIY16_NAV_GPSfix(gps_mtk.msg_buf)) { case MTK_DIY_FIX_3D: @@ -282,18 +271,6 @@ void gps_mtk_read_message(void) gps.fix = GPS_FIX_NONE; } /* HDOP? */ - /* Computes from (lat, long) in the referenced UTM zone */ - struct LlaCoor_f lla_f; - LLA_FLOAT_OF_BFP(lla_f, gps.lla_pos); - struct UtmCoor_f utm_f; - utm_f.zone = nav_utm_zone0; - /* convert to utm */ - utm_of_lla_f(&utm_f, &lla_f); - /* copy results of utm conversion */ - gps.utm_pos.east = utm_f.east * 100; - gps.utm_pos.north = utm_f.north * 100; - gps.utm_pos.alt = gps.lla_pos.alt; - gps.utm_pos.zone = nav_utm_zone0; #ifdef GPS_LED if (gps.fix == GPS_FIX_3D) { LED_ON(GPS_LED); diff --git a/sw/airborne/subsystems/gps/gps_nmea.c b/sw/airborne/subsystems/gps/gps_nmea.c index 664284e9c0..a9c7170de2 100644 --- a/sw/airborne/subsystems/gps/gps_nmea.c +++ b/sw/airborne/subsystems/gps/gps_nmea.c @@ -34,10 +34,6 @@ #include "subsystems/abi.h" #include "led.h" -#if GPS_USE_LATLONG -/* currently needed to get nav_utm_zone0 */ -#include "subsystems/navigation/common_nav.h" -#endif #include "math/pprz_geodetic_float.h" #include @@ -342,6 +338,7 @@ static void nmea_parse_RMC(void) double course = strtod(&gps_nmea.msg_buf[i], NULL); gps.course = RadOfDeg(course) * 1e7; NMEA_PRINT("p_RMC() - course: %f deg\n\r", course); + SetBit(gps.valid_fields, GPS_VALID_COURSE_BIT); } @@ -404,6 +401,7 @@ static void nmea_parse_GGA(void) lla_f.lon = RadOfDeg(lon); gps.lla_pos.lon = lon * 1e7; // convert to fixed-point NMEA_PRINT("p_GGA() - lon=%f gps_lon=%f time=%u\n\r", (lon * 1000), lla_f.lon, gps.tow); + SetBit(gps.valid_fields, GPS_VALID_POS_LLA_BIT); // get position fix status nmea_read_until(&i); @@ -432,6 +430,7 @@ static void nmea_parse_GGA(void) float hmsl = strtof(&gps_nmea.msg_buf[i], NULL); gps.hmsl = hmsl * 1000; NMEA_PRINT("p_GGA() - gps.hmsl=%i\n\r", gps.hmsl); + SetBit(gps.valid_fields, GPS_VALID_HMSL_BIT); // get altitude units (always M) nmea_read_until(&i); @@ -451,25 +450,13 @@ static void nmea_parse_GGA(void) nmea_read_until(&i); // get DGPS station ID -#if GPS_USE_LATLONG - /* convert to utm */ - struct UtmCoor_f utm_f; - utm_f.zone = nav_utm_zone0; - utm_of_lla_f(&utm_f, &lla_f); - - /* copy results of utm conversion */ - gps.utm_pos.east = utm_f.east * 100; - gps.utm_pos.north = utm_f.north * 100; - gps.utm_pos.alt = gps.lla_pos.alt; - gps.utm_pos.zone = nav_utm_zone0; -#endif - /* convert to ECEF */ struct EcefCoor_f ecef_f; ecef_of_lla_f(&ecef_f, &lla_f); gps.ecef_pos.x = ecef_f.x * 100; gps.ecef_pos.y = ecef_f.y * 100; gps.ecef_pos.z = ecef_f.z * 100; + SetBit(gps.valid_fields, GPS_VALID_POS_ECEF_BIT); } /** diff --git a/sw/airborne/subsystems/gps/gps_piksi.c b/sw/airborne/subsystems/gps/gps_piksi.c index 3067be0785..140a5240cd 100644 --- a/sw/airborne/subsystems/gps/gps_piksi.c +++ b/sw/airborne/subsystems/gps/gps_piksi.c @@ -35,11 +35,9 @@ #include "subsystems/abi.h" #include "mcu_periph/uart.h" #include "math/pprz_geodetic_double.h" -#if GPS_USE_LATLONG -#include "math/pprz_geodetic_float.h" -#include "subsystems/navigation/common_nav.h" + +// get NAV_MSL0 for geoid separation #include "generated/flight_plan.h" -#endif #include #include @@ -124,10 +122,11 @@ static void sbp_pos_ecef_callback(uint16_t sender_id __attribute__((unused)), && pos_ecef.flags == SBP_FIX_MODE_SPP) { return; } - + gps.ecef_pos.x = (int32_t)(pos_ecef.x * 100.0); gps.ecef_pos.y = (int32_t)(pos_ecef.y * 100.0); gps.ecef_pos.z = (int32_t)(pos_ecef.z * 100.0); + SetBit(gps.valid_fields, GPS_VALID_POS_ECEF_BIT); gps.pacc = (uint32_t)(pos_ecef.accuracy);// FIXME not implemented yet by libswiftnav gps.num_sv = pos_ecef.n_sats; gps.tow = pos_ecef.tow; @@ -143,6 +142,7 @@ static void sbp_vel_ecef_callback(uint16_t sender_id __attribute__((unused)), gps.ecef_vel.x = (int32_t)(vel_ecef.x / 10); gps.ecef_vel.y = (int32_t)(vel_ecef.y / 10); gps.ecef_vel.z = (int32_t)(vel_ecef.z / 10); + SetBit(gps.valid_fields, GPS_VALID_VEL_ECEF_BIT); gps.sacc = (uint32_t)(vel_ecef.accuracy); // Solution available (VEL_ECEF is the last message to be send) @@ -158,23 +158,11 @@ static void sbp_pos_llh_callback(uint16_t sender_id __attribute__((unused)), msg_pos_llh_t pos_llh = *(msg_pos_llh_t *)msg; // Check if we got RTK fix (FIXME when libsbp has a nicer way of doing this) - if(pos_llh.flags > 0 || last_flags == 0) { + if (pos_llh.flags > 0 || last_flags == 0) { gps.lla_pos.lat = (int32_t)(pos_llh.lat * 1e7); gps.lla_pos.lon = (int32_t)(pos_llh.lon * 1e7); + int32_t alt = (int32_t)(pos_llh.height * 1000.); -#if GPS_USE_LATLONG - /* Computes from (lat, long) in the referenced UTM zone */ - struct LlaCoor_f lla_f; - LLA_FLOAT_OF_BFP(lla_f, gps.lla_pos); - struct UtmCoor_f utm_f; - utm_f.zone = nav_utm_zone0; - /* convert to utm */ - utm_of_lla_f(&utm_f, &lla_f); - /* copy results of utm conversion */ - gps.utm_pos.east = utm_f.east * 100; - gps.utm_pos.north = utm_f.north * 100; - gps.utm_pos.alt = gps.lla_pos.alt; - gps.utm_pos.zone = nav_utm_zone0; // height is above ellipsoid or MSL according to bit flag (but not both are available) // 0: above ellipsoid // 1: above MSL @@ -186,11 +174,9 @@ static void sbp_pos_llh_callback(uint16_t sender_id __attribute__((unused)), gps.lla_pos.alt = alt; gps.hmsl = alt - NAV_MSL0; } -#else - // but here we fill the two alt with the same value since we don't know HMSL - gps.lla_pos.alt = alt; - gps.hmsl = alt; -#endif + + SetBit(gps.valid_fields, GPS_VALID_POS_LLA_BIT); + SetBit(gps.valid_fields, GPS_VALID_HMSL_BIT); } last_flags = pos_llh.flags; } @@ -204,10 +190,11 @@ static void sbp_vel_ned_callback(uint16_t sender_id __attribute__((unused)), gps.ned_vel.x = (int32_t)(vel_ned.n / 10); gps.ned_vel.y = (int32_t)(vel_ned.e / 10); gps.ned_vel.z = (int32_t)(vel_ned.d / 10); -#if GPS_USE_LATLONG + SetBit(gps.valid_fields, GPS_VALID_VEL_NED_BIT); + gps.gspeed = int32_sqrt(gps.ned_vel.x * gps.ned_vel.x + gps.ned_vel.y * gps.ned_vel.y); gps.course = (int32_t)(1e7 * atan2(gps.ned_vel.y, gps.ned_vel.x)); -#endif + SetBit(gps.valid_fields, GPS_VALID_COURSE_BIT); } static void sbp_dops_callback(uint16_t sender_id __attribute__((unused)), @@ -283,7 +270,7 @@ static void sbp_tracking_state_dep_a_callback(uint16_t sender_id __attribute__(( } else { return GPS_FIX_NONE; } - + } /* @@ -323,7 +310,7 @@ void gps_impl_init(void) * Event handler for reading the GPS UART bytes */ void gps_piksi_event(void) -{ +{ if ( get_sys_time_msec() - time_since_last_pos_update > POS_ECEF_TIMEOUT ) { gps.fix = GPS_FIX_NONE; } @@ -384,4 +371,4 @@ uint32_t gps_piksi_write(uint8_t *buff, uint32_t n, void *context __attribute__( void gps_inject_data(uint8_t packet_id, uint8_t length, uint8_t *data) { sbp_send_message(&sbp_state, packet_id, SBP_SENDER_ID, length, data, gps_piksi_write); -} \ No newline at end of file +} diff --git a/sw/airborne/subsystems/gps/gps_sim_nps.c b/sw/airborne/subsystems/gps/gps_sim_nps.c index 1865d93d76..852161c619 100644 --- a/sw/airborne/subsystems/gps/gps_sim_nps.c +++ b/sw/airborne/subsystems/gps/gps_sim_nps.c @@ -25,12 +25,6 @@ #include "nps_sensors.h" #include "nps_fdm.h" -#if GPS_USE_LATLONG -/* currently needed to get nav_utm_zone0 */ -#include "subsystems/navigation/common_nav.h" -#include "math/pprz_geodetic_float.h" -#endif - bool_t gps_has_fix; void gps_feed_value() @@ -42,14 +36,20 @@ void gps_feed_value() gps.ecef_pos.x = sensors.gps.ecef_pos.x * 100.; gps.ecef_pos.y = sensors.gps.ecef_pos.y * 100.; gps.ecef_pos.z = sensors.gps.ecef_pos.z * 100.; + SetBit(gps.valid_fields, GPS_VALID_POS_ECEF_BIT); gps.ecef_vel.x = sensors.gps.ecef_vel.x * 100.; gps.ecef_vel.y = sensors.gps.ecef_vel.y * 100.; gps.ecef_vel.z = sensors.gps.ecef_vel.z * 100.; + SetBit(gps.valid_fields, GPS_VALID_VEL_ECEF_BIT); + //ecef pos seems to be based on geocentric model, hence we get a very high alt when converted to lla gps.lla_pos.lat = DegOfRad(sensors.gps.lla_pos.lat) * 1e7; gps.lla_pos.lon = DegOfRad(sensors.gps.lla_pos.lon) * 1e7; gps.lla_pos.alt = sensors.gps.lla_pos.alt * 1000.; + SetBit(gps.valid_fields, GPS_VALID_POS_LLA_BIT); + gps.hmsl = sensors.gps.hmsl * 1000.; + SetBit(gps.valid_fields, GPS_VALID_HMSL_BIT); /* calc NED speed from ECEF */ struct LtpDef_d ref_ltp; @@ -59,6 +59,7 @@ void gps_feed_value() gps.ned_vel.x = ned_vel_d.x * 100; gps.ned_vel.y = ned_vel_d.y * 100; gps.ned_vel.z = ned_vel_d.z * 100; + SetBit(gps.valid_fields, GPS_VALID_VEL_NED_BIT); /* horizontal and 3d ground speed in cm/s */ gps.gspeed = sqrt(ned_vel_d.x * ned_vel_d.x + ned_vel_d.y * ned_vel_d.y) * 100; @@ -66,21 +67,7 @@ void gps_feed_value() /* ground course in radians * 1e7 */ gps.course = atan2(ned_vel_d.y, ned_vel_d.x) * 1e7; - -#if GPS_USE_LATLONG - /* Computes from (lat, long) in the referenced UTM zone */ - struct LlaCoor_f lla_f; - LLA_FLOAT_OF_BFP(lla_f, gps.lla_pos); - struct UtmCoor_f utm_f; - utm_f.zone = nav_utm_zone0; - /* convert to utm */ - utm_of_lla_f(&utm_f, &lla_f); - /* copy results of utm conversion */ - gps.utm_pos.east = utm_f.east * 100; - gps.utm_pos.north = utm_f.north * 100; - gps.utm_pos.alt = gps.lla_pos.alt; - gps.utm_pos.zone = nav_utm_zone0; -#endif + SetBit(gps.valid_fields, GPS_VALID_COURSE_BIT); if (gps_has_fix) { gps.fix = GPS_FIX_3D; diff --git a/sw/airborne/subsystems/gps/gps_sirf.c b/sw/airborne/subsystems/gps/gps_sirf.c index 7a4e7aad62..e6c3950fc4 100644 --- a/sw/airborne/subsystems/gps/gps_sirf.c +++ b/sw/airborne/subsystems/gps/gps_sirf.c @@ -25,10 +25,6 @@ #include "subsystems/abi.h" #include "led.h" -#if GPS_USE_LATLONG -/* currently needed to get nav_utm_zone0 */ -#include "subsystems/navigation/common_nav.h" -#endif #include "math/pprz_geodetic_float.h" #include @@ -121,6 +117,7 @@ void sirf_parse_41(void) gps.tow = Invert4Bytes(p->tow); gps.hmsl = Invert4Bytes(p->alt_msl) * 10; + SetBit(gps.valid_fields, GPS_VALID_HMSL_BIT); gps.num_sv = p->num_sat; gps.nb_channels = p ->num_sat; @@ -128,24 +125,11 @@ void sirf_parse_41(void) gps.lla_pos.lat = Invert4Bytes(p->latitude); gps.lla_pos.lon = Invert4Bytes(p->longitude); gps.lla_pos.alt = Invert4Bytes(p->alt_ellipsoid) * 10; - -#if GPS_USE_LATLONG - /* convert to utm */ - struct LlaCoor_f lla_f; - LLA_FLOAT_OF_BFP(lla_f, gps.lla_pos); - struct UtmCoor_f utm_f; - utm_f.zone = nav_utm_zone0; - utm_of_lla_f(&utm_f, &lla_f); - - /* copy results of utm conversion */ - gps.utm_pos.east = utm_f.east * 100; - gps.utm_pos.north = utm_f.north * 100; - gps.utm_pos.alt = gps.lla_pos.alt; - gps.utm_pos.zone = nav_utm_zone0; -#endif + SetBit(gps.valid_fields, GPS_VALID_POS_LLA_BIT); gps.sacc = (Invert2Bytes(p->ehve) >> 16); gps.course = RadOfDeg(Invert2Bytes(p->cog)) * pow(10, 5); + SetBit(gps.valid_fields, GPS_VALID_COURSE_BIT); gps.gspeed = RadOfDeg(Invert2Bytes(p->sog)) * pow(10, 5); gps.cacc = RadOfDeg(Invert2Bytes(p->heading_err)) * pow(10, 5); gps.pacc = Invert4Bytes(p->ehpe); @@ -173,10 +157,12 @@ void sirf_parse_2(void) gps.ecef_pos.x = Invert4Bytes(p->x_pos) * 100; gps.ecef_pos.y = Invert4Bytes(p->y_pos) * 100; gps.ecef_pos.z = Invert4Bytes(p->z_pos) * 100; + SetBit(gps.valid_fields, GPS_VALID_POS_ECEF_BIT); gps.ecef_vel.x = (Invert2Bytes(p->vx) >> 16) * 100 / 8; gps.ecef_vel.y = (Invert2Bytes(p->vy) >> 16) * 100 / 8; gps.ecef_vel.z = (Invert2Bytes(p->vz) >> 16) * 100 / 8; + SetBit(gps.valid_fields, GPS_VALID_VEL_ECEF_BIT); if (gps.fix == GPS_FIX_3D) { ticks++; diff --git a/sw/airborne/subsystems/gps/gps_skytraq.c b/sw/airborne/subsystems/gps/gps_skytraq.c index 276bdfd17d..c450886421 100644 --- a/sw/airborne/subsystems/gps/gps_skytraq.c +++ b/sw/airborne/subsystems/gps/gps_skytraq.c @@ -23,12 +23,6 @@ #include "subsystems/abi.h" #include "led.h" -#if GPS_USE_LATLONG -/* currently needed to get nav_utm_zone0 */ -#include "subsystems/navigation/common_nav.h" -#include "math/pprz_geodetic_float.h" -#endif - struct GpsSkytraq gps_skytraq; /* parser status */ @@ -123,13 +117,21 @@ void gps_skytraq_read_message(void) gps.ecef_pos.x = SKYTRAQ_NAVIGATION_DATA_ECEFX(gps_skytraq.msg_buf); gps.ecef_pos.y = SKYTRAQ_NAVIGATION_DATA_ECEFY(gps_skytraq.msg_buf); gps.ecef_pos.z = SKYTRAQ_NAVIGATION_DATA_ECEFZ(gps_skytraq.msg_buf); + SetBit(gps.valid_fields, GPS_VALID_POS_ECEF_BIT); + gps.ecef_vel.x = SKYTRAQ_NAVIGATION_DATA_ECEFVX(gps_skytraq.msg_buf); gps.ecef_vel.y = SKYTRAQ_NAVIGATION_DATA_ECEFVY(gps_skytraq.msg_buf); gps.ecef_vel.z = SKYTRAQ_NAVIGATION_DATA_ECEFVZ(gps_skytraq.msg_buf); + SetBit(gps.valid_fields, GPS_VALID_VEL_ECEF_BIT); + gps.lla_pos.lat = SKYTRAQ_NAVIGATION_DATA_LAT(gps_skytraq.msg_buf); gps.lla_pos.lon = SKYTRAQ_NAVIGATION_DATA_LON(gps_skytraq.msg_buf); gps.lla_pos.alt = SKYTRAQ_NAVIGATION_DATA_AEL(gps_skytraq.msg_buf) * 10; + SetBit(gps.valid_fields, GPS_VALID_POS_LLA_BIT); + gps.hmsl = SKYTRAQ_NAVIGATION_DATA_ASL(gps_skytraq.msg_buf) * 10; + SetBit(gps.valid_fields, GPS_VALID_HMSL_BIT); + // pacc; // sacc; gps.pdop = SKYTRAQ_NAVIGATION_DATA_PDOP(gps_skytraq.msg_buf); @@ -148,21 +150,6 @@ void gps_skytraq_read_message(void) gps.fix = GPS_FIX_NONE; } -#if GPS_USE_LATLONG - /* Computes from (lat, long) in the referenced UTM zone */ - struct LlaCoor_f lla_f; - LLA_FLOAT_OF_BFP(lla_f, gps.lla_pos); - struct UtmCoor_f utm_f; - utm_f.zone = nav_utm_zone0; - /* convert to utm */ - utm_of_lla_f(&utm_f, &lla_f); - /* copy results of utm conversion */ - gps.utm_pos.east = utm_f.east * 100; - gps.utm_pos.north = utm_f.north * 100; - gps.utm_pos.alt = gps.lla_pos.alt; - gps.utm_pos.zone = nav_utm_zone0; -#endif - if (gps.fix == GPS_FIX_3D) { if (distance_too_great(&gps_skytraq.ref_ltp.ecef, &gps.ecef_pos)) { // just grab current ecef_pos as reference. @@ -170,9 +157,11 @@ void gps_skytraq_read_message(void) } // convert ecef velocity vector to NED vector. ned_of_ecef_vect_i(&gps.ned_vel, &gps_skytraq.ref_ltp, &gps.ecef_vel); + SetBit(gps.valid_fields, GPS_VALID_VEL_NED_BIT); // ground course in radians gps.course = (atan2f((float)gps.ned_vel.y, (float)gps.ned_vel.x)) * 1e7; + SetBit(gps.valid_fields, GPS_VALID_COURSE_BIT); // GT: gps.cacc = ... ? what should course accuracy be? // ground speed diff --git a/sw/airborne/subsystems/gps/gps_ubx.c b/sw/airborne/subsystems/gps/gps_ubx.c index f173c4c636..60de79c337 100644 --- a/sw/airborne/subsystems/gps/gps_ubx.c +++ b/sw/airborne/subsystems/gps/gps_ubx.c @@ -24,12 +24,6 @@ #include "subsystems/abi.h" #include "led.h" -#if GPS_USE_LATLONG -/* currently needed to get nav_utm_zone0 */ -#include "subsystems/navigation/common_nav.h" -#include "math/pprz_geodetic_float.h" -#endif - /** Includes macros generated from ubx.xml */ #include "ubx_protocol.h" @@ -67,7 +61,6 @@ void gps_impl_init(void) gps_ubx.msg_available = FALSE; gps_ubx.error_cnt = 0; gps_ubx.error_last = GPS_UBX_ERR_NONE; - gps_ubx.have_velned = 0; } @@ -86,10 +79,12 @@ void gps_ubx_read_message(void) gps.ecef_pos.x = UBX_NAV_SOL_ECEF_X(gps_ubx.msg_buf); gps.ecef_pos.y = UBX_NAV_SOL_ECEF_Y(gps_ubx.msg_buf); gps.ecef_pos.z = UBX_NAV_SOL_ECEF_Z(gps_ubx.msg_buf); + SetBit(gps.valid_fields, GPS_VALID_POS_ECEF_BIT); gps.pacc = UBX_NAV_SOL_Pacc(gps_ubx.msg_buf); gps.ecef_vel.x = UBX_NAV_SOL_ECEFVX(gps_ubx.msg_buf); gps.ecef_vel.y = UBX_NAV_SOL_ECEFVY(gps_ubx.msg_buf); gps.ecef_vel.z = UBX_NAV_SOL_ECEFVZ(gps_ubx.msg_buf); + SetBit(gps.valid_fields, GPS_VALID_VEL_ECEF_BIT); gps.sacc = UBX_NAV_SOL_Sacc(gps_ubx.msg_buf); gps.pdop = UBX_NAV_SOL_PDOP(gps_ubx.msg_buf); gps.num_sv = UBX_NAV_SOL_numSV(gps_ubx.msg_buf); @@ -104,21 +99,9 @@ void gps_ubx_read_message(void) gps.lla_pos.lat = UBX_NAV_POSLLH_LAT(gps_ubx.msg_buf); gps.lla_pos.lon = UBX_NAV_POSLLH_LON(gps_ubx.msg_buf); gps.lla_pos.alt = UBX_NAV_POSLLH_HEIGHT(gps_ubx.msg_buf); + SetBit(gps.valid_fields, GPS_VALID_POS_LLA_BIT); gps.hmsl = UBX_NAV_POSLLH_HMSL(gps_ubx.msg_buf); -#if GPS_USE_LATLONG - /* Computes from (lat, long) in the referenced UTM zone */ - struct LlaCoor_f lla_f; - LLA_FLOAT_OF_BFP(lla_f, gps.lla_pos); - struct UtmCoor_f utm_f; - utm_f.zone = nav_utm_zone0; - /* convert to utm */ - utm_of_lla_f(&utm_f, &lla_f); - /* copy results of utm conversion */ - gps.utm_pos.east = utm_f.east * 100; - gps.utm_pos.north = utm_f.north * 100; - gps.utm_pos.alt = gps.lla_pos.alt; - gps.utm_pos.zone = nav_utm_zone0; -#else + SetBit(gps.valid_fields, GPS_VALID_HMSL_BIT); } else if (gps_ubx.msg_id == UBX_NAV_POSUTM_ID) { gps.utm_pos.east = UBX_NAV_POSUTM_EAST(gps_ubx.msg_buf); gps.utm_pos.north = UBX_NAV_POSUTM_NORTH(gps_ubx.msg_buf); @@ -127,24 +110,30 @@ void gps_ubx_read_message(void) gps.utm_pos.north -= 1000000000; /* Subtract false northing: -10000km */ } gps.utm_pos.alt = UBX_NAV_POSUTM_ALT(gps_ubx.msg_buf) * 10; - gps.hmsl = gps.utm_pos.alt; - gps.lla_pos.alt = gps.utm_pos.alt; // FIXME: with UTM only you do not receive ellipsoid altitude gps.utm_pos.zone = UBX_NAV_POSUTM_ZONE(gps_ubx.msg_buf); -#endif + SetBit(gps.valid_fields, GPS_VALID_POS_UTM_BIT); + + gps.hmsl = gps.utm_pos.alt; + SetBit(gps.valid_fields, GPS_VALID_HMSL_BIT); + /* with UTM only you do not receive ellipsoid altitude, so set only if no valid lla */ + if (!bit_is_set(gps.valid_fields, GPS_VALID_HMSL_BIT)) { + gps.lla_pos.alt = gps.utm_pos.alt; + } } else if (gps_ubx.msg_id == UBX_NAV_VELNED_ID) { gps.speed_3d = UBX_NAV_VELNED_Speed(gps_ubx.msg_buf); gps.gspeed = UBX_NAV_VELNED_GSpeed(gps_ubx.msg_buf); gps.ned_vel.x = UBX_NAV_VELNED_VEL_N(gps_ubx.msg_buf); gps.ned_vel.y = UBX_NAV_VELNED_VEL_E(gps_ubx.msg_buf); gps.ned_vel.z = UBX_NAV_VELNED_VEL_D(gps_ubx.msg_buf); + SetBit(gps.valid_fields, GPS_VALID_VEL_NED_BIT); // Ublox gives I4 heading in 1e-5 degrees, apparenty from 0 to 360 degrees (not -180 to 180) // I4 max = 2^31 = 214 * 1e5 * 100 < 360 * 1e7: overflow on angles over 214 deg -> casted to -214 deg // solution: First to radians, and then scale to 1e-7 radians // First x 10 for loosing less resolution, then to radians, then multiply x 10 again gps.course = (RadOfDeg(UBX_NAV_VELNED_Heading(gps_ubx.msg_buf) * 10)) * 10; + SetBit(gps.valid_fields, GPS_VALID_COURSE_BIT); gps.cacc = (RadOfDeg(UBX_NAV_VELNED_CAcc(gps_ubx.msg_buf) * 10)) * 10; gps.tow = UBX_NAV_VELNED_ITOW(gps_ubx.msg_buf); - gps_ubx.have_velned = 1; } else if (gps_ubx.msg_id == UBX_NAV_SVINFO_ID) { gps.nb_channels = Min(UBX_NAV_SVINFO_NCH(gps_ubx.msg_buf), GPS_NB_CHANNELS); uint8_t i; @@ -332,7 +321,7 @@ void gps_ubx_msg(void) if (gps_ubx.msg_class == UBX_NAV_ID && (gps_ubx.msg_id == UBX_NAV_VELNED_ID || (gps_ubx.msg_id == UBX_NAV_SOL_ID && - gps_ubx.have_velned == 0))) { + !bit_is_set(gps.valid_fields, GPS_VALID_VEL_NED_BIT)))) { if (gps.fix == GPS_FIX_3D) { gps.last_3dfix_ticks = sys_time.nb_sec_rem; gps.last_3dfix_time = sys_time.nb_sec; diff --git a/sw/airborne/subsystems/gps/gps_ubx.h b/sw/airborne/subsystems/gps/gps_ubx.h index b52e4954dc..8be5296f00 100644 --- a/sw/airborne/subsystems/gps/gps_ubx.h +++ b/sw/airborne/subsystems/gps/gps_ubx.h @@ -52,8 +52,6 @@ struct GpsUbx { uint8_t status_flags; uint8_t sol_flags; - uint8_t have_velned; - }; extern struct GpsUbx gps_ubx; diff --git a/sw/airborne/subsystems/gps/gps_udp.c b/sw/airborne/subsystems/gps/gps_udp.c index c312c6b391..7c54d3e0af 100644 --- a/sw/airborne/subsystems/gps/gps_udp.c +++ b/sw/airborne/subsystems/gps/gps_udp.c @@ -26,11 +26,6 @@ #include #include -#if GPS_USE_LATLONG -#include "subsystems/navigation/common_nav.h" -#include "math/pprz_geodetic_float.h" -#endif - //Check if variables are set and else define them #ifndef GPS_UDP_HOST #define GPS_UDP_HOST 192.168.1.2 @@ -65,33 +60,23 @@ void gps_parse(void) gps.lla_pos.lat = UDP_GPS_INT(gps_udp_read_buffer + 4); gps.lla_pos.lon = UDP_GPS_INT(gps_udp_read_buffer + 8); gps.lla_pos.alt = UDP_GPS_INT(gps_udp_read_buffer + 12); + SetBit(gps.valid_fields, GPS_VALID_POS_LLA_BIT); + gps.hmsl = UDP_GPS_INT(gps_udp_read_buffer + 16); + SetBit(gps.valid_fields, GPS_VALID_HMSL_BIT); gps.ecef_pos.x = UDP_GPS_INT(gps_udp_read_buffer + 20); gps.ecef_pos.y = UDP_GPS_INT(gps_udp_read_buffer + 24); gps.ecef_pos.z = UDP_GPS_INT(gps_udp_read_buffer + 28); + SetBit(gps.valid_fields, GPS_VALID_POS_ECEF_BIT); gps.ecef_vel.x = UDP_GPS_INT(gps_udp_read_buffer + 32); gps.ecef_vel.y = UDP_GPS_INT(gps_udp_read_buffer + 36); gps.ecef_vel.z = UDP_GPS_INT(gps_udp_read_buffer + 40); + SetBit(gps.valid_fields, GPS_VALID_VEL_ECEF_BIT); gps.fix = GPS_FIX_3D; -#if GPS_USE_LATLONG - // Computes from (lat, long) in the referenced UTM zone - struct LlaCoor_f lla_f; - LLA_FLOAT_OF_BFP(lla_f, gps.lla_pos); - struct UtmCoor_f utm_f; - utm_f.zone = nav_utm_zone0; - // convert to utm - utm_of_lla_f(&utm_f, &lla_f); - // copy results of utm conversion - gps.utm_pos.east = utm_f.east * 100; - gps.utm_pos.north = utm_f.north * 100; - gps.utm_pos.alt = gps.lla_pos.alt; - gps.utm_pos.zone = nav_utm_zone0; -#endif - // publish new GPS data uint32_t now_ts = get_sys_time_usec(); gps.last_msg_ticks = sys_time.nb_sec_rem; diff --git a/sw/airborne/subsystems/ins.c b/sw/airborne/subsystems/ins.c index 50e97d4d4d..556c5f4e6a 100644 --- a/sw/airborne/subsystems/ins.c +++ b/sw/airborne/subsystems/ins.c @@ -73,17 +73,20 @@ void WEAK ins_reset_local_origin(void) { #if USE_GPS struct UtmCoor_f utm; -#ifdef GPS_USE_LATLONG - /* Recompute UTM coordinates in this zone */ - struct LlaCoor_f lla; - LLA_FLOAT_OF_BFP(lla, gps.lla_pos); - utm.zone = (gps.lla_pos.lon / 1e7 + 180) / 6 + 1; - utm_of_lla_f(&utm, &lla); -#else - utm.zone = gps.utm_pos.zone; - utm.east = gps.utm_pos.east / 100.0f; - utm.north = gps.utm_pos.north / 100.0f; -#endif + + if (bit_is_set(gps.valid_fields, GPS_VALID_POS_UTM_BIT)) { + utm.zone = gps.utm_pos.zone; + utm.east = gps.utm_pos.east / 100.0f; + utm.north = gps.utm_pos.north / 100.0f; + } + else { + /* Recompute UTM coordinates in this zone */ + struct LlaCoor_f lla; + LLA_FLOAT_OF_BFP(lla, gps.lla_pos); + utm.zone = (gps.lla_pos.lon / 1e7 + 180) / 6 + 1; + utm_of_lla_f(&utm, &lla); + } + // ground_alt utm.alt = gps.hmsl / 1000.0f; @@ -99,11 +102,12 @@ void WEAK ins_reset_utm_zone(struct UtmCoor_f *utm) { struct LlaCoor_f lla0; lla_of_utm_f(&lla0, utm); -#ifdef GPS_USE_LATLONG - utm->zone = (gps.lla_pos.lon / 1e7 + 180) / 6 + 1; -#else - utm->zone = gps.utm_pos.zone; -#endif + if (bit_is_set(gps.valid_fields, GPS_VALID_POS_UTM_BIT)) { + utm->zone = gps.utm_pos.zone; + } + else { + utm->zone = (gps.lla_pos.lon / 1e7 + 180) / 6 + 1; + } utm_of_lla_f(utm, &lla0); stateSetLocalUtmOrigin_f(utm); diff --git a/sw/airborne/subsystems/ins/ins_alt_float.c b/sw/airborne/subsystems/ins/ins_alt_float.c index 346c3d4f03..9c0e71c4ef 100644 --- a/sw/airborne/subsystems/ins/ins_alt_float.c +++ b/sw/airborne/subsystems/ins/ins_alt_float.c @@ -114,17 +114,20 @@ void ins_alt_float_init(void) void ins_reset_local_origin(void) { struct UtmCoor_f utm; -#ifdef GPS_USE_LATLONG - /* Recompute UTM coordinates in this zone */ - struct LlaCoor_f lla; - LLA_FLOAT_OF_BFP(lla, gps.lla_pos); - utm.zone = (gps.lla_pos.lon / 1e7 + 180) / 6 + 1; - utm_of_lla_f(&utm, &lla); -#else - utm.zone = gps.utm_pos.zone; - utm.east = gps.utm_pos.east / 100.0f; - utm.north = gps.utm_pos.north / 100.0f; -#endif + + if (bit_is_set(gps.valid_fields, GPS_VALID_POS_UTM_BIT)) { + utm.zone = gps.utm_pos.zone; + utm.east = gps.utm_pos.east / 100.0f; + utm.north = gps.utm_pos.north / 100.0f; + } + else { + /* Recompute UTM coordinates in this zone */ + struct LlaCoor_f lla; + LLA_FLOAT_OF_BFP(lla, gps.lla_pos); + utm.zone = (gps.lla_pos.lon / 1e7 + 180) / 6 + 1; + utm_of_lla_f(&utm, &lla); + } + // ground_alt utm.alt = gps.hmsl / 1000.0f; diff --git a/sw/airborne/subsystems/ins/ins_float_invariant.c b/sw/airborne/subsystems/ins/ins_float_invariant.c index b2c6e84a7d..ea12c2e759 100644 --- a/sw/airborne/subsystems/ins/ins_float_invariant.c +++ b/sw/airborne/subsystems/ins/ins_float_invariant.c @@ -274,19 +274,21 @@ void ins_reset_local_origin(void) { #if INS_FINV_USE_UTM struct UtmCoor_f utm; -#ifdef GPS_USE_LATLONG - /* Recompute UTM coordinates in this zone */ - struct LlaCoor_f lla; - LLA_FLOAT_OF_BFP(lla, gps.lla_pos); - utm.zone = (gps.lla_pos.lon / 1e7 + 180) / 6 + 1; - utm_of_lla_f(&utm, &lla); -#else - utm.zone = gps.utm_pos.zone; - utm.east = gps.utm_pos.east / 100.0f; - utm.north = gps.utm_pos.north / 100.0f; -#endif + if (bit_is_set(gps.valid_fields, GPS_VALID_POS_UTM_BIT)) { + utm.zone = gps.utm_pos.zone; + utm.east = gps.utm_pos.east / 100.0f; + utm.north = gps.utm_pos.north / 100.0f; + } + else { + /* Recompute UTM coordinates in this zone */ + struct LlaCoor_f lla; + LLA_FLOAT_OF_BFP(lla, gps.lla_pos); + struct UtmCoor_f utm; + utm.zone = (gps.lla_pos.lon / 1e7 + 180) / 6 + 1; + utm_of_lla_f(&utm, &lla); + } // ground_alt - utm.alt = gps.hmsl / 1000.0f; + utm.alt = gps.hmsl / 1000.0f; // reset state UTM ref stateSetLocalUtmOrigin_f(&utm); #else diff --git a/sw/airborne/subsystems/ins/ins_gps_passthrough_utm.c b/sw/airborne/subsystems/ins/ins_gps_passthrough_utm.c index 7b640c862a..e400029898 100644 --- a/sw/airborne/subsystems/ins/ins_gps_passthrough_utm.c +++ b/sw/airborne/subsystems/ins/ins_gps_passthrough_utm.c @@ -47,17 +47,20 @@ void ins_gps_utm_init(void) void ins_reset_local_origin(void) { struct UtmCoor_f utm; -#ifdef GPS_USE_LATLONG - /* Recompute UTM coordinates in this zone */ - struct LlaCoor_f lla; - LLA_FLOAT_OF_BFP(lla, gps.lla_pos); - utm.zone = (gps.lla_pos.lon / 1e7 + 180) / 6 + 1; - utm_of_lla_f(&utm, &lla); -#else - utm.zone = gps.utm_pos.zone; - utm.east = gps.utm_pos.east / 100.0f; - utm.north = gps.utm_pos.north / 100.0f; -#endif + + if (bit_is_set(gps.valid_fields, GPS_VALID_POS_UTM_BIT)) { + utm.zone = gps.utm_pos.zone; + utm.east = gps.utm_pos.east / 100.0f; + utm.north = gps.utm_pos.north / 100.0f; + } + else { + /* Recompute UTM coordinates in this zone */ + struct LlaCoor_f lla; + LLA_FLOAT_OF_BFP(lla, gps.lla_pos); + utm.zone = (gps.lla_pos.lon / 1e7 + 180) / 6 + 1; + utm_of_lla_f(&utm, &lla); + } + // ground_alt utm.alt = gps.hmsl / 1000.0f; // reset state UTM ref diff --git a/sw/airborne/subsystems/ins/ins_vectornav.c b/sw/airborne/subsystems/ins/ins_vectornav.c index 1b2ae4d1b8..e22211e5eb 100644 --- a/sw/airborne/subsystems/ins/ins_vectornav.c +++ b/sw/airborne/subsystems/ins/ins_vectornav.c @@ -363,6 +363,7 @@ void ins_vectornav_propagate() ins_vn.lla_pos.lon = RadOfDeg((float)ins_vn.pos_lla[1]); // ins_impl.pos_lla[1] = lon ins_vn.lla_pos.alt = ((float)ins_vn.pos_lla[2]); // ins_impl.pos_lla[2] = alt LLA_BFP_OF_REAL(gps.lla_pos, ins_vn.lla_pos); + SetBit(gps.valid_fields, GPS_VALID_POS_LLA_BIT); stateSetPositionLla_i(&gps.lla_pos); // ECEF position @@ -371,27 +372,13 @@ void ins_vectornav_propagate() struct EcefCoor_f ecef_vel; ecef_of_ned_point_f(&ecef_vel, &def, &ins_vn.vel_ned); ECEF_BFP_OF_REAL(gps.ecef_vel, ecef_vel); + SetBit(gps.valid_fields, GPS_VALID_VEL_ECEF_BIT); // ECEF velocity gps.ecef_pos.x = stateGetPositionEcef_i()->x; gps.ecef_pos.y = stateGetPositionEcef_i()->y; gps.ecef_pos.z = stateGetPositionEcef_i()->z; - - -#if GPS_USE_LATLONG - // GPS UTM - /* Computes from (lat, long) in the referenced UTM zone */ - struct UtmCoor_f utm_f; - utm_f.zone = nav_utm_zone0; - /* convert to utm */ - //utm_of_lla_f(&utm_f, &lla_f); - utm_of_lla_f(&utm_f, &ins_vn.lla_pos); - /* copy results of utm conversion */ - gps.utm_pos.east = (int32_t)(utm_f.east * 100); - gps.utm_pos.north = (int32_t)(utm_f.north * 100); - gps.utm_pos.alt = (int32_t)(utm_f.alt * 1000); - gps.utm_pos.zone = (uint8_t)nav_utm_zone0; -#endif + SetBit(gps.valid_fields, GPS_VALID_POS_ECEF_BIT); // GPS Ground speed float speed = sqrt(ins_vn.vel_ned.x * ins_vn.vel_ned.x + ins_vn.vel_ned.y * ins_vn.vel_ned.y); @@ -399,6 +386,7 @@ void ins_vectornav_propagate() // GPS course gps.course = (int32_t)(1e7 * (atan2(ins_vn.vel_ned.y, ins_vn.vel_ned.x))); + SetBit(gps.valid_fields, GPS_VALID_COURSE_BIT); // Because we have not HMSL data from Vectornav, we are using LLA-Altitude // as a workaround diff --git a/sw/airborne/subsystems/ins/ins_vectornav.h b/sw/airborne/subsystems/ins/ins_vectornav.h index fa2fd9575b..02848f2583 100644 --- a/sw/airborne/subsystems/ins/ins_vectornav.h +++ b/sw/airborne/subsystems/ins/ins_vectornav.h @@ -53,12 +53,6 @@ // Abi #include "subsystems/abi.h" -#if GPS_USE_LATLONG -/* currently needed to get nav_utm_zone0 */ -#include "subsystems/navigation/common_nav.h" -#include "math/pprz_geodetic_float.h" -#endif - #if !defined INS_VN_BODY_TO_IMU_PHI && !defined INS_VN_BODY_TO_IMU_THETA && !defined INS_VN_BODY_TO_IMU_PSI #define INS_VN_BODY_TO_IMU_PHI 0 diff --git a/sw/tools/generators/gen_flight_plan.ml b/sw/tools/generators/gen_flight_plan.ml index fcff23af76..b9c6063e8b 100644 --- a/sw/tools/generators/gen_flight_plan.ml +++ b/sw/tools/generators/gen_flight_plan.ml @@ -702,7 +702,7 @@ let check_geo_ref = fun wgs84 xml -> let max_d = min 1000. (get_float "max_dist_from_home") in let check_zone = fun u -> if (utm_of WGS84 (of_utm WGS84 u)).utm_zone <> utm0.utm_zone then - prerr_endline "Warning: You are close (less than twice the max distance) to an UTM zone border ! The navigation will not work unless the GPS_USE_LATLONG flag is set and the GPS receiver configured to send the POSLLH message." in + prerr_endline "Warning: You are close (less than twice the max distance) to an UTM zone border ! The navigation will not work unless the GPS receiver configured to send the POSLLH message." in check_zone { utm0 with utm_x = utm0.utm_x +. 2.*.max_d }; check_zone { utm0 with utm_x = utm0.utm_x -. 2.*.max_d };