Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-05-10 06:59:54 +08:00
Code Issues Actions 26 Packages Projects Releases Wiki Activity

[gps] add fields_valid and remove GPS_USE_LATLONG

Browse Source 
add fields_valid bitfiled to gps struct and use that to decide whether a valid UTM pos is availabe instead of always computing UTM if GPS_USE_LATLONG is defined

should close #641
This commit is contained in:
Felix Ruess
2015-12-13 00:33:21 +01:00
parent a8d581f680
commit 29c4174be7
40 changed files with 205 additions and 432 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/conf_tests.xml
+1 -1
View File
@@ -161,7 +161,7 @@
telemetry="telemetry/default_fixedwing.xml"
flight_plan="flight_plans/basic.xml"
settings="settings/fixedwing_basic.xml settings/control/ctl_basic.xml"
settings_modules="modules/nav_smooth.xml modules/nav_survey_poly_osam.xml modules/infrared_adc.xml modules/tune_airspeed.xml"
settings_modules="modules/tune_airspeed.xml modules/infrared_adc.xml modules/nav_smooth.xml modules/nav_survey_poly_osam.xml"
gui_color="#6293ba"
/>
<aircraft
conf/firmwares/subsystems/fixedwing/gps_mediatek_diy.makefile
+3 -4
View File
@@ -5,7 +5,7 @@
GPS_LED ?= none
MTK_GPS_PORT_LOWER=$(shell echo $(GPS_PORT) | tr A-Z a-z)
ap.CFLAGS += -DUSE_GPS -DGPS_CONFIGURE -DGPS_USE_LATLONG
ap.CFLAGS += -DUSE_GPS -DGPS_CONFIGURE
ap.CFLAGS += -DGPS_LINK=$(MTK_GPS_PORT_LOWER)
ap.CFLAGS += -DUSE_$(GPS_PORT)
ap.CFLAGS += -D$(GPS_PORT)_BAUD=$(GPS_BAUD)
@@ -19,11 +19,10 @@ ap.srcs += $(SRC_SUBSYSTEMS)/gps/gps_mtk.c
$(TARGET).srcs += $(SRC_SUBSYSTEMS)/gps.c
sim.CFLAGS += -DUSE_GPS -DGPS_USE_LATLONG
sim.CFLAGS += -DUSE_GPS
sim.CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_sim.h\"
sim.srcs += $(SRC_SUBSYSTEMS)/gps/gps_sim.c
nps.CFLAGS += -DUSE_GPS -DGPS_USE_LATLONG
nps.srcs += $(SRC_SUBSYSTEMS)/gps.c
nps.CFLAGS += -DUSE_GPS
nps.CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_sim_nps.h\"
nps.srcs += $(SRC_SUBSYSTEMS)/gps/gps_sim_nps.c
conf/firmwares/subsystems/fixedwing/gps_piksi.makefile
+3 -3
View File
@@ -6,7 +6,7 @@ GPS_LED ?= none
GPS_PORT_LOWER=$(shell echo $(GPS_PORT) | tr A-Z a-z)
ap.CFLAGS += -DUSE_GPS -DGPS_USE_LATLONG
ap.CFLAGS += -DUSE_GPS
ap.CFLAGS += -DUSE_$(GPS_PORT) -D$(GPS_PORT)_BAUD=B115200
ap.CFLAGS += -DGPS_LINK=$(GPS_PORT_LOWER)
@@ -23,11 +23,11 @@ ap.srcs += $(SRC_SUBSYSTEMS)/gps/gps_piksi.c
ap.CFLAGS += -I$(PAPARAZZI_SRC)/sw/ext/libsbp/c/include
ap.srcs += $(PAPARAZZI_SRC)/sw/ext/libsbp/c/src/sbp.c $(PAPARAZZI_SRC)/sw/ext/libsbp/c/src/edc.c
sim.CFLAGS += -DUSE_GPS -DGPS_USE_LATLONG
sim.CFLAGS += -DUSE_GPS
sim.CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_sim.h\"
sim.srcs += $(SRC_SUBSYSTEMS)/gps/gps_sim.c
nps.CFLAGS += -DUSE_GPS -DGPS_USE_LATLONG
nps.CFLAGS += -DUSE_GPS
nps.CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_sim_nps.h\"
nps.srcs += $(SRC_SUBSYSTEMS)/gps/gps_sim_nps.c
conf/firmwares/subsystems/fixedwing/gps_ublox_hitl.makefile
+1 -1
View File
@@ -3,6 +3,6 @@
# UBlox Hardware In The Loop
ap.CFLAGS += -DUSE_GPS -DUBX -DGPS_USE_LATLONG
ap.CFLAGS += -DUSE_GPS -DUBX
ap.CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_ubx.h\"
ap.srcs += $(SRC_SUBSYSTEMS)/gps/gps_ubx.c $(SRC_SUBSYSTEMS)/gps.c
conf/firmwares/subsystems/fixedwing/gps_ublox_utm.makefile
+1 -1
View File
@@ -21,7 +21,7 @@ $(TARGET).srcs += $(SRC_SUBSYSTEMS)/gps.c
sim.CFLAGS += -DUSE_GPS -DGPS_TYPE_H=\"subsystems/gps/gps_sim.h\"
sim.srcs += $(SRC_SUBSYSTEMS)/gps/gps_sim.c
nps.CFLAGS += -DUSE_GPS -DGPS_USE_LATLONG
nps.CFLAGS += -DUSE_GPS
nps.srcs += $(SRC_SUBSYSTEMS)/gps.c
nps.CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_sim_nps.h\"
nps.srcs += $(SRC_SUBSYSTEMS)/gps/gps_sim_nps.c
conf/firmwares/subsystems/fixedwing/ins_vectornav.makefile
+2 -4
View File
@@ -4,8 +4,6 @@
include $(CFG_SHARED)/ins_vectornav.makefile
ap.CFLAGS += -DGPS_USE_LATLONG
#########################################
## Simulator
@@ -20,7 +18,7 @@ sim.srcs += $(SRC_SUBSYSTEMS)/ins.c
sim.CFLAGS += -DINS_TYPE_H=\"subsystems/ins/ins_gps_passthrough_utm.h\"
sim.srcs += $(SRC_SUBSYSTEMS)/ins/ins_gps_passthrough_utm.c
sim.CFLAGS += -DUSE_GPS -DGPS_USE_LATLONG
sim.CFLAGS += -DUSE_GPS
sim.CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_sim.h\"
sim.srcs += $(SRC_SUBSYSTEMS)/gps/gps_sim.c
sim.srcs += $(SRC_SUBSYSTEMS)/gps.c
@@ -32,7 +30,7 @@ sim.srcs += $(SRC_SUBSYSTEMS)/gps.c
nps.CFLAGS += -DIMU_TYPE_H=\"imu/imu_nps.h\" -DUSE_IMU
nps.srcs += $(SRC_SUBSYSTEMS)/imu.c $(SRC_SUBSYSTEMS)/imu/imu_nps.c
nps.CFLAGS += -DUSE_GPS -DGPS_USE_LATLONG
nps.CFLAGS += -DUSE_GPS
nps.srcs += $(SRC_SUBSYSTEMS)/gps.c
nps.CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_sim_nps.h\"
nps.srcs += $(SRC_SUBSYSTEMS)/gps/gps_sim_nps.c
conf/firmwares/subsystems/fixedwing/ins_xsens.makefile
+2 -2
View File
@@ -40,7 +40,7 @@ ap.CFLAGS += -DAHRS_TRIGGERED_ATTITUDE_LOOP
ap.CFLAGS += -DUSE_GPS_XSENS
ap.CFLAGS += -DUSE_GPS_XSENS_RAW_DATA
ap.CFLAGS += -DGPS_NB_CHANNELS=16
ap.CFLAGS += -DUSE_GPS -DGPS_USE_LATLONG
ap.CFLAGS += -DUSE_GPS
ap.CFLAGS += -DGPS_TYPE_H=\"modules/ins/ins_xsens.h\"
ap.srcs += $(SRC_SUBSYSTEMS)/gps.c
@@ -61,7 +61,7 @@ $(TARGET).srcs += $(SRC_SUBSYSTEMS)/ins.c
$(TARGET).CFLAGS += -DINS_TYPE_H=\"subsystems/ins/ins_gps_passthrough_utm.h\"
$(TARGET).srcs += $(SRC_SUBSYSTEMS)/ins/ins_gps_passthrough_utm.c
$(TARGET).CFLAGS += -DUSE_GPS -DGPS_USE_LATLONG
$(TARGET).CFLAGS += -DUSE_GPS
$(TARGET).CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_sim.h\"
$(TARGET).srcs += $(SRC_SUBSYSTEMS)/gps/gps_sim.c
$(TARGET).srcs += $(SRC_SUBSYSTEMS)/gps.c
conf/firmwares/subsystems/rotorcraft/gps_furuno.makefile
-25
View File
@@ -1,25 +0,0 @@
# Hey Emacs, this is a -*- makefile -*-
# Furuno NMEA GPS unit
FURUNO_GPS_PORT_LOWER=$(shell echo $(GPS_PORT) | tr A-Z a-z)
ap.CFLAGS += -DUSE_GPS
ap.CFLAGS += -DGPS_LINK=$(FURUNO_GPS_PORT_LOWER)
ap.CFLAGS += -DUSE_$(GPS_PORT)
ap.CFLAGS += -D$(GPS_PORT)_BAUD=$(GPS_BAUD)
ap.CFLAGS += -DNMEA_PARSE_PROP
ifneq ($(GPS_LED),none)
ap.CFLAGS += -DGPS_LED=$(GPS_LED)
endif
ap.CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_nmea.h\"
ap.srcs += $(SRC_SUBSYSTEMS)/gps/gps_nmea.c $(SRC_SUBSYSTEMS)/gps/gps_furuno.c
$(TARGET).srcs += $(SRC_SUBSYSTEMS)/gps.c
nps.CFLAGS += -DUSE_GPS
nps.CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_sim_nps.h\"
nps.srcs += $(SRC_SUBSYSTEMS)/gps/gps_sim_nps.c
conf/firmwares/subsystems/rotorcraft/gps_nmea.makefile
-24
View File
@@ -1,24 +0,0 @@
# Hey Emacs, this is a -*- makefile -*-
# NMEA GPS unit
NMEA_GPS_PORT_LOWER=$(shell echo $(GPS_PORT) | tr A-Z a-z)
ap.CFLAGS += -DUSE_GPS
ap.CFLAGS += -DGPS_LINK=$(NMEA_GPS_PORT_LOWER)
ap.CFLAGS += -DUSE_$(GPS_PORT)
ap.CFLAGS += -D$(GPS_PORT)_BAUD=$(GPS_BAUD)
ifneq ($(GPS_LED),none)
ap.CFLAGS += -DGPS_LED=$(GPS_LED)
endif
ap.CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_nmea.h\"
ap.srcs += $(SRC_SUBSYSTEMS)/gps/gps_nmea.c
$(TARGET).srcs += $(SRC_SUBSYSTEMS)/gps.c
nps.CFLAGS += -DUSE_GPS
nps.CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_sim.h\"
nps.srcs += $(SRC_SUBSYSTEMS)/gps/gps_sim_nps.c
conf/firmwares/subsystems/rotorcraft/gps_skytraq.makefile
-21
View File
@@ -1,21 +0,0 @@
# Hey Emacs, this is a -*- makefile -*-
GPS_LED ?= none
SKYTRAQ_GPS_PORT_LOWER=$(shell echo $(GPS_PORT) | tr A-Z a-z)
ap.CFLAGS += -DUSE_GPS
ap.CFLAGS += -DGPS_LINK=$(SKYTRAQ_GPS_PORT_LOWER)
ap.CFLAGS += -DUSE_$(GPS_PORT)
ap.CFLAGS += -D$(GPS_PORT)_BAUD=$(GPS_BAUD)
ifneq ($(GPS_LED),none)
ap.CFLAGS += -DGPS_LED=$(GPS_LED)
endif
ap.srcs += $(SRC_SUBSYSTEMS)/gps.c
ap.CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_skytraq.h\"
ap.srcs += $(SRC_SUBSYSTEMS)/gps/gps_skytraq.c
nps.CFLAGS += -DUSE_GPS
nps.srcs += $(SRC_SUBSYSTEMS)/gps.c
conf/firmwares/subsystems/rotorcraft/gps_ublox.makefile
-20
View File
@@ -1,20 +0,0 @@
# Hey Emacs, this is a -*- makefile -*-
GPS_LED ?= none
UBX_GPS_PORT_LOWER=$(shell echo $(GPS_PORT) | tr A-Z a-z)
ap.srcs += $(SRC_SUBSYSTEMS)/gps.c
ap.CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_ubx.h\"
ap.srcs += $(SRC_SUBSYSTEMS)/gps/gps_ubx.c
ap.CFLAGS += -DUSE_$(GPS_PORT) -D$(GPS_PORT)_BAUD=$(GPS_BAUD)
ap.CFLAGS += -DUSE_GPS -DGPS_LINK=$(UBX_GPS_PORT_LOWER)
ifneq ($(GPS_LED),none)
ap.CFLAGS += -DGPS_LED=$(GPS_LED)
endif
nps.CFLAGS += -DUSE_GPS
nps.srcs += $(SRC_SUBSYSTEMS)/gps.c
nps.CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_sim_nps.h\"
nps.srcs += $(SRC_SUBSYSTEMS)/gps/gps_sim_nps.c
conf/firmwares/subsystems/fixedwing/gps_furuno.makefile → conf/firmwares/subsystems/shared/gps_furuno.makefile
+3 -3
View File
@@ -5,7 +5,7 @@
GPS_LED ?= none
FURUNO_GPS_PORT_LOWER=$(shell echo $(GPS_PORT) | tr A-Z a-z)
ap.CFLAGS += -DUSE_GPS -DGPS_USE_LATLONG
ap.CFLAGS += -DUSE_GPS
ap.CFLAGS += -DGPS_LINK=$(FURUNO_GPS_PORT_LOWER)
ap.CFLAGS += -DUSE_$(GPS_PORT)
ap.CFLAGS += -D$(GPS_PORT)_BAUD=$(GPS_BAUD)
@@ -20,11 +20,11 @@ ap.srcs += $(SRC_SUBSYSTEMS)/gps/gps_nmea.c $(SRC_SUBSYSTEMS)/gps/gps_furuno.c
$(TARGET).srcs += $(SRC_SUBSYSTEMS)/gps.c
sim.CFLAGS += -DUSE_GPS -DGPS_USE_LATLONG
sim.CFLAGS += -DUSE_GPS
sim.CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_sim.h\"
sim.srcs += $(SRC_SUBSYSTEMS)/gps/gps_sim.c
nps.CFLAGS += -DUSE_GPS -DGPS_USE_LATLONG
nps.CFLAGS += -DUSE_GPS
nps.srcs += $(SRC_SUBSYSTEMS)/gps.c
nps.CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_sim_nps.h\"
nps.srcs += $(SRC_SUBSYSTEMS)/gps/gps_sim_nps.c
conf/firmwares/subsystems/fixedwing/gps_nmea.makefile → conf/firmwares/subsystems/shared/gps_nmea.makefile
+3 -4
View File
@@ -5,7 +5,7 @@
GPS_LED ?= none
NMEA_GPS_PORT_LOWER=$(shell echo $(GPS_PORT) | tr A-Z a-z)
ap.CFLAGS += -DUSE_GPS -DGPS_USE_LATLONG
ap.CFLAGS += -DUSE_GPS
ap.CFLAGS += -DGPS_LINK=$(NMEA_GPS_PORT_LOWER)
ap.CFLAGS += -DUSE_$(GPS_PORT)
ap.CFLAGS += -D$(GPS_PORT)_BAUD=$(GPS_BAUD)
@@ -19,11 +19,10 @@ ap.srcs += $(SRC_SUBSYSTEMS)/gps/gps_nmea.c
$(TARGET).srcs += $(SRC_SUBSYSTEMS)/gps.c
sim.CFLAGS += -DUSE_GPS -DGPS_USE_LATLONG
sim.CFLAGS += -DUSE_GPS
sim.CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_sim.h\"
sim.srcs += $(SRC_SUBSYSTEMS)/gps/gps_sim.c
nps.CFLAGS += -DUSE_GPS -DGPS_USE_LATLONG
nps.srcs += $(SRC_SUBSYSTEMS)/gps.c
nps.CFLAGS += -DUSE_GPS
nps.CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_sim_nps.h\"
nps.srcs += $(SRC_SUBSYSTEMS)/gps/gps_sim_nps.c
conf/firmwares/subsystems/fixedwing/gps_skytraq.makefile → conf/firmwares/subsystems/shared/gps_skytraq.makefile
+3 -3
View File
@@ -3,7 +3,7 @@
GPS_LED ?= none
SKYTRAQ_GPS_PORT_LOWER=$(shell echo $(GPS_PORT) | tr A-Z a-z)
ap.CFLAGS += -DUSE_GPS -DGPS_USE_LATLONG
ap.CFLAGS += -DUSE_GPS
ap.CFLAGS += -DGPS_LINK=$(SKYTRAQ_GPS_PORT_LOWER)
ap.CFLAGS += -DUSE_$(GPS_PORT)
ap.CFLAGS += -D$(GPS_PORT)_BAUD=$(GPS_BAUD)
@@ -17,11 +17,11 @@ ap.srcs += $(SRC_SUBSYSTEMS)/gps/gps_skytraq.c
$(TARGET).srcs += $(SRC_SUBSYSTEMS)/gps.c
sim.CFLAGS += -DUSE_GPS -DGPS_USE_LATLONG
sim.CFLAGS += -DUSE_GPS
sim.CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_sim.h\"
sim.srcs += $(SRC_SUBSYSTEMS)/gps/gps_sim.c
nps.CFLAGS += -DUSE_GPS -DGPS_USE_LATLONG
nps.CFLAGS += -DUSE_GPS
nps.srcs += $(SRC_SUBSYSTEMS)/gps.c
nps.CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_sim_nps.h\"
nps.srcs += $(SRC_SUBSYSTEMS)/gps/gps_sim_nps.c
conf/firmwares/subsystems/fixedwing/gps_ublox.makefile → conf/firmwares/subsystems/shared/gps_ublox.makefile
+3 -4
View File
@@ -4,7 +4,7 @@
GPS_LED ?= none
UBX_GPS_PORT_LOWER=$(shell echo $(GPS_PORT) | tr A-Z a-z)
ap.CFLAGS += -DUSE_GPS -DUBX -DGPS_USE_LATLONG
ap.CFLAGS += -DUSE_GPS -DUBX
ap.CFLAGS += -DGPS_LINK=$(UBX_GPS_PORT_LOWER)
ap.CFLAGS += -DUSE_$(GPS_PORT)
ap.CFLAGS += -D$(GPS_PORT)_BAUD=$(GPS_BAUD)
@@ -18,11 +18,10 @@ ap.srcs += $(SRC_SUBSYSTEMS)/gps/gps_ubx.c
$(TARGET).srcs += $(SRC_SUBSYSTEMS)/gps.c
sim.CFLAGS += -DUSE_GPS -DGPS_USE_LATLONG
sim.CFLAGS += -DUSE_GPS
sim.CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_sim.h\"
sim.srcs += $(SRC_SUBSYSTEMS)/gps/gps_sim.c
nps.CFLAGS += -DUSE_GPS -DGPS_USE_LATLONG
nps.srcs += $(SRC_SUBSYSTEMS)/gps.c
nps.CFLAGS += -DUSE_GPS
nps.CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_sim_nps.h\"
nps.srcs += $(SRC_SUBSYSTEMS)/gps/gps_sim_nps.c
conf/firmwares/subsystems/fixedwing/ins_xsens700.makefile → conf/firmwares/subsystems/shared/ins_xsens700.makefile
+1 -1
View File
@@ -55,7 +55,7 @@ $(TARGET).srcs += $(SRC_SUBSYSTEMS)/ins.c
$(TARGET).CFLAGS += -DINS_TYPE_H=\"subsystems/ins/ins_gps_passthrough_utm.h\"
$(TARGET).srcs += $(SRC_SUBSYSTEMS)/ins/ins_gps_passthrough_utm.c
$(TARGET).CFLAGS += -DUSE_GPS -DGPS_USE_LATLONG
$(TARGET).CFLAGS += -DUSE_GPS
$(TARGET).CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_sim.h\"
$(TARGET).srcs += $(SRC_SUBSYSTEMS)/gps/gps_sim.c
$(TARGET).srcs += $(SRC_SUBSYSTEMS)/gps.c
conf/modules/gps_ublox.xml
-3
View File
@@ -21,8 +21,6 @@
<file name="gps.c" dir="subsystems"/>
<file name="gps_ubx.c" dir="subsystems/gps"/>
<define name="USE_GPS"/>
<!-- only for fixedwings -->
<!--define name="GPS_USE_LATLONG"/-->
<raw>
ap.CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_ubx.h\"
ap.CFLAGS += -DUSE_$(GPS_PORT) -D$(GPS_PORT)_BAUD=$(GPS_BAUD)
@@ -44,7 +42,6 @@
<makefile target="sim">
<file name="gps_sim.c" dir="subsystems/gps"/>
<define name="USE_GPS"/>
<define name="GPS_USE_LATLONG"/>
<raw>
sim.CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_sim.h\"
jsbsim.CFLAGS += -DGPS_TYPE_H=\"subsystems/gps/gps_sim.h\"
sw/airborne/arch/sim/sim_gps.c
+7 -16
View File
@@ -22,38 +22,29 @@ value sim_use_gps_pos(value x, value y, value z, value c, value a, value s, valu
{
gps.fix = (Bool_val(m) ? 3 : 0);
gps.course = Double_val(c) * 1e7;
SetBit(gps.valid_fields, GPS_VALID_COURSE_BIT);
gps.hmsl = Double_val(a) * 1000.;
SetBit(gps.valid_fields, GPS_VALID_HMSL_BIT);
gps.gspeed = Double_val(s) * 100.;
gps.ned_vel.x = gps.gspeed * cos(Double_val(c));
gps.ned_vel.y = gps.gspeed * sin(Double_val(c));
gps.ned_vel.z = -Double_val(cl) * 100.;
SetBit(gps.valid_fields, GPS_VALID_VEL_NED_BIT);
gps.week = 0; // FIXME
gps.tow = Double_val(t) * 1000.;
//TODO set alt above ellipsoid and hmsl
#ifdef GPS_USE_LATLONG
struct LlaCoor_f lla_f;
struct UtmCoor_f utm_f;
lla_f.lat = Double_val(lat);
lla_f.lon = Double_val(lon);
//TODO set alt above ellipsoid, use hmsl for now
lla_f.alt = Double_val(a);
utm_f.zone = nav_utm_zone0;
utm_of_lla_f(&utm_f, &lla_f);
LLA_BFP_OF_REAL(gps.lla_pos, lla_f);
gps.utm_pos.east = utm_f.east * 100;
gps.utm_pos.north = utm_f.north * 100;
gps.utm_pos.zone = nav_utm_zone0;
x = y = z; /* Just to get rid of the "unused arg" warning */
y = x; /* Just to get rid of the "unused arg" warning */
#else // GPS_USE_LATLONG
SetBit(gps.valid_fields, GPS_VALID_POS_UTM_BIT);
gps.utm_pos.east = Int_val(x);
gps.utm_pos.north = Int_val(y);
gps.utm_pos.zone = Int_val(z);
lat = lon; /* Just to get rid of the "unused arg" warning */
lon = lat; /* Just to get rid of the "unused arg" warning */
#endif // GPS_USE_LATLONG
SetBit(gps.valid_fields, GPS_VALID_POS_UTM_BIT);
/** Space vehicle info simulation */
gps.nb_channels = 7;
sw/airborne/modules/cam_control/point.c
+4 -4
View File
@@ -79,6 +79,7 @@
#include "subsystems/navigation/common_nav.h"
#include "subsystems/gps.h"
#include "math/pprz_geodetic_float.h"
#include "state.h"
typedef struct {
float fx;
@@ -363,10 +364,9 @@ void vPoint(float fPlaneEast, float fPlaneNorth, float fPlaneAltitude,
cam_point_distance_from_home = distance_correction * (uint16_t)(sqrt((cam_point_x * cam_point_x) +
(cam_point_y * cam_point_y)));
struct UtmCoor_f utm;
utm.east = gps.utm_pos.east / 100. + sv_cam_projection.fx;
utm.north = gps.utm_pos.north / 100. + sv_cam_projection.fy;
utm.zone = gps.utm_pos.zone;
struct UtmCoor_f utm = *stateGetPositionUtm_f();
utm.east += sv_cam_projection.fx;
utm.north += sv_cam_projection.fy;
struct LlaCoor_f lla;
lla_of_utm_f(&lla, &utm);
cam_point_lon = lla.lon * (180 / M_PI);
sw/airborne/modules/ins/ins_xsens.c
+8
View File
@@ -469,6 +469,7 @@ void handle_ins_msg(void)
float fcourse = atan2f((float)ins_vy, (float)ins_vx);
gps.course = fcourse * 1e7;
SetBit(gps.valid_fields, GPS_VALID_COURSE_BIT);
#endif // USE_GPS_XSENS
}
@@ -534,6 +535,7 @@ void parse_ins_msg(void)
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);
SetBit(gps.valid_fields, GPS_VALID_POS_LLA_BIT);
/* Set the real UTM zone */
gps.utm_pos.zone = (gps.lla_pos.lon / 1e7 + 180) / 6 + 1;
@@ -545,6 +547,7 @@ void parse_ins_msg(void)
gps.utm_pos.east = utm_f.east * 100;
gps.utm_pos.north = utm_f.north * 100;
gps.utm_pos.alt = gps.lla_pos.alt;
SetBit(gps.valid_fields, GPS_VALID_POS_UTM_BIT);
ins_x = utm_f.east;
ins_y = utm_f.north;
@@ -554,6 +557,7 @@ void parse_ins_msg(void)
// Compute geoid (MSL) height
float hmsl = wgs84_ellipsoid_to_geoid(lla_f.lat, lla_f.lon);
gps.hmsl = XSENS_DATA_RAWGPS_alt(xsens_msg_buf, offset) - (hmsl * 1000.0f);
SetBit(gps.valid_fields, GPS_VALID_HMSL_BIT);
ins_vx = ((INS_FORMAT)XSENS_DATA_RAWGPS_vel_n(xsens_msg_buf, offset)) / 100.;
ins_vy = ((INS_FORMAT)XSENS_DATA_RAWGPS_vel_e(xsens_msg_buf, offset)) / 100.;
@@ -561,6 +565,7 @@ void parse_ins_msg(void)
gps.ned_vel.x = XSENS_DATA_RAWGPS_vel_n(xsens_msg_buf, offset);
gps.ned_vel.y = XSENS_DATA_RAWGPS_vel_e(xsens_msg_buf, offset);
gps.ned_vel.z = XSENS_DATA_RAWGPS_vel_d(xsens_msg_buf, offset);
SetBit(gps.valid_fields, GPS_VALID_VEL_NED_BIT);
gps.pacc = XSENS_DATA_RAWGPS_hacc(xsens_msg_buf, offset) / 100;
gps.sacc = XSENS_DATA_RAWGPS_sacc(xsens_msg_buf, offset) / 100;
gps.pdop = 5; // FIXME Not output by XSens
@@ -647,16 +652,19 @@ void parse_ins_msg(void)
lla_f.lon = RadOfDeg(XSENS_DATA_Position_lon(xsens_msg_buf, offset));
gps.lla_pos.lat = (int32_t)(DegOfRad(lla_f.lat) * 1e7);
gps.lla_pos.lon = (int32_t)(DegOfRad(lla_f.lon) * 1e7);
SetBit(gps.valid_fields, GPS_VALID_POS_LLA_BIT);
gps.utm_pos.zone = (DegOfRad(lla_f.lon) + 180) / 6 + 1;
/* convert to utm */
utm_of_lla_f(&utm_f, &lla_f);
/* copy results of utm conversion */
gps.utm_pos.east = utm_f.east * 100;
gps.utm_pos.north = utm_f.north * 100;
SetBit(gps.valid_fields, GPS_VALID_POS_UTM_BIT);
ins_x = utm_f.east;
ins_y = utm_f.north;
ins_z = XSENS_DATA_Position_alt(xsens_msg_buf, offset); //TODO is this hms or above ellipsoid?
gps.hmsl = ins_z * 1000;
SetBit(gps.valid_fields, GPS_VALID_HMSL_BIT);
// what about gps.lla_pos.alt and gps.utm_pos.alt ?
gps_xsens_publish();
sw/airborne/modules/ins/ins_xsens700.c
+3
View File
@@ -490,6 +490,7 @@ void parse_ins_msg(void)
// Compute geoid (MSL) height
float geoid_h = wgs84_ellipsoid_to_geoid(lla_f.lat, lla_f.lon);
gps.hmsl = gps.utm_pos.alt - (geoid_h * 1000.0f);
SetBit(gps.valid_fields, GPS_VALID_HMSL_BIT);
//gps.tow = geoid_h * 1000.0f; //gps.utm_pos.alt;
} else if (code2 == 0x40) {
@@ -512,6 +513,7 @@ void parse_ins_msg(void)
// copy results of utm conversion
gps.utm_pos.east = utm_f.east * 100;
gps.utm_pos.north = utm_f.north * 100;
SetBit(gps.valid_fields, GPS_VALID_POS_UTM_BIT);
gps_xsens_publish();
}
@@ -524,6 +526,7 @@ void parse_ins_msg(void)
gps.ned_vel.x = ins_vx;
gps.ned_vel.y = ins_vy;
gps.ned_vel.z = ins_vx;
SetBit(gps.valid_fields, GPS_VALID_VEL_NED_BIT);
}
}
sw/airborne/subsystems/gps.c
+1
View File
@@ -134,6 +134,7 @@ static void send_gps_sol(struct transport_tx *trans, struct link_device *dev)
void gps_init(void)
{
gps.valid_fields = 0;
gps.fix = GPS_FIX_NONE;
gps.week = 0;
gps.tow = 0;
sw/airborne/subsystems/gps.h
+10
View File
@@ -53,6 +53,14 @@
#define GPS_NB_CHANNELS 1
#endif
#define GPS_VALID_POS_ECEF_BIT 0
#define GPS_VALID_POS_LLA_BIT 1
#define GPS_VALID_POS_UTM_BIT 2
#define GPS_VALID_VEL_ECEF_BIT 3
#define GPS_VALID_VEL_NED_BIT 4
#define GPS_VALID_HMSL_BIT 5
#define GPS_VALID_COURSE_BIT 6
/** data structure for Space Vehicle Information of a single satellite */
struct SVinfo {
uint8_t svid; ///< Satellite ID
@@ -65,6 +73,8 @@ struct SVinfo {
/** data structure for GPS information */
struct GpsState {
uint8_t valid_fields; ///< bitfield indicating valid fields (GPS_VALID_x_BIT)
struct EcefCoor_i ecef_pos; ///< position in ECEF in cm
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)
sw/airborne/subsystems/gps/gps_datalink.c
+11 -32
View File
@@ -100,6 +100,7 @@ void parse_gps_datalink_small(uint8_t num_sv, uint32_t pos_xyz, uint32_t speed_x
lla_of_ecef_i(&lla_pos, &ecef_pos);
gps.lla_pos = lla_pos;
SetBit(gps.valid_fields, GPS_VALID_POS_LLA_BIT);
enu_speed.x = (int32_t)((speed_xy >> 22) & 0x3FF); // bits 31-22 speed x in cm/s
if (enu_speed.x & 0x200) {
@@ -114,37 +115,24 @@ void parse_gps_datalink_small(uint8_t num_sv, uint32_t pos_xyz, uint32_t speed_x
// printf("ENU Speed: %u (%d, %d, %d)\n", speed_xy, enu_speed.x, enu_speed.y, enu_speed.z);
ecef_of_enu_vect_i(&gps.ecef_vel , &tracking_ltp , &enu_speed);
SetBit(gps.valid_fields, GPS_VALID_VEL_ECEF_BIT);
gps.hmsl = tracking_ltp.hmsl + enu_pos.z * 10; // TODO: try to compensate for the loss in accuracy
SetBit(gps.valid_fields, GPS_VALID_HMSL_BIT);
gps.course = (int32_t)((speed_xy >> 2) & 0x3FF); // bits 11-2 heading in rad*1e2
if (gps.course & 0x200) {
gps.course |= 0xFFFFFC00; // fix for twos complements
}
// printf("Heading: %d\n", gps.course);
gps.course *= 1e5;
SetBit(gps.valid_fields, GPS_VALID_COURSE_BIT);
gps.num_sv = num_sv;
gps.tow = 0; // set time-of-weak to 0
gps.fix = GPS_FIX_3D; // set 3D fix to true
gps_available = TRUE; // set GPS available to true
#if GPS_USE_LATLONG
// Computes from (lat, long) in the referenced UTM zone
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;
// convert to utm
utm_of_lla_f(&utm_f, &lla_f);
// copy results of utm conversion
gps.utm_pos.east = utm_f.east * 100;
gps.utm_pos.north = utm_f.north * 100;
gps.utm_pos.alt = gps.lla_pos.alt;
gps.utm_pos.zone = nav_utm_zone0;
#endif
// publish new GPS data
uint32_t now_ts = get_sys_time_usec();
gps.last_msg_ticks = sys_time.nb_sec_rem;
@@ -165,37 +153,28 @@ void parse_gps_datalink(uint8_t numsv, int32_t ecef_x, int32_t ecef_y, int32_t e
gps.lla_pos.lat = lat;
gps.lla_pos.lon = lon;
gps.lla_pos.alt = alt;
SetBit(gps.valid_fields, GPS_VALID_POS_LLA_BIT);
gps.hmsl = hmsl;
SetBit(gps.valid_fields, GPS_VALID_HMSL_BIT);
gps.ecef_pos.x = ecef_x;
gps.ecef_pos.y = ecef_y;
gps.ecef_pos.z = ecef_z;
SetBit(gps.valid_fields, GPS_VALID_POS_ECEF_BIT);
gps.ecef_vel.x = ecef_xd;
gps.ecef_vel.y = ecef_yd;
gps.ecef_vel.z = ecef_zd;
SetBit(gps.valid_fields, GPS_VALID_VEL_ECEF_BIT);
gps.course = course;
SetBit(gps.valid_fields, GPS_VALID_COURSE_BIT);
gps.num_sv = numsv;
gps.tow = tow;
gps.fix = GPS_FIX_3D;
gps_available = TRUE;
#if GPS_USE_LATLONG
// Computes from (lat, long) in the referenced UTM zone
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;
// convert to utm
utm_of_lla_f(&utm_f, &lla_f);
// copy results of utm conversion
gps.utm_pos.east = utm_f.east * 100;
gps.utm_pos.north = utm_f.north * 100;
gps.utm_pos.alt = gps.lla_pos.alt;
gps.utm_pos.zone = nav_utm_zone0;
#endif
// publish new GPS data
uint32_t now_ts = get_sys_time_usec();
gps.last_msg_ticks = sys_time.nb_sec_rem;
sw/airborne/subsystems/gps/gps_mtk.c
+5 -28
View File
@@ -35,10 +35,6 @@
#include "subsystems/abi.h"
#include "led.h"
/* currently needed to get nav_utm_zone0 */
#include "subsystems/navigation/common_nav.h"
#include "math/pprz_geodetic_float.h"
#include "mcu_periph/sys_time.h"
#define MTK_DIY_OUTPUT_RATE MTK_DIY_OUTPUT_4HZ
@@ -196,18 +192,21 @@ void gps_mtk_read_message(void)
gps_time_sync.t0_tow_frac = 0;
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;
SetBit(gps.valid_fields, GPS_VALID_POS_LLA_BIT);
// 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 -
MTK_DIY14_NAV_HEIGHT(gps_mtk.msg_buf) * 10) * OUTPUT_RATE) / 10;
} else { gps.ned_vel.z = 0; }
gps.hmsl = MTK_DIY14_NAV_HEIGHT(gps_mtk.msg_buf) * 10;
SetBit(gps.valid_fields, GPS_VALID_HMSL_BIT);
// FIXME: with MTK you do not receive ellipsoid altitude
gps.lla_pos.alt = gps.hmsl;
gps.gspeed = MTK_DIY14_NAV_GSpeed(gps_mtk.msg_buf);
// FIXME: with MTK you do not receive speed 3D
gps.speed_3d = gps.gspeed;
gps.course = (RadOfDeg(MTK_DIY14_NAV_Heading(gps_mtk.msg_buf))) * 10;
SetBit(gps.valid_fields, GPS_VALID_COURSE_BIT);
gps.num_sv = MTK_DIY14_NAV_numSV(gps_mtk.msg_buf);
switch (MTK_DIY14_NAV_GPSfix(gps_mtk.msg_buf)) {
case MTK_DIY_FIX_3D:
@@ -222,18 +221,6 @@ void gps_mtk_read_message(void)
gps.tow = MTK_DIY14_NAV_ITOW(gps_mtk.msg_buf);;
// FIXME: with MTK DIY 1.4 you do not receive GPS week
gps.week = 0;
/* Computes from (lat, long) in the referenced UTM zone */
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;
/* convert to utm */
utm_of_lla_f(&utm_f, &lla_f);
/* copy results of utm conversion */
gps.utm_pos.east = utm_f.east * 100;
gps.utm_pos.north = utm_f.north * 100;
gps.utm_pos.alt = gps.lla_pos.alt;
gps.utm_pos.zone = nav_utm_zone0;
#ifdef GPS_LED
if (gps.fix == GPS_FIX_3D) {
LED_ON(GPS_LED);
@@ -264,12 +251,14 @@ void gps_mtk_read_message(void)
MTK_DIY16_NAV_HEIGHT(gps_mtk.msg_buf) * 10) * OUTPUT_RATE) / 10;
} else { gps.ned_vel.z = 0; }
gps.hmsl = MTK_DIY16_NAV_HEIGHT(gps_mtk.msg_buf) * 10;
SetBit(gps.valid_fields, GPS_VALID_HMSL_BIT);
// FIXME: with MTK you do not receive ellipsoid altitude
gps.lla_pos.alt = gps.hmsl;
gps.gspeed = MTK_DIY16_NAV_GSpeed(gps_mtk.msg_buf);
// FIXME: with MTK you do not receive speed 3D
gps.speed_3d = gps.gspeed;
gps.course = (RadOfDeg(MTK_DIY16_NAV_Heading(gps_mtk.msg_buf) * 10000)) * 10;
SetBit(gps.valid_fields, GPS_VALID_COURSE_BIT);
gps.num_sv = MTK_DIY16_NAV_numSV(gps_mtk.msg_buf);
switch (MTK_DIY16_NAV_GPSfix(gps_mtk.msg_buf)) {
case MTK_DIY_FIX_3D:
@@ -282,18 +271,6 @@ void gps_mtk_read_message(void)
gps.fix = GPS_FIX_NONE;
}
/* HDOP? */
/* Computes from (lat, long) in the referenced UTM zone */
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;
/* convert to utm */
utm_of_lla_f(&utm_f, &lla_f);
/* copy results of utm conversion */
gps.utm_pos.east = utm_f.east * 100;
gps.utm_pos.north = utm_f.north * 100;
gps.utm_pos.alt = gps.lla_pos.alt;
gps.utm_pos.zone = nav_utm_zone0;
#ifdef GPS_LED
if (gps.fix == GPS_FIX_3D) {
LED_ON(GPS_LED);
sw/airborne/subsystems/gps/gps_nmea.c
+4 -17
View File
@@ -34,10 +34,6 @@
#include "subsystems/abi.h"
#include "led.h"
#if GPS_USE_LATLONG
/* currently needed to get nav_utm_zone0 */
#include "subsystems/navigation/common_nav.h"
#endif
#include "math/pprz_geodetic_float.h"
#include <inttypes.h>
@@ -342,6 +338,7 @@ static void nmea_parse_RMC(void)
double course = strtod(&gps_nmea.msg_buf[i], NULL);
gps.course = RadOfDeg(course) * 1e7;
NMEA_PRINT("p_RMC() - course: %f deg\n\r", course);
SetBit(gps.valid_fields, GPS_VALID_COURSE_BIT);
}
@@ -404,6 +401,7 @@ static void nmea_parse_GGA(void)
lla_f.lon = RadOfDeg(lon);
gps.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.tow);
SetBit(gps.valid_fields, GPS_VALID_POS_LLA_BIT);
// get position fix status
nmea_read_until(&i);
@@ -432,6 +430,7 @@ static void nmea_parse_GGA(void)
float hmsl = strtof(&gps_nmea.msg_buf[i], NULL);
gps.hmsl = hmsl * 1000;
NMEA_PRINT("p_GGA() - gps.hmsl=%i\n\r", gps.hmsl);
SetBit(gps.valid_fields, GPS_VALID_HMSL_BIT);
// get altitude units (always M)
nmea_read_until(&i);
@@ -451,25 +450,13 @@ static void nmea_parse_GGA(void)
nmea_read_until(&i);
// get DGPS station ID
#if GPS_USE_LATLONG
/* convert to utm */
struct UtmCoor_f utm_f;
utm_f.zone = nav_utm_zone0;
utm_of_lla_f(&utm_f, &lla_f);
/* copy results of utm conversion */
gps.utm_pos.east = utm_f.east * 100;
gps.utm_pos.north = utm_f.north * 100;
gps.utm_pos.alt = gps.lla_pos.alt;
gps.utm_pos.zone = nav_utm_zone0;
#endif
/* convert to ECEF */
struct EcefCoor_f ecef_f;
ecef_of_lla_f(&ecef_f, &lla_f);
gps.ecef_pos.x = ecef_f.x * 100;
gps.ecef_pos.y = ecef_f.y * 100;
gps.ecef_pos.z = ecef_f.z * 100;
SetBit(gps.valid_fields, GPS_VALID_POS_ECEF_BIT);
}
/**
sw/airborne/subsystems/gps/gps_piksi.c
+16 -29
View File
@@ -35,11 +35,9 @@
#include "subsystems/abi.h"
#include "mcu_periph/uart.h"
#include "math/pprz_geodetic_double.h"
#if GPS_USE_LATLONG
#include "math/pprz_geodetic_float.h"
#include "subsystems/navigation/common_nav.h"
// get NAV_MSL0 for geoid separation
#include "generated/flight_plan.h"
#endif
#include <libsbp/sbp.h>
#include <libsbp/navigation.h>
@@ -124,10 +122,11 @@ static void sbp_pos_ecef_callback(uint16_t sender_id __attribute__((unused)),
&& pos_ecef.flags == SBP_FIX_MODE_SPP) {
return;
}
gps.ecef_pos.x = (int32_t)(pos_ecef.x * 100.0);
gps.ecef_pos.y = (int32_t)(pos_ecef.y * 100.0);
gps.ecef_pos.z = (int32_t)(pos_ecef.z * 100.0);
SetBit(gps.valid_fields, GPS_VALID_POS_ECEF_BIT);
gps.pacc = (uint32_t)(pos_ecef.accuracy);// FIXME not implemented yet by libswiftnav
gps.num_sv = pos_ecef.n_sats;
gps.tow = pos_ecef.tow;
@@ -143,6 +142,7 @@ static void sbp_vel_ecef_callback(uint16_t sender_id __attribute__((unused)),
gps.ecef_vel.x = (int32_t)(vel_ecef.x / 10);
gps.ecef_vel.y = (int32_t)(vel_ecef.y / 10);
gps.ecef_vel.z = (int32_t)(vel_ecef.z / 10);
SetBit(gps.valid_fields, GPS_VALID_VEL_ECEF_BIT);
gps.sacc = (uint32_t)(vel_ecef.accuracy);
// Solution available (VEL_ECEF is the last message to be send)
@@ -158,23 +158,11 @@ static void sbp_pos_llh_callback(uint16_t sender_id __attribute__((unused)),
msg_pos_llh_t pos_llh = *(msg_pos_llh_t *)msg;
// Check if we got RTK fix (FIXME when libsbp has a nicer way of doing this)
if(pos_llh.flags > 0 || last_flags == 0) {
if (pos_llh.flags > 0 || last_flags == 0) {
gps.lla_pos.lat = (int32_t)(pos_llh.lat * 1e7);
gps.lla_pos.lon = (int32_t)(pos_llh.lon * 1e7);
int32_t alt = (int32_t)(pos_llh.height * 1000.);
#if GPS_USE_LATLONG
/* Computes from (lat, long) in the referenced UTM zone */
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;
/* convert to utm */
utm_of_lla_f(&utm_f, &lla_f);
/* copy results of utm conversion */
gps.utm_pos.east = utm_f.east * 100;
gps.utm_pos.north = utm_f.north * 100;
gps.utm_pos.alt = gps.lla_pos.alt;
gps.utm_pos.zone = nav_utm_zone0;
// height is above ellipsoid or MSL according to bit flag (but not both are available)
// 0: above ellipsoid
// 1: above MSL
@@ -186,11 +174,9 @@ static void sbp_pos_llh_callback(uint16_t sender_id __attribute__((unused)),
gps.lla_pos.alt = alt;
gps.hmsl = alt - NAV_MSL0;
}
#else
// but here we fill the two alt with the same value since we don't know HMSL
gps.lla_pos.alt = alt;
gps.hmsl = alt;
#endif
SetBit(gps.valid_fields, GPS_VALID_POS_LLA_BIT);
SetBit(gps.valid_fields, GPS_VALID_HMSL_BIT);
}
last_flags = pos_llh.flags;
}
@@ -204,10 +190,11 @@ static void sbp_vel_ned_callback(uint16_t sender_id __attribute__((unused)),
gps.ned_vel.x = (int32_t)(vel_ned.n / 10);
gps.ned_vel.y = (int32_t)(vel_ned.e / 10);
gps.ned_vel.z = (int32_t)(vel_ned.d / 10);
#if GPS_USE_LATLONG
SetBit(gps.valid_fields, GPS_VALID_VEL_NED_BIT);
gps.gspeed = int32_sqrt(gps.ned_vel.x * gps.ned_vel.x + gps.ned_vel.y * gps.ned_vel.y);
gps.course = (int32_t)(1e7 * atan2(gps.ned_vel.y, gps.ned_vel.x));
#endif
SetBit(gps.valid_fields, GPS_VALID_COURSE_BIT);
}
static void sbp_dops_callback(uint16_t sender_id __attribute__((unused)),
@@ -283,7 +270,7 @@ static void sbp_tracking_state_dep_a_callback(uint16_t sender_id __attribute__((
} else {
return GPS_FIX_NONE;
}
}
/*
@@ -323,7 +310,7 @@ void gps_impl_init(void)
* Event handler for reading the GPS UART bytes
*/
void gps_piksi_event(void)
{
{
if ( get_sys_time_msec() - time_since_last_pos_update > POS_ECEF_TIMEOUT ) {
gps.fix = GPS_FIX_NONE;
}
@@ -384,4 +371,4 @@ uint32_t gps_piksi_write(uint8_t *buff, uint32_t n, void *context __attribute__(
void gps_inject_data(uint8_t packet_id, uint8_t length, uint8_t *data)
{
sbp_send_message(&sbp_state, packet_id, SBP_SENDER_ID, length, data, gps_piksi_write);
}
}
sw/airborne/subsystems/gps/gps_sim_nps.c
+8 -21
View File
@@ -25,12 +25,6 @@
#include "nps_sensors.h"
#include "nps_fdm.h"
#if GPS_USE_LATLONG
/* currently needed to get nav_utm_zone0 */
#include "subsystems/navigation/common_nav.h"
#include "math/pprz_geodetic_float.h"
#endif
bool_t gps_has_fix;
void gps_feed_value()
@@ -42,14 +36,20 @@ void gps_feed_value()
gps.ecef_pos.x = sensors.gps.ecef_pos.x * 100.;
gps.ecef_pos.y = sensors.gps.ecef_pos.y * 100.;
gps.ecef_pos.z = sensors.gps.ecef_pos.z * 100.;
SetBit(gps.valid_fields, GPS_VALID_POS_ECEF_BIT);
gps.ecef_vel.x = sensors.gps.ecef_vel.x * 100.;
gps.ecef_vel.y = sensors.gps.ecef_vel.y * 100.;
gps.ecef_vel.z = sensors.gps.ecef_vel.z * 100.;
SetBit(gps.valid_fields, GPS_VALID_VEL_ECEF_BIT);
//ecef pos seems to be based on geocentric model, hence we get a very high alt when converted to lla
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.;
SetBit(gps.valid_fields, GPS_VALID_POS_LLA_BIT);
gps.hmsl = sensors.gps.hmsl * 1000.;
SetBit(gps.valid_fields, GPS_VALID_HMSL_BIT);
/* calc NED speed from ECEF */
struct LtpDef_d ref_ltp;
@@ -59,6 +59,7 @@ void gps_feed_value()
gps.ned_vel.x = ned_vel_d.x * 100;
gps.ned_vel.y = ned_vel_d.y * 100;
gps.ned_vel.z = ned_vel_d.z * 100;
SetBit(gps.valid_fields, GPS_VALID_VEL_NED_BIT);
/* horizontal and 3d ground speed in cm/s */
gps.gspeed = sqrt(ned_vel_d.x * ned_vel_d.x + ned_vel_d.y * ned_vel_d.y) * 100;
@@ -66,21 +67,7 @@ void gps_feed_value()
/* ground course in radians * 1e7 */
gps.course = atan2(ned_vel_d.y, ned_vel_d.x) * 1e7;
#if GPS_USE_LATLONG
/* Computes from (lat, long) in the referenced UTM zone */
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;
/* convert to utm */
utm_of_lla_f(&utm_f, &lla_f);
/* copy results of utm conversion */
gps.utm_pos.east = utm_f.east * 100;
gps.utm_pos.north = utm_f.north * 100;
gps.utm_pos.alt = gps.lla_pos.alt;
gps.utm_pos.zone = nav_utm_zone0;
#endif
SetBit(gps.valid_fields, GPS_VALID_COURSE_BIT);
if (gps_has_fix) {
gps.fix = GPS_FIX_3D;
sw/airborne/subsystems/gps/gps_sirf.c
+5 -19
View File
@@ -25,10 +25,6 @@
#include "subsystems/abi.h"
#include "led.h"
#if GPS_USE_LATLONG
/* currently needed to get nav_utm_zone0 */
#include "subsystems/navigation/common_nav.h"
#endif
#include "math/pprz_geodetic_float.h"
#include <inttypes.h>
@@ -121,6 +117,7 @@ void sirf_parse_41(void)
gps.tow = Invert4Bytes(p->tow);
gps.hmsl = Invert4Bytes(p->alt_msl) * 10;
SetBit(gps.valid_fields, GPS_VALID_HMSL_BIT);
gps.num_sv = p->num_sat;
gps.nb_channels = p ->num_sat;
@@ -128,24 +125,11 @@ void sirf_parse_41(void)
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_f);
/* copy results of utm conversion */
gps.utm_pos.east = utm_f.east * 100;
gps.utm_pos.north = utm_f.north * 100;
gps.utm_pos.alt = gps.lla_pos.alt;
gps.utm_pos.zone = nav_utm_zone0;
#endif
SetBit(gps.valid_fields, GPS_VALID_POS_LLA_BIT);
gps.sacc = (Invert2Bytes(p->ehve) >> 16);
gps.course = RadOfDeg(Invert2Bytes(p->cog)) * pow(10, 5);
SetBit(gps.valid_fields, GPS_VALID_COURSE_BIT);
gps.gspeed = RadOfDeg(Invert2Bytes(p->sog)) * pow(10, 5);
gps.cacc = RadOfDeg(Invert2Bytes(p->heading_err)) * pow(10, 5);
gps.pacc = Invert4Bytes(p->ehpe);
@@ -173,10 +157,12 @@ void sirf_parse_2(void)
gps.ecef_pos.x = Invert4Bytes(p->x_pos) * 100;
gps.ecef_pos.y = Invert4Bytes(p->y_pos) * 100;
gps.ecef_pos.z = Invert4Bytes(p->z_pos) * 100;
SetBit(gps.valid_fields, GPS_VALID_POS_ECEF_BIT);
gps.ecef_vel.x = (Invert2Bytes(p->vx) >> 16) * 100 / 8;
gps.ecef_vel.y = (Invert2Bytes(p->vy) >> 16) * 100 / 8;
gps.ecef_vel.z = (Invert2Bytes(p->vz) >> 16) * 100 / 8;
SetBit(gps.valid_fields, GPS_VALID_VEL_ECEF_BIT);
if (gps.fix == GPS_FIX_3D) {
ticks++;
sw/airborne/subsystems/gps/gps_skytraq.c
+10 -21
View File
@@ -23,12 +23,6 @@
#include "subsystems/abi.h"
#include "led.h"
#if GPS_USE_LATLONG
/* currently needed to get nav_utm_zone0 */
#include "subsystems/navigation/common_nav.h"
#include "math/pprz_geodetic_float.h"
#endif
struct GpsSkytraq gps_skytraq;
/* parser status */
@@ -123,13 +117,21 @@ void gps_skytraq_read_message(void)
gps.ecef_pos.x = SKYTRAQ_NAVIGATION_DATA_ECEFX(gps_skytraq.msg_buf);
gps.ecef_pos.y = SKYTRAQ_NAVIGATION_DATA_ECEFY(gps_skytraq.msg_buf);
gps.ecef_pos.z = SKYTRAQ_NAVIGATION_DATA_ECEFZ(gps_skytraq.msg_buf);
SetBit(gps.valid_fields, GPS_VALID_POS_ECEF_BIT);
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);
SetBit(gps.valid_fields, GPS_VALID_VEL_ECEF_BIT);
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;
SetBit(gps.valid_fields, GPS_VALID_POS_LLA_BIT);
gps.hmsl = SKYTRAQ_NAVIGATION_DATA_ASL(gps_skytraq.msg_buf) * 10;
SetBit(gps.valid_fields, GPS_VALID_HMSL_BIT);
// pacc;
// sacc;
gps.pdop = SKYTRAQ_NAVIGATION_DATA_PDOP(gps_skytraq.msg_buf);
@@ -148,21 +150,6 @@ void gps_skytraq_read_message(void)
gps.fix = GPS_FIX_NONE;
}
#if GPS_USE_LATLONG
/* Computes from (lat, long) in the referenced UTM zone */
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;
/* convert to utm */
utm_of_lla_f(&utm_f, &lla_f);
/* copy results of utm conversion */
gps.utm_pos.east = utm_f.east * 100;
gps.utm_pos.north = utm_f.north * 100;
gps.utm_pos.alt = gps.lla_pos.alt;
gps.utm_pos.zone = nav_utm_zone0;
#endif
if (gps.fix == GPS_FIX_3D) {
if (distance_too_great(&gps_skytraq.ref_ltp.ecef, &gps.ecef_pos)) {
// just grab current ecef_pos as reference.
@@ -170,9 +157,11 @@ void gps_skytraq_read_message(void)
}
// convert ecef velocity vector to NED vector.
ned_of_ecef_vect_i(&gps.ned_vel, &gps_skytraq.ref_ltp, &gps.ecef_vel);
SetBit(gps.valid_fields, GPS_VALID_VEL_NED_BIT);
// ground course in radians
gps.course = (atan2f((float)gps.ned_vel.y, (float)gps.ned_vel.x)) * 1e7;
SetBit(gps.valid_fields, GPS_VALID_COURSE_BIT);
// GT: gps.cacc = ... ? what should course accuracy be?
// ground speed
sw/airborne/subsystems/gps/gps_ubx.c
+15 -26
View File
@@ -24,12 +24,6 @@
#include "subsystems/abi.h"
#include "led.h"
#if GPS_USE_LATLONG
/* currently needed to get nav_utm_zone0 */
#include "subsystems/navigation/common_nav.h"
#include "math/pprz_geodetic_float.h"
#endif
/** Includes macros generated from ubx.xml */
#include "ubx_protocol.h"
@@ -67,7 +61,6 @@ void gps_impl_init(void)
gps_ubx.msg_available = FALSE;
gps_ubx.error_cnt = 0;
gps_ubx.error_last = GPS_UBX_ERR_NONE;
gps_ubx.have_velned = 0;
}
@@ -86,10 +79,12 @@ void gps_ubx_read_message(void)
gps.ecef_pos.x = UBX_NAV_SOL_ECEF_X(gps_ubx.msg_buf);
gps.ecef_pos.y = UBX_NAV_SOL_ECEF_Y(gps_ubx.msg_buf);
gps.ecef_pos.z = UBX_NAV_SOL_ECEF_Z(gps_ubx.msg_buf);
SetBit(gps.valid_fields, GPS_VALID_POS_ECEF_BIT);
gps.pacc = UBX_NAV_SOL_Pacc(gps_ubx.msg_buf);
gps.ecef_vel.x = UBX_NAV_SOL_ECEFVX(gps_ubx.msg_buf);
gps.ecef_vel.y = UBX_NAV_SOL_ECEFVY(gps_ubx.msg_buf);
gps.ecef_vel.z = UBX_NAV_SOL_ECEFVZ(gps_ubx.msg_buf);
SetBit(gps.valid_fields, GPS_VALID_VEL_ECEF_BIT);
gps.sacc = UBX_NAV_SOL_Sacc(gps_ubx.msg_buf);
gps.pdop = UBX_NAV_SOL_PDOP(gps_ubx.msg_buf);
gps.num_sv = UBX_NAV_SOL_numSV(gps_ubx.msg_buf);
@@ -104,21 +99,9 @@ void gps_ubx_read_message(void)
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);
SetBit(gps.valid_fields, GPS_VALID_POS_LLA_BIT);
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_FLOAT_OF_BFP(lla_f, gps.lla_pos);
struct UtmCoor_f utm_f;
utm_f.zone = nav_utm_zone0;
/* convert to utm */
utm_of_lla_f(&utm_f, &lla_f);
/* copy results of utm conversion */
gps.utm_pos.east = utm_f.east * 100;
gps.utm_pos.north = utm_f.north * 100;
gps.utm_pos.alt = gps.lla_pos.alt;
gps.utm_pos.zone = nav_utm_zone0;
#else
SetBit(gps.valid_fields, GPS_VALID_HMSL_BIT);
} else if (gps_ubx.msg_id == UBX_NAV_POSUTM_ID) {
gps.utm_pos.east = UBX_NAV_POSUTM_EAST(gps_ubx.msg_buf);
gps.utm_pos.north = UBX_NAV_POSUTM_NORTH(gps_ubx.msg_buf);
@@ -127,24 +110,30 @@ void gps_ubx_read_message(void)
gps.utm_pos.north -= 1000000000; /* Subtract false northing: -10000km */
}
gps.utm_pos.alt = UBX_NAV_POSUTM_ALT(gps_ubx.msg_buf) * 10;
gps.hmsl = gps.utm_pos.alt;
gps.lla_pos.alt = gps.utm_pos.alt; // FIXME: with UTM only you do not receive ellipsoid altitude
gps.utm_pos.zone = UBX_NAV_POSUTM_ZONE(gps_ubx.msg_buf);
#endif
SetBit(gps.valid_fields, GPS_VALID_POS_UTM_BIT);
gps.hmsl = gps.utm_pos.alt;
SetBit(gps.valid_fields, GPS_VALID_HMSL_BIT);
/* with UTM only you do not receive ellipsoid altitude, so set only if no valid lla */
if (!bit_is_set(gps.valid_fields, GPS_VALID_HMSL_BIT)) {
gps.lla_pos.alt = gps.utm_pos.alt;
}
} else if (gps_ubx.msg_id == UBX_NAV_VELNED_ID) {
gps.speed_3d = UBX_NAV_VELNED_Speed(gps_ubx.msg_buf);
gps.gspeed = UBX_NAV_VELNED_GSpeed(gps_ubx.msg_buf);
gps.ned_vel.x = UBX_NAV_VELNED_VEL_N(gps_ubx.msg_buf);
gps.ned_vel.y = UBX_NAV_VELNED_VEL_E(gps_ubx.msg_buf);
gps.ned_vel.z = UBX_NAV_VELNED_VEL_D(gps_ubx.msg_buf);
SetBit(gps.valid_fields, GPS_VALID_VEL_NED_BIT);
// Ublox gives I4 heading in 1e-5 degrees, apparenty from 0 to 360 degrees (not -180 to 180)
// I4 max = 2^31 = 214 * 1e5 * 100 < 360 * 1e7: overflow on angles over 214 deg -> casted to -214 deg
// solution: First to radians, and then scale to 1e-7 radians
// First x 10 for loosing less resolution, then to radians, then multiply x 10 again
gps.course = (RadOfDeg(UBX_NAV_VELNED_Heading(gps_ubx.msg_buf) * 10)) * 10;
SetBit(gps.valid_fields, GPS_VALID_COURSE_BIT);
gps.cacc = (RadOfDeg(UBX_NAV_VELNED_CAcc(gps_ubx.msg_buf) * 10)) * 10;
gps.tow = UBX_NAV_VELNED_ITOW(gps_ubx.msg_buf);
gps_ubx.have_velned = 1;
} else if (gps_ubx.msg_id == UBX_NAV_SVINFO_ID) {
gps.nb_channels = Min(UBX_NAV_SVINFO_NCH(gps_ubx.msg_buf), GPS_NB_CHANNELS);
uint8_t i;
@@ -332,7 +321,7 @@ void gps_ubx_msg(void)
if (gps_ubx.msg_class == UBX_NAV_ID &&
(gps_ubx.msg_id == UBX_NAV_VELNED_ID ||
(gps_ubx.msg_id == UBX_NAV_SOL_ID &&
gps_ubx.have_velned == 0))) {
!bit_is_set(gps.valid_fields, GPS_VALID_VEL_NED_BIT)))) {
if (gps.fix == GPS_FIX_3D) {
gps.last_3dfix_ticks = sys_time.nb_sec_rem;
gps.last_3dfix_time = sys_time.nb_sec;
sw/airborne/subsystems/gps/gps_ubx.h
-2
View File
@@ -52,8 +52,6 @@ struct GpsUbx {
uint8_t status_flags;
uint8_t sol_flags;
uint8_t have_velned;
};
extern struct GpsUbx gps_ubx;
sw/airborne/subsystems/gps/gps_udp.c
+5 -20
View File
@@ -26,11 +26,6 @@
#include <string.h>
#include <stdio.h>
#if GPS_USE_LATLONG
#include "subsystems/navigation/common_nav.h"
#include "math/pprz_geodetic_float.h"
#endif
//Check if variables are set and else define them
#ifndef GPS_UDP_HOST
#define GPS_UDP_HOST 192.168.1.2
@@ -65,33 +60,23 @@ void gps_parse(void)
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);
SetBit(gps.valid_fields, GPS_VALID_POS_LLA_BIT);
gps.hmsl = UDP_GPS_INT(gps_udp_read_buffer + 16);
SetBit(gps.valid_fields, GPS_VALID_HMSL_BIT);
gps.ecef_pos.x = UDP_GPS_INT(gps_udp_read_buffer + 20);
gps.ecef_pos.y = UDP_GPS_INT(gps_udp_read_buffer + 24);
gps.ecef_pos.z = UDP_GPS_INT(gps_udp_read_buffer + 28);
SetBit(gps.valid_fields, GPS_VALID_POS_ECEF_BIT);
gps.ecef_vel.x = UDP_GPS_INT(gps_udp_read_buffer + 32);
gps.ecef_vel.y = UDP_GPS_INT(gps_udp_read_buffer + 36);
gps.ecef_vel.z = UDP_GPS_INT(gps_udp_read_buffer + 40);
SetBit(gps.valid_fields, GPS_VALID_VEL_ECEF_BIT);
gps.fix = GPS_FIX_3D;
#if GPS_USE_LATLONG
// Computes from (lat, long) in the referenced UTM zone
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;
// convert to utm
utm_of_lla_f(&utm_f, &lla_f);
// copy results of utm conversion
gps.utm_pos.east = utm_f.east * 100;
gps.utm_pos.north = utm_f.north * 100;
gps.utm_pos.alt = gps.lla_pos.alt;
gps.utm_pos.zone = nav_utm_zone0;
#endif
// publish new GPS data
uint32_t now_ts = get_sys_time_usec();
gps.last_msg_ticks = sys_time.nb_sec_rem;
sw/airborne/subsystems/ins.c
+20 -16
View File
@@ -73,17 +73,20 @@ void WEAK ins_reset_local_origin(void)
{
#if USE_GPS
struct UtmCoor_f utm;
#ifdef GPS_USE_LATLONG
/* Recompute UTM coordinates in this zone */
struct LlaCoor_f lla;
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;
utm.east = gps.utm_pos.east / 100.0f;
utm.north = gps.utm_pos.north / 100.0f;
#endif
if (bit_is_set(gps.valid_fields, GPS_VALID_POS_UTM_BIT)) {
utm.zone = gps.utm_pos.zone;
utm.east = gps.utm_pos.east / 100.0f;
utm.north = gps.utm_pos.north / 100.0f;
}
else {
/* Recompute UTM coordinates in this zone */
struct LlaCoor_f lla;
LLA_FLOAT_OF_BFP(lla, gps.lla_pos);
utm.zone = (gps.lla_pos.lon / 1e7 + 180) / 6 + 1;
utm_of_lla_f(&utm, &lla);
}
// ground_alt
utm.alt = gps.hmsl / 1000.0f;
@@ -99,11 +102,12 @@ 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 = (gps.lla_pos.lon / 1e7 + 180) / 6 + 1;
#else
utm->zone = gps.utm_pos.zone;
#endif
if (bit_is_set(gps.valid_fields, GPS_VALID_POS_UTM_BIT)) {
utm->zone = gps.utm_pos.zone;
}
else {
utm->zone = (gps.lla_pos.lon / 1e7 + 180) / 6 + 1;
}
utm_of_lla_f(utm, &lla0);
stateSetLocalUtmOrigin_f(utm);
sw/airborne/subsystems/ins/ins_alt_float.c
+14 -11
View File
@@ -114,17 +114,20 @@ void ins_alt_float_init(void)
void ins_reset_local_origin(void)
{
struct UtmCoor_f utm;
#ifdef GPS_USE_LATLONG
/* Recompute UTM coordinates in this zone */
struct LlaCoor_f lla;
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;
utm.east = gps.utm_pos.east / 100.0f;
utm.north = gps.utm_pos.north / 100.0f;
#endif
if (bit_is_set(gps.valid_fields, GPS_VALID_POS_UTM_BIT)) {
utm.zone = gps.utm_pos.zone;
utm.east = gps.utm_pos.east / 100.0f;
utm.north = gps.utm_pos.north / 100.0f;
}
else {
/* Recompute UTM coordinates in this zone */
struct LlaCoor_f lla;
LLA_FLOAT_OF_BFP(lla, gps.lla_pos);
utm.zone = (gps.lla_pos.lon / 1e7 + 180) / 6 + 1;
utm_of_lla_f(&utm, &lla);
}
// ground_alt
utm.alt = gps.hmsl / 1000.0f;
sw/airborne/subsystems/ins/ins_float_invariant.c
+14 -12
View File
@@ -274,19 +274,21 @@ void ins_reset_local_origin(void)
{
#if INS_FINV_USE_UTM
struct UtmCoor_f utm;
#ifdef GPS_USE_LATLONG
/* Recompute UTM coordinates in this zone */
struct LlaCoor_f lla;
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;
utm.east = gps.utm_pos.east / 100.0f;
utm.north = gps.utm_pos.north / 100.0f;
#endif
if (bit_is_set(gps.valid_fields, GPS_VALID_POS_UTM_BIT)) {
utm.zone = gps.utm_pos.zone;
utm.east = gps.utm_pos.east / 100.0f;
utm.north = gps.utm_pos.north / 100.0f;
}
else {
/* Recompute UTM coordinates in this zone */
struct LlaCoor_f lla;
LLA_FLOAT_OF_BFP(lla, gps.lla_pos);
struct UtmCoor_f utm;
utm.zone = (gps.lla_pos.lon / 1e7 + 180) / 6 + 1;
utm_of_lla_f(&utm, &lla);
}
// ground_alt
utm.alt = gps.hmsl / 1000.0f;
utm.alt = gps.hmsl / 1000.0f;
// reset state UTM ref
stateSetLocalUtmOrigin_f(&utm);
#else
sw/airborne/subsystems/ins/ins_gps_passthrough_utm.c
+14 -11
View File
@@ -47,17 +47,20 @@ void ins_gps_utm_init(void)
void ins_reset_local_origin(void)
{
struct UtmCoor_f utm;
#ifdef GPS_USE_LATLONG
/* Recompute UTM coordinates in this zone */
struct LlaCoor_f lla;
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;
utm.east = gps.utm_pos.east / 100.0f;
utm.north = gps.utm_pos.north / 100.0f;
#endif
if (bit_is_set(gps.valid_fields, GPS_VALID_POS_UTM_BIT)) {
utm.zone = gps.utm_pos.zone;
utm.east = gps.utm_pos.east / 100.0f;
utm.north = gps.utm_pos.north / 100.0f;
}
else {
/* Recompute UTM coordinates in this zone */
struct LlaCoor_f lla;
LLA_FLOAT_OF_BFP(lla, gps.lla_pos);
utm.zone = (gps.lla_pos.lon / 1e7 + 180) / 6 + 1;
utm_of_lla_f(&utm, &lla);
}
// ground_alt
utm.alt = gps.hmsl / 1000.0f;
// reset state UTM ref
sw/airborne/subsystems/ins/ins_vectornav.c
+4 -16
View File
@@ -363,6 +363,7 @@ void ins_vectornav_propagate()
ins_vn.lla_pos.lon = RadOfDeg((float)ins_vn.pos_lla[1]); // ins_impl.pos_lla[1] = lon
ins_vn.lla_pos.alt = ((float)ins_vn.pos_lla[2]); // ins_impl.pos_lla[2] = alt
LLA_BFP_OF_REAL(gps.lla_pos, ins_vn.lla_pos);
SetBit(gps.valid_fields, GPS_VALID_POS_LLA_BIT);
stateSetPositionLla_i(&gps.lla_pos);
// ECEF position
@@ -371,27 +372,13 @@ void ins_vectornav_propagate()
struct EcefCoor_f ecef_vel;
ecef_of_ned_point_f(&ecef_vel, &def, &ins_vn.vel_ned);
ECEF_BFP_OF_REAL(gps.ecef_vel, ecef_vel);
SetBit(gps.valid_fields, GPS_VALID_VEL_ECEF_BIT);
// ECEF velocity
gps.ecef_pos.x = stateGetPositionEcef_i()->x;
gps.ecef_pos.y = stateGetPositionEcef_i()->y;
gps.ecef_pos.z = stateGetPositionEcef_i()->z;
#if GPS_USE_LATLONG
// GPS UTM
/* Computes from (lat, long) in the referenced UTM zone */
struct UtmCoor_f utm_f;
utm_f.zone = nav_utm_zone0;
/* convert to utm */
//utm_of_lla_f(&utm_f, &lla_f);
utm_of_lla_f(&utm_f, &ins_vn.lla_pos);
/* copy results of utm conversion */
gps.utm_pos.east = (int32_t)(utm_f.east * 100);
gps.utm_pos.north = (int32_t)(utm_f.north * 100);
gps.utm_pos.alt = (int32_t)(utm_f.alt * 1000);
gps.utm_pos.zone = (uint8_t)nav_utm_zone0;
#endif
SetBit(gps.valid_fields, GPS_VALID_POS_ECEF_BIT);
// GPS Ground speed
float speed = sqrt(ins_vn.vel_ned.x * ins_vn.vel_ned.x + ins_vn.vel_ned.y * ins_vn.vel_ned.y);
@@ -399,6 +386,7 @@ void ins_vectornav_propagate()
// GPS course
gps.course = (int32_t)(1e7 * (atan2(ins_vn.vel_ned.y, ins_vn.vel_ned.x)));
SetBit(gps.valid_fields, GPS_VALID_COURSE_BIT);
// Because we have not HMSL data from Vectornav, we are using LLA-Altitude
// as a workaround
sw/airborne/subsystems/ins/ins_vectornav.h
-6
View File
@@ -53,12 +53,6 @@
// Abi
#include "subsystems/abi.h"
#if GPS_USE_LATLONG
/* currently needed to get nav_utm_zone0 */
#include "subsystems/navigation/common_nav.h"
#include "math/pprz_geodetic_float.h"
#endif
#if !defined INS_VN_BODY_TO_IMU_PHI && !defined INS_VN_BODY_TO_IMU_THETA && !defined INS_VN_BODY_TO_IMU_PSI
#define INS_VN_BODY_TO_IMU_PHI 0
sw/tools/generators/gen_flight_plan.ml
+1 -1
View File
@@ -702,7 +702,7 @@ let check_geo_ref = fun wgs84 xml ->
let max_d = min 1000. (get_float "max_dist_from_home") in
let check_zone = fun u ->
if (utm_of WGS84 (of_utm WGS84 u)).utm_zone <> utm0.utm_zone then
prerr_endline "Warning: You are close (less than twice the max distance) to an UTM zone border ! The navigation will not work unless the GPS_USE_LATLONG flag is set and the GPS receiver configured to send the POSLLH message." in
prerr_endline "Warning: You are close (less than twice the max distance) to an UTM zone border ! The navigation will not work unless the GPS receiver configured to send the POSLLH message." in
check_zone { utm0 with utm_x = utm0.utm_x +. 2.*.max_d };
check_zone { utm0 with utm_x = utm0.utm_x -. 2.*.max_d };