void stateCalcPositionEcef_f(void) { if (bit_is_set(state.pos_status, POS_ECEF_F)) { return; } if (bit_is_set(state.pos_status, POS_ECEF_I)) { ECEF_FLOAT_OF_BFP(state.ecef_pos_f, state.ecef_pos_i); } else if (bit_is_set(state.pos_status, POS_NED_F) && state.ned_initialized_f) { ecef_of_ned_point_f(&state.ecef_pos_f, &state.ned_origin_f, &state.ned_pos_f); } else if (bit_is_set(state.pos_status, POS_NED_I) && state.ned_initialized_i) { /* transform ned_i -> ecef_i -> ecef_f, set status bits */ ecef_of_ned_pos_i(&state.ecef_pos_i, &state.ned_origin_i, &state.ned_pos_i); SetBit(state.pos_status, POS_ECEF_F); ECEF_FLOAT_OF_BFP(state.ecef_pos_f, state.ecef_pos_i); } else if (bit_is_set(state.pos_status, POS_LLA_F)) { ecef_of_lla_f(&state.ecef_pos_f, &state.lla_pos_f); } else if (bit_is_set(state.pos_status, POS_LLA_I)) { LLA_FLOAT_OF_BFP(state.lla_pos_f, state.lla_pos_i); SetBit(state.pos_status, POS_LLA_F); ecef_of_lla_f(&state.ecef_pos_f, &state.lla_pos_f); } else { /* could not get this representation, set errno */ //struct EcefCoor_f _ecef_zero = {0.0f}; //return _ecef_zero; } /* set bit to indicate this representation is computed */ SetBit(state.pos_status, POS_ECEF_F); }
void ecef_of_lla_i(struct EcefCoor_i *out, struct LlaCoor_i *in) { #if USE_SINGLE_PRECISION_LLA_ECEF /* convert our input to floating point */ struct LlaCoor_f in_f; in_f.lon = RAD_OF_EM7DEG((float)in->lon); in_f.lat = RAD_OF_EM7DEG((float)in->lat); in_f.alt = M_OF_MM((float)in->alt); /* calls the floating point transformation */ struct EcefCoor_f out_f; ecef_of_lla_f(&out_f, &in_f); /* convert the output to fixed point */ out->x = (int32_t)CM_OF_M(out_f.x); out->y = (int32_t)CM_OF_M(out_f.y); out->z = (int32_t)CM_OF_M(out_f.z); #else // use double precision by default /* convert our input to floating point */ struct LlaCoor_d in_d; in_d.lon = RAD_OF_EM7DEG((double)in->lon); in_d.lat = RAD_OF_EM7DEG((double)in->lat); in_d.alt = M_OF_MM((double)in->alt); /* calls the floating point transformation */ struct EcefCoor_d out_d; ecef_of_lla_d(&out_d, &in_d); /* convert the output to fixed point */ out->x = (int32_t)CM_OF_M(out_d.x); out->y = (int32_t)CM_OF_M(out_d.y); out->z = (int32_t)CM_OF_M(out_d.z); #endif }
void stateCalcPositionEcef_i(void) { if (bit_is_set(state.pos_status, POS_ECEF_I)) return; if (bit_is_set(state.pos_status, POS_ECEF_F)) { ECEF_BFP_OF_REAL(state.ecef_pos_i, state.ecef_pos_f); } else if (bit_is_set(state.pos_status, POS_NED_I)) { /// @todo check if resolution is good enough ecef_of_ned_point_i(&state.ecef_pos_i, &state.ned_origin_i, &state.ned_pos_i); } else if (bit_is_set(state.pos_status, POS_NED_F)) { /* transform ned_f to ecef_f, set status bit, then convert to int */ ecef_of_ned_point_f(&state.ecef_pos_f, &state.ned_origin_f, &state.ned_pos_f); SetBit(state.pos_status, POS_ECEF_F); ECEF_BFP_OF_REAL(state.ecef_pos_i, state.ecef_pos_f); } else if (bit_is_set(state.pos_status, POS_LLA_F)) { /* transform lla_f to ecef_f, set status bit, then convert to int */ ecef_of_lla_f(&state.ecef_pos_f, &state.lla_pos_f); SetBit(state.pos_status, POS_ECEF_F); ECEF_BFP_OF_REAL(state.ecef_pos_i, state.ecef_pos_f); } else if (bit_is_set(state.pos_status, POS_LLA_I)) { ecef_of_lla_i(&state.ecef_pos_i, &state.lla_pos_i); } else { /* could not get this representation, set errno */ //struct EcefCoor_i _ecef_zero = {0}; //return _ecef_zero; } /* set bit to indicate this representation is computed */ SetBit(state.pos_status, POS_ECEF_I); }
void ltp_def_from_lla_f(struct LtpDef_f* def, struct LlaCoor_f* lla) { /* store the origin of the tangeant plane */ LLA_COPY(def->lla, *lla); /* compute the ecef representation of the origin */ ecef_of_lla_f(&def->ecef, &def->lla); /* store the rotation matrix */ const float sin_lat = sinf(def->lla.lat); const float cos_lat = cosf(def->lla.lat); const float sin_lon = sinf(def->lla.lon); const float cos_lon = cosf(def->lla.lon); def->ltp_of_ecef.m[0] = -sin_lon; def->ltp_of_ecef.m[1] = cos_lon; def->ltp_of_ecef.m[2] = 0.; /* this element is always zero http://en.wikipedia.org/wiki/Geodetic_system#From_ECEF_to_ENU */ def->ltp_of_ecef.m[3] = -sin_lat*cos_lon; def->ltp_of_ecef.m[4] = -sin_lat*sin_lon; def->ltp_of_ecef.m[5] = cos_lat; def->ltp_of_ecef.m[6] = cos_lat*cos_lon; def->ltp_of_ecef.m[7] = cos_lat*sin_lon; def->ltp_of_ecef.m[8] = sin_lat; }
/** Convert a LLA to ECEF. * @param[out] out ECEF in cm * @param[in] in LLA in degrees*1e7 and mm above ellipsoid */ void ecef_of_lla_i(struct EcefCoor_i *out, struct LlaCoor_i *in) { #if USE_SINGLE_PRECISION_LLA_ECEF /* convert our input to floating point */ struct LlaCoor_f in_f; LLA_FLOAT_OF_BFP(in_f, *in); /* calls the floating point transformation */ struct EcefCoor_f out_f; ecef_of_lla_f(&out_f, &in_f); /* convert the output to fixed point */ ECEF_BFP_OF_REAL(*out, out_f); #else // use double precision by default /* convert our input to floating point */ struct LlaCoor_d in_d; LLA_DOUBLE_OF_BFP(in_d, *in); /* calls the floating point transformation */ struct EcefCoor_d out_d; ecef_of_lla_d(&out_d, &in_d); /* convert the output to fixed point */ ECEF_BFP_OF_REAL(*out, out_d); #endif }
void ned_of_lla_point_f(struct NedCoor_f* ned, struct LtpDef_f* def, struct LlaCoor_f* lla) { struct EcefCoor_f ecef; ecef_of_lla_f(&ecef,lla); ned_of_ecef_point_f(ned,def,&ecef); }
void enu_of_lla_point_f(struct EnuCoor_f* enu, struct LtpDef_f* def, struct LlaCoor_f* lla) { struct EcefCoor_f ecef; ecef_of_lla_f(&ecef,lla); enu_of_ecef_point_f(enu,def,&ecef); }
void stateCalcPositionNed_i(void) { if (bit_is_set(state.pos_status, POS_NED_I)) { return; } int errno = 0; if (state.ned_initialized_i) { if (bit_is_set(state.pos_status, POS_NED_F)) { NED_BFP_OF_REAL(state.ned_pos_i, state.ned_pos_f); } else if (bit_is_set(state.pos_status, POS_ENU_I)) { INT32_VECT3_NED_OF_ENU(state.ned_pos_i, state.enu_pos_i); } else if (bit_is_set(state.pos_status, POS_ENU_F)) { ENU_BFP_OF_REAL(state.enu_pos_i, state.enu_pos_f); SetBit(state.pos_status, POS_ENU_I); INT32_VECT3_NED_OF_ENU(state.ned_pos_i, state.enu_pos_i); } else if (bit_is_set(state.pos_status, POS_ECEF_I)) { ned_of_ecef_pos_i(&state.ned_pos_i, &state.ned_origin_i, &state.ecef_pos_i); } else if (bit_is_set(state.pos_status, POS_ECEF_F)) { /* transform ecef_f -> ned_f, set status bit, then convert to int */ ned_of_ecef_point_f(&state.ned_pos_f, &state.ned_origin_f, &state.ecef_pos_f); SetBit(state.pos_status, POS_NED_F); NED_BFP_OF_REAL(state.ned_pos_i, state.ned_pos_f); } else if (bit_is_set(state.pos_status, POS_LLA_F)) { /* transform lla_f -> ecef_f -> ned_f, set status bits, then convert to int */ ecef_of_lla_f(&state.ecef_pos_f, &state.lla_pos_f); SetBit(state.pos_status, POS_ECEF_F); ned_of_ecef_point_f(&state.ned_pos_f, &state.ned_origin_f, &state.ecef_pos_f); SetBit(state.pos_status, POS_NED_F); NED_BFP_OF_REAL(state.ned_pos_i, state.ned_pos_f); } else if (bit_is_set(state.pos_status, POS_LLA_I)) { ned_of_lla_point_i(&state.ned_pos_i, &state.ned_origin_i, &state.lla_pos_i); } else { /* could not get this representation, set errno */ errno = 1; } } else if (state.utm_initialized_f) { if (bit_is_set(state.pos_status, POS_NED_F)) { NED_BFP_OF_REAL(state.ned_pos_i, state.ned_pos_f); } else if (bit_is_set(state.pos_status, POS_ENU_I)) { INT32_VECT3_NED_OF_ENU(state.ned_pos_i, state.enu_pos_i); } else if (bit_is_set(state.pos_status, POS_ENU_F)) { ENU_BFP_OF_REAL(state.enu_pos_i, state.enu_pos_f); SetBit(state.pos_status, POS_ENU_I); INT32_VECT3_NED_OF_ENU(state.ned_pos_i, state.enu_pos_i); } else if (bit_is_set(state.pos_status, POS_UTM_F)) { /* transform utm_f -> ned_f -> ned_i, set status bits */ NED_OF_UTM_DIFF(state.ned_pos_f, state.utm_pos_f, state.utm_origin_f); SetBit(state.pos_status, POS_NED_F); NED_BFP_OF_REAL(state.ned_pos_i, state.ned_pos_f); } else if (bit_is_set(state.pos_status, POS_LLA_F)) { /* transform lla_f -> utm_f -> ned_f -> ned_i, set status bits */ utm_of_lla_f(&state.utm_pos_f, &state.lla_pos_f); SetBit(state.pos_status, POS_UTM_F); NED_OF_UTM_DIFF(state.ned_pos_f, state.utm_pos_f, state.utm_origin_f); SetBit(state.pos_status, POS_NED_F); NED_BFP_OF_REAL(state.ned_pos_i, state.ned_pos_f); } else if (bit_is_set(state.pos_status, POS_LLA_I)) { /* transform lla_i -> lla_f -> utm_f -> ned_f -> ned_i, set status bits */ LLA_FLOAT_OF_BFP(state.lla_pos_f, state.lla_pos_i); SetBit(state.pos_status, POS_LLA_F); utm_of_lla_f(&state.utm_pos_f, &state.lla_pos_f); SetBit(state.pos_status, POS_UTM_F); NED_OF_UTM_DIFF(state.ned_pos_f, state.utm_pos_f, state.utm_origin_f); SetBit(state.pos_status, POS_NED_F); NED_BFP_OF_REAL(state.ned_pos_i, state.ned_pos_f); } else { /* could not get this representation, set errno */ errno = 2; } } else { /* ned coordinate system not initialized, set errno */ errno = 3; } if (errno) { //struct NedCoor_i _ned_zero = {0}; //return _ned_zero; } /* set bit to indicate this representation is computed */ SetBit(state.pos_status, POS_NED_I); }
/** * Parse GGA NMEA messages. * GGA has essential fix data providing 3D location and HDOP. * Msg stored in gps_nmea.msg_buf. */ static void nmea_parse_GGA(void) { int i = 6; // current position in the message, start after: GPGGA, double degrees, minutesfrac; struct LlaCoor_f lla_f; // attempt to reject empty packets right away if (gps_nmea.msg_buf[i] == ',' && gps_nmea.msg_buf[i + 1] == ',') { NMEA_PRINT("p_GGA() - skipping empty message\n\r"); return; } // get UTC time [hhmmss.sss] // ignored GpsInfo.PosLLA.TimeOfFix.f = strtod(&packet[i], NULL); // FIXME: parse UTC time correctly double utc_time = strtod(&gps_nmea.msg_buf[i], NULL); gps_nmea.state.tow = (uint32_t)((utc_time + 1) * 1000); // get latitude [ddmm.mmmmm] nmea_read_until(&i); double lat = strtod(&gps_nmea.msg_buf[i], NULL); // convert to pure degrees [dd.dddd] format minutesfrac = modf(lat / 100, °rees); lat = degrees + (minutesfrac * 100) / 60; // get latitute N/S nmea_read_until(&i); if (gps_nmea.msg_buf[i] == 'S') { lat = -lat; } // convert to radians lla_f.lat = RadOfDeg(lat); gps_nmea.state.lla_pos.lat = lat * 1e7; // convert to fixed-point NMEA_PRINT("p_GGA() - lat=%f gps_lat=%f\n\r", (lat * 1000), lla_f.lat); // get longitude [ddmm.mmmmm] nmea_read_until(&i); double lon = strtod(&gps_nmea.msg_buf[i], NULL); // convert to pure degrees [dd.dddd] format minutesfrac = modf(lon / 100, °rees); lon = degrees + (minutesfrac * 100) / 60; // get longitude E/W nmea_read_until(&i); if (gps_nmea.msg_buf[i] == 'W') { lon = -lon; } // convert to radians lla_f.lon = RadOfDeg(lon); gps_nmea.state.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_nmea.state.tow); SetBit(gps_nmea.state.valid_fields, GPS_VALID_POS_LLA_BIT); // get position fix status nmea_read_until(&i); // 0 = Invalid, 1 = Valid SPS, 2 = Valid DGPS, 3 = Valid PPS // check for good position fix if ((gps_nmea.msg_buf[i] != '0') && (gps_nmea.msg_buf[i] != ',')) { gps_nmea.pos_available = TRUE; NMEA_PRINT("p_GGA() - POS_AVAILABLE == TRUE\n\r"); } else { gps_nmea.pos_available = FALSE; NMEA_PRINT("p_GGA() - gps_pos_available == false\n\r"); } // get number of satellites used in GPS solution nmea_read_until(&i); gps_nmea.state.num_sv = atoi(&gps_nmea.msg_buf[i]); NMEA_PRINT("p_GGA() - gps_numSatlitesUsed=%i\n\r", gps_nmea.state.num_sv); // get HDOP, but we use PDOP from GSA message nmea_read_until(&i); //float hdop = strtof(&gps_nmea.msg_buf[i], NULL); //gps_nmea.state.pdop = hdop * 100; // get altitude (in meters) above geoid (MSL) nmea_read_until(&i); float hmsl = strtof(&gps_nmea.msg_buf[i], NULL); gps_nmea.state.hmsl = hmsl * 1000; NMEA_PRINT("p_GGA() - gps_nmea.state.hmsl=%i\n\r", gps_nmea.state.hmsl); SetBit(gps_nmea.state.valid_fields, GPS_VALID_HMSL_BIT); // get altitude units (always M) nmea_read_until(&i); // get geoid seperation nmea_read_until(&i); float geoid = strtof(&gps_nmea.msg_buf[i], NULL); NMEA_PRINT("p_GGA() - geoid alt=%f\n\r", geoid); // height above ellipsoid lla_f.alt = hmsl + geoid; gps_nmea.state.lla_pos.alt = lla_f.alt * 1000; NMEA_PRINT("p_GGA() - gps_nmea.state.alt=%i\n\r", gps_nmea.state.lla_pos.alt); // get seperations units nmea_read_until(&i); // get DGPS age nmea_read_until(&i); // get DGPS station ID /* convert to ECEF */ struct EcefCoor_f ecef_f; ecef_of_lla_f(&ecef_f, &lla_f); gps_nmea.state.ecef_pos.x = ecef_f.x * 100; gps_nmea.state.ecef_pos.y = ecef_f.y * 100; gps_nmea.state.ecef_pos.z = ecef_f.z * 100; SetBit(gps_nmea.state.valid_fields, GPS_VALID_POS_ECEF_BIT); }