Paparazzi UAS  v6.2_unstable
Paparazzi is a free software Unmanned Aircraft System.
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 "modules/gps/gps.h"
24 #include "modules/core/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 
37  gps_nps.ecef_pos.x = sensors.gps.ecef_pos.x * 100.;
38  gps_nps.ecef_pos.y = sensors.gps.ecef_pos.y * 100.;
39  gps_nps.ecef_pos.z = sensors.gps.ecef_pos.z * 100.;
41  gps_nps.ecef_vel.x = sensors.gps.ecef_vel.x * 100.;
42  gps_nps.ecef_vel.y = sensors.gps.ecef_vel.y * 100.;
43  gps_nps.ecef_vel.z = sensors.gps.ecef_vel.z * 100.;
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;
49  gps_nps.lla_pos.alt = sensors.gps.lla_pos.alt * 1000.;
51 
52  gps_nps.hmsl = sensors.gps.hmsl * 1000.;
54 
55  /* calc NED speed from ECEF */
56  struct LtpDef_d ref_ltp;
57  ltp_def_from_ecef_d(&ref_ltp, &sensors.gps.ecef_pos);
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 }
GPS_SIM_ID
#define GPS_SIM_ID
Definition: abi_sender_ids.h:268
NedCoor_d::y
double y
in meters
Definition: pprz_geodetic_double.h:69
LlaCoor_i::lon
int32_t lon
in degrees*1e7
Definition: pprz_geodetic_int.h:61
GpsState::valid_fields
uint8_t valid_fields
bitfield indicating valid fields (GPS_VALID_x_BIT)
Definition: gps.h:91
uint32_t
unsigned int uint32_t
Typedef defining 32 bit unsigned int type.
Definition: vl53l1_types.h:78
ltp_def_from_ecef_d
void ltp_def_from_ecef_d(struct LtpDef_d *def, struct EcefCoor_d *ecef)
Definition: pprz_geodetic_double.c:34
LlaCoor_i::alt
int32_t alt
in millimeters above WGS84 reference ellipsoid
Definition: pprz_geodetic_int.h:62
GPS_VALID_COURSE_BIT
#define GPS_VALID_COURSE_BIT
Definition: gps.h:57
abi.h
NedCoor_i::y
int32_t y
East.
Definition: pprz_geodetic_int.h:70
GpsState::tow
uint32_t tow
GPS time of week in ms.
Definition: gps.h:112
NedCoor_d::x
double x
in meters
Definition: pprz_geodetic_double.h:68
GpsState
data structure for GPS information
Definition: gps.h:90
EcefCoor_i::x
int32_t x
in centimeters
Definition: pprz_geodetic_int.h:51
GpsState::pacc
uint32_t pacc
position accuracy in cm
Definition: gps.h:103
DoubleEulers::psi
double psi
in radians
Definition: pprz_algebra_double.h:79
GpsState::sacc
uint32_t sacc
speed accuracy in cm/s
Definition: gps.h:106
gps_has_fix
bool gps_has_fix
Definition: gps_sim_nps.c:29
GpsState::vacc
uint32_t vacc
vertical accuracy in cm
Definition: gps.h:105
nps_fdm.h
GpsState::ned_vel
struct NedCoor_i ned_vel
speed NED in cm/s
Definition: gps.h:99
gps_nps
struct GpsState gps_nps
Definition: gps_sim_nps.c:28
EcefCoor_i::y
int32_t y
in centimeters
Definition: pprz_geodetic_int.h:52
GPS_VALID_POS_LLA_BIT
#define GPS_VALID_POS_LLA_BIT
Definition: gps.h:52
NedCoor_i::z
int32_t z
Down.
Definition: pprz_geodetic_int.h:71
get_sys_time_usec
uint32_t get_sys_time_usec(void)
Get the time in microseconds since startup.
Definition: sys_time_arch.c:75
fdm
struct NpsFdm fdm
Holds all necessary NPS FDM state information.
Definition: nps_fdm_crrcsim.c:84
LlaCoor_i::lat
int32_t lat
in degrees*1e7
Definition: pprz_geodetic_int.h:60
GpsState::last_msg_ticks
uint32_t last_msg_ticks
cpu time ticks at last received GPS message
Definition: gps.h:119
NpsFdm::time
double time
Definition: nps_fdm.h:46
GpsState::hacc
uint32_t hacc
horizontal accuracy in cm
Definition: gps.h:104
EcefCoor_i::z
int32_t z
in centimeters
Definition: pprz_geodetic_int.h:53
gps.h
Device independent GPS code (interface)
GpsState::fix
uint8_t fix
status of fix
Definition: gps.h:110
GpsState::speed_3d
uint16_t speed_3d
norm of 3d speed in cm/s
Definition: gps.h:101
GpsState::week
uint16_t week
GPS week.
Definition: gps.h:111
gps_feed_value
void gps_feed_value(void)
Definition: gps_sim_nps.c:31
NedCoor_d::z
double z
in meters
Definition: pprz_geodetic_double.h:70
NedCoor_d
vector in North East Down coordinates Units: meters
Definition: pprz_geodetic_double.h:67
GpsState::gspeed
uint16_t gspeed
norm of 2d ground speed in cm/s
Definition: gps.h:100
GpsState::last_3dfix_time
uint32_t last_3dfix_time
cpu time in sec at last valid 3D fix
Definition: gps.h:118
nps_sensors.h
GpsState::last_3dfix_ticks
uint32_t last_3dfix_ticks
cpu time ticks at last valid 3D fix
Definition: gps.h:117
LtpDef_d
definition of the local (flat earth) coordinate system
Definition: pprz_geodetic_double.h:97
GpsState::course
int32_t course
GPS course over ground in rad*1e7, [0, 2*Pi]*1e7 (CW/north)
Definition: gps.h:102
GpsState::last_msg_time
uint32_t last_msg_time
cpu time in sec at last received GPS message
Definition: gps.h:120
GpsState::ecef_pos
struct EcefCoor_i ecef_pos
position in ECEF in cm
Definition: gps.h:94
gps_sim_nps.h
NpsFdm::ltp_to_body_eulers
struct DoubleEulers ltp_to_body_eulers
Definition: nps_fdm.h:92
GPS_VALID_VEL_NED_BIT
#define GPS_VALID_VEL_NED_BIT
Definition: gps.h:55
GpsState::lla_pos
struct LlaCoor_i lla_pos
position in LLA (lat,lon: deg*1e7; alt: mm over ellipsoid)
Definition: gps.h:95
GPS_VALID_VEL_ECEF_BIT
#define GPS_VALID_VEL_ECEF_BIT
Definition: gps.h:54
sys_time
Definition: sys_time.h:71
GpsState::num_sv
uint8_t num_sv
number of sat in fix
Definition: gps.h:109
sys_time::nb_sec_rem
volatile uint32_t nb_sec_rem
remainder of seconds since startup in CPU_TICKS
Definition: sys_time.h:73
GpsState::hmsl
int32_t hmsl
height above mean sea level (MSL) in mm
Definition: gps.h:97
ned_of_ecef_vect_d
void ned_of_ecef_vect_d(struct NedCoor_d *ned, struct LtpDef_d *def, struct EcefCoor_d *ecef)
Definition: pprz_geodetic_double.c:159
NedCoor_i::x
int32_t x
North.
Definition: pprz_geodetic_int.h:69
GpsState::pdop
uint16_t pdop
position dilution of precision scaled by 100
Definition: gps.h:108
gps_nps_init
void gps_nps_init(void)
Definition: gps_sim_nps.c:103
GPS_VALID_POS_ECEF_BIT
#define GPS_VALID_POS_ECEF_BIT
Definition: gps.h:51
sys_time::nb_sec
volatile uint32_t nb_sec
full seconds since startup
Definition: sys_time.h:72
GpsState::ecef_vel
struct EcefCoor_i ecef_vel
speed ECEF in cm/s
Definition: gps.h:98
GPS_FIX_NONE
#define GPS_FIX_NONE
No GPS fix.
Definition: gps.h:40
GPS_FIX_3D
#define GPS_FIX_3D
3D GPS fix
Definition: gps.h:42
GPS_VALID_HMSL_BIT
#define GPS_VALID_HMSL_BIT
Definition: gps.h:56
sensors
#define sensors(...)
Definition: cc2500_compat.h:68