Paparazzi UAS v7.0_unstable
Paparazzi is a free software Unmanned Aircraft System.
Loading...
Searching...
No Matches
gps_sirf.c
Go to the documentation of this file.
1/*
2 * Copyright (C) 2012 Freek van Tienen
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
23
24#include "modules/gps/gps.h"
25#include "modules/core/abi.h"
26#include "led.h"
27
29
30#include <inttypes.h>
31#include <math.h>
32#include <stdio.h>
33#include "gps_sirf.h"
34
35#include "pprzlink/pprzlink_device.h"
36#include "mcu_periph/uart.h"
37
38//Invert bytes
39#define Invert2Bytes(x) ((x>>8) | (x<<8))
40#define Invert4Bytes(x) ((x>>24) | ((x<<8) & 0x00FF0000) | ((x>>8) & 0x0000FF00) | (x<<24))
41
70
71
111
112
114
116void sirf_parse_msg(void);
117void gps_sirf_msg(void);
118
119void sirf_parse_2(void);
120void sirf_parse_41(void);
121
123{
124 gps_sirf.msg_available = false;
125 gps_sirf.msg_valid = false;
126 gps_sirf.msg_len = 0;
128}
129
146
148{
149 switch (gps_sirf.read_state) {
150 case UNINIT:
151 if (c == 0xA0) {
152 gps_sirf.msg_len = 0;
156 }
157 break;
158 case GOT_A0:
159 if (c == 0xA2) {
163 } else {
164 goto restart;
165 }
166 break;
167 case GOT_A2:
170 if (c == 0xB0) {
172 }
173 break;
174 case GOT_B0:
175 if (c == 0xB3) {
178 gps_sirf.msg_available = true;
179 } else {
180 goto restart;
181 }
182 break;
183 default:
184 break;
185 }
186 return;
187
188restart:
190}
191
193int ticks = 0;
195int ticks2 = 0;
196
198{
199 struct sirf_msg_41 *p = (struct sirf_msg_41 *)&gps_sirf.msg_buf[4];
200
202 gps_sirf.state.hmsl = Invert4Bytes(p->alt_msl) * 10;
204 gps_sirf.state.num_sv = p->num_sat;
205 gps_sirf.state.nb_channels = p ->num_sat;
206
207 /* read latitude, longitude and altitude from packet */
208 gps_sirf.state.lla_pos.lat = Invert4Bytes(p->latitude);
209 gps_sirf.state.lla_pos.lon = Invert4Bytes(p->longitude);
210 gps_sirf.state.lla_pos.alt = Invert4Bytes(p->alt_ellipsoid) * 10;
212
213 gps_sirf.state.sacc = (Invert2Bytes(p->ehve) >> 16);
214 gps_sirf.state.course = RadOfDeg(Invert2Bytes(p->cog)) * pow(10, 5);
216 gps_sirf.state.gspeed = RadOfDeg(Invert2Bytes(p->sog)) * pow(10, 5);
217 gps_sirf.state.cacc = RadOfDeg(Invert2Bytes(p->heading_err)) * pow(10, 5);
219 gps_sirf.state.pdop = p->hdop * 20;
220
221 if ((p->nav_type >> 8 & 0x7) >= 0x4) {
223 } else if ((p->nav_type >> 8 & 0x7) >= 0x1) {
225 } else {
227 }
228
229 gps_sirf.msg_valid = true;
230}
231
232void sirf_parse_2(void)
233{
234 struct sirf_msg_2 *p = (struct sirf_msg_2 *)&gps_sirf.msg_buf[4];
235
237
238 gps_sirf.state.ecef_pos.x = Invert4Bytes(p->x_pos) * 100;
239 gps_sirf.state.ecef_pos.y = Invert4Bytes(p->y_pos) * 100;
240 gps_sirf.state.ecef_pos.z = Invert4Bytes(p->z_pos) * 100;
242
243 gps_sirf.state.ecef_vel.x = (Invert2Bytes(p->vx) >> 16) * 100 / 8;
244 gps_sirf.state.ecef_vel.y = (Invert2Bytes(p->vy) >> 16) * 100 / 8;
245 gps_sirf.state.ecef_vel.z = (Invert2Bytes(p->vz) >> 16) * 100 / 8;
247
248 if (gps_sirf.state.fix == GPS_FIX_3D) {
249 ticks++;
250#if DEBUG_SIRF
251 printf("GPS %i %i %i %i\n", ticks, (sys_time.nb_sec - start_time), ticks2, (sys_time.nb_sec - start_time2));
252#endif
253 } else if (sys_time.nb_sec - gps_sirf.state.last_3dfix_time > 10) {
255 ticks = 0;
256 }
257
258 gps_sirf.msg_valid = true;
259}
260
262{
263 //Set msg_valid to false and check if it is a valid message
264 gps_sirf.msg_valid = false;
265 if (gps_sirf.msg_len < 8) {
266 return;
267 }
268
269 if (start_time2 == 0) {
271 }
272 ticks2++;
273
274 //Check the message id and parse the message
276 switch (message_id) {
277 case 0x29:
279 break;
280 case 0x02:
281 sirf_parse_2();
282 break;
283 }
284
285}
286
288{
289 struct link_device *dev = &((SIRF_GPS_LINK).device);
290
291 while (dev->char_available(dev->periph)) {
292 sirf_parse_char(dev->get_byte(dev->periph));
294 gps_sirf_msg();
295 }
296 }
297}
Main include for ABI (AirBorneInterface).
#define GPS_SIRF_ID
uint32_t get_sys_time_usec(void)
Get the time in microseconds since startup.
Device independent GPS code (interface)
uint32_t tow
GPS time of week in ms.
Definition gps.h:109
int32_t hmsl
height above mean sea level (MSL) in mm
Definition gps.h:94
struct LlaCoor_i lla_pos
position in LLA (lat,lon: deg*1e7; alt: mm over ellipsoid)
Definition gps.h:92
uint32_t sacc
speed accuracy in cm/s
Definition gps.h:103
uint32_t cacc
course accuracy in rad*1e7
Definition gps.h:104
int32_t course
GPS course over ground in rad*1e7, [0, 2*Pi]*1e7 (CW/north)
Definition gps.h:99
#define GPS_VALID_VEL_ECEF_BIT
Definition gps.h:51
struct EcefCoor_i ecef_pos
position in ECEF in cm
Definition gps.h:91
#define GPS_VALID_POS_LLA_BIT
Definition gps.h:49
uint32_t last_3dfix_ticks
cpu time ticks at last valid 3D fix
Definition gps.h:114
struct EcefCoor_i ecef_vel
speed ECEF in cm/s
Definition gps.h:95
uint16_t pdop
position dilution of precision scaled by 100
Definition gps.h:105
#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
uint32_t last_msg_time
cpu time in sec at last received GPS message
Definition gps.h:117
uint8_t nb_channels
Number of scanned satellites.
Definition gps.h:111
uint32_t last_3dfix_time
cpu time in sec at last valid 3D fix
Definition gps.h:115
uint32_t pacc
position accuracy in cm
Definition gps.h:100
uint16_t gspeed
norm of 2d ground speed in cm/s
Definition gps.h:97
uint8_t valid_fields
bitfield indicating valid fields (GPS_VALID_x_BIT)
Definition gps.h:88
#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
uint32_t last_msg_ticks
cpu time ticks at last received GPS message
Definition gps.h:116
uint8_t num_sv
number of sat in fix
Definition gps.h:106
uint16_t week
GPS week.
Definition gps.h:108
uint8_t fix
status of fix
Definition gps.h:107
uint8_t ch5prn
Definition gps_sirf.c:61
uint16_t extended_week_number
Definition gps_sirf.c:77
uint8_t ch2prn
Definition gps_sirf.c:58
int32_t alt_msl
in meters *10^2
Definition gps_sirf.c:89
int32_t y_pos
y-position in m
Definition gps_sirf.c:46
uint8_t ch1prn
pseudo-random noise, 12 channels
Definition gps_sirf.c:57
uint8_t ch7prn
Definition gps_sirf.c:63
int32_t clock_bias
in m * 10^2
Definition gps_sirf.c:100
uint8_t num_sat
Number of satellites used for solution.
Definition gps_sirf.c:107
uint8_t minute
Definition gps_sirf.c:83
uint8_t msg_id
hex value 0x29 (= decimal 41)
Definition gps_sirf.c:74
uint32_t ehpe
estimated horizontal position error, in meters * 10^2
Definition gps_sirf.c:96
int8_t map_datum
Definition gps_sirf.c:90
uint8_t month
Definition gps_sirf.c:80
int32_t longitude
in degrees (+= East) *10*7
Definition gps_sirf.c:87
uint32_t evpe
estimated vertical position error, in meters * 10^2
Definition gps_sirf.c:97
int32_t latitude
in degrees (+= North) *10^7
Definition gps_sirf.c:86
uint8_t add_info
Additional mode info.
Definition gps_sirf.c:109
uint32_t tow
time of week in seconds * 10^2
Definition gps_sirf.c:55
uint16_t week
Definition gps_sirf.c:54
uint8_t hour
Definition gps_sirf.c:82
void sirf_parse_char(uint8_t c)
Definition gps_sirf.c:147
uint8_t day
Definition gps_sirf.c:81
uint16_t ehve
estimated horizontal velocity error in m/s * 10^2
Definition gps_sirf.c:99
uint16_t cog
course over ground, in degrees clockwise from true north * 10^2
Definition gps_sirf.c:92
uint16_t distance_err
in meters
Definition gps_sirf.c:105
uint32_t clock_drift_err
in m/s * 10^2
Definition gps_sirf.c:103
void gps_sirf_init(void)
Definition gps_sirf.c:122
uint8_t ch8prn
Definition gps_sirf.c:64
uint32_t tow
time of week in seconds *10^3]
Definition gps_sirf.c:78
uint8_t ch12prn
Definition gps_sirf.c:68
uint16_t year
Definition gps_sirf.c:79
uint8_t mode1
Definition gps_sirf.c:51
uint8_t hdop
Horizontal dilution of precision x 5 (0.2 precision)
Definition gps_sirf.c:108
#define Invert2Bytes(x)
Definition gps_sirf.c:39
uint16_t sog
speed over ground, in m/s * 10^2
Definition gps_sirf.c:91
void sirf_parse_msg(void)
Definition gps_sirf.c:261
int ticks
Definition gps_sirf.c:193
void gps_sirf_event(void)
Definition gps_sirf.c:287
int16_t mag_var
not implemented
Definition gps_sirf.c:93
uint16_t heading_err
in degrees * 10^2
Definition gps_sirf.c:106
uint16_t nav_valid
if equal to 0x0000, then navigation solution is valid
Definition gps_sirf.c:75
int32_t x_pos
x-position in m
Definition gps_sirf.c:45
int ticks2
Definition gps_sirf.c:195
void sirf_parse_41(void)
Definition gps_sirf.c:197
int start_time
Definition gps_sirf.c:192
int16_t vy
y-velocity * 8 in m/s
Definition gps_sirf.c:49
uint8_t hdop
horizontal dilution of precision *5 (0.2 precision)
Definition gps_sirf.c:52
int16_t heading_rate
in deg/s * 10^2
Definition gps_sirf.c:95
uint32_t distance
Distance traveled since reset in m.
Definition gps_sirf.c:104
uint32_t clock_bias_err
in m * 10^2
Definition gps_sirf.c:101
uint8_t ch4prn
Definition gps_sirf.c:60
uint16_t nav_type
Definition gps_sirf.c:76
uint8_t msg_id
hex value 0x02 ( = decimal 2)
Definition gps_sirf.c:44
void sirf_parse_2(void)
Definition gps_sirf.c:232
uint8_t ch3prn
Definition gps_sirf.c:59
uint8_t ch9prn
Definition gps_sirf.c:65
int32_t alt_ellipsoid
in meters *10^2
Definition gps_sirf.c:88
int32_t clock_drift
in m/s * 10^2
Definition gps_sirf.c:102
int32_t z_pos
z-position in m
Definition gps_sirf.c:47
uint8_t ch10prn
Definition gps_sirf.c:66
#define Invert4Bytes(x)
Definition gps_sirf.c:40
struct GpsSirf gps_sirf
Definition gps_sirf.c:113
uint8_t ch6prn
Definition gps_sirf.c:62
uint32_t sat_id
satellites used in solution. Each satellite corresponds with a bit, e.g. bit 1 ON = SV 1 is used in s...
Definition gps_sirf.c:85
int16_t vz
z-velocity * 8 in m/s
Definition gps_sirf.c:50
void gps_sirf_msg(void)
Definition gps_sirf.c:130
int16_t climb_rate
in m/s * 10^2
Definition gps_sirf.c:94
uint8_t num_sat
Number of satellites in fix.
Definition gps_sirf.c:56
uint16_t second
Definition gps_sirf.c:84
uint8_t ch11prn
Definition gps_sirf.c:67
int start_time2
Definition gps_sirf.c:194
int16_t vx
x-velocity * 8 in m/s
Definition gps_sirf.c:48
uint32_t ete
estimated time error, in seconds * 10^2
Definition gps_sirf.c:98
uint8_t mode2
Definition gps_sirf.c:53
Message ID 2 from GPS.
Definition gps_sirf.c:43
Message ID 41 from GPS.
Definition gps_sirf.c:73
Sirf protocol specific code.
int read_state
Definition gps_sirf.h:53
bool msg_valid
Definition gps_sirf.h:50
bool msg_available
Definition gps_sirf.h:49
int msg_len
Definition gps_sirf.h:52
char msg_buf[SIRF_MAXLEN]
buffer for storing one nmea-line
Definition gps_sirf.h:51
#define GOT_B0
Definition gps_sirf.h:46
struct GpsState state
Definition gps_sirf.h:54
#define GOT_A2
Definition gps_sirf.h:45
#define GOT_A0
Definition gps_sirf.h:44
int32_t lat
in degrees*1e7
int32_t alt
in millimeters above WGS84 reference ellipsoid
int32_t z
in centimeters
int32_t x
in centimeters
int32_t y
in centimeters
int32_t lon
in degrees*1e7
static float p[2][2]
arch independent LED (Light Emitting Diodes) API
uint16_t foo
Definition main_demo5.c:58
Paparazzi floating point math for geodetic calculations.
#define UNINIT
Receiving pprz messages.
Definition protocol.c:11
static const struct usb_device_descriptor dev
Definition usb_ser_hw.c:74
volatile uint32_t nb_sec
full seconds since startup
Definition sys_time.h:72
volatile uint32_t nb_sec_rem
remainder of seconds since startup in CPU_TICKS
Definition sys_time.h:73
arch independent UART (Universal Asynchronous Receiver/Transmitter) API
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.
short int16_t
Typedef defining 16 bit short type.
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.
signed char int8_t
Typedef defining 8 bit char type.