Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-05-09 22:49:53 +08:00
Code Issues Actions 27 Packages Projects Releases Wiki Activity

lat/lon int in 1e7 deg instead of rad

Browse Source 
so we don't loose resolution by converting and storing as radians
This commit is contained in:
Felix Ruess
2014-08-21 11:50:09 +02:00
parent 231e47a545
commit b03dbfee57
26 changed files with 104 additions and 114 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/messages.xml
+14 -14
View File
@@ -365,8 +365,8 @@
<message name="WP_MOVED_LLA" id="47">
<field name="wp_id" type="uint8"/>
<field name="lat" type="int32" unit="deg"/>
<field name="lon" type="int32" unit="deg"/>
<field name="lat" type="int32" unit="1e7deg" alt_unit="deg" alt_unit_coef="0.0000001"/>
<field name="lon" type="int32" unit="1e7deg" alt_unit="deg" alt_unit_coef="0.0000001"/>
<field name="alt" type="int32" unit="cm"/>
</message>
@@ -469,8 +469,8 @@
</message>
<message name="GPS_LLA" id="59">
<field name="lat" type="int32" alt_unit="deg" alt_unit_coef="0.0000057296"/>
<field name="lon" type="int32" alt_unit="deg" alt_unit_coef="0.0000057296"/>
<field name="lat" type="int32" unit="1e7deg" alt_unit="deg" alt_unit_coef="0.0000001"/>
<field name="lon" type="int32" unit="1e7deg" alt_unit="deg" alt_unit_coef="0.0000001"/>
<field name="alt" type="int32" unit="mm" alt_unit="m"/>
<field name="course" type="int16" unit="decideg" alt_unit="deg"/>
<field name="speed" type="uint16" unit="cm/s" alt_unit="m/s"/>
@@ -692,8 +692,8 @@
</message>
<message name="GENERIC_COM" id="81">
<field name="lat" type="int32" unit="1e7deg"/>
<field name="lon" type="int32" unit="1e7deg"/>
<field name="lat" type="int32" unit="1e7deg" alt_unit="deg" alt_unit_coef="0.0000001"/>
<field name="lon" type="int32" unit="1e7deg" alt_unit="deg" alt_unit_coef="0.0000001"/>
<field name="alt" type="int16" unit="m"/>
<field name="gspeed" type="uint16" unit="cm/s" alt_unit="m/s"/>
<field name="course" type="int16" unit="decideg" alt_unit="deg"/>
@@ -1364,8 +1364,8 @@
<field name="ecef_x0" type="int32" alt_unit="m" alt_unit_coef="0.01"/>
<field name="ecef_y0" type="int32" alt_unit="m" alt_unit_coef="0.01"/>
<field name="ecef_z0" type="int32" alt_unit="m" alt_unit_coef="0.01"/>
<field name="lat0" type="int32" alt_unit="deg" alt_unit_coef="0.0000057296"/>
<field name="lon0" type="int32" alt_unit="deg" alt_unit_coef="0.0000057296"/>
<field name="lat0" type="int32" unit="1e7deg" alt_unit="deg" alt_unit_coef="0.0000001"/>
<field name="lon0" type="int32" unit="1e7deg" alt_unit="deg" alt_unit_coef="0.0000001"/>
<field name="alt0" type="int32" alt_unit="m" alt_unit_coef="0.001"/>
<field name="hmsl0" type="int32" alt_unit="m" alt_unit_coef="0.001"/>
<field name="baro_qfe" type="float" unit="pascal"/>
@@ -1375,8 +1375,8 @@
<field name="ecef_x" type="int32" unit="cm" alt_unit="m"/>
<field name="ecef_y" type="int32" unit="cm" alt_unit="m"/>
<field name="ecef_z" type="int32" unit="cm" alt_unit="m"/>
<field name="lat" type="int32" alt_unit="deg" alt_unit_coef="0.0000057296"/>
<field name="lon" type="int32" alt_unit="deg" alt_unit_coef="0.0000057296"/>
<field name="lat" type="int32" unit="1e7deg" alt_unit="deg" alt_unit_coef="0.0000001"/>
<field name="lon" type="int32" unit="1e7deg" alt_unit="deg" alt_unit_coef="0.0000001"/>
<field name="alt" type="int32" unit="mm" alt_unit="m"/>
<field name="hmsl" type="int32" unit="mm" alt_unit="m"/>
<field name="ecef_xd" type="int32" unit="cm/s" alt_unit="m/s"/>
@@ -2204,8 +2204,8 @@
<message name="MOVE_WP" id="2" link="forwarded">
<field name="wp_id" type="uint8"/>
<field name="ac_id" type="uint8"/>
<field name="lat" type="int32" unit="e-7deg"/>
<field name="lon" type="int32" unit="e-7deg"/>
<field name="lat" type="int32" unit="1e7deg" alt_unit="deg" alt_unit_coef="0.0000001"/>
<field name="lon" type="int32" unit="1e7deg" alt_unit="deg" alt_unit_coef="0.0000001"/>
<field name="alt" type="int32" unit="cm"/>
</message>
@@ -2448,8 +2448,8 @@
<field name="ecef_x" type="int32" unit="cm" alt_unit="m"/>
<field name="ecef_y" type="int32" unit="cm" alt_unit="m"/>
<field name="ecef_z" type="int32" unit="cm" alt_unit="m"/>
<field name="lat" type="int32" alt_unit="deg" alt_unit_coef="0.0000057296"/>
<field name="lon" type="int32" alt_unit="deg" alt_unit_coef="0.0000057296"/>
<field name="lat" type="int32" unit="1e7deg" alt_unit="deg" alt_unit_coef="0.0000001"/>
<field name="lon" type="int32" unit="1e7deg" alt_unit="deg" alt_unit_coef="0.0000001"/>
<field name="alt" type="int32" unit="mm" alt_unit="m"/>
<field name="hmsl" type="int32" unit="mm" alt_unit="m"/>
<field name="ecef_xd" type="int32" unit="cm/s" alt_unit="m/s"/>
sw/airborne/firmwares/rotorcraft/datalink.c
+2 -2
View File
@@ -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
*/
sw/airborne/math/pprz_geodetic_int.c
+12 -12
View File
@@ -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;
sw/airborne/math/pprz_geodetic_int.h
+12 -12
View File
@@ -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); \
sw/airborne/modules/geo_mag/geo_mag.c
+2 -2
View File
@@ -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
sw/airborne/modules/ins/ins_xsens.c
+6 -8
View File
@@ -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);
sw/airborne/state.h
+1 -1
View File
@@ -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;
sw/airborne/subsystems/gps.h
+1 -1
View File
@@ -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
sw/airborne/subsystems/gps/gps_datalink.c
+3 -4
View File
@@ -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
sw/airborne/subsystems/gps/gps_mtk.c
+6 -8
View File
@@ -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 */
sw/airborne/subsystems/gps/gps_nmea.c
+2 -2
View File
@@ -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);
sw/airborne/subsystems/gps/gps_sim_nps.c
+3 -4
View File
@@ -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 */
sw/airborne/subsystems/gps/gps_sirf.c
+5 -3
View File
@@ -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;
sw/airborne/subsystems/gps/gps_skytraq.c
+3 -4
View File
@@ -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 */
sw/airborne/subsystems/gps/gps_ubx.c
+3 -4
View File
@@ -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 */
sw/airborne/subsystems/gps/gps_udp.c
+3 -4
View File
@@ -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
sw/airborne/subsystems/ins.c
+1 -1
View File
@@ -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
sw/airborne/subsystems/ins/ins_alt_float.c
+2 -3
View File
@@ -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;
sw/airborne/subsystems/ins/ins_ardrone2.c
+2 -2
View File
@@ -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;
sw/airborne/subsystems/ins/ins_float_invariant.c
+4 -5
View File
@@ -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;
sw/airborne/subsystems/ins/ins_gps_passthrough.c
+2 -2
View File
@@ -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;
sw/airborne/subsystems/ins/ins_gps_passthrough_utm.c
+2 -3
View File
@@ -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;
sw/airborne/subsystems/ins/ins_int.c
+2 -2
View File
@@ -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;
sw/ground_segment/misc/natnet2ivy.c
+5 -5
View File
@@ -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
sw/ground_segment/tmtc/fw_server.ml
+4 -4
View File
@@ -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.;
sw/simulator/nps/nps_ivy_rotorcraft.c
+2 -2
View File
@@ -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
*/