Skip to content

Commit

Permalink
[math] factor out function to calc ltp_of_ecef rmat
Browse files Browse the repository at this point in the history
  • Loading branch information
@flixr
flixr committed Aug 22, 2013
1 parent 393a21b commit c90cd6f
Show file tree
Hide file tree
Showing 2 changed files with 28 additions and 46 deletions.
73 changes: 27 additions & 46 deletions sw/airborne/math/pprz_geodetic_int.c
View file
Expand Up @@ -24,36 +24,39 @@

#define HIGH_RES_TRIG_FRAC 20

void ltp_of_ecef_rmat_from_lla_i(struct Int32Mat33* ltp_of_ecef, struct LlaCoor_i* lla) {

#if USE_DOUBLE_PRECISION_TRIG
int32_t sin_lat = rint(BFP_OF_REAL(sin(RAD_OF_EM7RAD((double)lla->lat)), HIGH_RES_TRIG_FRAC));
int32_t cos_lat = rint(BFP_OF_REAL(cos(RAD_OF_EM7RAD((double)lla->lat)), HIGH_RES_TRIG_FRAC));
int32_t sin_lon = rint(BFP_OF_REAL(sin(RAD_OF_EM7RAD((double)lla->lon)), HIGH_RES_TRIG_FRAC));
int32_t cos_lon = rint(BFP_OF_REAL(cos(RAD_OF_EM7RAD((double)lla->lon)), HIGH_RES_TRIG_FRAC));
#else
int32_t sin_lat = rint(BFP_OF_REAL(sinf(RAD_OF_EM7RAD((float)lla->lat)), HIGH_RES_TRIG_FRAC));
int32_t cos_lat = rint(BFP_OF_REAL(cosf(RAD_OF_EM7RAD((float)lla->lat)), HIGH_RES_TRIG_FRAC));
int32_t sin_lon = rint(BFP_OF_REAL(sinf(RAD_OF_EM7RAD((float)lla->lon)), HIGH_RES_TRIG_FRAC));
int32_t cos_lon = rint(BFP_OF_REAL(cosf(RAD_OF_EM7RAD((float)lla->lon)), HIGH_RES_TRIG_FRAC));
#endif

ltp_of_ecef->m[0] = -sin_lon;
ltp_of_ecef->m[1] = cos_lon;
ltp_of_ecef->m[2] = 0; /* this element is always zero http://en.wikipedia.org/wiki/Geodetic_system#From_ECEF_to_ENU */
ltp_of_ecef->m[3] = (int32_t)((-(int64_t)sin_lat*(int64_t)cos_lon)>>HIGH_RES_TRIG_FRAC);
ltp_of_ecef->m[4] = (int32_t)((-(int64_t)sin_lat*(int64_t)sin_lon)>>HIGH_RES_TRIG_FRAC);
ltp_of_ecef->m[5] = cos_lat;
ltp_of_ecef->m[6] = (int32_t)(( (int64_t)cos_lat*(int64_t)cos_lon)>>HIGH_RES_TRIG_FRAC);
ltp_of_ecef->m[7] = (int32_t)(( (int64_t)cos_lat*(int64_t)sin_lon)>>HIGH_RES_TRIG_FRAC);
ltp_of_ecef->m[8] = sin_lat;
}

void ltp_def_from_ecef_i(struct LtpDef_i* def, struct EcefCoor_i* ecef) {

/* store the origin of the tangeant plane */
VECT3_COPY(def->ecef, *ecef);
/* compute the lla representation of the origin */
lla_of_ecef_i(&def->lla, &def->ecef);
/* store the rotation matrix */

#if 1
int32_t sin_lat = rint(BFP_OF_REAL(sinf(RAD_OF_EM7RAD((float)def->lla.lat)), HIGH_RES_TRIG_FRAC));
int32_t cos_lat = rint(BFP_OF_REAL(cosf(RAD_OF_EM7RAD((float)def->lla.lat)), HIGH_RES_TRIG_FRAC));
int32_t sin_lon = rint(BFP_OF_REAL(sinf(RAD_OF_EM7RAD((float)def->lla.lon)), HIGH_RES_TRIG_FRAC));
int32_t cos_lon = rint(BFP_OF_REAL(cosf(RAD_OF_EM7RAD((float)def->lla.lon)), HIGH_RES_TRIG_FRAC));
#else
int32_t sin_lat = rint(BFP_OF_REAL(sin(RAD_OF_EM7RAD((double)def->lla.lat)), HIGH_RES_TRIG_FRAC));
int32_t cos_lat = rint(BFP_OF_REAL(cos(RAD_OF_EM7RAD((double)def->lla.lat)), HIGH_RES_TRIG_FRAC));
int32_t sin_lon = rint(BFP_OF_REAL(sin(RAD_OF_EM7RAD((double)def->lla.lon)), HIGH_RES_TRIG_FRAC));
int32_t cos_lon = rint(BFP_OF_REAL(cos(RAD_OF_EM7RAD((double)def->lla.lon)), HIGH_RES_TRIG_FRAC));
#endif


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] = (int32_t)((-(int64_t)sin_lat*(int64_t)cos_lon)>>HIGH_RES_TRIG_FRAC);
def->ltp_of_ecef.m[4] = (int32_t)((-(int64_t)sin_lat*(int64_t)sin_lon)>>HIGH_RES_TRIG_FRAC);
def->ltp_of_ecef.m[5] = cos_lat;
def->ltp_of_ecef.m[6] = (int32_t)(( (int64_t)cos_lat*(int64_t)cos_lon)>>HIGH_RES_TRIG_FRAC);
def->ltp_of_ecef.m[7] = (int32_t)(( (int64_t)cos_lat*(int64_t)sin_lon)>>HIGH_RES_TRIG_FRAC);
def->ltp_of_ecef.m[8] = sin_lat;
ltp_of_ecef_rmat_from_lla_i(&def->ltp_of_ecef, &def->lla);

}

Expand All @@ -64,29 +67,7 @@ void ltp_def_from_lla_i(struct LtpDef_i* def, struct LlaCoor_i* lla) {
/* compute the ecef representation of the origin */
ecef_of_lla_i(&def->ecef, &def->lla);
/* store the rotation matrix */

#if 1
int32_t sin_lat = rint(BFP_OF_REAL(sinf(RAD_OF_EM7RAD((float)def->lla.lat)), HIGH_RES_TRIG_FRAC));
int32_t cos_lat = rint(BFP_OF_REAL(cosf(RAD_OF_EM7RAD((float)def->lla.lat)), HIGH_RES_TRIG_FRAC));
int32_t sin_lon = rint(BFP_OF_REAL(sinf(RAD_OF_EM7RAD((float)def->lla.lon)), HIGH_RES_TRIG_FRAC));
int32_t cos_lon = rint(BFP_OF_REAL(cosf(RAD_OF_EM7RAD((float)def->lla.lon)), HIGH_RES_TRIG_FRAC));
#else
int32_t sin_lat = rint(BFP_OF_REAL(sin(RAD_OF_EM7RAD((double)def->lla.lat)), HIGH_RES_TRIG_FRAC));
int32_t cos_lat = rint(BFP_OF_REAL(cos(RAD_OF_EM7RAD((double)def->lla.lat)), HIGH_RES_TRIG_FRAC));
int32_t sin_lon = rint(BFP_OF_REAL(sin(RAD_OF_EM7RAD((double)def->lla.lon)), HIGH_RES_TRIG_FRAC));
int32_t cos_lon = rint(BFP_OF_REAL(cos(RAD_OF_EM7RAD((double)def->lla.lon)), HIGH_RES_TRIG_FRAC));
#endif


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] = (int32_t)((-(int64_t)sin_lat*(int64_t)cos_lon)>>HIGH_RES_TRIG_FRAC);
def->ltp_of_ecef.m[4] = (int32_t)((-(int64_t)sin_lat*(int64_t)sin_lon)>>HIGH_RES_TRIG_FRAC);
def->ltp_of_ecef.m[5] = cos_lat;
def->ltp_of_ecef.m[6] = (int32_t)(( (int64_t)cos_lat*(int64_t)cos_lon)>>HIGH_RES_TRIG_FRAC);
def->ltp_of_ecef.m[7] = (int32_t)(( (int64_t)cos_lat*(int64_t)sin_lon)>>HIGH_RES_TRIG_FRAC);
def->ltp_of_ecef.m[8] = sin_lat;
ltp_of_ecef_rmat_from_lla_i(&def->ltp_of_ecef, &def->lla);

}

Expand Down
1 change: 1 addition & 0 deletions sw/airborne/math/pprz_geodetic_int.h
View file
Expand Up @@ -99,6 +99,7 @@ struct LtpDef_i {
int32_t hmsl; ///< Height above mean sea level in mm
};

extern void ltp_of_ecef_rmat_from_lla_i(struct Int32Mat33* ltp_of_ecef, struct LlaCoor_i* lla);
extern void ltp_def_from_ecef_i(struct LtpDef_i* def, struct EcefCoor_i* ecef);
extern void ltp_def_from_lla_i(struct LtpDef_i* def, struct LlaCoor_i* lla);
extern void lla_of_ecef_i(struct LlaCoor_i* out, struct EcefCoor_i* in);
Expand Down

0 comments on commit c90cd6f

Please sign in to comment.