Paparazzi UAS v7.1_unstable
Paparazzi is a free software Unmanned Aircraft System.
Loading...
Searching...
No Matches
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
55void gps_skytraq_msg(void);
56
57static 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
91static int distance_too_great(struct EcefCoor_i *ecef_ref, struct EcefCoor_i *ecef_pos);
92
94{
95 gps_skytraq.status = UNINIT;
96}
97
99{
100 // current timestamp
101 uint32_t now_ts = get_sys_time_usec();
102 gps_skytraq.state.last_msg_ticks = sys_time.nb_sec_rem;
103 gps_skytraq.state.last_msg_time = sys_time.nb_sec;
106 if (gps_skytraq.state.fix == GPS_FIX_3D) {
107 gps_skytraq.state.last_3dfix_ticks = sys_time.nb_sec_rem;
108 gps_skytraq.state.last_3dfix_time = sys_time.nb_sec;
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));
121 if (gps_skytraq.msg_available) {
123 }
124 }
125}
126
128{
129
131 gps_skytraq.state.ecef_pos.x = SKYTRAQ_NAVIGATION_DATA_ECEFX(gps_skytraq.msg_buf);
132 gps_skytraq.state.ecef_pos.y = SKYTRAQ_NAVIGATION_DATA_ECEFY(gps_skytraq.msg_buf);
133 gps_skytraq.state.ecef_pos.z = SKYTRAQ_NAVIGATION_DATA_ECEFZ(gps_skytraq.msg_buf);
134 SetBit(gps_skytraq.state.valid_fields, GPS_VALID_POS_ECEF_BIT);
135
136 gps_skytraq.state.ecef_vel.x = SKYTRAQ_NAVIGATION_DATA_ECEFVX(gps_skytraq.msg_buf);
137 gps_skytraq.state.ecef_vel.y = SKYTRAQ_NAVIGATION_DATA_ECEFVY(gps_skytraq.msg_buf);
138 gps_skytraq.state.ecef_vel.z = SKYTRAQ_NAVIGATION_DATA_ECEFVZ(gps_skytraq.msg_buf);
139 SetBit(gps_skytraq.state.valid_fields, GPS_VALID_VEL_ECEF_BIT);
140
141 gps_skytraq.state.lla_pos.lat = SKYTRAQ_NAVIGATION_DATA_LAT(gps_skytraq.msg_buf);
142 gps_skytraq.state.lla_pos.lon = SKYTRAQ_NAVIGATION_DATA_LON(gps_skytraq.msg_buf);
143 gps_skytraq.state.lla_pos.alt = SKYTRAQ_NAVIGATION_DATA_AEL(gps_skytraq.msg_buf) * 10;
144 SetBit(gps_skytraq.state.valid_fields, GPS_VALID_POS_LLA_BIT);
145
146 gps_skytraq.state.hmsl = SKYTRAQ_NAVIGATION_DATA_ASL(gps_skytraq.msg_buf) * 10;
147 SetBit(gps_skytraq.state.valid_fields, GPS_VALID_HMSL_BIT);
148
149 // pacc;
150 // sacc;
153 gps_skytraq.state.tow = SKYTRAQ_NAVIGATION_DATA_TOW(gps_skytraq.msg_buf) * 10;
154
157 case SKYTRAQ_FIX_3D:
158 gps_skytraq.state.fix = GPS_FIX_3D;
159 break;
160 case SKYTRAQ_FIX_2D:
161 gps_skytraq.state.fix = GPS_FIX_2D;
162 break;
163 default:
164 gps_skytraq.state.fix = GPS_FIX_NONE;
165 }
166
167 if (gps_skytraq.state.fix == GPS_FIX_3D) {
168 if (distance_too_great(&gps_skytraq.ref_ltp.ecef, &gps_skytraq.state.ecef_pos)) {
169 // just grab current ecef_pos as reference.
170 ltp_def_from_ecef_i(&gps_skytraq.ref_ltp, &gps_skytraq.state.ecef_pos);
171 }
172 // convert ecef velocity vector to NED vector.
173 ned_of_ecef_vect_i(&gps_skytraq.state.ned_vel, &gps_skytraq.ref_ltp, &gps_skytraq.state.ecef_vel);
174 SetBit(gps_skytraq.state.valid_fields, GPS_VALID_VEL_NED_BIT);
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;
178 SetBit(gps_skytraq.state.valid_fields, GPS_VALID_COURSE_BIT);
179 // GT: gps_skytraq.state.cacc = ... ? what should course accuracy be?
180
181 // ground speed
182 gps_skytraq.state.gspeed = sqrt(gps_skytraq.state.ned_vel.x * gps_skytraq.state.ned_vel.x + gps_skytraq.state.ned_vel.y * gps_skytraq.state.ned_vel.y);
183 gps_skytraq.state.speed_3d = sqrt(gps_skytraq.state.ned_vel.x * gps_skytraq.state.ned_vel.x + gps_skytraq.state.ned_vel.y * gps_skytraq.state.ned_vel.y + gps_skytraq.state.ned_vel.z * gps_skytraq.state.ned_vel.z);
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{
203 if (gps_skytraq.status < GOT_PAYLOAD) {
204 gps_skytraq.checksum ^= c;
205 }
206 switch (gps_skytraq.status) {
207 case UNINIT:
208 if (c == SKYTRAQ_SYNC1) {
209 gps_skytraq.status = GOT_SYNC1;
210 }
211 break;
212 case GOT_SYNC1:
213 if (c != SKYTRAQ_SYNC2) {
215 goto error;
216 }
217 gps_skytraq.status = GOT_SYNC2;
218 break;
219 case GOT_SYNC2:
220 gps_skytraq.len = c << 8;
221 gps_skytraq.status = GOT_LEN1;
222 break;
223 case GOT_LEN1:
224 gps_skytraq.len += c;
225 gps_skytraq.status = GOT_LEN2;
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;
235 gps_skytraq.status = GOT_ID;
236 break;
237 case GOT_ID:
238 gps_skytraq.msg_buf[gps_skytraq.msg_idx] = c;
239 gps_skytraq.msg_idx++;
240 if (gps_skytraq.msg_idx >= gps_skytraq.len - 1) {
241 gps_skytraq.status = GOT_PAYLOAD;
242 }
243 break;
244 case GOT_PAYLOAD:
245 if (c != gps_skytraq.checksum) {
247 goto error;
248 }
249 gps_skytraq.status = GOT_CHECKSUM;
250 break;
251 case GOT_CHECKSUM:
252 if (c != SKYTRAQ_SYNC3) {
254 goto error;
255 }
256 gps_skytraq.status = GOT_SYNC3;
257 break;
258 case GOT_SYNC3:
259 gps_skytraq.msg_available = true;
260 goto restart;
261 default:
263 goto error;
264 }
265 return;
266error:
267 gps_skytraq.error_cnt++;
268restart:
269 gps_skytraq.status = UNINIT;
270 return;
271}
272
273static 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}
Main include for ABI (AirBorneInterface).
#define GPS_SKYTRAQ_ID
#define LED_ON(i)
Definition led_hw.h:51
#define LED_TOGGLE(i)
Definition led_hw.h:53
uint32_t get_sys_time_usec(void)
Get the time in microseconds since startup.
static struct uart_periph * dev
Device independent GPS code (interface).
#define GPS_VALID_VEL_ECEF_BIT
Definition gps.h:51
#define GPS_VALID_VEL_NED_BIT
Definition gps.h:52
#define GPS_VALID_POS_LLA_BIT
Definition gps.h:49
#define GPS_FIX_NONE
No GPS fix.
Definition gps.h:42
#define GPS_VALID_POS_ECEF_BIT
Definition gps.h:48
#define GPS_VALID_HMSL_BIT
Definition gps.h:53
#define GPS_FIX_2D
2D GPS fix
Definition gps.h:43
#define GPS_FIX_3D
3D GPS fix
Definition gps.h:44
#define GPS_VALID_COURSE_BIT
Definition gps.h:54
#define GOT_ID
Definition gps_mtk.c:55
#define GOT_CHECKSUM
Definition gps_nmea.c:56
#define MAX_DISTANCE
Definition gps_skytraq.c:89
#define SKYTRAQ_NAVIGATION_DATA_ECEFX(_payload)
Definition gps_skytraq.c:80
#define SKYTRAQ_NAVIGATION_DATA_TOW(_payload)
Definition gps_skytraq.c:66
#define SKYTRAQ_NAVIGATION_DATA_ECEFVZ(_payload)
Definition gps_skytraq.c:85
#define SKYTRAQ_FIX_3D_DGPS
Definition gps_skytraq.c:42
#define GOT_SYNC2
Definition gps_skytraq.c:31
#define SKYTRAQ_NAVIGATION_DATA_AEL(_payload)
Definition gps_skytraq.c:71
static uint16_t bswap16(uint16_t a)
Definition gps_skytraq.c:57
#define SKYTRAQ_NAVIGATION_DATA_ECEFY(_payload)
Definition gps_skytraq.c:81
#define SKYTRAQ_NAVIGATION_DATA_ECEFVX(_payload)
Definition gps_skytraq.c:83
#define GOT_LEN1
Definition gps_skytraq.c:32
#define GOT_SYNC3
Definition gps_skytraq.c:37
#define SKYTRAQ_NAVIGATION_DATA_PDOP(_payload)
Definition gps_skytraq.c:75
#define GOT_SYNC1
Definition gps_skytraq.c:30
#define SKYTRAQ_NAVIGATION_DATA_LAT(_payload)
Definition gps_skytraq.c:68
void gps_skytraq_init(void)
Definition gps_skytraq.c:93
void gps_skytraq_event(void)
void gps_skytraq_read_message(void)
#define SKYTRAQ_NAVIGATION_DATA_NumSV(_payload)
Definition gps_skytraq.c:63
#define SKYTRAQ_NAVIGATION_DATA_FixMode(_payload)
Definition gps_skytraq.c:62
#define SKYTRAQ_SYNC2
Definition gps_skytraq.c:46
#define SKYTRAQ_NAVIGATION_DATA_ECEFZ(_payload)
Definition gps_skytraq.c:82
void gps_skytraq_msg(void)
Definition gps_skytraq.c:98
void gps_skytraq_parse(uint8_t c)
#define SKYTRAQ_FIX_3D
Definition gps_skytraq.c:41
#define SKYTRAQ_FIX_2D
Definition gps_skytraq.c:40
#define SKYTRAQ_SYNC1
Definition gps_skytraq.c:45
struct GpsSkytraq gps_skytraq
Definition gps_skytraq.c:51
static int distance_too_great(struct EcefCoor_i *ecef_ref, struct EcefCoor_i *ecef_pos)
#define SKYTRAQ_SYNC3
Definition gps_skytraq.c:48
#define SKYTRAQ_NAVIGATION_DATA_ASL(_payload)
Definition gps_skytraq.c:72
#define GOT_LEN2
Definition gps_skytraq.c:33
#define SKYTRAQ_NAVIGATION_DATA_LON(_payload)
Definition gps_skytraq.c:69
#define SKYTRAQ_NAVIGATION_DATA_ECEFVY(_payload)
Definition gps_skytraq.c:84
#define SKYTRAQ_ID_NAVIGATION_DATA
Definition gps_skytraq.h:27
#define GPS_SKYTRAQ_MAX_PAYLOAD
Definition gps_skytraq.h:43
@ GPS_SKYTRAQ_ERR_UNEXPECTED
Definition gps_skytraq.h:40
@ GPS_SKYTRAQ_ERR_MSG_TOO_LONG
Definition gps_skytraq.h:37
@ GPS_SKYTRAQ_ERR_CHECKSUM
Definition gps_skytraq.h:38
@ GPS_SKYTRAQ_ERR_OUT_OF_SYNC
Definition gps_skytraq.h:39
int32_t z
in centimeters
int32_t x
in centimeters
int32_t y
in centimeters
void ltp_def_from_ecef_i(struct LtpDef_i *def, struct EcefCoor_i *ecef)
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.
vector in EarthCenteredEarthFixed coordinates
arch independent LED (Light Emitting Diodes) API
#define GOT_PAYLOAD
Definition protocol.c:15
#define UNINIT
Receiving pprz messages.
Definition protocol.c:11
volatile uint32_t nb_sec
full seconds since startup
Definition sys_time.h:80
volatile uint32_t nb_sec_rem
remainder of seconds since startup in CPU_TICKS
Definition sys_time.h:81
unsigned short uint16_t
Typedef defining 16 bit unsigned short type.
int int32_t
Typedef defining 32 bit int type.
unsigned int uint32_t
Typedef defining 32 bit unsigned int type.
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.