Paparazzi UAS  v5.15_devel-105-g9de12e2
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
gps_skytraq.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2010 Antoine Drouin <poinix@gmail.com>
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 "subsystems/gps.h"
24 #include "subsystems/abi.h"
25 #include "led.h"
26 #include "pprzlink/pprzlink_device.h"
27 
28 /* parser status */
29 #define UNINIT 0
30 #define GOT_SYNC1 1
31 #define GOT_SYNC2 2
32 #define GOT_LEN1 3
33 #define GOT_LEN2 4
34 #define GOT_ID 5
35 #define GOT_PAYLOAD 6
36 #define GOT_CHECKSUM 7
37 #define GOT_SYNC3 8
38 
39 #define SKYTRAQ_FIX_NONE 0x00
40 #define SKYTRAQ_FIX_2D 0x01
41 #define SKYTRAQ_FIX_3D 0x02
42 #define SKYTRAQ_FIX_3D_DGPS 0x03
43 
44 
45 #define SKYTRAQ_SYNC1 0xA0
46 #define SKYTRAQ_SYNC2 0xA1
47 
48 #define SKYTRAQ_SYNC3 0x0D
49 #define SKYTRAQ_SYNC4 0x0A
50 
52 
53 void gps_skytraq_read_message(void);
55 void gps_skytraq_msg(void);
56 
57 static inline uint16_t bswap16(uint16_t a)
58 {
59  return (a << 8) | (a >> 8);
60 }
61 
62 #define SKYTRAQ_NAVIGATION_DATA_FixMode(_payload) (uint8_t) (*((uint8_t*)_payload+2-2))
63 #define SKYTRAQ_NAVIGATION_DATA_NumSV(_payload) (uint8_t) (*((uint8_t*)_payload+3-2))
64 
65 #define SKYTRAQ_NAVIGATION_DATA_WEEK(_payload) bswap16(*(uint16_t*)&_payload[4-2])
66 #define SKYTRAQ_NAVIGATION_DATA_TOW(_payload) __builtin_bswap32(*(uint32_t*)&_payload[6-2])
67 
68 #define SKYTRAQ_NAVIGATION_DATA_LAT(_payload) (int32_t)__builtin_bswap32(*( int32_t*)&_payload[10-2])
69 #define SKYTRAQ_NAVIGATION_DATA_LON(_payload) (int32_t)__builtin_bswap32(*( int32_t*)&_payload[14-2])
70 
71 #define SKYTRAQ_NAVIGATION_DATA_AEL(_payload) __builtin_bswap32(*(uint32_t*)&_payload[18-2])
72 #define SKYTRAQ_NAVIGATION_DATA_ASL(_payload) __builtin_bswap32(*(uint32_t*)&_payload[22-2])
73 
74 #define SKYTRAQ_NAVIGATION_DATA_GDOP(_payload) bswap16(*(uint16_t*)&_payload[26-2])
75 #define SKYTRAQ_NAVIGATION_DATA_PDOP(_payload) bswap16(*(uint16_t*)&_payload[28-2])
76 #define SKYTRAQ_NAVIGATION_DATA_HDOP(_payload) bswap16(*(uint16_t*)&_payload[30-2])
77 #define SKYTRAQ_NAVIGATION_DATA_VDOP(_payload) bswap16(*(uint16_t*)&_payload[32-2])
78 #define SKYTRAQ_NAVIGATION_DATA_TDOP(_payload) bswap16(*(uint16_t*)&_payload[34-2])
79 
80 #define SKYTRAQ_NAVIGATION_DATA_ECEFX(_payload) (int32_t)__builtin_bswap32(*(uint32_t*)&_payload[36-2])
81 #define SKYTRAQ_NAVIGATION_DATA_ECEFY(_payload) (int32_t)__builtin_bswap32(*(uint32_t*)&_payload[40-2])
82 #define SKYTRAQ_NAVIGATION_DATA_ECEFZ(_payload) (int32_t)__builtin_bswap32(*(uint32_t*)&_payload[44-2])
83 #define SKYTRAQ_NAVIGATION_DATA_ECEFVX(_payload) (int32_t)__builtin_bswap32(*(uint32_t*)&_payload[48-2])
84 #define SKYTRAQ_NAVIGATION_DATA_ECEFVY(_payload) (int32_t)__builtin_bswap32(*(uint32_t*)&_payload[52-2])
85 #define SKYTRAQ_NAVIGATION_DATA_ECEFVZ(_payload) (int32_t)__builtin_bswap32(*(uint32_t*)&_payload[56-2])
86 
87 
88 // distance in cm (10km dist max any direction)
89 #define MAX_DISTANCE 1000000
90 
91 static int distance_too_great(struct EcefCoor_i *ecef_ref, struct EcefCoor_i *ecef_pos);
92 
93 void gps_skytraq_init(void)
94 {
96 }
97 
98 void gps_skytraq_msg(void)
99 {
100  // current timestamp
101  uint32_t now_ts = get_sys_time_usec();
106  if (gps_skytraq.state.fix == GPS_FIX_3D) {
109  }
110  AbiSendMsgGPS(GPS_SKYTRAQ_ID, now_ts, &gps_skytraq.state);
111  }
112  gps_skytraq.msg_available = false;
113 }
114 
116 {
117  struct link_device *dev = &((SKYTRAQ_GPS_LINK).device);
118 
119  while (dev->char_available(dev->periph)) {
120  gps_skytraq_parse(dev->get_byte(dev->periph));
122  gps_skytraq_msg();
123  }
124  }
125 }
126 
128 {
129 
135 
140 
145 
148 
149  // pacc;
150  // sacc;
154 
156  case SKYTRAQ_FIX_3D_DGPS:
157  case SKYTRAQ_FIX_3D:
159  break;
160  case SKYTRAQ_FIX_2D:
162  break;
163  default:
165  }
166 
167  if (gps_skytraq.state.fix == GPS_FIX_3D) {
169  // just grab current ecef_pos as reference.
171  }
172  // convert ecef velocity vector to NED vector.
175 
176  // ground course in radians
177  gps_skytraq.state.course = (atan2f((float)gps_skytraq.state.ned_vel.y, (float)gps_skytraq.state.ned_vel.x)) * 1e7;
179  // GT: gps_skytraq.state.cacc = ... ? what should course accuracy be?
180 
181  // ground speed
184 
185  // vertical speed (climb)
186  // solved by gps_skytraq.state.ned.z?
187  }
188 
189 
190 #ifdef GPS_LED
191  if (gps_skytraq.state.fix == GPS_FIX_3D) {
192  LED_ON(GPS_LED);
193  } else {
194  LED_TOGGLE(GPS_LED);
195  }
196 #endif
197  }
198 
199 }
200 
202 {
204  gps_skytraq.checksum ^= c;
205  }
206  switch (gps_skytraq.status) {
207  case UNINIT:
208  if (c == SKYTRAQ_SYNC1) {
210  }
211  break;
212  case GOT_SYNC1:
213  if (c != SKYTRAQ_SYNC2) {
215  goto error;
216  }
218  break;
219  case GOT_SYNC2:
220  gps_skytraq.len = c << 8;
222  break;
223  case GOT_LEN1:
224  gps_skytraq.len += c;
228  goto error;
229  }
230  break;
231  case GOT_LEN2:
232  gps_skytraq.msg_id = c;
233  gps_skytraq.msg_idx = 0;
234  gps_skytraq.checksum = c;
236  break;
237  case GOT_ID:
240  if (gps_skytraq.msg_idx >= gps_skytraq.len - 1) {
242  }
243  break;
244  case GOT_PAYLOAD:
245  if (c != gps_skytraq.checksum) {
247  goto error;
248  }
250  break;
251  case GOT_CHECKSUM:
252  if (c != SKYTRAQ_SYNC3) {
254  goto error;
255  }
257  break;
258  case GOT_SYNC3:
259  gps_skytraq.msg_available = true;
260  goto restart;
261  default:
263  goto error;
264  }
265  return;
266 error:
268 restart:
270  return;
271 }
272 
273 static int distance_too_great(struct EcefCoor_i *ecef_ref, struct EcefCoor_i *ecef_pos)
274 {
275  int32_t xdiff = abs(ecef_ref->x - ecef_pos->x);
276  if (xdiff > MAX_DISTANCE) {
277  return true;
278  }
279  int32_t ydiff = abs(ecef_ref->y - ecef_pos->y);
280  if (ydiff > MAX_DISTANCE) {
281  return true;
282  }
283  int32_t zdiff = abs(ecef_ref->z - ecef_pos->z);
284  if (zdiff > MAX_DISTANCE) {
285  return true;
286  }
287 
288  return false;
289 }
unsigned short uint16_t
Definition: types.h:16
int32_t z
in centimeters
#define GOT_CHECKSUM
Definition: gps_skytraq.c:36
#define GOT_SYNC3
Definition: gps_skytraq.c:37
struct LtpDef_i ref_ltp
Definition: gps_skytraq.h:56
#define SKYTRAQ_NAVIGATION_DATA_ASL(_payload)
Definition: gps_skytraq.c:72
#define SKYTRAQ_FIX_3D
Definition: gps_skytraq.c:41
#define MAX_DISTANCE
Definition: gps_skytraq.c:89
enum GpsSkytraqError error_last
Definition: gps_skytraq.h:54
#define GPS_VALID_VEL_NED_BIT
Definition: gps.h:52
#define SKYTRAQ_SYNC1
Definition: gps_skytraq.c:45
vector in EarthCenteredEarthFixed coordinates
uint8_t valid_fields
bitfield indicating valid fields (GPS_VALID_x_BIT)
Definition: gps.h:88
int32_t y
in centimeters
#define SKYTRAQ_NAVIGATION_DATA_ECEFVY(_payload)
Definition: gps_skytraq.c:84
uint8_t msg_buf[GPS_SKYTRAQ_MAX_PAYLOAD]
Definition: gps_skytraq.h:45
Main include for ABI (AirBorneInterface).
#define GPS_SKYTRAQ_ID
bool msg_available
Definition: gps_skytraq.h:46
#define GOT_ID
Definition: gps_skytraq.c:34
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
int32_t z
Down.
void gps_skytraq_event(void)
Definition: gps_skytraq.c:115
#define SKYTRAQ_NAVIGATION_DATA_LON(_payload)
Definition: gps_skytraq.c:69
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.
#define SKYTRAQ_NAVIGATION_DATA_AEL(_payload)
Definition: gps_skytraq.c:71
#define SKYTRAQ_NAVIGATION_DATA_ECEFY(_payload)
Definition: gps_skytraq.c:81
struct EcefCoor_i ecef
Reference point in ecef.
uint32_t last_3dfix_ticks
cpu time ticks at last valid 3D fix
Definition: gps.h:114
void gps_skytraq_init(void)
Definition: gps_skytraq.c:93
int32_t alt
in millimeters above WGS84 reference ellipsoid
int32_t y
East.
struct GpsState state
Definition: gps_skytraq.h:58
uint32_t last_msg_time
cpu time in sec at last received GPS message
Definition: gps.h:117
#define GOT_SYNC2
Definition: gps_skytraq.c:31
#define GPS_VALID_COURSE_BIT
Definition: gps.h:54
int32_t hmsl
height above mean sea level (MSL) in mm
Definition: gps.h:94
#define GOT_LEN1
Definition: gps_skytraq.c:32
uint32_t tow
GPS time of week in ms.
Definition: gps.h:109
#define GPS_FIX_NONE
No GPS fix.
Definition: gps.h:37
#define GPS_FIX_2D
2D GPS fix
Definition: gps.h:38
#define GOT_PAYLOAD
Definition: gps_skytraq.c:35
#define SKYTRAQ_ID_NAVIGATION_DATA
Definition: gps_skytraq.h:27
Device independent GPS code (interface)
uint16_t pdop
position dilution of precision scaled by 100
Definition: gps.h:105
#define SKYTRAQ_NAVIGATION_DATA_ECEFX(_payload)
Definition: gps_skytraq.c:80
void gps_skytraq_read_message(void)
Definition: gps_skytraq.c:127
#define SKYTRAQ_SYNC3
Definition: gps_skytraq.c:48
#define SKYTRAQ_NAVIGATION_DATA_ECEFVZ(_payload)
Definition: gps_skytraq.c:85
int32_t x
North.
#define SKYTRAQ_FIX_3D_DGPS
Definition: gps_skytraq.c:42
unsigned long uint32_t
Definition: types.h:18
struct EcefCoor_i ecef_pos
position in ECEF in cm
Definition: gps.h:91
#define GPS_VALID_HMSL_BIT
Definition: gps.h:53
int32_t lon
in degrees*1e7
#define SKYTRAQ_FIX_2D
Definition: gps_skytraq.c:40
volatile uint32_t nb_sec_rem
remainder of seconds since startup in CPU_TICKS
Definition: sys_time.h:73
#define LED_TOGGLE(i)
Definition: led_hw.h:53
#define GPS_SKYTRAQ_MAX_PAYLOAD
Definition: gps_skytraq.h:43
signed long int32_t
Definition: types.h:19
static const struct usb_device_descriptor dev
Definition: usb_ser_hw.c:73
#define SKYTRAQ_NAVIGATION_DATA_ECEFZ(_payload)
Definition: gps_skytraq.c:82
uint8_t msg_id
Definition: gps_skytraq.h:47
#define SKYTRAQ_SYNC2
Definition: gps_skytraq.c:46
uint32_t last_3dfix_time
cpu time in sec at last valid 3D fix
Definition: gps.h:115
static int distance_too_great(struct EcefCoor_i *ecef_ref, struct EcefCoor_i *ecef_pos)
Definition: gps_skytraq.c:273
unsigned char uint8_t
Definition: types.h:14
int32_t course
GPS course over ground in rad*1e7, [0, 2*Pi]*1e7 (CW/north)
Definition: gps.h:99
volatile uint32_t nb_sec
full seconds since startup
Definition: sys_time.h:72
#define GOT_SYNC1
Definition: gps_skytraq.c:30
#define SKYTRAQ_NAVIGATION_DATA_ECEFVX(_payload)
Definition: gps_skytraq.c:83
uint16_t len
Definition: gps_skytraq.h:50
#define SKYTRAQ_NAVIGATION_DATA_PDOP(_payload)
Definition: gps_skytraq.c:75
uint8_t status
Definition: gps_skytraq.h:49
#define GPS_VALID_POS_ECEF_BIT
Definition: gps.h:48
void gps_skytraq_parse(uint8_t c)
Definition: gps_skytraq.c:201
#define UNINIT
Definition: gps_skytraq.c:29
#define SKYTRAQ_NAVIGATION_DATA_LAT(_payload)
Definition: gps_skytraq.c:68
#define SKYTRAQ_NAVIGATION_DATA_NumSV(_payload)
Definition: gps_skytraq.c:63
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
arch independent LED (Light Emitting Diodes) API
int32_t x
in centimeters
uint16_t gspeed
norm of 2d ground speed in cm/s
Definition: gps.h:97
#define LED_ON(i)
Definition: led_hw.h:51
uint8_t error_cnt
Definition: gps_skytraq.h:53
struct EcefCoor_i ecef_vel
speed ECEF in cm/s
Definition: gps.h:95
struct GpsSkytraq gps_skytraq
Definition: gps_skytraq.c:51
#define GOT_LEN2
Definition: gps_skytraq.c:33
#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
static uint16_t bswap16(uint16_t a)
Definition: gps_skytraq.c:57
#define SKYTRAQ_NAVIGATION_DATA_FixMode(_payload)
Definition: gps_skytraq.c:62
uint32_t get_sys_time_usec(void)
Get the time in microseconds since startup.
Definition: sys_time_arch.c:68
int32_t lat
in degrees*1e7
uint8_t fix
status of fix
Definition: gps.h:107
void gps_skytraq_msg(void)
Definition: gps_skytraq.c:98
struct NedCoor_i ned_vel
speed NED in cm/s
Definition: gps.h:96
uint8_t checksum
Definition: gps_skytraq.h:52
uint8_t msg_idx
Definition: gps_skytraq.h:51
void ltp_def_from_ecef_i(struct LtpDef_i *def, struct EcefCoor_i *ecef)
#define SKYTRAQ_NAVIGATION_DATA_TOW(_payload)
Definition: gps_skytraq.c:66