Paparazzi UAS  v7.0_unstable
Paparazzi is a free software Unmanned Aircraft System.
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 "modules/gps/gps.h"
34 #include "modules/core/abi.h"
35 #include "modules/imu/imu.h"
38 
39 #ifndef EXTERNAL_POSE_SMALL_POS_RES
40 #define EXTERNAL_POSE_SMALL_POS_RES 1.0
41 #endif
42 
43 #ifndef EXTERNAL_POSE_SMALL_SPEED_RES
44 #define EXTERNAL_POSE_SMALL_SPEED_RES 1.0
45 #endif
46 
47 struct GpsState gps_datalink;
48 static struct LtpDef_i ltp_def;
49 
52 {
54  gps_datalink.pdop = 10;
55  gps_datalink.sacc = 5;
56  gps_datalink.pacc = 1;
57  gps_datalink.cacc = 1;
58 
60 
61  struct LlaCoor_i llh_nav0; /* Height above the ellipsoid */
62  llh_nav0.lat = NAV_LAT0;
63  llh_nav0.lon = NAV_LON0;
64  /* NAV_ALT0 = ground alt above msl, NAV_MSL0 = geoid-height (msl) over ellipsoid */
65  llh_nav0.alt = NAV_ALT0 + NAV_MSL0;
66 
67  ltp_def_from_lla_i(&ltp_def, &llh_nav0);
68 }
69 
71 static void gps_datalink_publish(uint32_t tow, struct EnuCoor_f *enu_pos, struct EnuCoor_f *enu_speed, float course)
72 {
73  struct EnuCoor_i enu_pos_i, enu_speed_i;
74 
75  enu_pos_i.x = (int32_t)CM_OF_M(enu_pos->x);
76  enu_pos_i.y = (int32_t)CM_OF_M(enu_pos->y);
77  enu_pos_i.z = (int32_t)CM_OF_M(enu_pos->z);
78 
79  // Convert the ENU coordinates to ECEF
82 
85 
86  enu_speed_i.x = (int32_t)CM_OF_M(enu_speed->x);
87  enu_speed_i.y = (int32_t)CM_OF_M(enu_speed->y);
88  enu_speed_i.z = (int32_t)CM_OF_M(enu_speed->z);
89 
92 
93  ecef_of_enu_vect_i(&gps_datalink.ecef_vel , &ltp_def , &enu_speed_i);
95 
98 
99  gps_datalink.hmsl = ltp_def.hmsl + enu_pos_i.z * 10;
101 
104 
105  gps_datalink.num_sv = 7;
106  gps_datalink.tow = tow;
107  gps_datalink.fix = GPS_FIX_3D; // set 3D fix to true
108 
109  // set gps msg time
112 
115 
116  uint32_t now_ts = get_sys_time_usec();
117 
118  // Publish GPS data
119  AbiSendMsgGPS(GPS_DATALINK_ID, now_ts, &gps_datalink);
120 }
121 
124 {
125  if (DL_EXTERNAL_POSE_ac_id(buf) != AC_ID) { return; } // not for this aircraft
126 
127  uint32_t tow = DL_EXTERNAL_POSE_timestamp(buf);
128 
129  struct EnuCoor_f enu_pos, enu_speed;
130  enu_pos.x = DL_EXTERNAL_POSE_enu_x(buf);
131  enu_pos.y = DL_EXTERNAL_POSE_enu_y(buf);
132  enu_pos.z = DL_EXTERNAL_POSE_enu_z(buf);
133  enu_speed.x = DL_EXTERNAL_POSE_enu_xd(buf);
134  enu_speed.y = DL_EXTERNAL_POSE_enu_yd(buf);
135  enu_speed.z = DL_EXTERNAL_POSE_enu_zd(buf);
136 
137  struct FloatQuat body_q; // converted to NED
138  body_q.qi = DL_EXTERNAL_POSE_body_qi(buf);
139  body_q.qx = DL_EXTERNAL_POSE_body_qy(buf);
140  body_q.qy = DL_EXTERNAL_POSE_body_qx(buf);
141  body_q.qz = -DL_EXTERNAL_POSE_body_qz(buf);
142 
143  struct FloatEulers body_e;
144  float_eulers_of_quat(&body_e, &body_q);
145  float heading = body_e.psi;
146 
147  gps_datalink_publish(tow, &enu_pos, &enu_speed, heading);
148 }
149 
152 {
153  if (DL_EXTERNAL_POSE_SMALL_ac_id(buf) != AC_ID) { return; } // not for this aircraft
154 
155  uint32_t tow = DL_EXTERNAL_POSE_SMALL_timestamp(buf);
156 
157  struct EnuCoor_i enu_pos_cm, enu_speed_cm;
158  struct EnuCoor_f enu_pos, enu_speed;
159 
160  /* Convert the 32 bit xyz position to cm seperated */
161  uint32_t enu_pos_u = DL_EXTERNAL_POSE_SMALL_enu_pos(buf);
162  enu_pos_cm.x = (int32_t)((enu_pos_u >> 21) & 0x7FF); // bits 31-21 x position in cm
163  if (enu_pos_cm.x & 0x400) {
164  enu_pos_cm.x |= 0xFFFFF800; // sign extend for twos complements
165  }
166  enu_pos_cm.y = (int32_t)((enu_pos_u >> 10) & 0x7FF); // bits 20-10 y position in cm
167  if (enu_pos_cm.y & 0x400) {
168  enu_pos_cm.y |= 0xFFFFF800; // sign extend for twos complements
169  }
170  enu_pos_cm.z = (int32_t)(enu_pos_u & 0x3FF); // bits 9-0 z position in cm
171 
172  /* Convert the cm position with a resolution to meters */
173  enu_pos.x = enu_pos_cm.x / 100.0f * EXTERNAL_POSE_SMALL_POS_RES;
174  enu_pos.y = enu_pos_cm.y / 100.0f * EXTERNAL_POSE_SMALL_POS_RES;
175  enu_pos.z = enu_pos_cm.z / 100.0f * EXTERNAL_POSE_SMALL_POS_RES;
176 
177  /* Convert the 32 bit xyz speed to cm seperated */
178  uint32_t enu_speed_u = DL_EXTERNAL_POSE_SMALL_enu_speed(buf);
179  enu_speed_cm.x = (int32_t)((enu_speed_u >> 21) & 0x7FF); // bits 31-21 speed x in cm/s
180  if (enu_speed_cm.x & 0x400) {
181  enu_speed_cm.x |= 0xFFFFF800; // sign extend for twos complements
182  }
183  enu_speed_cm.y = (int32_t)((enu_speed_u >> 10) & 0x7FF); // bits 20-10 speed y in cm/s
184  if (enu_speed_cm.y & 0x400) {
185  enu_speed_cm.y |= 0xFFFFF800; // sign extend for twos complements
186  }
187  enu_speed_cm.z = (int32_t)((enu_speed_u) & 0x3FF); // bits 9-0 speed z in cm/s
188  if (enu_speed_cm.z & 0x200) {
189  enu_speed_cm.z |= 0xFFFFFC00; // sign extend for twos complements
190  }
191 
192  /* Convert the cm/s speed with a resolution to meters/s */
193  enu_speed.x = enu_speed_cm.x / 100.0f * EXTERNAL_POSE_SMALL_SPEED_RES;
194  enu_speed.y = enu_speed_cm.y / 100.0f * EXTERNAL_POSE_SMALL_SPEED_RES;
195  enu_speed.z = enu_speed_cm.z / 100.0f * EXTERNAL_POSE_SMALL_SPEED_RES;
196 
197  /* Convert the heading with the 1e4 fraction to radians */
198  float heading = DL_EXTERNAL_POSE_SMALL_heading(buf) / 1e4;
199 
200  gps_datalink_publish(tow, &enu_pos, &enu_speed, heading);
201 }
202 
204 {
205  if (DL_REMOTE_GPS_LOCAL_ac_id(buf) != AC_ID) { return; } // not for this aircraft
206 
207  uint32_t tow = DL_REMOTE_GPS_LOCAL_tow(buf);
208 
209  struct EnuCoor_f enu_pos, enu_speed;
210  enu_pos.x = DL_REMOTE_GPS_LOCAL_enu_x(buf);
211  enu_pos.y = DL_REMOTE_GPS_LOCAL_enu_y(buf);
212  enu_pos.z = DL_REMOTE_GPS_LOCAL_enu_z(buf);
213 
214  enu_speed.x = DL_REMOTE_GPS_LOCAL_enu_xd(buf);
215  enu_speed.y = DL_REMOTE_GPS_LOCAL_enu_yd(buf);
216  enu_speed.z = DL_REMOTE_GPS_LOCAL_enu_zd(buf);
217 
218  float course = RadOfDeg(DL_REMOTE_GPS_LOCAL_course(buf));
219 
220  gps_datalink_publish(tow, &enu_pos, &enu_speed, course);
221 }
Main include for ABI (AirBorneInterface).
#define GPS_DATALINK_ID
static int16_t course[3]
Definition: airspeed_uADC.c:58
uint32_t get_sys_time_usec(void)
Get the time in microseconds since startup.
Definition: sys_time_arch.c:71
Device independent GPS code (interface)
uint32_t tow
GPS time of week in ms.
Definition: gps.h:108
int32_t hmsl
height above mean sea level (MSL) in mm
Definition: gps.h:93
struct LlaCoor_i lla_pos
position in LLA (lat,lon: deg*1e7; alt: mm over ellipsoid)
Definition: gps.h:91
uint32_t sacc
speed accuracy in cm/s
Definition: gps.h:102
uint32_t cacc
course accuracy in rad*1e7
Definition: gps.h:103
int32_t course
GPS course over ground in rad*1e7, [0, 2*Pi]*1e7 (CW/north)
Definition: gps.h:98
#define GPS_VALID_VEL_ECEF_BIT
Definition: gps.h:50
#define GPS_VALID_VEL_NED_BIT
Definition: gps.h:51
struct EcefCoor_i ecef_pos
position in ECEF in cm
Definition: gps.h:90
#define GPS_VALID_POS_LLA_BIT
Definition: gps.h:48
uint32_t last_3dfix_ticks
cpu time ticks at last valid 3D fix
Definition: gps.h:113
struct EcefCoor_i ecef_vel
speed ECEF in cm/s
Definition: gps.h:94
uint16_t pdop
position dilution of precision scaled by 100
Definition: gps.h:104
#define GPS_FIX_NONE
No GPS fix.
Definition: gps.h:41
#define GPS_VALID_POS_ECEF_BIT
Definition: gps.h:47
#define GPS_VALID_HMSL_BIT
Definition: gps.h:52
struct NedCoor_i ned_vel
speed NED in cm/s
Definition: gps.h:95
uint32_t last_msg_time
cpu time in sec at last received GPS message
Definition: gps.h:116
uint32_t last_3dfix_time
cpu time in sec at last valid 3D fix
Definition: gps.h:114
uint32_t pacc
position accuracy in cm
Definition: gps.h:99
uint16_t gspeed
norm of 2d ground speed in cm/s
Definition: gps.h:96
uint8_t valid_fields
bitfield indicating valid fields (GPS_VALID_x_BIT)
Definition: gps.h:87
uint8_t comp_id
id of current gps
Definition: gps.h:88
#define GPS_FIX_3D
3D GPS fix
Definition: gps.h:43
uint16_t speed_3d
norm of 3d speed in cm/s
Definition: gps.h:97
#define GPS_VALID_COURSE_BIT
Definition: gps.h:53
uint32_t last_msg_ticks
cpu time ticks at last received GPS message
Definition: gps.h:115
uint8_t num_sv
number of sat in fix
Definition: gps.h:105
uint8_t fix
status of fix
Definition: gps.h:106
data structure for GPS information
Definition: gps.h:86
float psi
in radians
#define FLOAT_VECT3_NORM(_v)
void float_eulers_of_quat(struct FloatEulers *e, struct FloatQuat *q)
euler rotation 'ZYX'
#define FLOAT_VECT2_NORM(_v)
euler angles
Roation quaternion.
int32_t lat
in degrees*1e7
int32_t y
North.
int32_t hmsl
Height above mean sea level in mm.
int32_t alt
in millimeters above WGS84 reference ellipsoid
int32_t z
Up.
int32_t x
East.
int32_t lon
in degrees*1e7
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.
#define VECT3_NED_OF_ENU(_o, _i)
#define CM_OF_M(_m)
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.
void lla_of_ecef_i(struct LlaCoor_i *out, struct EcefCoor_i *in)
Convert a ECEF to LLA.
void ltp_def_from_lla_i(struct LtpDef_i *def, struct LlaCoor_i *lla)
vector in East North Up coordinates
vector in Latitude, Longitude and Altitude
definition of the local (flat earth) coordinate system
Inertial Measurement Unit interface.
float y
in meters
float x
in meters
float z
in meters
vector in East North Up coordinates Units: meters
volatile uint32_t nb_sec
full seconds since startup
Definition: sys_time.h:72
volatile uint32_t nb_sec_rem
remainder of seconds since startup in CPU_TICKS
Definition: sys_time.h:73
int int32_t
Typedef defining 32 bit int type.
Definition: vl53l1_types.h:83
unsigned int uint32_t
Typedef defining 32 bit unsigned int type.
Definition: vl53l1_types.h:78
short int16_t
Typedef defining 16 bit short type.
Definition: vl53l1_types.h:93
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.
Definition: vl53l1_types.h:98
float heading
Definition: wedgebug.c:258