Paparazzi UAS  v5.18.0_stable
Paparazzi is a free software Unmanned Aircraft System.
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 "subsystems/gps.h"
25 #include "subsystems/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 
43 struct sirf_msg_2 {
69 } __attribute__((packed));
70 
71 
73 struct sirf_msg_41 {
110 } __attribute__((packed));
111 
112 
114 
116 void sirf_parse_msg(void);
117 void gps_sirf_msg(void);
118 
119 void sirf_parse_2(void);
120 void sirf_parse_41(void);
121 
122 void gps_sirf_init(void)
123 {
124  gps_sirf.msg_available = false;
125  gps_sirf.msg_valid = false;
126  gps_sirf.msg_len = 0;
127  gps_sirf.read_state = 0;
128 }
129 
130 void gps_sirf_msg(void)
131 {
132  // current timestamp
133  uint32_t now_ts = get_sys_time_usec();
136  sirf_parse_msg();
137  if (gps_sirf.msg_valid) {
138  if (gps_sirf.state.fix == GPS_FIX_3D) {
141  }
142  AbiSendMsgGPS(GPS_SIRF_ID, now_ts, &gps_sirf.state);
143  }
144  gps_sirf.msg_valid = false;
145 }
146 
148 {
149  switch (gps_sirf.read_state) {
150  case UNINIT:
151  if (c == 0xA0) {
152  gps_sirf.msg_len = 0;
154  gps_sirf.msg_len++;
156  }
157  break;
158  case GOT_A0:
159  if (c == 0xA2) {
161  gps_sirf.msg_len++;
163  } else {
164  goto restart;
165  }
166  break;
167  case GOT_A2:
169  gps_sirf.msg_len++;
170  if (c == 0xB0) {
172  }
173  break;
174  case GOT_B0:
175  if (c == 0xB3) {
177  gps_sirf.msg_len++;
178  gps_sirf.msg_available = true;
179  } else {
180  goto restart;
181  }
182  break;
183  default:
184  break;
185  }
186  return;
187 
188 restart:
190 }
191 
192 int start_time = 0;
193 int ticks = 0;
194 int start_time2 = 0;
195 int ticks2 = 0;
196 
197 void sirf_parse_41(void)
198 {
199  struct sirf_msg_41 *p = (struct sirf_msg_41 *)&gps_sirf.msg_buf[4];
200 
201  gps_sirf.state.tow = Invert4Bytes(p->tow);
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);
218  gps_sirf.state.pacc = Invert4Bytes(p->ehpe);
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 
232 void sirf_parse_2(void)
233 {
234  struct sirf_msg_2 *p = (struct sirf_msg_2 *)&gps_sirf.msg_buf[4];
235 
236  gps_sirf.state.week = Invert2Bytes(p->week);
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 
261 void sirf_parse_msg(void)
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
275  uint8_t message_id = gps_sirf.msg_buf[4];
276  switch (message_id) {
277  case 0x29:
278  sirf_parse_41();
279  break;
280  case 0x02:
281  sirf_parse_2();
282  break;
283  }
284 
285 }
286 
287 void gps_sirf_event(void)
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));
293  if (gps_sirf.msg_available) {
294  gps_sirf_msg();
295  }
296  }
297 }
c
VIC slots used for the LPC2148 define name e g gps UART1_VIC_SLOT e g modem SPI1_VIC_SLOT SPI1 in mcu_periph spi_arch c or spi_slave_hs_arch c(and some others not using the SPI peripheral yet..) I2C0_VIC_SLOT 8 mcu_periph/i2c_arch.c I2C1_VIC_SLOT 9 mcu_periph/i2c_arch.c USB_VIC_SLOT 10 usb
sirf_msg_2::ch2prn
uint8_t ch2prn
Definition: gps_sirf.c:58
sirf_parse_msg
void sirf_parse_msg(void)
Definition: gps_sirf.c:261
LlaCoor_i::lon
int32_t lon
in degrees*1e7
Definition: pprz_geodetic_int.h:61
uint16_t
unsigned short uint16_t
Definition: types.h:16
GpsState::valid_fields
uint8_t valid_fields
bitfield indicating valid fields (GPS_VALID_x_BIT)
Definition: gps.h:88
sirf_msg_41::map_datum
int8_t map_datum
Definition: gps_sirf.c:90
GOT_A0
#define GOT_A0
Definition: gps_sirf.h:44
LlaCoor_i::alt
int32_t alt
in millimeters above WGS84 reference ellipsoid
Definition: pprz_geodetic_int.h:62
sirf_msg_2::msg_id
uint8_t msg_id
hex value 0x02 ( = decimal 2)
Definition: gps_sirf.c:44
sirf_msg_41
Message ID 41 from GPS.
Definition: gps_sirf.c:73
GPS_VALID_COURSE_BIT
#define GPS_VALID_COURSE_BIT
Definition: gps.h:54
GPS_FIX_2D
#define GPS_FIX_2D
2D GPS fix
Definition: gps.h:38
UNINIT
#define UNINIT
Receiving pprz messages.
Definition: protocol.c:11
sirf_msg_2::ch6prn
uint8_t ch6prn
Definition: gps_sirf.c:62
sirf_msg_41::clock_drift_err
uint32_t clock_drift_err
in m/s * 10^2
Definition: gps_sirf.c:103
sirf_msg_41::second
uint16_t second
Definition: gps_sirf.c:84
sirf_msg_41::climb_rate
int16_t climb_rate
in m/s * 10^2
Definition: gps_sirf.c:94
sirf_parse_char
void sirf_parse_char(uint8_t c)
Definition: gps_sirf.c:147
sirf_msg_2::z_pos
int32_t z_pos
z-position in m
Definition: gps_sirf.c:47
GpsSirf::msg_buf
char msg_buf[SIRF_MAXLEN]
buffer for storing one nmea-line
Definition: gps_sirf.h:51
sirf_msg_2::tow
uint32_t tow
time of week in seconds * 10^2
Definition: gps_sirf.c:55
sirf_msg_2::vx
int16_t vx
x-velocity * 8 in m/s
Definition: gps_sirf.c:48
abi.h
GpsState::tow
uint32_t tow
GPS time of week in ms.
Definition: gps.h:109
sirf_msg_41::ehpe
uint32_t ehpe
estimated horizontal position error, in meters * 10^2
Definition: gps_sirf.c:96
GpsSirf::msg_len
int msg_len
Definition: gps_sirf.h:52
sirf_msg_2::x_pos
int32_t x_pos
x-position in m
Definition: gps_sirf.c:45
sirf_msg_41::latitude
int32_t latitude
in degrees (+= North) *10^7
Definition: gps_sirf.c:86
Invert4Bytes
#define Invert4Bytes(x)
Definition: gps_sirf.c:40
start_time
int start_time
Definition: gps_sirf.c:192
sirf_msg_2::ch9prn
uint8_t ch9prn
Definition: gps_sirf.c:65
GpsState::nb_channels
uint8_t nb_channels
Number of scanned satellites.
Definition: gps.h:111
sirf_msg_41::distance
uint32_t distance
Distance traveled since reset in m.
Definition: gps_sirf.c:104
EcefCoor_i::x
int32_t x
in centimeters
Definition: pprz_geodetic_int.h:51
GpsState::pacc
uint32_t pacc
position accuracy in cm
Definition: gps.h:100
GpsState::sacc
uint32_t sacc
speed accuracy in cm/s
Definition: gps.h:103
uint32_t
unsigned long uint32_t
Definition: types.h:18
sirf_msg_41::clock_bias_err
uint32_t clock_bias_err
in m * 10^2
Definition: gps_sirf.c:101
sirf_msg_41::clock_drift
int32_t clock_drift
in m/s * 10^2
Definition: gps_sirf.c:102
sirf_msg_2
Message ID 2 from GPS.
Definition: gps_sirf.c:43
sirf_msg_2::ch1prn
uint8_t ch1prn
pseudo-random noise, 12 channels
Definition: gps_sirf.c:57
sirf_msg_41::year
uint16_t year
Definition: gps_sirf.c:79
GpsSirf::state
struct GpsState state
Definition: gps_sirf.h:54
GOT_B0
#define GOT_B0
Definition: gps_sirf.h:46
sirf_msg_41::nav_type
uint16_t nav_type
Definition: gps_sirf.c:76
sirf_msg_2::ch4prn
uint8_t ch4prn
Definition: gps_sirf.c:60
sirf_msg_41::sog
uint16_t sog
speed over ground, in m/s * 10^2
Definition: gps_sirf.c:91
sirf_msg_41::month
uint8_t month
Definition: gps_sirf.c:80
sirf_msg_2::ch3prn
uint8_t ch3prn
Definition: gps_sirf.c:59
gps_sirf.h
Sirf protocol specific code.
sirf_msg_41::day
uint8_t day
Definition: gps_sirf.c:81
EcefCoor_i::y
int32_t y
in centimeters
Definition: pprz_geodetic_int.h:52
sirf_msg_2::vy
int16_t vy
y-velocity * 8 in m/s
Definition: gps_sirf.c:49
gps_sirf_msg
void gps_sirf_msg(void)
Definition: gps_sirf.c:130
GPS_VALID_POS_LLA_BIT
#define GPS_VALID_POS_LLA_BIT
Definition: gps.h:49
ticks2
int ticks2
Definition: gps_sirf.c:195
get_sys_time_usec
uint32_t get_sys_time_usec(void)
Get the time in microseconds since startup.
Definition: sys_time_arch.c:68
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:116
pprz_geodetic_float.h
Paparazzi floating point math for geodetic calculations.
sirf_msg_2::ch7prn
uint8_t ch7prn
Definition: gps_sirf.c:63
uart.h
arch independent UART (Universal Asynchronous Receiver/Transmitter) API
sirf_msg_41::tow
uint32_t tow
time of week in seconds *10^3]
Definition: gps_sirf.c:78
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:107
sirf_msg_2::vz
int16_t vz
z-velocity * 8 in m/s
Definition: gps_sirf.c:50
GpsState::week
uint16_t week
GPS week.
Definition: gps.h:108
sirf_msg_41::heading_rate
int16_t heading_rate
in deg/s * 10^2
Definition: gps_sirf.c:95
dev
static const struct usb_device_descriptor dev
Definition: usb_ser_hw.c:74
sirf_msg_41::hour
uint8_t hour
Definition: gps_sirf.c:82
sirf_msg_41::evpe
uint32_t evpe
estimated vertical position error, in meters * 10^2
Definition: gps_sirf.c:97
sirf_msg_2::week
uint16_t week
Definition: gps_sirf.c:54
int16_t
signed short int16_t
Definition: types.h:17
sirf_msg_41::msg_id
uint8_t msg_id
hex value 0x29 (= decimal 41)
Definition: gps_sirf.c:74
GpsSirf::msg_available
bool msg_available
Definition: gps_sirf.h:49
uint8_t
unsigned char uint8_t
Definition: types.h:14
sirf_msg_41::mag_var
int16_t mag_var
not implemented
Definition: gps_sirf.c:93
sirf_msg_2::ch10prn
uint8_t ch10prn
Definition: gps_sirf.c:66
GpsState::gspeed
uint16_t gspeed
norm of 2d ground speed in cm/s
Definition: gps.h:97
sirf_msg_41::longitude
int32_t longitude
in degrees (+= East) *10*7
Definition: gps_sirf.c:87
sirf_msg_2::mode1
uint8_t mode1
Definition: gps_sirf.c:51
gps_sirf
struct GpsSirf gps_sirf
Definition: gps_sirf.c:113
GpsState::last_3dfix_time
uint32_t last_3dfix_time
cpu time in sec at last valid 3D fix
Definition: gps.h:115
sirf_msg_41::alt_msl
int32_t alt_msl
in meters *10^2
Definition: gps_sirf.c:89
sirf_msg_41::num_sat
uint8_t num_sat
Number of satellites used for solution.
Definition: gps_sirf.c:107
GpsState::last_3dfix_ticks
uint32_t last_3dfix_ticks
cpu time ticks at last valid 3D fix
Definition: gps.h:114
sirf_parse_41
void sirf_parse_41(void)
Definition: gps_sirf.c:197
led.h
arch independent LED (Light Emitting Diodes) API
sirf_msg_41::clock_bias
int32_t clock_bias
in m * 10^2
Definition: gps_sirf.c:100
sirf_msg_2::ch11prn
uint8_t ch11prn
Definition: gps_sirf.c:67
sirf_msg_41::distance_err
uint16_t distance_err
in meters
Definition: gps_sirf.c:105
GpsState::course
int32_t course
GPS course over ground in rad*1e7, [0, 2*Pi]*1e7 (CW/north)
Definition: gps.h:99
GpsState::last_msg_time
uint32_t last_msg_time
cpu time in sec at last received GPS message
Definition: gps.h:117
GpsState::ecef_pos
struct EcefCoor_i ecef_pos
position in ECEF in cm
Definition: gps.h:91
sirf_msg_41::cog
uint16_t cog
course over ground, in degrees clockwise from true north * 10^2
Definition: gps_sirf.c:92
GOT_A2
#define GOT_A2
Definition: gps_sirf.h:45
int8_t
signed char int8_t
Definition: types.h:15
sirf_parse_2
void sirf_parse_2(void)
Definition: gps_sirf.c:232
Invert2Bytes
#define Invert2Bytes(x)
Definition: gps_sirf.c:39
sirf_msg_2::ch8prn
uint8_t ch8prn
Definition: gps_sirf.c:64
GpsState::lla_pos
struct LlaCoor_i lla_pos
position in LLA (lat,lon: deg*1e7; alt: mm over ellipsoid)
Definition: gps.h:92
int32_t
signed long int32_t
Definition: types.h:19
GPS_VALID_VEL_ECEF_BIT
#define GPS_VALID_VEL_ECEF_BIT
Definition: gps.h:51
sys_time
Definition: sys_time.h:71
GpsState::num_sv
uint8_t num_sv
number of sat in fix
Definition: gps.h:106
sys_time::nb_sec_rem
volatile uint32_t nb_sec_rem
remainder of seconds since startup in CPU_TICKS
Definition: sys_time.h:73
sirf_msg_41::alt_ellipsoid
int32_t alt_ellipsoid
in meters *10^2
Definition: gps_sirf.c:88
GpsState::hmsl
int32_t hmsl
height above mean sea level (MSL) in mm
Definition: gps.h:94
sirf_msg_2::mode2
uint8_t mode2
Definition: gps_sirf.c:53
start_time2
int start_time2
Definition: gps_sirf.c:194
ticks
int ticks
Definition: gps_sirf.c:193
GpsSirf::read_state
int read_state
Definition: gps_sirf.h:53
GPS_SIRF_ID
#define GPS_SIRF_ID
Definition: abi_sender_ids.h:220
GpsState::pdop
uint16_t pdop
position dilution of precision scaled by 100
Definition: gps.h:105
gps_sirf_event
void gps_sirf_event(void)
Definition: gps_sirf.c:287
GpsSirf::msg_valid
bool msg_valid
Definition: gps_sirf.h:50
sirf_msg_41::ehve
uint16_t ehve
estimated horizontal velocity error in m/s * 10^2
Definition: gps_sirf.c:99
sirf_msg_41::add_info
uint8_t add_info
Additional mode info.
Definition: gps_sirf.c:109
sirf_msg_41::ete
uint32_t ete
estimated time error, in seconds * 10^2
Definition: gps_sirf.c:98
sirf_msg_41::nav_valid
uint16_t nav_valid
if equal to 0x0000, then navigation solution is valid
Definition: gps_sirf.c:75
sirf_msg_2::ch12prn
uint8_t ch12prn
Definition: gps_sirf.c:68
GPS_VALID_POS_ECEF_BIT
#define GPS_VALID_POS_ECEF_BIT
Definition: gps.h:48
GpsState::cacc
uint32_t cacc
course accuracy in rad*1e7
Definition: gps.h:104
sirf_msg_41::sat_id
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
sirf_msg_41::minute
uint8_t minute
Definition: gps_sirf.c:83
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:95
sirf_msg_41::hdop
uint8_t hdop
Horizontal dilution of precision x 5 (0.2 precision)
Definition: gps_sirf.c:108
gps_sirf_init
void gps_sirf_init(void)
Definition: gps_sirf.c:122
sirf_msg_41::extended_week_number
uint16_t extended_week_number
Definition: gps_sirf.c:77
sirf_msg_2::hdop
uint8_t hdop
horizontal dilution of precision *5 (0.2 precision)
Definition: gps_sirf.c:52
inttypes.h
GPS_FIX_NONE
#define GPS_FIX_NONE
No GPS fix.
Definition: gps.h:37
sirf_msg_2::y_pos
int32_t y_pos
y-position in m
Definition: gps_sirf.c:46
GPS_FIX_3D
#define GPS_FIX_3D
3D GPS fix
Definition: gps.h:39
GPS_VALID_HMSL_BIT
#define GPS_VALID_HMSL_BIT
Definition: gps.h:53
p
static float p[2][2]
Definition: ins_alt_float.c:268
sirf_msg_2::num_sat
uint8_t num_sat
Number of satellites in fix.
Definition: gps_sirf.c:56
sirf_msg_2::ch5prn
uint8_t ch5prn
Definition: gps_sirf.c:61
sirf_msg_41::heading_err
uint16_t heading_err
in degrees * 10^2
Definition: gps_sirf.c:106
GpsSirf
Definition: gps_sirf.h:48