Paparazzi UAS v7.1_unstable
Paparazzi is a free software Unmanned Aircraft System.
Loading...
Searching...
No Matches
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
22#include <time.h>
24#include "modules/gps/gps.h"
25#include "modules/core/abi.h"
26#include "nps_sensors.h"
27#include "nps_fdm.h"
29
30struct GpsState gps_nps = {0};
32
34#ifndef GPS_NPS_PACC
35#define GPS_NPS_PACC 200
36#endif
37
39#ifndef GPS_NPS_HACC
40#define GPS_NPS_HACC 120
41#endif
42
44#ifndef GPS_NPS_VACC
45#define GPS_NPS_VACC 180
46#endif
47
49#ifndef GPS_NPS_SACC
50#define GPS_NPS_SACC 20
51#endif
52
54#ifndef GPS_NPS_PDOP
55#define GPS_NPS_PDOP 140
56#endif
57
59{
60 // GPS time from the host clock at the start of the simulation
61 static uint64_t gps_start_ms = 0;
62 if (gps_start_ms == 0) {
63 struct timespec now;
64 clock_gettime(CLOCK_REALTIME, &now);
65 uint64_t unix_ms = (uint64_t)now.tv_sec * 1000 + now.tv_nsec / 1000000;
66 gps_start_ms = unix_ms - SYS_TIME_UNIX_GPS_EPOCH_MS + SYS_TIME_GPS_LEAP_SECONDS * 1000ULL
67 - (uint64_t)(fdm.time * 1000);
68 }
69 uint64_t gps_ms = gps_start_ms + (uint64_t)(fdm.time * 1000);
70 gps_nps.week = gps_ms / SYS_TIME_MSEC_PER_WEEK;
71 gps_nps.tow = gps_ms % SYS_TIME_MSEC_PER_WEEK;
72
73 gps_nps.ecef_pos.x = sensors.gps.ecef_pos.x * 100.;
74 gps_nps.ecef_pos.y = sensors.gps.ecef_pos.y * 100.;
75 gps_nps.ecef_pos.z = sensors.gps.ecef_pos.z * 100.;
76 SetBit(gps_nps.valid_fields, GPS_VALID_POS_ECEF_BIT);
77 gps_nps.ecef_vel.x = sensors.gps.ecef_vel.x * 100.;
78 gps_nps.ecef_vel.y = sensors.gps.ecef_vel.y * 100.;
79 gps_nps.ecef_vel.z = sensors.gps.ecef_vel.z * 100.;
80 SetBit(gps_nps.valid_fields, GPS_VALID_VEL_ECEF_BIT);
81
82 //ecef pos seems to be based on geocentric model, hence we get a very high alt when converted to lla
83 gps_nps.lla_pos.lat = DegOfRad(sensors.gps.lla_pos.lat) * 1e7;
84 gps_nps.lla_pos.lon = DegOfRad(sensors.gps.lla_pos.lon) * 1e7;
85 gps_nps.lla_pos.alt = sensors.gps.lla_pos.alt * 1000.;
86 SetBit(gps_nps.valid_fields, GPS_VALID_POS_LLA_BIT);
87
88 gps_nps.hmsl = sensors.gps.hmsl * 1000.;
89 SetBit(gps_nps.valid_fields, GPS_VALID_HMSL_BIT);
90
91 /* calc NED speed from ECEF */
92 struct LtpDef_d ref_ltp;
93 ltp_def_from_ecef_d(&ref_ltp, &sensors.gps.ecef_pos);
94 struct NedCoor_d ned_vel_d;
95 ned_of_ecef_vect_d(&ned_vel_d, &ref_ltp, &sensors.gps.ecef_vel);
96 gps_nps.ned_vel.x = ned_vel_d.x * 100;
97 gps_nps.ned_vel.y = ned_vel_d.y * 100;
98 gps_nps.ned_vel.z = ned_vel_d.z * 100;
99 SetBit(gps_nps.valid_fields, GPS_VALID_VEL_NED_BIT);
100
101 /* horizontal and 3d ground speed in cm/s */
102 gps_nps.gspeed = sqrt(ned_vel_d.x * ned_vel_d.x + ned_vel_d.y * ned_vel_d.y) * 100;
103 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;
104
105#if GpsId(PRIMARY_GPS) == GPS_DATALINK_ID
106 /* vehicle heading in radians * 1e7 */
107 gps_nps.course = fdm.ltp_to_body_eulers.psi * 1e7;
108#else
109 /* ground course in radians * 1e7 */
110 gps_nps.course = atan2(ned_vel_d.y, ned_vel_d.x) * 1e7;
111#endif
112 SetBit(gps_nps.valid_fields, GPS_VALID_COURSE_BIT);
113
114 /* accuracies of a good GNSS receiver with a clear sky view */
115 gps_nps.pacc = GPS_NPS_PACC;
116 gps_nps.hacc = GPS_NPS_HACC;
117 gps_nps.vacc = GPS_NPS_VACC;
118 gps_nps.sacc = GPS_NPS_SACC;
119 gps_nps.pdop = GPS_NPS_PDOP;
120
121 if (gps_has_fix) {
122 gps_nps.num_sv = 11;
123 gps_nps.fix = GPS_FIX_3D;
124 } else {
125 gps_nps.num_sv = 1;
126 gps_nps.fix = GPS_FIX_NONE;
127 }
128
129 // publish gps data
130 uint32_t now_ts = get_sys_time_usec();
131 gps_nps.last_msg_ticks = sys_time.nb_sec_rem;
132 gps_nps.last_msg_time = sys_time.nb_sec;
133 if (gps_nps.fix == GPS_FIX_3D) {
134 gps_nps.last_3dfix_ticks = sys_time.nb_sec_rem;
135 gps_nps.last_3dfix_time = sys_time.nb_sec;
136 }
137 AbiSendMsgGPS(GPS_SIM_ID, now_ts, &gps_nps);
138}
139
140void gps_nps_init(void)
141{
142 gps_has_fix = true;
143}
144
Main include for ABI (AirBorneInterface).
#define GPS_SIM_ID
#define sensors(...)
uint32_t get_sys_time_usec(void)
Get the time in microseconds since startup.
Device independent GPS code (interface).
#define GPS_VALID_VEL_ECEF_BIT
Definition gps.h:51
#define GPS_VALID_VEL_NED_BIT
Definition gps.h:52
#define GPS_VALID_POS_LLA_BIT
Definition gps.h:49
#define GPS_FIX_NONE
No GPS fix.
Definition gps.h:42
#define GPS_VALID_POS_ECEF_BIT
Definition gps.h:48
#define GPS_VALID_HMSL_BIT
Definition gps.h:53
#define GPS_FIX_3D
3D GPS fix
Definition gps.h:44
#define GPS_VALID_COURSE_BIT
Definition gps.h:54
data structure for GPS information
Definition gps.h:90
void gps_feed_value(void)
Definition gps_sim_nps.c:58
#define GPS_NPS_PACC
Simulated position accuracy (cm).
Definition gps_sim_nps.c:35
bool gps_has_fix
Definition gps_sim_nps.c:31
#define GPS_NPS_PDOP
Simulated position dilution of precision (x100).
Definition gps_sim_nps.c:55
struct GpsState gps_nps
Definition gps_sim_nps.c:30
#define GPS_NPS_VACC
Simulated vertical position accuracy (cm).
Definition gps_sim_nps.c:45
#define GPS_NPS_SACC
Simulated speed accuracy (cm/s).
Definition gps_sim_nps.c:50
void gps_nps_init(void)
#define GPS_NPS_HACC
Simulated horizontal position accuracy (cm).
Definition gps_sim_nps.c:40
double y
in meters
double z
in meters
double x
in meters
void ltp_def_from_ecef_d(struct LtpDef_d *def, struct EcefCoor_d *ecef)
void ned_of_ecef_vect_d(struct NedCoor_d *ned, struct LtpDef_d *def, struct EcefCoor_d *ecef)
definition of the local (flat earth) coordinate system
vector in North East Down coordinates Units: meters
struct NpsFdm fdm
Holds all necessary NPS FDM state information.
volatile uint32_t nb_sec
full seconds since startup
Definition sys_time.h:80
#define SYS_TIME_GPS_LEAP_SECONDS
Difference between GPS time and UTC.
Definition sys_time.h:65
volatile uint32_t nb_sec_rem
remainder of seconds since startup in CPU_TICKS
Definition sys_time.h:81
#define SYS_TIME_MSEC_PER_WEEK
Definition sys_time.h:60
#define SYS_TIME_UNIX_GPS_EPOCH_MS
unix time of the GPS epoch (1980-01-06) in ms
Definition sys_time.h:61
unsigned int uint32_t
Typedef defining 32 bit unsigned int type.
unsigned long long uint64_t