Paparazzi UAS  v6.0_unstable-92-g17422e4-dirty
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
gps_datalink.c
Go to the documentation of this file.
1 
2 /*
3  * Copyright (C) 2014 Freek van Tienen
4  *
5  * This file is part of paparazzi.
6  *
7  * paparazzi is free software; you can redistribute it and/or modify
8  * it under the terms of the GNU General Public License as published by
9  * the Free Software Foundation; either version 2, or (at your option)
10  * any later version.
11  *
12  * paparazzi is distributed in the hope that it will be useful,
13  * but WITHOUT ANY WARRANTY; without even the implied warranty of
14  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
15  * GNU General Public License for more details.
16  *
17  * You should have received a copy of the GNU General Public License
18  * along with paparazzi; see the file COPYING. If not, write to
19  * the Free Software Foundation, 59 Temple Place - Suite 330,
20  * Boston, MA 02111-1307, USA.
21  */
22 
31 #include "generated/flight_plan.h" // reference lla NAV_XXX0
32 
33 #include "subsystems/gps.h"
34 #include "subsystems/abi.h"
35 #include "subsystems/imu.h"
38 
40 #ifndef GPS_DATALINK_USE_MAG
41 #define GPS_DATALINK_USE_MAG 1
42 #endif
43 PRINT_CONFIG_VAR(GPS_DATALINK_USE_MAG)
44 
46 
48 
51 {
52  gps_datalink.fix = GPS_FIX_NONE;
53  gps_datalink.pdop = 10;
54  gps_datalink.sacc = 5;
55  gps_datalink.pacc = 1;
56  gps_datalink.cacc = 1;
57 
58  gps_datalink.comp_id = GPS_DATALINK_ID;
59 
60  struct LlaCoor_i llh_nav0; /* Height above the ellipsoid */
61  llh_nav0.lat = NAV_LAT0;
62  llh_nav0.lon = NAV_LON0;
63  /* NAV_ALT0 = ground alt above msl, NAV_MSL0 = geoid-height (msl) over ellipsoid */
64  llh_nav0.alt = NAV_ALT0 + NAV_MSL0;
65 
66  ltp_def_from_lla_i(&ltp_def, &llh_nav0);
67 }
68 
69 // Send GPS heading info as magnetometer messages
71 {
72  struct Int32Vect3 mag;
73  struct FloatVect3 mag_real;
74  // course from gps in [0, 2*Pi]*1e7 (CW/north)
75  float heading = course/1e7;
76  mag_real.x = cos(heading);
77  mag_real.y = -sin(heading);
78  mag_real.z = 0;
79  MAGS_BFP_OF_REAL(mag, mag_real);
80 
81  // update IMU information
84 
85  // Send fake ABI for GPS, Magnetometer and Optical Flow for GPS fusion
86  AbiSendMsgIMU_MAG_INT32(MAG_DATALINK_SENDER_ID, now_ts, &mag);
87 }
88 
89 // Parse the REMOTE_GPS_SMALL datalink packet
90 static void parse_gps_datalink_small(int16_t heading, uint32_t pos_xyz, uint32_t speed_xyz, uint32_t tow)
91 {
92  struct EnuCoor_i enu_pos, enu_speed;
93 
94  // Position in ENU coordinates
95  enu_pos.x = (int32_t)((pos_xyz >> 21) & 0x7FF); // bits 31-21 x position in cm
96  if (enu_pos.x & 0x400) {
97  enu_pos.x |= 0xFFFFF800; // sign extend for twos complements
98  }
99  enu_pos.y = (int32_t)((pos_xyz >> 10) & 0x7FF); // bits 20-10 y position in cm
100  if (enu_pos.y & 0x400) {
101  enu_pos.y |= 0xFFFFF800; // sign extend for twos complements
102  }
103  enu_pos.z = (int32_t)(pos_xyz & 0x3FF); // bits 9-0 z position in cm
104 
105  // Convert the ENU coordinates to ECEF
108 
111 
112  enu_speed.x = (int32_t)((speed_xyz >> 21) & 0x7FF); // bits 31-21 speed x in cm/s
113  if (enu_speed.x & 0x400) {
114  enu_speed.x |= 0xFFFFF800; // sign extend for twos complements
115  }
116  enu_speed.y = (int32_t)((speed_xyz >> 10) & 0x7FF); // bits 20-10 speed y in cm/s
117  if (enu_speed.y & 0x400) {
118  enu_speed.y |= 0xFFFFF800; // sign extend for twos complements
119  }
120  enu_speed.z = (int32_t)((speed_xyz) & 0x3FF); // bits 9-0 speed z in cm/s
121  if (enu_speed.z & 0x200) {
122  enu_speed.z |= 0xFFFFFC00; // sign extend for twos complements
123  }
124 
127 
130 
133 
134  gps_datalink.hmsl = ltp_def.hmsl + enu_pos.z * 10;
136 
137  gps_datalink.course = ((int32_t)heading) * 1e3;
139 
140  gps_datalink.num_sv = 30;
141  gps_datalink.tow = tow;
142  gps_datalink.fix = GPS_FIX_3D; // set 3D fix to true
143 
144  // set gps msg time
147 
150 
151  // publish new GPS data
152  uint32_t now_ts = get_sys_time_usec();
153  AbiSendMsgGPS(GPS_DATALINK_ID, now_ts, &gps_datalink);
154 }
155 
157 static void parse_gps_datalink(uint8_t numsv, int32_t ecef_x, int32_t ecef_y, int32_t ecef_z,
158  int32_t lat, int32_t lon, int32_t alt, int32_t hmsl,
159  int32_t ecef_xd, int32_t ecef_yd, int32_t ecef_zd,
160  uint32_t tow, int32_t course)
161 {
162  gps_datalink.lla_pos.lat = lat;
163  gps_datalink.lla_pos.lon = lon;
164  gps_datalink.lla_pos.alt = alt;
166 
167  gps_datalink.hmsl = hmsl;
169 
170  gps_datalink.ecef_pos.x = ecef_x;
171  gps_datalink.ecef_pos.y = ecef_y;
172  gps_datalink.ecef_pos.z = ecef_z;
174 
175  gps_datalink.ecef_vel.x = ecef_xd;
176  gps_datalink.ecef_vel.y = ecef_yd;
177  gps_datalink.ecef_vel.z = ecef_zd;
179 
182 
185 
188 
189  gps_datalink.num_sv = numsv;
190  gps_datalink.tow = tow;
192 
193  // set gps msg time
196 
199 
200  uint32_t now_ts = get_sys_time_usec();
201 
202  // if selected, publish magnetometer data
203  #if GPS_DATALINK_USE_MAG
204  send_magnetometer(course, now_ts);
205  #endif
206 
207  // publish new GPS data
208  AbiSendMsgGPS(GPS_DATALINK_ID, now_ts, &gps_datalink);
209 }
210 
212 static void parse_gps_datalink_local(float enu_x, float enu_y, float enu_z,
213  float enu_xd, float enu_yd, float enu_zd,
214  uint32_t tow, float course)
215 {
216 
217  struct EnuCoor_i enu_pos, enu_speed;
218 
219  // Position in ENU coordinates
220  enu_pos.x = (int32_t)CM_OF_M(enu_x);
221  enu_pos.y = (int32_t)CM_OF_M(enu_y);
222  enu_pos.z = (int32_t)CM_OF_M(enu_z);
223 
224  // Convert the ENU coordinates to ECEF
227 
230 
231  enu_speed.x = (int32_t)CM_OF_M(enu_xd);
232  enu_speed.y = (int32_t)CM_OF_M(enu_yd);
233  enu_speed.z = (int32_t)CM_OF_M(enu_zd);
234 
237 
240 
243 
244  gps_datalink.hmsl = ltp_def.hmsl + enu_pos.z * 10;
246 
247  gps_datalink.course = (int32_t)(RadOfDeg(course)*1e7);
249 
250  gps_datalink.num_sv = 7;
251  gps_datalink.tow = tow;
252  gps_datalink.fix = GPS_FIX_3D; // set 3D fix to true
253 
254  // set gps msg time
257 
260 
261  uint32_t now_ts = get_sys_time_usec();
262 
263  // if selected, publish magnetometer data
264  #if GPS_DATALINK_USE_MAG
265  send_magnetometer(course, now_ts);
266  #endif
267 
268  // Publish GPS data
269  AbiSendMsgGPS(GPS_DATALINK_ID, now_ts, &gps_datalink);
270 }
271 
273 {
274  if (DL_REMOTE_GPS_ac_id(buf) != AC_ID) { return; } // not for this aircraft
275 
276  parse_gps_datalink(DL_REMOTE_GPS_numsv(buf),
277  DL_REMOTE_GPS_ecef_x(buf),
278  DL_REMOTE_GPS_ecef_y(buf),
279  DL_REMOTE_GPS_ecef_z(buf),
280  DL_REMOTE_GPS_lat(buf),
281  DL_REMOTE_GPS_lon(buf),
282  DL_REMOTE_GPS_alt(buf),
283  DL_REMOTE_GPS_hmsl(buf),
284  DL_REMOTE_GPS_ecef_xd(buf),
285  DL_REMOTE_GPS_ecef_yd(buf),
286  DL_REMOTE_GPS_ecef_zd(buf),
287  DL_REMOTE_GPS_tow(buf),
288  DL_REMOTE_GPS_course(buf));
289 }
290 
292 {
293  if (DL_REMOTE_GPS_SMALL_ac_id(buf) != AC_ID) { return; } // not for this aircraft
294 
295  parse_gps_datalink_small(DL_REMOTE_GPS_SMALL_heading(buf),
296  DL_REMOTE_GPS_SMALL_pos_xyz(buf),
297  DL_REMOTE_GPS_SMALL_speed_xyz(buf),
298  DL_REMOTE_GPS_SMALL_tow(buf));
299 }
300 
302 {
303  if (DL_REMOTE_GPS_LOCAL_ac_id(buf) != AC_ID) { return; } // not for this aircraft
304 
305  parse_gps_datalink_local(DL_REMOTE_GPS_LOCAL_enu_x(buf),
306  DL_REMOTE_GPS_LOCAL_enu_y(buf),
307  DL_REMOTE_GPS_LOCAL_enu_z(buf),
308  DL_REMOTE_GPS_LOCAL_enu_xd(buf),
309  DL_REMOTE_GPS_LOCAL_enu_yd(buf),
310  DL_REMOTE_GPS_LOCAL_enu_zd(buf),
311  DL_REMOTE_GPS_LOCAL_tow(buf),
312  DL_REMOTE_GPS_LOCAL_course(buf));
313 }
int32_t z
in centimeters
uint32_t pacc
position accuracy in cm
Definition: gps.h:100
definition of the local (flat earth) coordinate system
int32_t y
North.
int32_t x
East.
float heading
Definition: wedgebug.c:258
#define GPS_VALID_VEL_NED_BIT
Definition: gps.h:52
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.
uint8_t valid_fields
bitfield indicating valid fields (GPS_VALID_x_BIT)
Definition: gps.h:88
int32_t y
in centimeters
#define VECT3_COPY(_a, _b)
Definition: pprz_algebra.h:140
#define CM_OF_M(_m)
Main include for ABI (AirBorneInterface).
short int16_t
Typedef defining 16 bit short type.
Definition: vl53l1_types.h:93
struct Imu imu
global IMU state
Definition: imu.c:108
vector in Latitude, Longitude and Altitude
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
void ltp_def_from_lla_i(struct LtpDef_i *def, struct LlaCoor_i *lla)
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.
int32_t hmsl
Height above mean sea level in mm.
#define FLOAT_VECT3_NORM(_v)
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
uint32_t last_msg_time
cpu time in sec at last received GPS message
Definition: gps.h:117
uint32_t cacc
course accuracy in rad*1e7
Definition: gps.h:104
struct Int32Vect3 mag_unscaled
unscaled magnetometer measurements
Definition: imu.h:48
#define GPS_VALID_COURSE_BIT
Definition: gps.h:54
#define MAGS_BFP_OF_REAL(_ef, _ei)
Definition: pprz_algebra.h:813
#define FLOAT_VECT2_NORM(_v)
int32_t hmsl
height above mean sea level (MSL) in mm
Definition: gps.h:94
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)
uint16_t pdop
position dilution of precision scaled by 100
Definition: gps.h:105
struct EcefCoor_i ecef_pos
position in ECEF in cm
Definition: gps.h:91
#define MAG_DATALINK_SENDER_ID
int int32_t
Typedef defining 32 bit int type.
Definition: vl53l1_types.h:83
#define GPS_VALID_HMSL_BIT
Definition: gps.h:53
int32_t lon
in degrees*1e7
int32_t z
Up.
Inertial Measurement Unit interface.
volatile uint32_t nb_sec_rem
remainder of seconds since startup in CPU_TICKS
Definition: sys_time.h:69
#define VECT3_NED_OF_ENU(_o, _i)
uint32_t last_3dfix_time
cpu time in sec at last valid 3D fix
Definition: gps.h:115
vector in East North Up coordinates
void imu_scale_mag(struct Imu *_imu)
Definition: ahrs_gx3.c:352
int32_t course
GPS course over ground in rad*1e7, [0, 2*Pi]*1e7 (CW/north)
Definition: gps.h:99
uint8_t comp_id
id of current gps
Definition: gps.h:89
volatile uint32_t nb_sec
full seconds since startup
Definition: sys_time.h:68
static int16_t course[3]
Definition: airspeed_uADC.c:58
#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
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
void lla_of_ecef_i(struct LlaCoor_i *out, struct EcefCoor_i *in)
Convert a ECEF to LLA.
uint32_t get_sys_time_usec(void)
Get the time in microseconds since startup.
Definition: sys_time_arch.c:71
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
#define GPS_DATALINK_ID
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.
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.
Definition: vl53l1_types.h:98
unsigned int uint32_t
Typedef defining 32 bit unsigned int type.
Definition: vl53l1_types.h:78