Paparazzi UAS  v5.18.0_stable
Paparazzi is a free software Unmanned Aircraft System.
gps_datalink.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2014 Freek van Tienen
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 
30 #include "generated/flight_plan.h" // reference lla NAV_XXX0
31 
32 #include "subsystems/gps.h"
33 #include "subsystems/abi.h"
34 #include "subsystems/datalink/datalink.h"
35 
36 struct LtpDef_i ltp_def;
37 
38 struct GpsState gps_datalink;
39 
41 void gps_datalink_init(void)
42 {
43  gps_datalink.fix = GPS_FIX_NONE;
44  gps_datalink.pdop = 0;
45  gps_datalink.sacc = 0;
46  gps_datalink.pacc = 0;
47  gps_datalink.cacc = 0;
48 
49  gps_datalink.comp_id = GPS_DATALINK_ID;
50 
51  struct LlaCoor_i llh_nav0; /* Height above the ellipsoid */
52  llh_nav0.lat = NAV_LAT0;
53  llh_nav0.lon = NAV_LON0;
54  /* NAV_ALT0 = ground alt above msl, NAV_MSL0 = geoid-height (msl) over ellipsoid */
55  llh_nav0.alt = NAV_ALT0 + NAV_MSL0;
56 
57  ltp_def_from_lla_i(&ltp_def, &llh_nav0);
58 }
59 
60 // Parse the REMOTE_GPS_SMALL datalink packet
61 static void parse_gps_datalink_small(int16_t heading, uint32_t pos_xyz, uint32_t speed_xyz, uint32_t tow)
62 {
63  struct EnuCoor_i enu_pos, enu_speed;
64 
65  // Position in ENU coordinates
66  enu_pos.x = (int32_t)((pos_xyz >> 21) & 0x7FF); // bits 31-21 x position in cm
67  if (enu_pos.x & 0x400) {
68  enu_pos.x |= 0xFFFFF800; // sign extend for twos complements
69  }
70  enu_pos.y = (int32_t)((pos_xyz >> 10) & 0x7FF); // bits 20-10 y position in cm
71  if (enu_pos.y & 0x400) {
72  enu_pos.y |= 0xFFFFF800; // sign extend for twos complements
73  }
74  enu_pos.z = (int32_t)(pos_xyz & 0x3FF); // bits 9-0 z position in cm
75 
76  // Convert the ENU coordinates to ECEF
77  ecef_of_enu_point_i(&gps_datalink.ecef_pos, &ltp_def, &enu_pos);
78  SetBit(gps_datalink.valid_fields, GPS_VALID_POS_ECEF_BIT);
79 
80  lla_of_ecef_i(&gps_datalink.lla_pos, &gps_datalink.ecef_pos);
81  SetBit(gps_datalink.valid_fields, GPS_VALID_POS_LLA_BIT);
82 
83  enu_speed.x = (int32_t)((speed_xyz >> 21) & 0x7FF); // bits 31-21 speed x in cm/s
84  if (enu_speed.x & 0x400) {
85  enu_speed.x |= 0xFFFFF800; // sign extend for twos complements
86  }
87  enu_speed.y = (int32_t)((speed_xyz >> 10) & 0x7FF); // bits 20-10 speed y in cm/s
88  if (enu_speed.y & 0x400) {
89  enu_speed.y |= 0xFFFFF800; // sign extend for twos complements
90  }
91  enu_speed.z = (int32_t)((speed_xyz) & 0x3FF); // bits 9-0 speed z in cm/s
92  if (enu_speed.z & 0x200) {
93  enu_speed.z |= 0xFFFFFC00; // sign extend for twos complements
94  }
95 
96  VECT3_NED_OF_ENU(gps_datalink.ned_vel, enu_speed);
97  SetBit(gps_datalink.valid_fields, GPS_VALID_VEL_NED_BIT);
98 
99  ecef_of_enu_vect_i(&gps_datalink.ecef_vel , &ltp_def , &enu_speed);
100  SetBit(gps_datalink.valid_fields, GPS_VALID_VEL_ECEF_BIT);
101 
102  gps_datalink.gspeed = (int16_t)FLOAT_VECT2_NORM(enu_speed);
103  gps_datalink.speed_3d = (int16_t)FLOAT_VECT3_NORM(enu_speed);
104 
105  gps_datalink.hmsl = ltp_def.hmsl + enu_pos.z * 10;
106  SetBit(gps_datalink.valid_fields, GPS_VALID_HMSL_BIT);
107 
108  gps_datalink.course = ((int32_t)heading) * 1e3;
109  SetBit(gps_datalink.valid_fields, GPS_VALID_COURSE_BIT);
110 
111  gps_datalink.num_sv = 7;
112  gps_datalink.tow = tow;
113  gps_datalink.fix = GPS_FIX_3D; // set 3D fix to true
114 
115  // set gps msg time
116  gps_datalink.last_msg_ticks = sys_time.nb_sec_rem;
117  gps_datalink.last_msg_time = sys_time.nb_sec;
118 
119  gps_datalink.last_3dfix_ticks = sys_time.nb_sec_rem;
120  gps_datalink.last_3dfix_time = sys_time.nb_sec;
121 
122  // publish new GPS data
123  uint32_t now_ts = get_sys_time_usec();
124  AbiSendMsgGPS(GPS_DATALINK_ID, now_ts, &gps_datalink);
125 }
126 
128 static void parse_gps_datalink(uint8_t numsv, int32_t ecef_x, int32_t ecef_y, int32_t ecef_z,
129  int32_t lat, int32_t lon, int32_t alt, int32_t hmsl,
130  int32_t ecef_xd, int32_t ecef_yd, int32_t ecef_zd,
131  uint32_t tow, int32_t course)
132 {
133  gps_datalink.lla_pos.lat = lat;
134  gps_datalink.lla_pos.lon = lon;
135  gps_datalink.lla_pos.alt = alt;
136  SetBit(gps_datalink.valid_fields, GPS_VALID_POS_LLA_BIT);
137 
138  gps_datalink.hmsl = hmsl;
139  SetBit(gps_datalink.valid_fields, GPS_VALID_HMSL_BIT);
140 
141  gps_datalink.ecef_pos.x = ecef_x;
142  gps_datalink.ecef_pos.y = ecef_y;
143  gps_datalink.ecef_pos.z = ecef_z;
144  SetBit(gps_datalink.valid_fields, GPS_VALID_POS_ECEF_BIT);
145 
146  gps_datalink.ecef_vel.x = ecef_xd;
147  gps_datalink.ecef_vel.y = ecef_yd;
148  gps_datalink.ecef_vel.z = ecef_zd;
149  SetBit(gps_datalink.valid_fields, GPS_VALID_VEL_ECEF_BIT);
150 
151  ned_of_ecef_vect_i(&gps_datalink.ned_vel, &ltp_def , &gps_datalink.ecef_vel);
152  SetBit(gps_datalink.valid_fields, GPS_VALID_VEL_NED_BIT);
153 
154  gps_datalink.gspeed = (int16_t)FLOAT_VECT2_NORM(gps_datalink.ned_vel);
155  gps_datalink.speed_3d = (int16_t)FLOAT_VECT3_NORM(gps_datalink.ned_vel);
156 
157  gps_datalink.course = course;
158  SetBit(gps_datalink.valid_fields, GPS_VALID_COURSE_BIT);
159 
160  gps_datalink.num_sv = numsv;
161  gps_datalink.tow = tow;
162  gps_datalink.fix = GPS_FIX_3D;
163 
164  // set gps msg time
165  gps_datalink.last_msg_ticks = sys_time.nb_sec_rem;
166  gps_datalink.last_msg_time = sys_time.nb_sec;
167 
168  gps_datalink.last_3dfix_ticks = sys_time.nb_sec_rem;
169  gps_datalink.last_3dfix_time = sys_time.nb_sec;
170 
171  // publish new GPS data
172  uint32_t now_ts = get_sys_time_usec();
173  AbiSendMsgGPS(GPS_DATALINK_ID, now_ts, &gps_datalink);
174 }
175 
177 static void parse_gps_datalink_local(float enu_x, float enu_y, float enu_z,
178  float enu_xd, float enu_yd, float enu_zd,
179  uint32_t tow, float course)
180 {
181 
182  struct EnuCoor_i enu_pos, enu_speed;
183 
184  // Position in ENU coordinates
185  enu_pos.x = (int32_t)CM_OF_M(enu_x);
186  enu_pos.y = (int32_t)CM_OF_M(enu_y);
187  enu_pos.z = (int32_t)CM_OF_M(enu_z);
188 
189  // Convert the ENU coordinates to ECEF
190  ecef_of_enu_point_i(&gps_datalink.ecef_pos, &ltp_def, &enu_pos);
191  SetBit(gps_datalink.valid_fields, GPS_VALID_POS_ECEF_BIT);
192 
193  lla_of_ecef_i(&gps_datalink.lla_pos, &gps_datalink.ecef_pos);
194  SetBit(gps_datalink.valid_fields, GPS_VALID_POS_LLA_BIT);
195 
196  enu_speed.x = (int32_t)CM_OF_M(enu_xd);
197  enu_speed.y = (int32_t)CM_OF_M(enu_yd);
198  enu_speed.z = (int32_t)CM_OF_M(enu_zd);
199 
200  VECT3_NED_OF_ENU(gps_datalink.ned_vel, enu_speed);
201  SetBit(gps_datalink.valid_fields, GPS_VALID_VEL_NED_BIT);
202 
203  ecef_of_enu_vect_i(&gps_datalink.ecef_vel , &ltp_def , &enu_speed);
204  SetBit(gps_datalink.valid_fields, GPS_VALID_VEL_ECEF_BIT);
205 
206  gps_datalink.gspeed = (int16_t)FLOAT_VECT2_NORM(enu_speed);
207  gps_datalink.speed_3d = (int16_t)FLOAT_VECT3_NORM(enu_speed);
208 
209  gps_datalink.hmsl = ltp_def.hmsl + enu_pos.z * 10;
210  SetBit(gps_datalink.valid_fields, GPS_VALID_HMSL_BIT);
211 
212  gps_datalink.course = (int32_t)(RadOfDeg(course)*1e7);
213  SetBit(gps_datalink.valid_fields, GPS_VALID_COURSE_BIT);
214 
215  gps_datalink.num_sv = 7;
216  gps_datalink.tow = tow;
217  gps_datalink.fix = GPS_FIX_3D; // set 3D fix to true
218 
219  // set gps msg time
220  gps_datalink.last_msg_ticks = sys_time.nb_sec_rem;
221  gps_datalink.last_msg_time = sys_time.nb_sec;
222 
223  gps_datalink.last_3dfix_ticks = sys_time.nb_sec_rem;
224  gps_datalink.last_3dfix_time = sys_time.nb_sec;
225 
226  // publish new GPS data
227  uint32_t now_ts = get_sys_time_usec();
228  AbiSendMsgGPS(GPS_DATALINK_ID, now_ts, &gps_datalink);
229 }
230 
231 void gps_datalink_parse_REMOTE_GPS(uint8_t *buf)
232 {
233  if (DL_REMOTE_GPS_ac_id(buf) != AC_ID) { return; } // not for this aircraft
234 
235  parse_gps_datalink(DL_REMOTE_GPS_numsv(buf),
236  DL_REMOTE_GPS_ecef_x(buf),
237  DL_REMOTE_GPS_ecef_y(buf),
238  DL_REMOTE_GPS_ecef_z(buf),
239  DL_REMOTE_GPS_lat(buf),
240  DL_REMOTE_GPS_lon(buf),
241  DL_REMOTE_GPS_alt(buf),
242  DL_REMOTE_GPS_hmsl(buf),
243  DL_REMOTE_GPS_ecef_xd(buf),
244  DL_REMOTE_GPS_ecef_yd(buf),
245  DL_REMOTE_GPS_ecef_zd(buf),
246  DL_REMOTE_GPS_tow(buf),
247  DL_REMOTE_GPS_course(buf));
248 }
249 
250 void gps_datalink_parse_REMOTE_GPS_SMALL(uint8_t *buf)
251 {
252  if (DL_REMOTE_GPS_SMALL_ac_id(buf) != AC_ID) { return; } // not for this aircraft
253 
254  parse_gps_datalink_small(DL_REMOTE_GPS_SMALL_heading(buf),
255  DL_REMOTE_GPS_SMALL_pos_xyz(buf),
256  DL_REMOTE_GPS_SMALL_speed_xyz(buf),
257  DL_REMOTE_GPS_SMALL_tow(buf));
258 }
259 
260 void gps_datalink_parse_REMOTE_GPS_LOCAL(uint8_t *buf)
261 {
262  if (DL_REMOTE_GPS_LOCAL_ac_id(buf) != AC_ID) { return; } // not for this aircraft
263 
264  parse_gps_datalink_local(DL_REMOTE_GPS_LOCAL_enu_x(buf),
265  DL_REMOTE_GPS_LOCAL_enu_y(buf),
266  DL_REMOTE_GPS_LOCAL_enu_z(buf),
267  DL_REMOTE_GPS_LOCAL_enu_xd(buf),
268  DL_REMOTE_GPS_LOCAL_enu_yd(buf),
269  DL_REMOTE_GPS_LOCAL_enu_zd(buf),
270  DL_REMOTE_GPS_LOCAL_tow(buf),
271  DL_REMOTE_GPS_LOCAL_course(buf));
272 }
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:88
VECT3_NED_OF_ENU
#define VECT3_NED_OF_ENU(_o, _i)
Definition: pprz_geodetic_int.h:150
GPS_DATALINK_ID
#define GPS_DATALINK_ID
Definition: abi_sender_ids.h:240
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:54
abi.h
EnuCoor_i::y
int32_t y
North.
Definition: pprz_geodetic_int.h:79
lla_of_ecef_i
void lla_of_ecef_i(struct LlaCoor_i *out, struct EcefCoor_i *in)
Convert a ECEF to LLA.
Definition: pprz_geodetic_int.c:363
EnuCoor_i::x
int32_t x
East.
Definition: pprz_geodetic_int.h:78
GpsState::tow
uint32_t tow
GPS time of week in ms.
Definition: gps.h:109
LtpDef_i
definition of the local (flat earth) coordinate system
Definition: pprz_geodetic_int.h:98
GpsState
data structure for GPS information
Definition: gps.h:87
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:100
GpsState::sacc
uint32_t sacc
speed accuracy in cm/s
Definition: gps.h:103
uint32_t
unsigned long uint32_t
Definition: types.h:18
ecef_of_enu_vect_i
void ecef_of_enu_vect_i(struct EcefCoor_i *ecef, struct LtpDef_i *def, struct EnuCoor_i *enu)
Rotate a vector from ENU to ECEF.
Definition: pprz_geodetic_int.c:199
FLOAT_VECT3_NORM
#define FLOAT_VECT3_NORM(_v)
Definition: pprz_algebra_float.h:164
gps_datalink_parse_REMOTE_GPS_SMALL
void gps_datalink_parse_REMOTE_GPS_SMALL(uint8_t *buf)
Definition: gps_datalink.c:250
GpsState::ned_vel
struct NedCoor_i ned_vel
speed NED in cm/s
Definition: gps.h:96
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:49
get_sys_time_usec
uint32_t get_sys_time_usec(void)
Get the time in microseconds since startup.
Definition: sys_time_arch.c:68
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:116
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:107
GpsState::speed_3d
uint16_t speed_3d
norm of 3d speed in cm/s
Definition: gps.h:98
FLOAT_VECT2_NORM
#define FLOAT_VECT2_NORM(_v)
Definition: pprz_algebra_float.h:132
ltp_def
struct LtpDef_i ltp_def
Definition: gps_datalink.c:36
int16_t
signed short int16_t
Definition: types.h:17
gps_datalink_parse_REMOTE_GPS_LOCAL
void gps_datalink_parse_REMOTE_GPS_LOCAL(uint8_t *buf)
Definition: gps_datalink.c:260
uint8_t
unsigned char uint8_t
Definition: types.h:14
GpsState::gspeed
uint16_t gspeed
norm of 2d ground speed in cm/s
Definition: gps.h:97
gps_datalink_parse_REMOTE_GPS
void gps_datalink_parse_REMOTE_GPS(uint8_t *buf)
Definition: gps_datalink.c:231
parse_gps_datalink_small
static void parse_gps_datalink_small(int16_t heading, uint32_t pos_xyz, uint32_t speed_xyz, uint32_t tow)
Definition: gps_datalink.c:61
GpsState::last_3dfix_time
uint32_t last_3dfix_time
cpu time in sec at last valid 3D fix
Definition: gps.h:115
GpsState::last_3dfix_ticks
uint32_t last_3dfix_ticks
cpu time ticks at last valid 3D fix
Definition: gps.h:114
parse_gps_datalink
static void parse_gps_datalink(uint8_t numsv, int32_t ecef_x, int32_t ecef_y, int32_t ecef_z, int32_t lat, int32_t lon, int32_t alt, int32_t hmsl, int32_t ecef_xd, int32_t ecef_yd, int32_t ecef_zd, uint32_t tow, int32_t course)
Parse the REMOTE_GPS datalink packet.
Definition: gps_datalink.c:128
ned_of_ecef_vect_i
void ned_of_ecef_vect_i(struct NedCoor_i *ned, struct LtpDef_i *def, struct EcefCoor_i *ecef)
Rotate a vector from ECEF to NED.
Definition: pprz_geodetic_int.c:186
GpsState::comp_id
uint8_t comp_id
id of current gps
Definition: gps.h:89
ltp_def_from_lla_i
void ltp_def_from_lla_i(struct LtpDef_i *def, struct LlaCoor_i *lla)
Definition: pprz_geodetic_int.c:72
GpsState::course
int32_t course
GPS course over ground in rad*1e7, [0, 2*Pi]*1e7 (CW/north)
Definition: gps.h:99
GpsState::last_msg_time
uint32_t last_msg_time
cpu time in sec at last received GPS message
Definition: gps.h:117
GpsState::ecef_pos
struct EcefCoor_i ecef_pos
position in ECEF in cm
Definition: gps.h:91
course
static int16_t course[3]
Definition: airspeed_uADC.c:58
GPS_VALID_VEL_NED_BIT
#define GPS_VALID_VEL_NED_BIT
Definition: gps.h:52
LlaCoor_i
vector in Latitude, Longitude and Altitude
Definition: pprz_geodetic_int.h:59
gps_datalink_init
void gps_datalink_init(void)
GPS initialization.
Definition: gps_datalink.c:41
GpsState::lla_pos
struct LlaCoor_i lla_pos
position in LLA (lat,lon: deg*1e7; alt: mm over ellipsoid)
Definition: gps.h:92
LtpDef_i::hmsl
int32_t hmsl
Height above mean sea level in mm.
Definition: pprz_geodetic_int.h:102
int32_t
signed long int32_t
Definition: types.h:19
CM_OF_M
#define CM_OF_M(_m)
Definition: pprz_geodetic_int.h:131
GPS_VALID_VEL_ECEF_BIT
#define GPS_VALID_VEL_ECEF_BIT
Definition: gps.h:51
sys_time
Definition: sys_time.h:71
GpsState::num_sv
uint8_t num_sv
number of sat in fix
Definition: gps.h:106
sys_time::nb_sec_rem
volatile uint32_t nb_sec_rem
remainder of seconds since startup in CPU_TICKS
Definition: sys_time.h:73
parse_gps_datalink_local
static void parse_gps_datalink_local(float enu_x, float enu_y, float enu_z, float enu_xd, float enu_yd, float enu_zd, uint32_t tow, float course)
Parse the REMOTE_GPS_LOCAL datalink packet.
Definition: gps_datalink.c:177
GpsState::hmsl
int32_t hmsl
height above mean sea level (MSL) in mm
Definition: gps.h:94
gps_datalink
struct GpsState gps_datalink
Definition: gps_datalink.c:38
GpsState::pdop
uint16_t pdop
position dilution of precision scaled by 100
Definition: gps.h:105
ecef_of_enu_point_i
void ecef_of_enu_point_i(struct EcefCoor_i *ecef, struct LtpDef_i *def, struct EnuCoor_i *enu)
Convert a point in local ENU to ECEF.
Definition: pprz_geodetic_int.c:238
GPS_VALID_POS_ECEF_BIT
#define GPS_VALID_POS_ECEF_BIT
Definition: gps.h:48
GpsState::cacc
uint32_t cacc
course accuracy in rad*1e7
Definition: gps.h:104
EnuCoor_i::z
int32_t z
Up.
Definition: pprz_geodetic_int.h:80
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:95
GPS_FIX_NONE
#define GPS_FIX_NONE
No GPS fix.
Definition: gps.h:37
GPS_FIX_3D
#define GPS_FIX_3D
3D GPS fix
Definition: gps.h:39
EnuCoor_i
vector in East North Up coordinates
Definition: pprz_geodetic_int.h:77
GPS_VALID_HMSL_BIT
#define GPS_VALID_HMSL_BIT
Definition: gps.h:53
heading
float heading
Definition: wedgebug.c:258