Paparazzi UAS  v6.2_unstable
Paparazzi is a free software Unmanned Aircraft System.
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 "modules/gps/gps.h"
24 #include "modules/core/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 }
SKYTRAQ_NAVIGATION_DATA_ECEFZ
#define SKYTRAQ_NAVIGATION_DATA_ECEFZ(_payload)
Definition: gps_skytraq.c:82
gps_skytraq
struct GpsSkytraq gps_skytraq
Definition: gps_skytraq.c:51
GPS_SKYTRAQ_ERR_UNEXPECTED
@ GPS_SKYTRAQ_ERR_UNEXPECTED
Definition: gps_skytraq.h:40
LlaCoor_i::lon
int32_t lon
in degrees*1e7
Definition: pprz_geodetic_int.h:61
SKYTRAQ_NAVIGATION_DATA_PDOP
#define SKYTRAQ_NAVIGATION_DATA_PDOP(_payload)
Definition: gps_skytraq.c:75
GpsState::valid_fields
uint8_t valid_fields
bitfield indicating valid fields (GPS_VALID_x_BIT)
Definition: gps.h:91
uint32_t
unsigned int uint32_t
Typedef defining 32 bit unsigned int type.
Definition: vl53l1_types.h:78
uint8_t
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.
Definition: vl53l1_types.h:98
GpsSkytraq::error_cnt
uint8_t error_cnt
Definition: gps_skytraq.h:53
GOT_LEN2
#define GOT_LEN2
Definition: gps_skytraq.c:33
LlaCoor_i::alt
int32_t alt
in millimeters above WGS84 reference ellipsoid
Definition: pprz_geodetic_int.h:62
GPS_VALID_COURSE_BIT
#define GPS_VALID_COURSE_BIT
Definition: gps.h:57
GPS_FIX_2D
#define GPS_FIX_2D
2D GPS fix
Definition: gps.h:41
GpsSkytraq::msg_buf
uint8_t msg_buf[GPS_SKYTRAQ_MAX_PAYLOAD]
Definition: gps_skytraq.h:45
SKYTRAQ_NAVIGATION_DATA_ECEFVX
#define SKYTRAQ_NAVIGATION_DATA_ECEFVX(_payload)
Definition: gps_skytraq.c:83
ltp_def_from_ecef_i
void ltp_def_from_ecef_i(struct LtpDef_i *def, struct EcefCoor_i *ecef)
Definition: pprz_geodetic_int.c:60
SKYTRAQ_FIX_2D
#define SKYTRAQ_FIX_2D
Definition: gps_skytraq.c:40
abi.h
NedCoor_i::y
int32_t y
East.
Definition: pprz_geodetic_int.h:70
GpsState::tow
uint32_t tow
GPS time of week in ms.
Definition: gps.h:112
GpsSkytraq::checksum
uint8_t checksum
Definition: gps_skytraq.h:52
LtpDef_i::ecef
struct EcefCoor_i ecef
Reference point in ecef.
Definition: pprz_geodetic_int.h:99
GOT_CHECKSUM
#define GOT_CHECKSUM
Definition: gps_skytraq.c:36
EcefCoor_i::x
int32_t x
in centimeters
Definition: pprz_geodetic_int.h:51
GOT_SYNC3
#define GOT_SYNC3
Definition: gps_skytraq.c:37
SKYTRAQ_NAVIGATION_DATA_ASL
#define SKYTRAQ_NAVIGATION_DATA_ASL(_payload)
Definition: gps_skytraq.c:72
SKYTRAQ_ID_NAVIGATION_DATA
#define SKYTRAQ_ID_NAVIGATION_DATA
Definition: gps_skytraq.h:27
LED_ON
#define LED_ON(i)
Definition: led_hw.h:51
SKYTRAQ_NAVIGATION_DATA_AEL
#define SKYTRAQ_NAVIGATION_DATA_AEL(_payload)
Definition: gps_skytraq.c:71
GPS_SKYTRAQ_ERR_CHECKSUM
@ GPS_SKYTRAQ_ERR_CHECKSUM
Definition: gps_skytraq.h:38
LED_TOGGLE
#define LED_TOGGLE(i)
Definition: led_hw.h:53
GpsState::ned_vel
struct NedCoor_i ned_vel
speed NED in cm/s
Definition: gps.h:99
GOT_ID
#define GOT_ID
Definition: gps_skytraq.c:34
EcefCoor_i::y
int32_t y
in centimeters
Definition: pprz_geodetic_int.h:52
GPS_VALID_POS_LLA_BIT
#define GPS_VALID_POS_LLA_BIT
Definition: gps.h:52
NedCoor_i::z
int32_t z
Down.
Definition: pprz_geodetic_int.h:71
GPS_SKYTRAQ_ERR_OUT_OF_SYNC
@ GPS_SKYTRAQ_ERR_OUT_OF_SYNC
Definition: gps_skytraq.h:39
get_sys_time_usec
uint32_t get_sys_time_usec(void)
Get the time in microseconds since startup.
Definition: sys_time_arch.c:75
SKYTRAQ_NAVIGATION_DATA_ECEFX
#define SKYTRAQ_NAVIGATION_DATA_ECEFX(_payload)
Definition: gps_skytraq.c:80
LlaCoor_i::lat
int32_t lat
in degrees*1e7
Definition: pprz_geodetic_int.h:60
GpsState::last_msg_ticks
uint32_t last_msg_ticks
cpu time ticks at last received GPS message
Definition: gps.h:119
SKYTRAQ_NAVIGATION_DATA_ECEFY
#define SKYTRAQ_NAVIGATION_DATA_ECEFY(_payload)
Definition: gps_skytraq.c:81
GOT_SYNC2
#define GOT_SYNC2
Definition: gps_skytraq.c:31
EcefCoor_i::z
int32_t z
in centimeters
Definition: pprz_geodetic_int.h:53
gps.h
Device independent GPS code (interface)
GpsState::fix
uint8_t fix
status of fix
Definition: gps.h:110
GpsState::speed_3d
uint16_t speed_3d
norm of 3d speed in cm/s
Definition: gps.h:101
GpsSkytraq::msg_available
bool msg_available
Definition: gps_skytraq.h:46
EcefCoor_i
vector in EarthCenteredEarthFixed coordinates
Definition: pprz_geodetic_int.h:50
GpsSkytraq::error_last
enum GpsSkytraqError error_last
Definition: gps_skytraq.h:54
dev
static const struct usb_device_descriptor dev
Definition: usb_ser_hw.c:74
GpsSkytraq::len
uint16_t len
Definition: gps_skytraq.h:50
GPS_SKYTRAQ_ID
#define GPS_SKYTRAQ_ID
Definition: abi_sender_ids.h:240
GpsState::gspeed
uint16_t gspeed
norm of 2d ground speed in cm/s
Definition: gps.h:100
gps_skytraq_event
void gps_skytraq_event(void)
Definition: gps_skytraq.c:115
GpsState::last_3dfix_time
uint32_t last_3dfix_time
cpu time in sec at last valid 3D fix
Definition: gps.h:118
GpsSkytraq
Definition: gps_skytraq.h:44
SKYTRAQ_SYNC2
#define SKYTRAQ_SYNC2
Definition: gps_skytraq.c:46
GpsSkytraq::ref_ltp
struct LtpDef_i ref_ltp
Definition: gps_skytraq.h:56
SKYTRAQ_NAVIGATION_DATA_LON
#define SKYTRAQ_NAVIGATION_DATA_LON(_payload)
Definition: gps_skytraq.c:69
GpsState::last_3dfix_ticks
uint32_t last_3dfix_ticks
cpu time ticks at last valid 3D fix
Definition: gps.h:117
ned_of_ecef_vect_i
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.
Definition: pprz_geodetic_int.c:186
led.h
arch independent LED (Light Emitting Diodes) API
GPS_SKYTRAQ_ERR_MSG_TOO_LONG
@ GPS_SKYTRAQ_ERR_MSG_TOO_LONG
Definition: gps_skytraq.h:37
gps_skytraq_parse
void gps_skytraq_parse(uint8_t c)
Definition: gps_skytraq.c:201
SKYTRAQ_FIX_3D
#define SKYTRAQ_FIX_3D
Definition: gps_skytraq.c:41
gps_skytraq_read_message
void gps_skytraq_read_message(void)
Definition: gps_skytraq.c:127
GpsSkytraq::state
struct GpsState state
Definition: gps_skytraq.h:58
SKYTRAQ_NAVIGATION_DATA_LAT
#define SKYTRAQ_NAVIGATION_DATA_LAT(_payload)
Definition: gps_skytraq.c:68
GpsState::course
int32_t course
GPS course over ground in rad*1e7, [0, 2*Pi]*1e7 (CW/north)
Definition: gps.h:102
GpsState::last_msg_time
uint32_t last_msg_time
cpu time in sec at last received GPS message
Definition: gps.h:120
GpsState::ecef_pos
struct EcefCoor_i ecef_pos
position in ECEF in cm
Definition: gps.h:94
int32_t
int int32_t
Typedef defining 32 bit int type.
Definition: vl53l1_types.h:83
SKYTRAQ_FIX_3D_DGPS
#define SKYTRAQ_FIX_3D_DGPS
Definition: gps_skytraq.c:42
GpsSkytraq::status
uint8_t status
Definition: gps_skytraq.h:49
SKYTRAQ_NAVIGATION_DATA_ECEFVY
#define SKYTRAQ_NAVIGATION_DATA_ECEFVY(_payload)
Definition: gps_skytraq.c:84
GPS_VALID_VEL_NED_BIT
#define GPS_VALID_VEL_NED_BIT
Definition: gps.h:55
GpsState::lla_pos
struct LlaCoor_i lla_pos
position in LLA (lat,lon: deg*1e7; alt: mm over ellipsoid)
Definition: gps.h:95
GPS_VALID_VEL_ECEF_BIT
#define GPS_VALID_VEL_ECEF_BIT
Definition: gps.h:54
sys_time
Definition: sys_time.h:71
MAX_DISTANCE
#define MAX_DISTANCE
Definition: gps_skytraq.c:89
GpsState::num_sv
uint8_t num_sv
number of sat in fix
Definition: gps.h:109
distance_too_great
static int distance_too_great(struct EcefCoor_i *ecef_ref, struct EcefCoor_i *ecef_pos)
Definition: gps_skytraq.c:273
sys_time::nb_sec_rem
volatile uint32_t nb_sec_rem
remainder of seconds since startup in CPU_TICKS
Definition: sys_time.h:73
GpsState::hmsl
int32_t hmsl
height above mean sea level (MSL) in mm
Definition: gps.h:97
SKYTRAQ_NAVIGATION_DATA_NumSV
#define SKYTRAQ_NAVIGATION_DATA_NumSV(_payload)
Definition: gps_skytraq.c:63
gps_skytraq.h
gps_skytraq_msg
void gps_skytraq_msg(void)
Definition: gps_skytraq.c:98
NedCoor_i::x
int32_t x
North.
Definition: pprz_geodetic_int.h:69
GpsState::pdop
uint16_t pdop
position dilution of precision scaled by 100
Definition: gps.h:108
GOT_LEN1
#define GOT_LEN1
Definition: gps_skytraq.c:32
GPS_SKYTRAQ_MAX_PAYLOAD
#define GPS_SKYTRAQ_MAX_PAYLOAD
Definition: gps_skytraq.h:43
UNINIT
#define UNINIT
Definition: gps_skytraq.c:29
GOT_PAYLOAD
#define GOT_PAYLOAD
Definition: gps_skytraq.c:35
GPS_VALID_POS_ECEF_BIT
#define GPS_VALID_POS_ECEF_BIT
Definition: gps.h:51
SKYTRAQ_NAVIGATION_DATA_TOW
#define SKYTRAQ_NAVIGATION_DATA_TOW(_payload)
Definition: gps_skytraq.c:66
SKYTRAQ_NAVIGATION_DATA_ECEFVZ
#define SKYTRAQ_NAVIGATION_DATA_ECEFVZ(_payload)
Definition: gps_skytraq.c:85
sys_time::nb_sec
volatile uint32_t nb_sec
full seconds since startup
Definition: sys_time.h:72
GpsState::ecef_vel
struct EcefCoor_i ecef_vel
speed ECEF in cm/s
Definition: gps.h:98
SKYTRAQ_SYNC1
#define SKYTRAQ_SYNC1
Definition: gps_skytraq.c:45
GpsSkytraq::msg_id
uint8_t msg_id
Definition: gps_skytraq.h:47
SKYTRAQ_NAVIGATION_DATA_FixMode
#define SKYTRAQ_NAVIGATION_DATA_FixMode(_payload)
Definition: gps_skytraq.c:62
uint16_t
unsigned short uint16_t
Typedef defining 16 bit unsigned short type.
Definition: vl53l1_types.h:88
bswap16
static uint16_t bswap16(uint16_t a)
Definition: gps_skytraq.c:57
GPS_FIX_NONE
#define GPS_FIX_NONE
No GPS fix.
Definition: gps.h:40
GpsSkytraq::msg_idx
uint8_t msg_idx
Definition: gps_skytraq.h:51
GPS_FIX_3D
#define GPS_FIX_3D
3D GPS fix
Definition: gps.h:42
GOT_SYNC1
#define GOT_SYNC1
Definition: gps_skytraq.c:30
GPS_VALID_HMSL_BIT
#define GPS_VALID_HMSL_BIT
Definition: gps.h:56
SKYTRAQ_SYNC3
#define SKYTRAQ_SYNC3
Definition: gps_skytraq.c:48
gps_skytraq_init
void gps_skytraq_init(void)
Definition: gps_skytraq.c:93