Paparazzi UAS  v5.15_devel-112-g521f3cf
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 #if GpsId(PRIMARY_GPS) == GPS_DATALINK_ID
70  /* vehicle heading in radians * 1e7 */
72 #else
73  /* ground course in radians * 1e7 */
74  gps_nps.course = atan2(ned_vel_d.y, ned_vel_d.x) * 1e7;
75 #endif
77 
78  gps_nps.pacc = 650;
79  gps_nps.hacc = 450;
80  gps_nps.vacc = 200;
81  gps_nps.sacc = 100;
82  gps_nps.pdop = 650;
83 
84  if (gps_has_fix) {
85  gps_nps.num_sv = 11;
87  } else {
88  gps_nps.num_sv = 1;
90  }
91 
92  // publish gps data
93  uint32_t now_ts = get_sys_time_usec();
96  if (gps_nps.fix == GPS_FIX_3D) {
99  }
100  AbiSendMsgGPS(GPS_SIM_ID, now_ts, &gps_nps);
101 }
102 
103 void gps_nps_init(void)
104 {
105  gps_has_fix = true;
106 }
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
uint32_t pacc
position accuracy in cm
Definition: gps.h:100
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:88
int32_t y
in centimeters
uint32_t hacc
horizontal accuracy in cm
Definition: gps.h:101
uint16_t week
GPS week.
Definition: gps.h:108
double psi
in radians
double lat
in radians
double alt
in meters above WGS84 reference ellipsoid
Main include for ABI (AirBorneInterface).
uint32_t vacc
vertical accuracy in cm
Definition: gps.h:102
uint16_t speed_3d
norm of 3d speed in cm/s
Definition: gps.h:98
#define GPS_FIX_3D
3D GPS fix
Definition: gps.h:39
int32_t z
Down.
struct DoubleEulers ltp_to_body_eulers
Definition: nps_fdm.h:92
double x
in meters
uint32_t last_3dfix_ticks
cpu time ticks at last valid 3D fix
Definition: gps.h:114
int32_t alt
in millimeters above WGS84 reference ellipsoid
uint32_t sacc
speed accuracy in cm/s
Definition: gps.h:103
int32_t y
East.
uint32_t last_msg_time
cpu time in sec at last received GPS message
Definition: gps.h:117
#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:94
double x
in meters
data structure for GPS information
Definition: gps.h:87
uint32_t tow
GPS time of week in ms.
Definition: gps.h:109
#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
uint16_t pdop
position dilution of precision scaled by 100
Definition: gps.h:105
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:91
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:115
int32_t course
GPS course over ground in rad*1e7, [0, 2*Pi]*1e7 (CW/north)
Definition: gps.h:99
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:103
#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:116
#define GPS_VALID_POS_LLA_BIT
Definition: gps.h:49
double z
in meters
uint8_t num_sv
number of sat in fix
Definition: gps.h:106
int32_t x
in centimeters
uint16_t gspeed
norm of 2d ground speed in cm/s
Definition: gps.h:97
struct EcefCoor_i ecef_vel
speed ECEF in cm/s
Definition: gps.h:95
#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:92
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:107
struct NedCoor_i ned_vel
speed NED in cm/s
Definition: gps.h:96
double y
in meters
struct NpsSensorGps gps
Definition: nps_sensors.h:22
void ned_of_ecef_vect_d(struct NedCoor_d *ned, struct LtpDef_d *def, struct EcefCoor_d *ecef)