Skip to content

Commit

Permalink
Merge pull request #1476 from paparazzi/gps_utm_cleanup
Browse files Browse the repository at this point in the history 
[gps] add fields_valid and remove GPS_USE_LATLONG

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
  • Loading branch information
@flixr
flixr committed Dec 21, 2015
2 parents 7c10df2 + 29c4174 commit 5629c64
Show file tree
Hide file tree
Showing 40 changed files with 205 additions and 432 deletions.
2 changes: 1 addition & 1 deletion conf/conf_tests.xml
View file
Expand Up @@ -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
Expand Down
7 changes: 3 additions & 4 deletions conf/firmwares/subsystems/fixedwing/gps_mediatek_diy.makefile
View file
Expand Up @@ -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)
Expand All @@ -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
6 changes: 3 additions & 3 deletions conf/firmwares/subsystems/fixedwing/gps_piksi.makefile
View file
Expand Up @@ -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)

Expand All @@ -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

2 changes: 1 addition & 1 deletion conf/firmwares/subsystems/fixedwing/gps_ublox_hitl.makefile
View file
Expand Up @@ -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
2 changes: 1 addition & 1 deletion conf/firmwares/subsystems/fixedwing/gps_ublox_utm.makefile
View file
Expand Up @@ -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
6 changes: 2 additions & 4 deletions conf/firmwares/subsystems/fixedwing/ins_vectornav.makefile
View file
Expand Up @@ -4,8 +4,6 @@

include $(CFG_SHARED)/ins_vectornav.makefile

ap.CFLAGS += -DGPS_USE_LATLONG


#########################################
## Simulator
Expand All @@ -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
Expand All @@ -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
Expand Down
4 changes: 2 additions & 2 deletions conf/firmwares/subsystems/fixedwing/ins_xsens.makefile
View file
Expand Up @@ -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

Expand All @@ -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
Expand Down
25 changes: 0 additions & 25 deletions conf/firmwares/subsystems/rotorcraft/gps_furuno.makefile
View file

This file was deleted.

24 changes: 0 additions & 24 deletions conf/firmwares/subsystems/rotorcraft/gps_nmea.makefile
View file

This file was deleted.

21 changes: 0 additions & 21 deletions conf/firmwares/subsystems/rotorcraft/gps_skytraq.makefile
View file

This file was deleted.

20 changes: 0 additions & 20 deletions conf/firmwares/subsystems/rotorcraft/gps_ublox.makefile
View file

This file was deleted.

6 changes: 3 additions & 3 deletions .../subsystems/fixedwing/gps_furuno.makefile → ...res/subsystems/shared/gps_furuno.makefile
View file
Expand Up @@ -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)
Expand All @@ -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
7 changes: 3 additions & 4 deletions ...es/subsystems/fixedwing/gps_nmea.makefile → ...wares/subsystems/shared/gps_nmea.makefile
View file
Expand Up @@ -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)
Expand All @@ -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
6 changes: 3 additions & 3 deletions ...subsystems/fixedwing/gps_skytraq.makefile → ...es/subsystems/shared/gps_skytraq.makefile
View file
Expand Up @@ -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)
Expand All @@ -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
7 changes: 3 additions & 4 deletions ...s/subsystems/fixedwing/gps_ublox.makefile → ...ares/subsystems/shared/gps_ublox.makefile
View file
Expand Up @@ -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)
Expand All @@ -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
2 changes: 1 addition & 1 deletion ...ubsystems/fixedwing/ins_xsens700.makefile → ...s/subsystems/shared/ins_xsens700.makefile
View file
Expand Up @@ -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
Expand Down
3 changes: 0 additions & 3 deletions conf/modules/gps_ublox.xml
View file
Expand Up @@ -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)
Expand All @@ -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\"
Expand Down
23 changes: 7 additions & 16 deletions sw/airborne/arch/sim/sim_gps.c
View file
Expand Up @@ -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;
Expand Down
8 changes: 4 additions & 4 deletions sw/airborne/modules/cam_control/point.c
View file
Expand Up @@ -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;
Expand Down Expand Up @@ -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);
Expand Down

0 comments on commit 5629c64

Please sign in to comment.