Paparazzi UAS  v5.2.2_stable-0-gd6b9f29
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures 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"
23 #include "led.h"
24 
25 #if GPS_USE_LATLONG
26 /* currently needed to get nav_utm_zone0 */
29 #endif
30 
32 
33 /* parser status */
34 #define UNINIT 0
35 #define GOT_SYNC1 1
36 #define GOT_SYNC2 2
37 #define GOT_LEN1 3
38 #define GOT_LEN2 4
39 #define GOT_ID 5
40 #define GOT_PAYLOAD 6
41 #define GOT_CHECKSUM 7
42 #define GOT_SYNC3 8
43 
44 #define SKYTRAQ_FIX_NONE 0x00
45 #define SKYTRAQ_FIX_2D 0x01
46 #define SKYTRAQ_FIX_3D 0x02
47 #define SKYTRAQ_FIX_3D_DGPS 0x03
48 
49 
50 #define SKYTRAQ_SYNC1 0xA0
51 #define SKYTRAQ_SYNC2 0xA1
52 
53 #define SKYTRAQ_SYNC3 0x0D
54 #define SKYTRAQ_SYNC4 0x0A
55 
56 
57 static inline uint16_t bswap16(uint16_t a) {
58  return (a<<8)|(a>>8);
59 }
60 
61 #define SKYTRAQ_NAVIGATION_DATA_FixMode(_payload) (uint8_t) (*((uint8_t*)_payload+2-2))
62 #define SKYTRAQ_NAVIGATION_DATA_NumSV(_payload) (uint8_t) (*((uint8_t*)_payload+3-2))
63 
64 #define SKYTRAQ_NAVIGATION_DATA_WEEK(_payload) bswap16(*(uint16_t*)&_payload[4-2])
65 #define SKYTRAQ_NAVIGATION_DATA_TOW(_payload) __builtin_bswap32(*(uint32_t*)&_payload[6-2])
66 
67 #define SKYTRAQ_NAVIGATION_DATA_LAT(_payload) (int32_t)__builtin_bswap32(*( int32_t*)&_payload[10-2])
68 #define SKYTRAQ_NAVIGATION_DATA_LON(_payload) (int32_t)__builtin_bswap32(*( int32_t*)&_payload[14-2])
69 
70 #define SKYTRAQ_NAVIGATION_DATA_AEL(_payload) __builtin_bswap32(*(uint32_t*)&_payload[18-2])
71 #define SKYTRAQ_NAVIGATION_DATA_ASL(_payload) __builtin_bswap32(*(uint32_t*)&_payload[22-2])
72 
73 #define SKYTRAQ_NAVIGATION_DATA_GDOP(_payload) bswap16(*(uint16_t*)&_payload[26-2])
74 #define SKYTRAQ_NAVIGATION_DATA_PDOP(_payload) bswap16(*(uint16_t*)&_payload[28-2])
75 #define SKYTRAQ_NAVIGATION_DATA_HDOP(_payload) bswap16(*(uint16_t*)&_payload[30-2])
76 #define SKYTRAQ_NAVIGATION_DATA_VDOP(_payload) bswap16(*(uint16_t*)&_payload[32-2])
77 #define SKYTRAQ_NAVIGATION_DATA_TDOP(_payload) bswap16(*(uint16_t*)&_payload[34-2])
78 
79 #define SKYTRAQ_NAVIGATION_DATA_ECEFX(_payload) (int32_t)__builtin_bswap32(*(uint32_t*)&_payload[36-2])
80 #define SKYTRAQ_NAVIGATION_DATA_ECEFY(_payload) (int32_t)__builtin_bswap32(*(uint32_t*)&_payload[40-2])
81 #define SKYTRAQ_NAVIGATION_DATA_ECEFZ(_payload) (int32_t)__builtin_bswap32(*(uint32_t*)&_payload[44-2])
82 #define SKYTRAQ_NAVIGATION_DATA_ECEFVX(_payload) (int32_t)__builtin_bswap32(*(uint32_t*)&_payload[48-2])
83 #define SKYTRAQ_NAVIGATION_DATA_ECEFVY(_payload) (int32_t)__builtin_bswap32(*(uint32_t*)&_payload[52-2])
84 #define SKYTRAQ_NAVIGATION_DATA_ECEFVZ(_payload) (int32_t)__builtin_bswap32(*(uint32_t*)&_payload[56-2])
85 
86 
87 // distance in cm (10km dist max any direction)
88 #define MAX_DISTANCE 1000000
89 
90 static int distance_too_great(struct EcefCoor_i *ecef_ref, struct EcefCoor_i *ecef_pos);
91 
92 void gps_impl_init(void) {
93 
95 
96 }
97 
98 
100 
112  // pacc;
113  // sacc;
117 
119  case SKYTRAQ_FIX_3D_DGPS:
120  case SKYTRAQ_FIX_3D:
121  gps.fix = GPS_FIX_3D;
122  break;
123  case SKYTRAQ_FIX_2D:
124  gps.fix = GPS_FIX_2D;
125  break;
126  default:
127  gps.fix = GPS_FIX_NONE;
128  }
129 
130 #if GPS_USE_LATLONG
131  /* Computes from (lat, long) in the referenced UTM zone */
132  struct LlaCoor_f lla_f;
133  LLA_FLOAT_OF_BFP(lla_f, gps.lla_pos);
134  struct UtmCoor_f utm_f;
135  utm_f.zone = nav_utm_zone0;
136  /* convert to utm */
137  utm_of_lla_f(&utm_f, &lla_f);
138  /* copy results of utm conversion */
139  gps.utm_pos.east = utm_f.east*100;
140  gps.utm_pos.north = utm_f.north*100;
143 #endif
144 
145  if (gps.fix == GPS_FIX_3D) {
147  // just grab current ecef_pos as reference.
149  }
150  // convert ecef velocity vector to NED vector.
152 
153  // ground course in radians
154  gps.course = (atan2f( (float)gps.ned_vel.y, (float)gps.ned_vel.x )) * 1e7;
155  // GT: gps.cacc = ... ? what should course accuracy be?
156 
157  // ground speed
160 
161  // vertical speed (climb)
162  // solved by gps.ned.z?
163  }
164 
165 
166 #ifdef GPS_LED
167  if (gps.fix == GPS_FIX_3D) {
168  LED_ON(GPS_LED);
169  }
170  else {
171  LED_TOGGLE(GPS_LED);
172  }
173 #endif
174  }
175 
176 }
177 
180  gps_skytraq.checksum ^= c;
181  }
182  switch (gps_skytraq.status) {
183  case UNINIT:
184  if (c == SKYTRAQ_SYNC1)
186  break;
187  case GOT_SYNC1:
188  if (c != SKYTRAQ_SYNC2) {
190  goto error;
191  }
193  break;
194  case GOT_SYNC2:
195  gps_skytraq.len = c<<8;
197  break;
198  case GOT_LEN1:
199  gps_skytraq.len += c;
203  goto error;
204  }
205  break;
206  case GOT_LEN2:
207  gps_skytraq.msg_id = c;
208  gps_skytraq.msg_idx = 0;
209  gps_skytraq.checksum = c;
211  break;
212  case GOT_ID:
215  if (gps_skytraq.msg_idx >= gps_skytraq.len-1) {
217  }
218  break;
219  case GOT_PAYLOAD:
220  if (c != gps_skytraq.checksum) {
222  goto error;
223  }
225  break;
226  case GOT_CHECKSUM:
227  if (c != SKYTRAQ_SYNC3) {
229  goto error;
230  }
232  break;
233  case GOT_SYNC3:
235  goto restart;
236  default:
238  goto error;
239  }
240  return;
241  error:
243  restart:
245  return;
246 }
247 
248 static int distance_too_great(struct EcefCoor_i *ecef_ref, struct EcefCoor_i *ecef_pos) {
249  int32_t xdiff = abs(ecef_ref->x - ecef_pos->x);
250  if (xdiff > MAX_DISTANCE) {
251  return TRUE;
252  }
253  int32_t ydiff = abs(ecef_ref->y - ecef_pos->y);
254  if (ydiff > MAX_DISTANCE) {
255  return TRUE;
256  }
257  int32_t zdiff = abs(ecef_ref->z - ecef_pos->z);
258  if (zdiff > MAX_DISTANCE) {
259  return TRUE;
260  }
261 
262  return FALSE;
263 }
unsigned short uint16_t
Definition: types.h:16
int32_t y
in centimeters
struct LlaCoor_i lla_pos
position in LLA (lat,lon: deg*1e7; alt: mm over ellipsoid)
Definition: gps.h:65
#define GOT_CHECKSUM
Definition: gps_skytraq.c:41
#define GOT_SYNC3
Definition: gps_skytraq.c:42
struct NedCoor_i ned_vel
speed NED in cm/s
Definition: gps.h:69
int32_t lat
in degrees*1e7
struct EcefCoor_i ecef_vel
speed ECEF in cm/s
Definition: gps.h:68
#define SKYTRAQ_NAVIGATION_DATA_ASL(_payload)
Definition: gps_skytraq.c:71
#define SKYTRAQ_FIX_3D
Definition: gps_skytraq.c:46
int32_t course
GPS course over ground in rad*1e7, [0, 2*Pi]*1e7 (CW/north)
Definition: gps.h:72
void gps_impl_init(void)
GPS initialization.
Definition: gps_skytraq.c:92
#define MAX_DISTANCE
Definition: gps_skytraq.c:88
vector in EarthCenteredEarthFixed coordinates
uint8_t zone
UTM zone number.
uint8_t msg_id
Definition: gps_skytraq.h:43
#define SKYTRAQ_SYNC1
Definition: gps_skytraq.c:50
#define SKYTRAQ_NAVIGATION_DATA_ECEFVY(_payload)
Definition: gps_skytraq.c:83
uint16_t pdop
position dilution of precision scaled by 100
Definition: gps.h:76
int32_t x
North.
uint8_t nav_utm_zone0
Definition: common_nav.c:44
#define LED_ON(i)
Definition: led_hw.h:28
int32_t hmsl
height above mean sea level in mm
Definition: gps.h:67
uint8_t msg_buf[GPS_SKYTRAQ_MAX_PAYLOAD]
Definition: gps_skytraq.h:41
#define GOT_ID
Definition: gps_skytraq.c:39
uint8_t fix
status of fix
Definition: gps.h:78
#define GPS_FIX_3D
Definition: gps.h:43
int32_t z
in centimeters
struct EcefCoor_i ecef_pos
position in ECEF in cm
Definition: gps.h:64
#define SKYTRAQ_NAVIGATION_DATA_LON(_payload)
Definition: gps_skytraq.c:68
#define SKYTRAQ_NAVIGATION_DATA_AEL(_payload)
Definition: gps_skytraq.c:70
#define SKYTRAQ_NAVIGATION_DATA_ECEFY(_payload)
Definition: gps_skytraq.c:80
vector in Latitude, Longitude and Altitude
#define FALSE
Definition: imu_chimu.h:141
int32_t alt
in millimeters above WGS84 reference ellipsoid
#define GOT_SYNC2
Definition: gps_skytraq.c:36
Paparazzi floating point math for geodetic calculations.
uint32_t tow
GPS time of week in ms.
Definition: gps.h:80
int32_t z
Down.
uint8_t status
Definition: gps_skytraq.h:45
uint8_t zone
UTM zone number.
uint8_t msg_idx
Definition: gps_skytraq.h:47
int16_t gspeed
norm of 2d ground speed in cm/s
Definition: gps.h:70
uint8_t error_cnt
Definition: gps_skytraq.h:49
#define GOT_LEN1
Definition: gps_skytraq.c:37
float north
in meters
#define GPS_FIX_NONE
Definition: gps.h:41
#define GPS_FIX_2D
Definition: gps.h:42
#define GOT_PAYLOAD
Definition: gps_skytraq.c:40
#define SKYTRAQ_ID_NAVIGATION_DATA
Definition: gps_skytraq.h:27
Device independent GPS code (interface)
#define SKYTRAQ_NAVIGATION_DATA_ECEFX(_payload)
Definition: gps_skytraq.c:79
void gps_skytraq_read_message(void)
Definition: gps_skytraq.c:99
#define SKYTRAQ_SYNC3
Definition: gps_skytraq.c:53
#define SKYTRAQ_NAVIGATION_DATA_ECEFVZ(_payload)
Definition: gps_skytraq.c:84
#define SKYTRAQ_FIX_3D_DGPS
Definition: gps_skytraq.c:47
position in UTM coordinates Units: meters
int32_t alt
in millimeters above WGS84 reference ellipsoid
#define SKYTRAQ_FIX_2D
Definition: gps_skytraq.c:45
int32_t y
East.
int32_t north
in centimeters
int32_t x
in centimeters
#define LED_TOGGLE(i)
Definition: led_hw.h:30
#define GPS_SKYTRAQ_MAX_PAYLOAD
Definition: gps_skytraq.h:39
bool_t msg_available
Definition: gps_skytraq.h:42
signed long int32_t
Definition: types.h:19
uint8_t checksum
Definition: gps_skytraq.h:48
#define TRUE
Definition: imu_chimu.h:144
#define SKYTRAQ_NAVIGATION_DATA_ECEFZ(_payload)
Definition: gps_skytraq.c:81
void ltp_def_from_ecef_i(struct LtpDef_i *def, struct EcefCoor_i *ecef)
int16_t speed_3d
norm of 3d speed in cm/s
Definition: gps.h:71
#define SKYTRAQ_SYNC2
Definition: gps_skytraq.c:51
int32_t east
in centimeters
static int distance_too_great(struct EcefCoor_i *ecef_ref, struct EcefCoor_i *ecef_pos)
Definition: gps_skytraq.c:248
uint16_t len
Definition: gps_skytraq.h:46
unsigned char uint8_t
Definition: types.h:14
int32_t lon
in degrees*1e7
#define GOT_SYNC1
Definition: gps_skytraq.c:35
#define LLA_FLOAT_OF_BFP(_o, _i)
#define SKYTRAQ_NAVIGATION_DATA_ECEFVX(_payload)
Definition: gps_skytraq.c:82
struct EcefCoor_i ecef
Reference point in ecef.
#define SKYTRAQ_NAVIGATION_DATA_PDOP(_payload)
Definition: gps_skytraq.c:74
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.
void gps_skytraq_parse(uint8_t c)
Definition: gps_skytraq.c:178
#define UNINIT
Definition: gps_skytraq.c:34
#define SKYTRAQ_NAVIGATION_DATA_LAT(_payload)
Definition: gps_skytraq.c:67
#define SKYTRAQ_NAVIGATION_DATA_NumSV(_payload)
Definition: gps_skytraq.c:62
arch independent LED (Light Emitting Diodes) API
enum GpsSkytraqError error_last
Definition: gps_skytraq.h:50
struct UtmCoor_i utm_pos
position in UTM (north,east: cm; alt: mm over ellipsoid)
Definition: gps.h:66
struct GpsSkytraq gps_skytraq
Definition: gps_skytraq.c:31
#define GOT_LEN2
Definition: gps_skytraq.c:38
static uint16_t bswap16(uint16_t a)
Definition: gps_skytraq.c:57
#define SKYTRAQ_NAVIGATION_DATA_FixMode(_payload)
Definition: gps_skytraq.c:61
float east
in meters
struct GpsState gps
global GPS state
Definition: gps.c:41
void utm_of_lla_f(struct UtmCoor_f *utm, struct LlaCoor_f *lla)
#define SKYTRAQ_NAVIGATION_DATA_TOW(_payload)
Definition: gps_skytraq.c:65
struct LtpDef_i ref_ltp
Definition: gps_skytraq.h:52
uint8_t num_sv
number of sat in fix
Definition: gps.h:77