/** Convert a LLA to UTM. * @param[out] out UTM in cm and mm hmsl alt * @param[in] in LLA in degrees*1e7 and mm above ellipsoid */ void utm_of_lla_i(struct UtmCoor_i *utm, struct LlaCoor_i *lla) { #if USE_SINGLE_PRECISION_LLA_UTM /* convert our input to floating point */ struct LlaCoor_f lla_f; LLA_FLOAT_OF_BFP(lla_f, *lla); /* calls the floating point transformation */ struct UtmCoor_f utm_f; utm_f.zone = utm->zone; utm_of_lla_f(&utm_f, &lla_f); /* convert the output to fixed point */ UTM_BFP_OF_REAL(*utm, utm_f); #else // use double precision by default /* convert our input to floating point */ struct LlaCoor_d lla_d; LLA_DOUBLE_OF_BFP(lla_d, *lla); /* calls the floating point transformation */ struct UtmCoor_d utm_d; utm_d.zone = utm->zone; utm_of_lla_d(&utm_d, &lla_d); /* convert the output to fixed point */ UTM_BFP_OF_REAL(*utm, utm_d); #endif }
void WEAK ins_reset_utm_zone(struct UtmCoor_f *utm) { struct LlaCoor_f lla0; lla_of_utm_f(&lla0, utm); 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); struct UtmCoor_i utm_i; UTM_BFP_OF_REAL(utm_i, *utm); stateSetLocalUtmOrigin_i(&utm_i); }