diff --git a/conf/messages.xml b/conf/messages.xml
index aa55389bcc..d100003256 100644
--- a/conf/messages.xml
+++ b/conf/messages.xml
@@ -365,8 +365,8 @@
-
-
+
+
@@ -469,8 +469,8 @@
-
-
+
+
@@ -692,8 +692,8 @@
-
-
+
+
@@ -1364,8 +1364,8 @@
-
-
+
+
@@ -1375,8 +1375,8 @@
-
-
+
+
@@ -2204,8 +2204,8 @@
-
-
+
+
@@ -2448,8 +2448,8 @@
-
-
+
+
diff --git a/sw/airborne/firmwares/rotorcraft/datalink.c b/sw/airborne/firmwares/rotorcraft/datalink.c
index 928a35492e..0bc0f349a4 100644
--- a/sw/airborne/firmwares/rotorcraft/datalink.c
+++ b/sw/airborne/firmwares/rotorcraft/datalink.c
@@ -105,8 +105,8 @@ void dl_parse_msg(void) {
if (stateIsLocalCoordinateValid()) {
uint8_t wp_id = DL_MOVE_WP_wp_id(dl_buffer);
struct LlaCoor_i lla;
- lla.lat = INT32_RAD_OF_DEG(DL_MOVE_WP_lat(dl_buffer));
- lla.lon = INT32_RAD_OF_DEG(DL_MOVE_WP_lon(dl_buffer));
+ lla.lat = DL_MOVE_WP_lat(dl_buffer);
+ lla.lon = DL_MOVE_WP_lon(dl_buffer);
/* WP_alt from message is alt above MSL in cm
* lla.alt is above ellipsoid in mm
*/
diff --git a/sw/airborne/math/pprz_geodetic_int.c b/sw/airborne/math/pprz_geodetic_int.c
index e98cdc4136..352bdb4cb0 100644
--- a/sw/airborne/math/pprz_geodetic_int.c
+++ b/sw/airborne/math/pprz_geodetic_int.c
@@ -26,15 +26,15 @@
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));
+ int32_t sin_lat = rint(BFP_OF_REAL(sin(RAD_OF_EM7DEG((double)lla->lat)), HIGH_RES_TRIG_FRAC));
+ int32_t cos_lat = rint(BFP_OF_REAL(cos(RAD_OF_EM7DEG((double)lla->lat)), HIGH_RES_TRIG_FRAC));
+ int32_t sin_lon = rint(BFP_OF_REAL(sin(RAD_OF_EM7DEG((double)lla->lon)), HIGH_RES_TRIG_FRAC));
+ int32_t cos_lon = rint(BFP_OF_REAL(cos(RAD_OF_EM7DEG((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));
+ int32_t sin_lat = rint(BFP_OF_REAL(sinf(RAD_OF_EM7DEG((float)lla->lat)), HIGH_RES_TRIG_FRAC));
+ int32_t cos_lat = rint(BFP_OF_REAL(cosf(RAD_OF_EM7DEG((float)lla->lat)), HIGH_RES_TRIG_FRAC));
+ int32_t sin_lon = rint(BFP_OF_REAL(sinf(RAD_OF_EM7DEG((float)lla->lon)), HIGH_RES_TRIG_FRAC));
+ int32_t cos_lon = rint(BFP_OF_REAL(cosf(RAD_OF_EM7DEG((float)lla->lon)), HIGH_RES_TRIG_FRAC));
#endif
ltp_of_ecef->m[0] = -sin_lon;
@@ -307,8 +307,8 @@ void lla_of_ecef_i(struct LlaCoor_i* out, struct EcefCoor_i* in) {
struct LlaCoor_d out_d;
lla_of_ecef_d(&out_d, &in_d);
/* convert the output to fixed point */
- out->lon = (int32_t)rint(EM7RAD_OF_RAD(out_d.lon));
- out->lat = (int32_t)rint(EM7RAD_OF_RAD(out_d.lat));
+ out->lon = (int32_t)rint(EM7DEG_OF_RAD(out_d.lon));
+ out->lat = (int32_t)rint(EM7DEG_OF_RAD(out_d.lat));
out->alt = (int32_t)MM_OF_M(out_d.alt);
}
@@ -317,8 +317,8 @@ void ecef_of_lla_i(struct EcefCoor_i* out, struct LlaCoor_i* in) {
/* convert our input to floating point */
struct LlaCoor_d in_d;
- in_d.lon = RAD_OF_EM7RAD((double)in->lon);
- in_d.lat = RAD_OF_EM7RAD((double)in->lat);
+ 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;
diff --git a/sw/airborne/math/pprz_geodetic_int.h b/sw/airborne/math/pprz_geodetic_int.h
index 62a2f847b9..d6a4efc54f 100644
--- a/sw/airborne/math/pprz_geodetic_int.h
+++ b/sw/airborne/math/pprz_geodetic_int.h
@@ -52,8 +52,8 @@ struct EcefCoor_i {
* @brief vector in Latitude, Longitude and Altitude
*/
struct LlaCoor_i {
- int32_t lon; ///< in radians*1e7
- int32_t lat; ///< in radians*1e7
+ int32_t lon; ///< in degrees*1e7
+ int32_t lat; ///< in degrees*1e7
int32_t alt; ///< in millimeters above WGS84 reference ellipsoid
};
@@ -125,6 +125,8 @@ extern void ecef_of_ned_vect_i(struct EcefCoor_i* ecef, struct LtpDef_i* def, st
#define M_OF_MM(_mm) ((_mm)/1e3)
#define EM7RAD_OF_RAD(_r) ((_r)*1e7)
#define RAD_OF_EM7RAD(_r) ((_r)/1e7)
+#define EM7DEG_OF_RAD(_r) (DegOfRad(_r)*1e7)
+#define RAD_OF_EM7DEG(_r) (RadOfDeg(_r)/1e7)
#define HIGH_RES_TRIG_FRAC 20
@@ -155,25 +157,23 @@ extern void ecef_of_ned_vect_i(struct EcefCoor_i* ecef, struct LtpDef_i* def, st
(_o).y = (double)M_OF_CM((_i).y); \
(_o).z = (double)M_OF_CM((_i).z); \
}
-
#define LLA_BFP_OF_REAL(_o, _i) { \
- (_o).lat = (int32_t)EM7RAD_OF_RAD((_i).lat); \
- (_o).lon = (int32_t)EM7RAD_OF_RAD((_i).lon); \
+ (_o).lat = (int32_t)EM7DEG_OF_RAD((_i).lat); \
+ (_o).lon = (int32_t)EM7DEG_OF_RAD((_i).lon); \
(_o).alt = (int32_t)MM_OF_M((_i).alt); \
}
#define LLA_FLOAT_OF_BFP(_o, _i) { \
- (_o).lat = (float)RAD_OF_EM7RAD((_i).lat); \
- (_o).lon = (float)RAD_OF_EM7RAD((_i).lon); \
- (_o).alt = (float)M_OF_MM((_i).alt); \
+ (_o).lat = RAD_OF_EM7DEG((float)(_i).lat); \
+ (_o).lon = RAD_OF_EM7DEG((float)(_i).lon); \
+ (_o).alt = M_OF_MM((float)(_i).alt); \
}
#define LLA_DOUBLE_OF_BFP(_o, _i) { \
- (_o).lat = (double)RAD_OF_EM7RAD((_i).lat); \
- (_o).lon = (double)RAD_OF_EM7RAD((_i).lon); \
- (_o).alt = (double)M_OF_MM((_i).alt); \
+ (_o).lat = RAD_OF_EM7DEG((double)(_i).lat); \
+ (_o).lon = RAD_OF_EM7DEG((double)(_i).lon); \
+ (_o).alt = M_OF_MM((double)(_i).alt); \
}
-
#define NED_BFP_OF_REAL(_o, _i) { \
(_o).x = POS_BFP_OF_REAL((_i).x); \
(_o).y = POS_BFP_OF_REAL((_i).y); \
diff --git a/sw/airborne/modules/geo_mag/geo_mag.c b/sw/airborne/modules/geo_mag/geo_mag.c
index 8b0d57bc38..3f9ee5081c 100644
--- a/sw/airborne/modules/geo_mag/geo_mag.c
+++ b/sw/airborne/modules/geo_mag/geo_mag.c
@@ -58,8 +58,8 @@ void geo_mag_event(void) {
(double)gps.tow/1000/SECS_IN_YEAR;
/* LLA Position in decimal degrees and altitude in km */
- double latitude = DegOfRad((double)gps.lla_pos.lat / 1e7);
- double longitude = DegOfRad((double)gps.lla_pos.lon / 1e7);
+ double latitude = (double)gps.lla_pos.lat / 1e7;
+ double longitude = (double)gps.lla_pos.lon / 1e7;
double alt = (double)gps.lla_pos.alt / 1e6;
// Calculates additional coeffs
diff --git a/sw/airborne/modules/ins/ins_xsens.c b/sw/airborne/modules/ins/ins_xsens.c
index 5f3186d9a1..b9687c6207 100644
--- a/sw/airborne/modules/ins/ins_xsens.c
+++ b/sw/airborne/modules/ins/ins_xsens.c
@@ -483,15 +483,13 @@ void parse_ins_msg(void) {
gps.week = 0; // FIXME
gps.tow = XSENS_DATA_RAWGPS_itow(xsens_msg_buf,offset) * 10;
- gps.lla_pos.lat = RadOfDeg(XSENS_DATA_RAWGPS_lat(xsens_msg_buf,offset));
- gps.lla_pos.lon = RadOfDeg(XSENS_DATA_RAWGPS_lon(xsens_msg_buf,offset));
+ gps.lla_pos.lat = XSENS_DATA_RAWGPS_lat(xsens_msg_buf,offset);
+ gps.lla_pos.lon = XSENS_DATA_RAWGPS_lon(xsens_msg_buf,offset);
gps.lla_pos.alt = XSENS_DATA_RAWGPS_alt(xsens_msg_buf,offset);
/* Set the real UTM zone */
- gps.utm_pos.zone = (DegOfRad(gps.lla_pos.lon/1e7)+180) / 6 + 1;
-
- lla_f.lat = ((float) gps.lla_pos.lat) / 1e7;
- lla_f.lon = ((float) gps.lla_pos.lon) / 1e7;
+ gps.utm_pos.zone = (gps.lla_pos.lon / 1e7 + 180) / 6 + 1;
+ LLA_FLOAT_OF_BFP(lla_f, gps.lla_pos);
utm_f.zone = nav_utm_zone0;
/* convert to utm */
utm_of_lla_f(&utm_f, &lla_f);
@@ -600,8 +598,8 @@ void parse_ins_msg(void) {
#if (! USE_GPS_XSENS_RAW_DATA) && USE_GPS_XSENS
lla_f.lat = RadOfDeg(XSENS_DATA_Position_lat(xsens_msg_buf,offset));
lla_f.lon = RadOfDeg(XSENS_DATA_Position_lon(xsens_msg_buf,offset));
- gps.lla_pos.lat = (int32_t)(lla_f.lat * 1e7);
- gps.lla_pos.lon = (int32_t)(lla_f.lon * 1e7);
+ gps.lla_pos.lat = (int32_t)(DegOfRad(lla_f.lat) * 1e7);
+ gps.lla_pos.lon = (int32_t)(DegOfRad(lla_f.lon) * 1e7);
gps.utm_pos.zone = (DegOfRad(lla_f.lon)+180) / 6 + 1;
/* convert to utm */
utm_of_lla_f(&utm_f, &lla_f);
diff --git a/sw/airborne/state.h b/sw/airborne/state.h
index 3f7f81925f..893b1f39ce 100644
--- a/sw/airborne/state.h
+++ b/sw/airborne/state.h
@@ -147,7 +147,7 @@ struct State {
/**
* Position in Latitude, Longitude and Altitude.
- * Units lat,lon: radians*1e7
+ * Units lat,lon: degrees*1e7
* Units alt: milimeters above reference ellipsoid
*/
struct LlaCoor_i lla_pos_i;
diff --git a/sw/airborne/subsystems/gps.h b/sw/airborne/subsystems/gps.h
index 6202952f1d..ef4d7178c0 100644
--- a/sw/airborne/subsystems/gps.h
+++ b/sw/airborne/subsystems/gps.h
@@ -62,7 +62,7 @@ struct SVinfo {
/** data structure for GPS information */
struct GpsState {
struct EcefCoor_i ecef_pos; ///< position in ECEF in cm
- struct LlaCoor_i lla_pos; ///< position in LLA (lat,lon: rad*1e7; alt: mm over ellipsoid)
+ struct LlaCoor_i lla_pos; ///< position in LLA (lat,lon: deg*1e7; alt: mm over ellipsoid)
struct UtmCoor_i utm_pos; ///< position in UTM (north,east: cm; alt: mm over ellipsoid)
int32_t hmsl; ///< height above mean sea level in mm
struct EcefCoor_i ecef_vel; ///< speed ECEF in cm/s
diff --git a/sw/airborne/subsystems/gps/gps_datalink.c b/sw/airborne/subsystems/gps/gps_datalink.c
index 09987daeeb..31dfcbe6c3 100644
--- a/sw/airborne/subsystems/gps/gps_datalink.c
+++ b/sw/airborne/subsystems/gps/gps_datalink.c
@@ -43,8 +43,8 @@ void gps_impl_init(void) {
void parse_gps_datalink(uint8_t numsv, int32_t ecef_x, int32_t ecef_y, int32_t ecef_z, int32_t lat, int32_t lon, int32_t alt,
int32_t hmsl, int32_t ecef_xd, int32_t ecef_yd, int32_t ecef_zd, uint32_t tow, int32_t course) {
- gps.lla_pos.lat = RadOfDeg(lat);
- gps.lla_pos.lon = RadOfDeg(lon);
+ gps.lla_pos.lat = lat;
+ gps.lla_pos.lon = lon;
gps.lla_pos.alt = alt;
gps.hmsl = hmsl;
@@ -65,8 +65,7 @@ void parse_gps_datalink(uint8_t numsv, int32_t ecef_x, int32_t ecef_y, int32_t e
#if GPS_USE_LATLONG
// Computes from (lat, long) in the referenced UTM zone
struct LlaCoor_f lla_f;
- lla_f.lat = ((float) gps.lla_pos.lat) / 1e7;
- lla_f.lon = ((float) gps.lla_pos.lon) / 1e7;
+ LLA_FLOAT_OF_BFP(lla_f, gps.lla_pos);
struct UtmCoor_f utm_f;
utm_f.zone = nav_utm_zone0;
// convert to utm
diff --git a/sw/airborne/subsystems/gps/gps_mtk.c b/sw/airborne/subsystems/gps/gps_mtk.c
index 5b3886804f..e1097b567e 100644
--- a/sw/airborne/subsystems/gps/gps_mtk.c
+++ b/sw/airborne/subsystems/gps/gps_mtk.c
@@ -164,8 +164,8 @@ void gps_mtk_read_message(void) {
gps_time_sync.t0_ticks = sys_time.nb_tick;
gps_time_sync.t0_tow = MTK_DIY14_NAV_ITOW(gps_mtk.msg_buf);
gps_time_sync.t0_tow_frac = 0;
- gps.lla_pos.lat = RadOfDeg(MTK_DIY14_NAV_LAT(gps_mtk.msg_buf))*10;
- gps.lla_pos.lon = RadOfDeg(MTK_DIY14_NAV_LON(gps_mtk.msg_buf))*10;
+ gps.lla_pos.lat = MTK_DIY14_NAV_LAT(gps_mtk.msg_buf)*10;
+ gps.lla_pos.lon = MTK_DIY14_NAV_LON(gps_mtk.msg_buf)*10;
// FIXME: with MTK you do not receive vertical speed
if (sys_time.nb_sec - gps.last_3dfix_time < 2) {
gps.ned_vel.z = ((gps.hmsl -
@@ -194,8 +194,7 @@ void gps_mtk_read_message(void) {
gps.week = 0;
/* Computes from (lat, long) in the referenced UTM zone */
struct LlaCoor_f lla_f;
- lla_f.lat = ((float) gps.lla_pos.lat) / 1e7;
- lla_f.lon = ((float) gps.lla_pos.lon) / 1e7;
+ LLA_FLOAT_OF_BFP(lla_f, gps.lla_pos);
struct UtmCoor_f utm_f;
utm_f.zone = nav_utm_zone0;
/* convert to utm */
@@ -228,8 +227,8 @@ void gps_mtk_read_message(void) {
gps.t0_tow = gps.tow;
gps.t0_tow_frac = 0;
#endif
- gps.lla_pos.lat = RadOfDeg(MTK_DIY16_NAV_LAT(gps_mtk.msg_buf))*10;
- gps.lla_pos.lon = RadOfDeg(MTK_DIY16_NAV_LON(gps_mtk.msg_buf))*10;
+ gps.lla_pos.lat = MTK_DIY16_NAV_LAT(gps_mtk.msg_buf)*10;
+ gps.lla_pos.lon = MTK_DIY16_NAV_LON(gps_mtk.msg_buf)*10;
// FIXME: with MTK you do not receive vertical speed
if (sys_time.nb_sec - gps.last_3dfix_time < 2) {
gps.ned_vel.z = ((gps.hmsl -
@@ -256,8 +255,7 @@ void gps_mtk_read_message(void) {
/* HDOP? */
/* Computes from (lat, long) in the referenced UTM zone */
struct LlaCoor_f lla_f;
- lla_f.lat = ((float) gps.lla_pos.lat) / 1e7;
- lla_f.lon = ((float) gps.lla_pos.lon) / 1e7;
+ LLA_FLOAT_OF_BFP(lla_f, gps.lla_pos);
struct UtmCoor_f utm_f;
utm_f.zone = nav_utm_zone0;
/* convert to utm */
diff --git a/sw/airborne/subsystems/gps/gps_nmea.c b/sw/airborne/subsystems/gps/gps_nmea.c
index 10acf76215..1190294c51 100644
--- a/sw/airborne/subsystems/gps/gps_nmea.c
+++ b/sw/airborne/subsystems/gps/gps_nmea.c
@@ -238,7 +238,7 @@ void parse_nmea_GPGGA(void) {
// convert to radians
lla_f.lat = RadOfDeg(lat);
- gps.lla_pos.lat = lla_f.lat * 1e7; // convert to fixed-point
+ gps.lla_pos.lat = lat * 1e7; // convert to fixed-point
NMEA_PRINT("p_GPGGA() - lat=%d gps_lat=%i\n\r", (lat*1000), lla_f.lat);
@@ -266,7 +266,7 @@ void parse_nmea_GPGGA(void) {
// convert to radians
lla_f.lon = RadOfDeg(lon);
- gps.lla_pos.lon = lla_f.lon * 1e7; // convert to fixed-point
+ gps.lla_pos.lon = lon * 1e7; // convert to fixed-point
NMEA_PRINT("p_GPGGA() - lon=%d gps_lon=%i time=%u\n\r", (lon*1000), lla_f.lon, gps.tow);
diff --git a/sw/airborne/subsystems/gps/gps_sim_nps.c b/sw/airborne/subsystems/gps/gps_sim_nps.c
index 865ff0efa2..5e70503bbd 100644
--- a/sw/airborne/subsystems/gps/gps_sim_nps.c
+++ b/sw/airborne/subsystems/gps/gps_sim_nps.c
@@ -46,8 +46,8 @@ void gps_feed_value() {
gps.ecef_vel.y = sensors.gps.ecef_vel.y * 100.;
gps.ecef_vel.z = sensors.gps.ecef_vel.z * 100.;
//ecef pos seems to be based on geocentric model, hence we get a very high alt when converted to lla
- gps.lla_pos.lat = sensors.gps.lla_pos.lat * 1e7;
- gps.lla_pos.lon = sensors.gps.lla_pos.lon * 1e7;
+ gps.lla_pos.lat = DegOfRad(sensors.gps.lla_pos.lat) * 1e7;
+ gps.lla_pos.lon = DegOfRad(sensors.gps.lla_pos.lon) * 1e7;
gps.lla_pos.alt = sensors.gps.lla_pos.alt * 1000.;
gps.hmsl = sensors.gps.hmsl * 1000.;
@@ -70,8 +70,7 @@ void gps_feed_value() {
#if GPS_USE_LATLONG
/* Computes from (lat, long) in the referenced UTM zone */
struct LlaCoor_f lla_f;
- lla_f.lat = ((float) gps.lla_pos.lat) / 1e7;
- lla_f.lon = ((float) gps.lla_pos.lon) / 1e7;
+ LLA_FLOAT_OF_BFP(lla_f, gps.lla_pos);
struct UtmCoor_f utm_f;
utm_f.zone = nav_utm_zone0;
/* convert to utm */
diff --git a/sw/airborne/subsystems/gps/gps_sirf.c b/sw/airborne/subsystems/gps/gps_sirf.c
index 600391ef39..b832cac79b 100644
--- a/sw/airborne/subsystems/gps/gps_sirf.c
+++ b/sw/airborne/subsystems/gps/gps_sirf.c
@@ -104,15 +104,17 @@ void sirf_parse_41(void) {
gps.nb_channels = p ->num_sat;
/* read latitude, longitude and altitude from packet */
- gps.lla_pos.lat = RadOfDeg(Invert4Bytes(p->latitude));
- gps.lla_pos.lon = RadOfDeg(Invert4Bytes(p->longitude));
+ gps.lla_pos.lat = Invert4Bytes(p->latitude);
+ gps.lla_pos.lon = Invert4Bytes(p->longitude);
gps.lla_pos.alt = Invert4Bytes(p->alt_ellipsoid) * 10;
#if GPS_USE_LATLONG
/* convert to utm */
+ struct LlaCoor_f lla_f;
+ LLA_FLOAT_OF_BFP(lla_f, gps.lla_pos);
struct UtmCoor_f utm_f;
utm_f.zone = nav_utm_zone0;
- utm_of_lla_f(&utm_f, &lla_pos);
+ utm_of_lla_f(&utm_f, &lla_f);
/* copy results of utm conversion */
gps.utm_pos.east = utm_f.east*100;
diff --git a/sw/airborne/subsystems/gps/gps_skytraq.c b/sw/airborne/subsystems/gps/gps_skytraq.c
index b22b71c816..d3fedc36ea 100644
--- a/sw/airborne/subsystems/gps/gps_skytraq.c
+++ b/sw/airborne/subsystems/gps/gps_skytraq.c
@@ -105,8 +105,8 @@ void gps_skytraq_read_message(void) {
gps.ecef_vel.x = SKYTRAQ_NAVIGATION_DATA_ECEFVX(gps_skytraq.msg_buf);
gps.ecef_vel.y = SKYTRAQ_NAVIGATION_DATA_ECEFVY(gps_skytraq.msg_buf);
gps.ecef_vel.z = SKYTRAQ_NAVIGATION_DATA_ECEFVZ(gps_skytraq.msg_buf);
- gps.lla_pos.lat = RadOfDeg(SKYTRAQ_NAVIGATION_DATA_LAT(gps_skytraq.msg_buf));
- gps.lla_pos.lon = RadOfDeg(SKYTRAQ_NAVIGATION_DATA_LON(gps_skytraq.msg_buf));
+ gps.lla_pos.lat = SKYTRAQ_NAVIGATION_DATA_LAT(gps_skytraq.msg_buf);
+ gps.lla_pos.lon = SKYTRAQ_NAVIGATION_DATA_LON(gps_skytraq.msg_buf);
gps.lla_pos.alt = SKYTRAQ_NAVIGATION_DATA_AEL(gps_skytraq.msg_buf)*10;
gps.hmsl = SKYTRAQ_NAVIGATION_DATA_ASL(gps_skytraq.msg_buf)*10;
// pacc;
@@ -130,8 +130,7 @@ void gps_skytraq_read_message(void) {
#if GPS_USE_LATLONG
/* Computes from (lat, long) in the referenced UTM zone */
struct LlaCoor_f lla_f;
- lla_f.lat = ((float) gps.lla_pos.lat) / 1e7;
- lla_f.lon = ((float) gps.lla_pos.lon) / 1e7;
+ LLA_FLOAT_OF_BFP(lla_f, gps.lla_pos);
struct UtmCoor_f utm_f;
utm_f.zone = nav_utm_zone0;
/* convert to utm */
diff --git a/sw/airborne/subsystems/gps/gps_ubx.c b/sw/airborne/subsystems/gps/gps_ubx.c
index bc012d96f8..03183eb2cb 100644
--- a/sw/airborne/subsystems/gps/gps_ubx.c
+++ b/sw/airborne/subsystems/gps/gps_ubx.c
@@ -100,15 +100,14 @@ void gps_ubx_read_message(void) {
}
#endif
} else if (gps_ubx.msg_id == UBX_NAV_POSLLH_ID) {
- gps.lla_pos.lat = RadOfDeg(UBX_NAV_POSLLH_LAT(gps_ubx.msg_buf));
- gps.lla_pos.lon = RadOfDeg(UBX_NAV_POSLLH_LON(gps_ubx.msg_buf));
+ gps.lla_pos.lat = UBX_NAV_POSLLH_LAT(gps_ubx.msg_buf);
+ gps.lla_pos.lon = UBX_NAV_POSLLH_LON(gps_ubx.msg_buf);
gps.lla_pos.alt = UBX_NAV_POSLLH_HEIGHT(gps_ubx.msg_buf);
gps.hmsl = UBX_NAV_POSLLH_HMSL(gps_ubx.msg_buf);
#if GPS_USE_LATLONG
/* Computes from (lat, long) in the referenced UTM zone */
struct LlaCoor_f lla_f;
- lla_f.lat = ((float) gps.lla_pos.lat) / 1e7;
- lla_f.lon = ((float) gps.lla_pos.lon) / 1e7;
+ LLA_FLOAT_OF_BFP(lla_f, gps.lla_pos);
struct UtmCoor_f utm_f;
utm_f.zone = nav_utm_zone0;
/* convert to utm */
diff --git a/sw/airborne/subsystems/gps/gps_udp.c b/sw/airborne/subsystems/gps/gps_udp.c
index 8c7d3151a0..64430feed3 100644
--- a/sw/airborne/subsystems/gps/gps_udp.c
+++ b/sw/airborne/subsystems/gps/gps_udp.c
@@ -65,8 +65,8 @@ void gps_parse(void) {
// Correct MSG
if ((size == GPS_UDP_MSG_LEN) && (gps_udp_read_buffer[0] == STX))
{
- gps.lla_pos.lat = RadOfDeg(UDP_GPS_INT(gps_udp_read_buffer+4));
- gps.lla_pos.lon = RadOfDeg(UDP_GPS_INT(gps_udp_read_buffer+8));
+ gps.lla_pos.lat = UDP_GPS_INT(gps_udp_read_buffer+4);
+ gps.lla_pos.lon = UDP_GPS_INT(gps_udp_read_buffer+8);
gps.lla_pos.alt = UDP_GPS_INT(gps_udp_read_buffer+12);
gps.hmsl = UDP_GPS_INT(gps_udp_read_buffer+16);
@@ -84,8 +84,7 @@ void gps_parse(void) {
#if GPS_USE_LATLONG
// Computes from (lat, long) in the referenced UTM zone
struct LlaCoor_f lla_f;
- lla_f.lat = ((float) gps.lla_pos.lat) / 1e7;
- lla_f.lon = ((float) gps.lla_pos.lon) / 1e7;
+ LLA_FLOAT_OF_BFP(lla_f, gps.lla_pos);
struct UtmCoor_f utm_f;
utm_f.zone = nav_utm_zone0;
// convert to utm
diff --git a/sw/airborne/subsystems/ins.c b/sw/airborne/subsystems/ins.c
index c60ddc05fd..78749ada24 100644
--- a/sw/airborne/subsystems/ins.c
+++ b/sw/airborne/subsystems/ins.c
@@ -50,7 +50,7 @@ void WEAK ins_reset_utm_zone(struct UtmCoor_f * utm) {
struct LlaCoor_f lla0;
lla_of_utm_f(&lla0, utm);
#ifdef GPS_USE_LATLONG
- utm->zone = (DegOfRad(gps.lla_pos.lon/1e7)+180) / 6 + 1;
+ utm->zone = (gps.lla_pos.lon/1e7 + 180) / 6 + 1;
#else
utm->zone = gps.utm_pos.zone;
#endif
diff --git a/sw/airborne/subsystems/ins/ins_alt_float.c b/sw/airborne/subsystems/ins/ins_alt_float.c
index 3149b5b1cc..ea39e5bc90 100644
--- a/sw/airborne/subsystems/ins/ins_alt_float.c
+++ b/sw/airborne/subsystems/ins/ins_alt_float.c
@@ -103,9 +103,8 @@ void ins_reset_local_origin(void) {
#ifdef GPS_USE_LATLONG
/* Recompute UTM coordinates in this zone */
struct LlaCoor_f lla;
- lla.lat = gps.lla_pos.lat / 1e7;
- lla.lon = gps.lla_pos.lon / 1e7;
- utm.zone = (DegOfRad(gps.lla_pos.lon/1e7)+180) / 6 + 1;
+ LLA_FLOAT_OF_BFP(lla, gps.lla_pos);
+ utm.zone = (gps.lla_pos.lon/1e7 + 180) / 6 + 1;
utm_of_lla_f(&utm, &lla);
#else
utm.zone = gps.utm_pos.zone;
diff --git a/sw/airborne/subsystems/ins/ins_ardrone2.c b/sw/airborne/subsystems/ins/ins_ardrone2.c
index 625c146dae..eccffb71b8 100644
--- a/sw/airborne/subsystems/ins/ins_ardrone2.c
+++ b/sw/airborne/subsystems/ins/ins_ardrone2.c
@@ -46,8 +46,8 @@ struct InsArdrone2 ins_impl;
void ins_init() {
#if USE_INS_NAV_INIT
struct LlaCoor_i llh_nav0; /* Height above the ellipsoid */
- llh_nav0.lat = INT32_RAD_OF_DEG(NAV_LAT0);
- llh_nav0.lon = INT32_RAD_OF_DEG(NAV_LON0);
+ llh_nav0.lat = NAV_LAT0);
+ llh_nav0.lon = NAV_LON0;
/* NAV_ALT0 = ground alt above msl, NAV_MSL0 = geoid-height (msl) over ellipsoid */
llh_nav0.alt = NAV_ALT0 + NAV_MSL0;
diff --git a/sw/airborne/subsystems/ins/ins_float_invariant.c b/sw/airborne/subsystems/ins/ins_float_invariant.c
index 8256e3aa37..e1a3c027fa 100644
--- a/sw/airborne/subsystems/ins/ins_float_invariant.c
+++ b/sw/airborne/subsystems/ins/ins_float_invariant.c
@@ -236,8 +236,8 @@ void ins_init() {
stateSetPositionUtm_f(&utm0);
#else
struct LlaCoor_i llh_nav0; /* Height above the ellipsoid */
- llh_nav0.lat = INT32_RAD_OF_DEG(NAV_LAT0);
- llh_nav0.lon = INT32_RAD_OF_DEG(NAV_LON0);
+ llh_nav0.lat = NAV_LAT0;
+ llh_nav0.lon = NAV_LON0;
/* NAV_ALT0 = ground alt above msl, NAV_MSL0 = geoid-height (msl) over ellipsoid */
llh_nav0.alt = NAV_ALT0 + NAV_MSL0;
struct EcefCoor_i ecef_nav0;
@@ -284,9 +284,8 @@ void ins_reset_local_origin( void ) {
#ifdef GPS_USE_LATLONG
/* Recompute UTM coordinates in this zone */
struct LlaCoor_f lla;
- lla.lat = gps.lla_pos.lat / 1e7;
- lla.lon = gps.lla_pos.lon / 1e7;
- utm.zone = (DegOfRad(gps.lla_pos.lon/1e7)+180) / 6 + 1;
+ LLA_FLOAT_OF_BFP(lla, gps.lla_pos);
+ utm.zone = (gps.lla_pos.lon/1e7 + 180) / 6 + 1;
utm_of_lla_f(&utm, &lla);
#else
utm.zone = gps.utm_pos.zone;
diff --git a/sw/airborne/subsystems/ins/ins_gps_passthrough.c b/sw/airborne/subsystems/ins/ins_gps_passthrough.c
index a04b4dc1fc..c7384c286a 100644
--- a/sw/airborne/subsystems/ins/ins_gps_passthrough.c
+++ b/sw/airborne/subsystems/ins/ins_gps_passthrough.c
@@ -86,8 +86,8 @@ void ins_init(void) {
#if USE_INS_NAV_INIT
struct LlaCoor_i llh_nav0; /* Height above the ellipsoid */
- llh_nav0.lat = INT32_RAD_OF_DEG(NAV_LAT0);
- llh_nav0.lon = INT32_RAD_OF_DEG(NAV_LON0);
+ llh_nav0.lat = NAV_LAT0;
+ llh_nav0.lon = NAV_LON0;
/* NAV_ALT0 = ground alt above msl, NAV_MSL0 = geoid-height (msl) over ellipsoid */
llh_nav0.alt = NAV_ALT0 + NAV_MSL0;
diff --git a/sw/airborne/subsystems/ins/ins_gps_passthrough_utm.c b/sw/airborne/subsystems/ins/ins_gps_passthrough_utm.c
index cee3eeeafe..fdeae62121 100644
--- a/sw/airborne/subsystems/ins/ins_gps_passthrough_utm.c
+++ b/sw/airborne/subsystems/ins/ins_gps_passthrough_utm.c
@@ -48,9 +48,8 @@ void ins_reset_local_origin(void) {
#ifdef GPS_USE_LATLONG
/* Recompute UTM coordinates in this zone */
struct LlaCoor_f lla;
- lla.lat = gps.lla_pos.lat/1e7;
- lla.lon = gps.lla_pos.lon/1e7;
- utm.zone = (DegOfRad(gps.lla_pos.lon/1e7)+180) / 6 + 1;
+ LLA_FLOAT_OF_BFP(lla, gps.lla_pos);
+ utm.zone = (gps.lla_pos.lon/1e7 + 180) / 6 + 1;
utm_of_lla_f(&utm, &lla);
#else
utm.zone = gps.utm_pos.zone;
diff --git a/sw/airborne/subsystems/ins/ins_int.c b/sw/airborne/subsystems/ins/ins_int.c
index d2eafd4f1a..25d8fd3eef 100644
--- a/sw/airborne/subsystems/ins/ins_int.c
+++ b/sw/airborne/subsystems/ins/ins_int.c
@@ -359,8 +359,8 @@ static void sonar_cb(uint8_t __attribute__((unused)) sender_id, const float *dis
static void ins_init_origin_from_flightplan(void) {
struct LlaCoor_i llh_nav0; /* Height above the ellipsoid */
- llh_nav0.lat = INT32_RAD_OF_DEG(NAV_LAT0);
- llh_nav0.lon = INT32_RAD_OF_DEG(NAV_LON0);
+ llh_nav0.lat = NAV_LAT0;
+ llh_nav0.lon = NAV_LON0;
/* NAV_ALT0 = ground alt above msl, NAV_MSL0 = geoid-height (msl) over ellipsoid */
llh_nav0.alt = NAV_ALT0 + NAV_MSL0;
diff --git a/sw/ground_segment/misc/natnet2ivy.c b/sw/ground_segment/misc/natnet2ivy.c
index 96a8a442b6..d1e8095324 100644
--- a/sw/ground_segment/misc/natnet2ivy.c
+++ b/sw/ground_segment/misc/natnet2ivy.c
@@ -502,13 +502,13 @@ gboolean timeout_transmit_callback(gpointer data) {
(int)(ecef_pos.x*100.0), //int32 ECEF X in CM
(int)(ecef_pos.y*100.0), //int32 ECEF Y in CM
(int)(ecef_pos.z*100.0), //int32 ECEF Z in CM
- (int)(lla_pos.lat*10000000.0), //int32 LLA latitude in rad*1e7
- (int)(lla_pos.lon*10000000.0), //int32 LLA longitude in rad*1e7
+ (int)(DegOfRad(lla_pos.lat)*1e7), //int32 LLA latitude in deg*1e7
+ (int)(DegOfRad(lla_pos.lon)*1e7), //int32 LLA longitude in deg*1e7
(int)(rigidBodies[i].z*1000.0), //int32 LLA altitude in mm above elipsoid
(int)(rigidBodies[i].z*1000.0), //int32 HMSL height above mean sea level in mm
- (int)(rigidBodies[i].ecef_vel.x*100.0), //int32 ECEF velocity X in m/s
- (int)(rigidBodies[i].ecef_vel.y*100.0), //int32 ECEF velocity Y in m/s
- (int)(rigidBodies[i].ecef_vel.z*100.0), //int32 ECEF velocity Z in m/s
+ (int)(rigidBodies[i].ecef_vel.x*100.0), //int32 ECEF velocity X in cm/s
+ (int)(rigidBodies[i].ecef_vel.y*100.0), //int32 ECEF velocity Y in cm/s
+ (int)(rigidBodies[i].ecef_vel.z*100.0), //int32 ECEF velocity Z in cm/s
0,
(int)(heading*10000000.0)); //int32 Course in rad*1e7
diff --git a/sw/ground_segment/tmtc/fw_server.ml b/sw/ground_segment/tmtc/fw_server.ml
index 5c3a17fa83..b65be589ab 100644
--- a/sw/ground_segment/tmtc/fw_server.ml
+++ b/sw/ground_segment/tmtc/fw_server.ml
@@ -122,7 +122,7 @@ let log_and_parse = fun ac_name (a:Aircraft.aircraft) msg values ->
| "GPS_LLA" ->
let lat = ivalue "lat"
and lon = ivalue "lon" in
- let geo = make_geo (float lat /. 1e7) (float lon /. 1e7) in
+ let geo = make_geo_deg (float lat /. 1e7) (float lon /. 1e7) in
a.pos <- geo;
a.unix_time <- LL.unix_time_of_tow (truncate (fvalue "itow" /. 1000.));
a.itow <- Int32.of_float (fvalue "itow");
@@ -167,7 +167,7 @@ let log_and_parse = fun ac_name (a:Aircraft.aircraft) msg values ->
| "NAVIGATION_REF_LLA" ->
let lat = ivalue "lat"
and lon = ivalue "lon" in
- let geo = make_geo (float lat /. 1e7) (float lon /. 1e7) in
+ let geo = make_geo_deg (float lat /. 1e7) (float lon /. 1e7) in
a.nav_ref <- Some (Geo geo);
a.ground_alt <- fvalue "ground_alt";
if a.gps_mode = _3D then
@@ -317,7 +317,7 @@ let log_and_parse = fun ac_name (a:Aircraft.aircraft) msg values ->
let lat = ivalue "lat"
and lon = ivalue "lon"
and alt = ivalue "alt" in
- let geo = make_geo (float lat /. 1e7) (float lon /. 1e7) in
+ let geo = make_geo_deg (float lat /. 1e7) (float lon /. 1e7) in
update_waypoint a (ivalue "wp_id") geo (float alt /. 100.)
| "GENERIC_COM" ->
let flight_time = ivalue "flight_time" in
@@ -325,7 +325,7 @@ let log_and_parse = fun ac_name (a:Aircraft.aircraft) msg values ->
a.flight_time <- flight_time;
let lat = fvalue "lat"
and lon = fvalue "lon" in
- let geo = make_geo (lat /. 1e7) (lon /. 1e7) in
+ let geo = make_geo_deg (lat /. 1e7) (lon /. 1e7) in
a.pos <- geo;
a.alt <- fvalue "alt";
a.gspeed <- fvalue "gspeed" /. 100.;
diff --git a/sw/simulator/nps/nps_ivy_rotorcraft.c b/sw/simulator/nps/nps_ivy_rotorcraft.c
index d267fc57ed..fcb0b99964 100644
--- a/sw/simulator/nps/nps_ivy_rotorcraft.c
+++ b/sw/simulator/nps/nps_ivy_rotorcraft.c
@@ -44,8 +44,8 @@ static void on_DL_MOVE_WP(IvyClientPtr app __attribute__ ((unused)),
uint8_t wp_id = atoi(argv[1]);
struct LlaCoor_i lla;
- lla.lat = INT32_RAD_OF_DEG(atoi(argv[3]));
- lla.lon = INT32_RAD_OF_DEG(atoi(argv[4]));
+ lla.lat = atoi(argv[3]);
+ lla.lon = atoi(argv[4]);
/* WP_alt from message is alt above MSL in cm
* lla.alt is above ellipsoid in mm
*/