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);
}
