Paparazzi UAS  v5.2.2_stable-0-gd6b9f29
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
gps_sim_nps.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2008-2011 The Paparazzi Team
3  *
4  * This file is part of paparazzi.
5  *
6  * paparazzi is free software; you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License as published by
8  * the Free Software Foundation; either version 2, or (at your option)
9  * any later version.
10  *
11  * paparazzi is distributed in the hope that it will be useful,
12  * but WITHOUT ANY WARRANTY; without even the implied warranty of
13  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
14  * GNU General Public License for more details.
15  *
16  * You should have received a copy of the GNU General Public License
17  * along with paparazzi; see the file COPYING. If not, write to
18  * the Free Software Foundation, 59 Temple Place - Suite 330,
19  * Boston, MA 02111-1307, USA.
20  */
21 
23 #include "subsystems/gps.h"
24 
25 #include "nps_sensors.h"
26 #include "nps_fdm.h"
27 
28 #if GPS_USE_LATLONG
29 /* currently needed to get nav_utm_zone0 */
32 #endif
33 
35 bool_t gps_has_fix;
36 
38  // FIXME, set proper time instead of hardcoded to May 2014
39  gps.week = 1794;
40  gps.tow = fdm.time * 1000;
41 
42  gps.ecef_pos.x = sensors.gps.ecef_pos.x * 100.;
43  gps.ecef_pos.y = sensors.gps.ecef_pos.y * 100.;
44  gps.ecef_pos.z = sensors.gps.ecef_pos.z * 100.;
45  gps.ecef_vel.x = sensors.gps.ecef_vel.x * 100.;
46  gps.ecef_vel.y = sensors.gps.ecef_vel.y * 100.;
47  gps.ecef_vel.z = sensors.gps.ecef_vel.z * 100.;
48  //ecef pos seems to be based on geocentric model, hence we get a very high alt when converted to lla
49  gps.lla_pos.lat = DegOfRad(sensors.gps.lla_pos.lat) * 1e7;
50  gps.lla_pos.lon = DegOfRad(sensors.gps.lla_pos.lon) * 1e7;
51  gps.lla_pos.alt = sensors.gps.lla_pos.alt * 1000.;
52  gps.hmsl = sensors.gps.hmsl * 1000.;
53 
54  /* calc NED speed from ECEF */
55  struct LtpDef_d ref_ltp;
56  ltp_def_from_ecef_d(&ref_ltp, &sensors.gps.ecef_pos);
57  struct NedCoor_d ned_vel_d;
58  ned_of_ecef_vect_d(&ned_vel_d, &ref_ltp, &sensors.gps.ecef_vel);
59  gps.ned_vel.x = ned_vel_d.x * 100;
60  gps.ned_vel.y = ned_vel_d.y * 100;
61  gps.ned_vel.z = ned_vel_d.z * 100;
62 
63  /* horizontal and 3d ground speed in cm/s */
64  gps.gspeed = sqrt(ned_vel_d.x * ned_vel_d.x + ned_vel_d.y * ned_vel_d.y) * 100;
65  gps.speed_3d = sqrt(ned_vel_d.x * ned_vel_d.x + ned_vel_d.y * ned_vel_d.y + ned_vel_d.z * ned_vel_d.z) * 100;
66 
67  /* ground course in radians * 1e7 */
68  gps.course = atan2(ned_vel_d.y, ned_vel_d.x) * 1e7;
69 
70 #if GPS_USE_LATLONG
71  /* Computes from (lat, long) in the referenced UTM zone */
72  struct LlaCoor_f lla_f;
74  struct UtmCoor_f utm_f;
75  utm_f.zone = nav_utm_zone0;
76  /* convert to utm */
77  utm_of_lla_f(&utm_f, &lla_f);
78  /* copy results of utm conversion */
79  gps.utm_pos.east = utm_f.east*100;
80  gps.utm_pos.north = utm_f.north*100;
83 #endif
84 
85  if (gps_has_fix)
86  gps.fix = GPS_FIX_3D;
87  else
90 }
91 
92 void gps_impl_init() {
94  gps_has_fix = TRUE;
95 }
int32_t y
in centimeters
struct LlaCoor_i lla_pos
position in LLA (lat,lon: deg*1e7; alt: mm over ellipsoid)
Definition: gps.h:65
vector in North East Down coordinates Units: meters
struct NedCoor_i ned_vel
speed NED in cm/s
Definition: gps.h:69
int32_t lat
in degrees*1e7
struct EcefCoor_i ecef_vel
speed ECEF in cm/s
Definition: gps.h:68
int32_t course
GPS course over ground in rad*1e7, [0, 2*Pi]*1e7 (CW/north)
Definition: gps.h:72
uint8_t zone
UTM zone number.
definition of the local (flat earth) coordinate system
int32_t x
North.
uint8_t nav_utm_zone0
Definition: common_nav.c:44
int32_t hmsl
height above mean sea level in mm
Definition: gps.h:67
uint8_t fix
status of fix
Definition: gps.h:78
#define GPS_FIX_3D
Definition: gps.h:43
int32_t z
in centimeters
struct EcefCoor_i ecef_pos
position in ECEF in cm
Definition: gps.h:64
int16_t week
GPS week.
Definition: gps.h:79
vector in Latitude, Longitude and Altitude
void gps_impl_init()
GPS initialization.
Definition: gps_sim_nps.c:92
#define FALSE
Definition: imu_chimu.h:141
int32_t alt
in millimeters above WGS84 reference ellipsoid
void gps_feed_value()
Definition: gps_sim_nps.c:37
Paparazzi floating point math for geodetic calculations.
uint32_t tow
GPS time of week in ms.
Definition: gps.h:80
int32_t z
Down.
uint8_t zone
UTM zone number.
int16_t gspeed
norm of 2d ground speed in cm/s
Definition: gps.h:70
float north
in meters
#define GPS_FIX_NONE
Definition: gps.h:41
double z
in meters
Device independent GPS code (interface)
position in UTM coordinates Units: meters
void ned_of_ecef_vect_d(struct NedCoor_d *ned, struct LtpDef_d *def, struct EcefCoor_d *ecef)
int32_t alt
in millimeters above WGS84 reference ellipsoid
int32_t y
East.
int32_t north
in centimeters
int32_t x
in centimeters
void ltp_def_from_ecef_d(struct LtpDef_d *def, struct EcefCoor_d *ecef)
#define TRUE
Definition: imu_chimu.h:144
bool_t gps_available
Is set to TRUE when a new REMOTE_GPS packet is received and parsed.
Definition: gps_sim_nps.c:34
int16_t speed_3d
norm of 3d speed in cm/s
Definition: gps.h:71
int32_t east
in centimeters
int32_t lon
in degrees*1e7
#define LLA_FLOAT_OF_BFP(_o, _i)
bool_t gps_has_fix
Definition: gps_sim_nps.c:35
double x
in meters
struct UtmCoor_i utm_pos
position in UTM (north,east: cm; alt: mm over ellipsoid)
Definition: gps.h:66
float east
in meters
struct GpsState gps
global GPS state
Definition: gps.c:41
double y
in meters
void utm_of_lla_f(struct UtmCoor_f *utm, struct LlaCoor_f *lla)