Paparazzi UAS  v5.0.5_stable-7-g4b8bbb7
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_f.lat = ((float) gps.lla_pos.lat) / 1e7;
134  lla_f.lon = ((float) gps.lla_pos.lon) / 1e7;
135  struct UtmCoor_f utm_f;
136  utm_f.zone = nav_utm_zone0;
137  /* convert to utm */
138  utm_of_lla_f(&utm_f, &lla_f);
139  /* copy results of utm conversion */
140  gps.utm_pos.east = utm_f.east*100;
141  gps.utm_pos.north = utm_f.north*100;
144 #endif
145 
146  if (gps.fix == GPS_FIX_3D) {
148  // just grab current ecef_pos as reference.
150  }
151  // convert ecef velocity vector to NED vector.
153 
154  // ground course in radians
155  gps.course = (atan2f( (float)gps.ned_vel.y, (float)gps.ned_vel.x )) * 1e7;
156  // GT: gps.cacc = ... ? what should course accuracy be?
157 
158  // ground speed
161 
162  // vertical speed (climb)
163  // solved by gps.ned.z?
164  }
165 
166 
167 #ifdef GPS_LED
168  if (gps.fix == GPS_FIX_3D) {
169  LED_ON(GPS_LED);
170  }
171  else {
172  LED_TOGGLE(GPS_LED);
173  }
174 #endif
175  }
176 
177 }
178 
182  }
183  switch (gps_skytraq.status) {
184  case UNINIT:
185  if (c == SKYTRAQ_SYNC1)
187  break;
188  case GOT_SYNC1:
189  if (c != SKYTRAQ_SYNC2) {
191  goto error;
192  }
194  break;
195  case GOT_SYNC2:
196  gps_skytraq.len = c<<8;
198  break;
199  case GOT_LEN1:
200  gps_skytraq.len += c;
204  goto error;
205  }
206  break;
207  case GOT_LEN2:
208  gps_skytraq.msg_id = c;
209  gps_skytraq.msg_idx = 0;
212  break;
213  case GOT_ID:
216  if (gps_skytraq.msg_idx >= gps_skytraq.len-1) {
218  }
219  break;
220  case GOT_PAYLOAD:
221  if (c != gps_skytraq.checksum) {
223  goto error;
224  }
226  break;
227  case GOT_CHECKSUM:
228  if (c != SKYTRAQ_SYNC3) {
230  goto error;
231  }
233  break;
234  case GOT_SYNC3:
236  goto restart;
237  default:
239  goto error;
240  }
241  return;
242  error:
244  restart:
246  return;
247 }
248 
249 static int distance_too_great(struct EcefCoor_i *ecef_ref, struct EcefCoor_i *ecef_pos) {
250  int32_t xdiff = abs(ecef_ref->x - ecef_pos->x);
251  if (xdiff > MAX_DISTANCE) {
252  return TRUE;
253  }
254  int32_t ydiff = abs(ecef_ref->y - ecef_pos->y);
255  if (ydiff > MAX_DISTANCE) {
256  return TRUE;
257  }
258  int32_t zdiff = abs(ecef_ref->z - ecef_pos->z);
259  if (zdiff > MAX_DISTANCE) {
260  return TRUE;
261  }
262 
263  return FALSE;
264 }
unsigned short uint16_t
Definition: types.h:16
int32_t y
in centimeters
struct LlaCoor_i lla_pos
position in LLA (lat,lon: rad*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 radians*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)
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
float lat
in radians
#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:249
uint16_t len
Definition: gps_skytraq.h:46
unsigned char uint8_t
Definition: types.h:14
int32_t lon
in radians*1e7
#define GOT_SYNC1
Definition: gps_skytraq.c:35
#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)
void gps_skytraq_parse(uint8_t c)
Definition: gps_skytraq.c:179
#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
float lon
in radians
#define SKYTRAQ_NAVIGATION_DATA_FixMode(_payload)
Definition: gps_skytraq.c:61
static struct point c
Definition: discsurvey.c:39
float east
in meters
struct GpsState gps
global GPS state
Definition: gps.c:33
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