struct UtmCoor_f utm_float_from_gps(struct GpsState *gps_s, uint8_t zone)
{
struct UtmCoor_f utm = {.east = 0., .north=0., .alt=0., .zone=zone};
if (bit_is_set(gps_s->valid_fields, GPS_VALID_POS_UTM_BIT)) {
/* A real UTM position is available, use the correct zone */
UTM_FLOAT_OF_BFP(utm, gps_s->utm_pos);
} else if (bit_is_set(gps_s->valid_fields, GPS_VALID_POS_LLA_BIT))
{
/* Recompute UTM coordinates in this zone */
struct UtmCoor_i utm_i;
utm_i.zone = zone;
utm_of_lla_i(&utm_i, &gps_s->lla_pos);
UTM_FLOAT_OF_BFP(utm, utm_i);
/* set utm.alt in hsml */
if (bit_is_set(gps_s->valid_fields, GPS_VALID_HMSL_BIT)) {
utm.alt = gps_s->hmsl/1000.;
} else {
utm.alt = wgs84_ellipsoid_to_geoid_i(gps_s->lla_pos.lat, gps_s->lla_pos.lon)/1000.;
}
}
return utm;
}
struct UtmCoor_i utm_int_from_gps(struct GpsState *gps_s, uint8_t zone)
{
struct UtmCoor_i utm = {.east = 0, .north=0, .alt=0, .zone=zone};
if (bit_is_set(gps_s->valid_fields, GPS_VALID_POS_UTM_BIT)) {
// A real UTM position is available, use the correct zone
UTM_COPY(utm, gps_s->utm_pos);
}
else if (bit_is_set(gps_s->valid_fields, GPS_VALID_POS_LLA_BIT)){
/* Recompute UTM coordinates in zone */
utm_of_lla_i(&utm, &gps_s->lla_pos);
/* set utm.alt in hsml */
if (bit_is_set(gps_s->valid_fields, GPS_VALID_HMSL_BIT)) {
utm.alt = gps_s->hmsl;
} else {
utm.alt = wgs84_ellipsoid_to_geoid_i(gps_s->lla_pos.lat, gps_s->lla_pos.lon);
}
}
return utm;
}
/** Convert a local NED position to ECEF.
* @param[out] ecef ECEF position in cm
* @param[in] def local coordinate system definition
* @param[in] ned NED position in meter << #INT32_POS_FRAC
*/
void lla_of_utm_i(struct LlaCoor_i *lla, struct UtmCoor_i *utm)
{
#if USE_SINGLE_PRECISION_LLA_UTM
/* convert our input to floating point */
struct UtmCoor_f utm_f;
UTM_FLOAT_OF_BFP(utm_f, *utm);
/* calls the floating point transformation */
struct LlaCoor_f lla_f;
lla_of_utm_f(&lla_f, &utm_f);
/* convert the output to fixed point */
LLA_BFP_OF_REAL(*lla, lla_f);
#else // use double precision by default
/* convert our input to floating point */
struct UtmCoor_d utm_d;
UTM_DOUBLE_OF_BFP(utm_d, *utm);
/* calls the floating point transformation */
struct LlaCoor_d lla_d;
lla_of_utm_d(&lla_d, &utm_d);
/* convert the output to fixed point */
LLA_BFP_OF_REAL(*lla, lla_d);
#endif
}
