diff --git a/sw/airborne/modules/cam_control/point.c b/sw/airborne/modules/cam_control/point.c index 2be9eae0fb..37d6d5662c 100644 --- a/sw/airborne/modules/cam_control/point.c +++ b/sw/airborne/modules/cam_control/point.c @@ -408,7 +408,7 @@ void vPoint(float fPlaneEast, float fPlaneNorth, float fPlaneAltitude, struct UtmCoor_f utm; utm.east = nav_utm_east0 + fObjectEast; utm.north = nav_utm_north0 + fObjectNorth; - utm.zone = gps.utm_pos.zone; + utm.zone = nav_utm_zone0; 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 8a98e60c8c..852c2c3ea7 100644 --- a/sw/airborne/modules/ins/ins_xsens.c +++ b/sw/airborne/modules/ins/ins_xsens.c @@ -275,10 +275,7 @@ void ins_xsens_register(void) void ins_xsens_update_gps(struct GpsState *gps_s) { - struct UtmCoor_f utm; - utm.east = gps_s->utm_pos.east / 100.; - utm.north = gps_s->utm_pos.north / 100.; - utm.zone = nav_utm_zone0; + struct UtmCoor_f utm = utm_float_from_gps(gps_s, nav_utm_zone0); utm.alt = gps_s->hmsl / 1000.; // set position diff --git a/sw/airborne/modules/ins/ins_xsens700.c b/sw/airborne/modules/ins/ins_xsens700.c index 727ad4943b..7e62ba91b4 100644 --- a/sw/airborne/modules/ins/ins_xsens700.c +++ b/sw/airborne/modules/ins/ins_xsens700.c @@ -206,10 +206,7 @@ void ins_xsens_register(void) void ins_xsens_update_gps(struct GpsState *gps_s) { - struct UtmCoor_f utm; - utm.east = gps_s->utm_pos.east / 100.; - utm.north = gps_s->utm_pos.north / 100.; - utm.zone = nav_utm_zone0; + struct UtmCoor_f utm = utm_float_from_gps(gps_s, nav_utm_zone0); utm.alt = gps_s->hmsl / 1000.; // set position diff --git a/sw/airborne/modules/meteo/meteo_france_DAQ.c b/sw/airborne/modules/meteo/meteo_france_DAQ.c index 7f000f5897..59db68ba77 100644 --- a/sw/airborne/modules/meteo/meteo_france_DAQ.c +++ b/sw/airborne/modules/meteo/meteo_france_DAQ.c @@ -106,10 +106,12 @@ void mf_daq_send_report(void) uint8_t foo = 0; int16_t climb = -gps.ned_vel.z; int16_t course = (DegOfRad(gps.course) / ((int32_t)1e6)); + struct UtmCoor_f utm = stateGetPositionEnu_f(); + int32_t east = utm.east * 100; + int32_t north = utm.north * 100; DOWNLINK_SEND_GPS(pprzlog_tp, chibios_sdlog, &gps.fix, - &gps.utm_pos.east, &gps.utm_pos.north, - &course, &gps.hmsl, &gps.gspeed, &climb, - &gps.week, &gps.tow, &gps.utm_pos.zone, &foo); + &east, &north, &course, &gps.hmsl, &gps.gspeed, &climb, + &gps.week, &gps.tow, &utm.zone, &foo); } } diff --git a/sw/airborne/subsystems/gps.c b/sw/airborne/subsystems/gps.c index a15ae97b2a..235f554e70 100644 --- a/sw/airborne/subsystems/gps.c +++ b/sw/airborne/subsystems/gps.c @@ -199,3 +199,33 @@ uint32_t gps_tow_from_sys_ticks(uint32_t sys_ticks) void WEAK gps_inject_data(uint8_t packet_id __attribute__((unused)), uint8_t length __attribute__((unused)), uint8_t *data __attribute__((unused))){ } + +/** + * Convenience function to get utm position from GPS state + */ +struct UtmCoor_f utm_float_from_gps(struct GpsState *gps_s, uint8_t zone) +{ + struct UtmCoor_f utm; + utm.alt = 0.f; + + if (bit_is_set(gps_s->valid_fields, GPS_VALID_POS_UTM_BIT)) { + // A real UTM position is available, use the correct zone + utm.zone = gps_s->utm_pos.zone; + utm.east = gps_s->utm_pos.east / 100.0f; + utm.north = gps_s->utm_pos.north / 100.0f; + } + else { + struct LlaCoor_f lla; + LLA_FLOAT_OF_BFP(lla, gps_s->lla_pos); + // Check if zone should be computed + if (zone > 0) { + utm.zone = zone; + } else { + utm.zone = (gps_s->lla_pos.lon / 1e7 + 180) / 6 + 1; + } + /* Recompute UTM coordinates in this zone */ + utm_of_lla_f(&utm, &lla); + } + + return utm; +} diff --git a/sw/airborne/subsystems/gps.h b/sw/airborne/subsystems/gps.h index 7e2679d88e..7fd4b01bab 100644 --- a/sw/airborne/subsystems/gps.h +++ b/sw/airborne/subsystems/gps.h @@ -30,6 +30,7 @@ #include "std.h" #include "math/pprz_geodetic_int.h" +#include "math/pprz_geodetic_float.h" #include "mcu_periph/sys_time.h" @@ -162,4 +163,13 @@ extern struct GpsTimeSync gps_time_sync; */ extern uint32_t gps_tow_from_sys_ticks(uint32_t sys_ticks); +/** + * 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 + * @param[in] gps pointer to the gps structure + * @param[in] zone set the utm zone in which the position should be computed, 0 to try to get it automatically from lla position + * @return utm position in float + */ +extern struct UtmCoor_f utm_float_from_gps(struct GpsState *gps, uint8_t zone); + #endif /* GPS_H */ diff --git a/sw/airborne/subsystems/ins.c b/sw/airborne/subsystems/ins.c index 556c5f4e6a..178e76c0cd 100644 --- a/sw/airborne/subsystems/ins.c +++ b/sw/airborne/subsystems/ins.c @@ -72,20 +72,7 @@ void ins_init(void) void WEAK ins_reset_local_origin(void) { #if USE_GPS - struct UtmCoor_f utm; - - 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); - } + struct UtmCoor_f utm = utm_float_from_gps(&gps, 0); // ground_alt utm.alt = gps.hmsl / 1000.0f; diff --git a/sw/airborne/subsystems/ins/ins_alt_float.c b/sw/airborne/subsystems/ins/ins_alt_float.c index 9c0e71c4ef..870e1ecc83 100644 --- a/sw/airborne/subsystems/ins/ins_alt_float.c +++ b/sw/airborne/subsystems/ins/ins_alt_float.c @@ -113,20 +113,8 @@ void ins_alt_float_init(void) /** Reset the geographic reference to the current GPS fix */ void ins_reset_local_origin(void) { - struct UtmCoor_f utm; - - 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); - } + // get utm pos + struct UtmCoor_f utm = utm_float_from_gps(&gps, 0); // ground_alt utm.alt = gps.hmsl / 1000.0f; @@ -198,10 +186,7 @@ void ins_alt_float_update_baro(float pressure __attribute__((unused))) void ins_alt_float_update_gps(struct GpsState *gps_s) { #if USE_GPS - struct UtmCoor_f utm; - utm.east = gps_s->utm_pos.east / 100.0f; - utm.north = gps_s->utm_pos.north / 100.0f; - utm.zone = nav_utm_zone0; + struct UtmCoor_f utm = utm_float_from_gps(gps_s, nav_utm_zone0); #if !USE_BAROMETER #ifdef GPS_DT diff --git a/sw/airborne/subsystems/ins/ins_float_invariant.c b/sw/airborne/subsystems/ins/ins_float_invariant.c index b81cffcab6..124c191d74 100644 --- a/sw/airborne/subsystems/ins/ins_float_invariant.c +++ b/sw/airborne/subsystems/ins/ins_float_invariant.c @@ -273,20 +273,7 @@ void ins_float_invariant_init(void) void ins_reset_local_origin(void) { #if INS_FINV_USE_UTM - struct UtmCoor_f utm; - 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); - } + struct UtmCoor_f utm = utm_float_from_gps(&gps, 0); // ground_alt utm.alt = gps.hmsl / 1000.0f; // reset state UTM ref @@ -441,9 +428,10 @@ void ins_float_invariant_update_gps(struct GpsState *gps_s) #if INS_FINV_USE_UTM if (state.utm_initialized_f) { + struct UtmCoor_f utm = utm_float_from_gps(gps_s, nav_utm_zone0); // position (local ned) - ins_float_inv.meas.pos_gps.x = (gps_s->utm_pos.north / 100.0f) - state.utm_origin_f.north; - ins_float_inv.meas.pos_gps.y = (gps_s->utm_pos.east / 100.0f) - state.utm_origin_f.east; + ins_float_inv.meas.pos_gps.x = utm.north - state.utm_origin_f.north; + 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 - (gps_s->hmsl / 1000.0f); // speed ins_float_inv.meas.speed_gps.x = gps_s->ned_vel.x / 100.0f; diff --git a/sw/airborne/subsystems/ins/ins_gps_passthrough_utm.c b/sw/airborne/subsystems/ins/ins_gps_passthrough_utm.c index e400029898..6aa8127229 100644 --- a/sw/airborne/subsystems/ins/ins_gps_passthrough_utm.c +++ b/sw/airborne/subsystems/ins/ins_gps_passthrough_utm.c @@ -46,20 +46,7 @@ void ins_gps_utm_init(void) void ins_reset_local_origin(void) { - struct UtmCoor_f utm; - - 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); - } + struct UtmCoor_f utm = utm_float_from_gps(&gps, 0); // ground_alt utm.alt = gps.hmsl / 1000.0f; @@ -82,10 +69,7 @@ static void gps_cb(uint8_t sender_id __attribute__((unused)), uint32_t stamp __attribute__((unused)), struct GpsState *gps_s) { - struct UtmCoor_f utm; - utm.east = gps_s->utm_pos.east / 100.0f; - utm.north = gps_s->utm_pos.north / 100.0f; - utm.zone = nav_utm_zone0; + struct UtmCoor_f utm = utm_float_from_gps(gps_s, nav_utm_zone0); utm.alt = gps_s->hmsl / 1000.0f; // set position