Paparazzi UAS  v5.10_stable-5-g83a0da5-dirty
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces 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 #include "subsystems/abi.h"
25 #include "nps_sensors.h"
26 #include "nps_fdm.h"
27 
30 
31 void gps_feed_value(void)
32 {
33  // FIXME, set proper time instead of hardcoded to May 2014
34  gps_nps.week = 1794;
35  gps_nps.tow = fdm.time * 1000;
36 
45 
46  //ecef pos seems to be based on geocentric model, hence we get a very high alt when converted to lla
47  gps_nps.lla_pos.lat = DegOfRad(sensors.gps.lla_pos.lat) * 1e7;
48  gps_nps.lla_pos.lon = DegOfRad(sensors.gps.lla_pos.lon) * 1e7;
51 
52  gps_nps.hmsl = sensors.gps.hmsl * 1000.;
54 
55  /* calc NED speed from ECEF */
56  struct LtpDef_d ref_ltp;
58  struct NedCoor_d ned_vel_d;
59  ned_of_ecef_vect_d(&ned_vel_d, &ref_ltp, &sensors.gps.ecef_vel);
60  gps_nps.ned_vel.x = ned_vel_d.x * 100;
61  gps_nps.ned_vel.y = ned_vel_d.y * 100;
62  gps_nps.ned_vel.z = ned_vel_d.z * 100;
64 
65  /* horizontal and 3d ground speed in cm/s */
66  gps_nps.gspeed = sqrt(ned_vel_d.x * ned_vel_d.x + ned_vel_d.y * ned_vel_d.y) * 100;
67  gps_nps.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;
68 
69  /* ground course in radians * 1e7 */
70  gps_nps.course = atan2(ned_vel_d.y, ned_vel_d.x) * 1e7;
72 
73  if (gps_has_fix) {
75  } else {
77  }
78 
79  // publish gps data
80  uint32_t now_ts = get_sys_time_usec();
83  if (gps_nps.fix == GPS_FIX_3D) {
86  }
87  AbiSendMsgGPS(GPS_SIM_ID, now_ts, &gps_nps);
88 }
89 
90 void gps_nps_init(void)
91 {
92  gps_has_fix = true;
93 }
double z
in meters
struct EcefCoor_d ecef_vel
int32_t z
in centimeters
double time
Definition: nps_fdm.h:46
vector in North East Down coordinates Units: meters
bool gps_has_fix
Definition: gps_sim_nps.c:29
struct NpsFdm fdm
Holds all necessary NPS FDM state information.
#define GPS_VALID_VEL_NED_BIT
Definition: gps.h:52
struct EcefCoor_d ecef_pos
uint8_t valid_fields
bitfield indicating valid fields (GPS_VALID_x_BIT)
Definition: gps.h:82
int32_t y
in centimeters
uint16_t week
GPS week.
Definition: gps.h:100
double lat
in radians
double alt
in meters above WGS84 reference ellipsoid
Main include for ABI (AirBorneInterface).
uint16_t speed_3d
norm of 3d speed in cm/s
Definition: gps.h:92
#define GPS_FIX_3D
3D GPS fix
Definition: gps.h:39
int32_t z
Down.
double x
in meters
uint32_t last_3dfix_ticks
cpu time ticks at last valid 3D fix
Definition: gps.h:106
int32_t alt
in millimeters above WGS84 reference ellipsoid
int32_t y
East.
uint32_t last_msg_time
cpu time in sec at last received GPS message
Definition: gps.h:109
#define GPS_VALID_COURSE_BIT
Definition: gps.h:54
double y
in meters
struct LlaCoor_d lla_pos
int32_t hmsl
height above mean sea level (MSL) in mm
Definition: gps.h:88
double x
in meters
data structure for GPS information
Definition: gps.h:81
uint32_t tow
GPS time of week in ms.
Definition: gps.h:101
#define GPS_FIX_NONE
No GPS fix.
Definition: gps.h:37
Device independent GPS code (interface)
struct GpsState gps_nps
Definition: gps_sim_nps.c:28
int32_t x
North.
unsigned long uint32_t
Definition: types.h:18
struct EcefCoor_i ecef_pos
position in ECEF in cm
Definition: gps.h:85
definition of the local (flat earth) coordinate system
#define GPS_VALID_HMSL_BIT
Definition: gps.h:53
int32_t lon
in degrees*1e7
struct NpsSensors sensors
Definition: nps_sensors.c:6
#define GPS_SIM_ID
volatile uint32_t nb_sec_rem
remainder of seconds since startup in CPU_TICKS
Definition: sys_time.h:73
void gps_feed_value(void)
Definition: gps_sim_nps.c:31
void ltp_def_from_ecef_d(struct LtpDef_d *def, struct EcefCoor_d *ecef)
uint32_t last_3dfix_time
cpu time in sec at last valid 3D fix
Definition: gps.h:107
int32_t course
GPS course over ground in rad*1e7, [0, 2*Pi]*1e7 (CW/north)
Definition: gps.h:93
volatile uint32_t nb_sec
full seconds since startup
Definition: sys_time.h:72
double lon
in radians
void gps_nps_init(void)
Definition: gps_sim_nps.c:90
#define GPS_VALID_POS_ECEF_BIT
Definition: gps.h:48
uint32_t last_msg_ticks
cpu time ticks at last received GPS message
Definition: gps.h:108
#define GPS_VALID_POS_LLA_BIT
Definition: gps.h:49
double z
in meters
int32_t x
in centimeters
uint16_t gspeed
norm of 2d ground speed in cm/s
Definition: gps.h:91
struct EcefCoor_i ecef_vel
speed ECEF in cm/s
Definition: gps.h:89
#define GPS_VALID_VEL_ECEF_BIT
Definition: gps.h:51
struct LlaCoor_i lla_pos
position in LLA (lat,lon: deg*1e7; alt: mm over ellipsoid)
Definition: gps.h:86
uint32_t get_sys_time_usec(void)
Get the time in microseconds since startup.
Definition: sys_time_arch.c:68
int32_t lat
in degrees*1e7
uint8_t fix
status of fix
Definition: gps.h:99
struct NedCoor_i ned_vel
speed NED in cm/s
Definition: gps.h:90
double y
in meters
struct NpsSensorGps gps
Definition: nps_sensors.h:20
void ned_of_ecef_vect_d(struct NedCoor_d *ned, struct LtpDef_d *def, struct EcefCoor_d *ecef)