From b03dbfee57e9c40c4fb5eb4eb403ae49d47ebdd8 Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Thu, 21 Aug 2014 11:50:09 +0200 Subject: [PATCH] lat/lon int in 1e7 deg instead of rad so we don't loose resolution by converting and storing as radians --- conf/messages.xml | 28 +++++++++---------- sw/airborne/firmwares/rotorcraft/datalink.c | 4 +-- sw/airborne/math/pprz_geodetic_int.c | 24 ++++++++-------- sw/airborne/math/pprz_geodetic_int.h | 24 ++++++++-------- sw/airborne/modules/geo_mag/geo_mag.c | 4 +-- sw/airborne/modules/ins/ins_xsens.c | 14 ++++------ sw/airborne/state.h | 2 +- sw/airborne/subsystems/gps.h | 2 +- sw/airborne/subsystems/gps/gps_datalink.c | 7 ++--- sw/airborne/subsystems/gps/gps_mtk.c | 14 ++++------ sw/airborne/subsystems/gps/gps_nmea.c | 4 +-- sw/airborne/subsystems/gps/gps_sim_nps.c | 7 ++--- sw/airborne/subsystems/gps/gps_sirf.c | 8 ++++-- sw/airborne/subsystems/gps/gps_skytraq.c | 7 ++--- sw/airborne/subsystems/gps/gps_ubx.c | 7 ++--- sw/airborne/subsystems/gps/gps_udp.c | 7 ++--- sw/airborne/subsystems/ins.c | 2 +- sw/airborne/subsystems/ins/ins_alt_float.c | 5 ++-- sw/airborne/subsystems/ins/ins_ardrone2.c | 4 +-- .../subsystems/ins/ins_float_invariant.c | 9 +++--- .../subsystems/ins/ins_gps_passthrough.c | 4 +-- .../subsystems/ins/ins_gps_passthrough_utm.c | 5 ++-- sw/airborne/subsystems/ins/ins_int.c | 4 +-- sw/ground_segment/misc/natnet2ivy.c | 10 +++---- sw/ground_segment/tmtc/fw_server.ml | 8 +++--- sw/simulator/nps/nps_ivy_rotorcraft.c | 4 +-- 26 files changed, 104 insertions(+), 114 deletions(-) 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 */