Paparazzi UAS  v4.0.4_stable-3-gf39211a
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Files Functions Variables Typedefs Enumerations Enumerator Macros 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 //#include "my_debug_servo.h"
50 
51 void gps_impl_init(void) {
52 
54 
55 
56  //DEBUG_SERVO1_INIT();
57 
58 }
59 
60 
62 
63  //DEBUG_S1_ON();
64 
76  // pacc;
77  // sacc;
78  // gps.pdop = SKYTRAQ_NAVIGATION_DATA_PDOP(gps_skytraq.msg_buf);
81 
84  case SKYTRAQ_FIX_3D:
85  gps.fix = GPS_FIX_3D;
86  break;
87  case SKYTRAQ_FIX_2D:
88  gps.fix = GPS_FIX_2D;
89  break;
90  default:
92  }
93 
94 #if GPS_USE_LATLONG
95  /* Computes from (lat, long) in the referenced UTM zone */
96  struct LlaCoor_f lla_f;
97  lla_f.lat = ((float) gps.lla_pos.lat) / 1e7;
98  lla_f.lon = ((float) gps.lla_pos.lon) / 1e7;
99  struct UtmCoor_f utm_f;
100  utm_f.zone = nav_utm_zone0;
101  /* convert to utm */
102  utm_of_lla_f(&utm_f, &lla_f);
103  /* copy results of utm conversion */
104  gps.utm_pos.east = utm_f.east*100;
105  gps.utm_pos.north = utm_f.north*100;
108 #endif
109 
110  //DEBUG_S2_TOGGLE();
111 
112 #ifdef GPS_LED
113  if (gps.fix == GPS_FIX_3D) {
114  LED_ON(GPS_LED);
115  }
116  else {
117  LED_TOGGLE(GPS_LED);
118  }
119 #endif
120  }
121 
122  //DEBUG_S1_OFF();
123 }
124 
128  switch (gps_skytraq.status) {
129  case UNINIT:
130  if (c == SKYTRAQ_SYNC1)
132  break;
133  case GOT_SYNC1:
134  if (c != SKYTRAQ_SYNC2) {
136  goto error;
137  }
139  break;
140  case GOT_SYNC2:
141  gps_skytraq.len = c<<8;
143  break;
144  case GOT_LEN1:
145  gps_skytraq.len += c;
149  goto error;
150  }
151  break;
152  case GOT_LEN2:
153  gps_skytraq.msg_id = c;
154  gps_skytraq.msg_idx = 0;
157  break;
158  case GOT_ID:
159  gps_skytraq.msg_buf[gps_skytraq.msg_idx] = c;
161  if (gps_skytraq.msg_idx >= gps_skytraq.len-1) {
163  }
164  break;
165  case GOT_PAYLOAD:
166  if (c != gps_skytraq.checksum) {
168  goto error;
169  }
171  break;
172  case GOT_CHECKSUM:
173  if (c != SKYTRAQ_SYNC3) {
175  goto error;
176  }
178  break;
179  case GOT_SYNC3:
181  goto restart;
182  default:
184  goto error;
185  }
186  return;
187  error:
189  restart:
191  return;
192 }
int32_t y
in centimeters
struct LlaCoor_i lla_pos
position in LLA (lat,lon: rad*1e7; alt: mm over ellipsoid)
Definition: gps.h:64
#define GOT_CHECKSUM
Definition: gps_skytraq.c:41
#define GOT_SYNC3
Definition: gps_skytraq.c:42
int32_t lat
in radians*1e7
struct EcefCoor_i ecef_vel
speed ECEF in cm/s
Definition: gps.h:67
#define SKYTRAQ_FIX_3D
Definition: gps_skytraq.c:46
void gps_impl_init(void)
Definition: gps_skytraq.c:51
uint8_t error_last
Definition: gps_skytraq.h:76
uint8_t zone
UTM zone number.
uint8_t msg_id
Definition: gps_skytraq.h:69
#define SKYTRAQ_SYNC3
Definition: gps_skytraq.h:30
#define SKYTRAQ_NAVIGATION_DATA_FixMode(_payload)
Definition: gps_skytraq.h:36
#define SKYTRAQ_NAVIGATION_DATA_ECEFZ(_payload)
Definition: gps_skytraq.h:50
uint8_t nav_utm_zone0
Definition: common_nav.c:43
#define LED_ON(i)
Definition: led_hw.h:28
int32_t hmsl
height above mean sea level in mm
Definition: gps.h:66
float lat
in radians
#define GOT_ID
Definition: gps_skytraq.c:39
#define SKYTRAQ_NAVIGATION_DATA_ASL(_payload)
Definition: gps_skytraq.h:44
uint8_t fix
status of fix
Definition: gps.h:77
#define GPS_FIX_3D
Definition: gps.h:42
int32_t z
in centimeters
#define SKYTRAQ_NAVIGATION_DATA_ECEFX(_payload)
Definition: gps_skytraq.h:48
struct EcefCoor_i ecef_pos
position in ECEF in cm
Definition: gps.h:63
vector in Latitude, Longitude and Altitude
#define GPS_SKYTRAQ_ERR_UNEXPECTED
Definition: gps_skytraq.h:63
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:79
uint8_t status
Definition: gps_skytraq.h:71
#define SKYTRAQ_NAVIGATION_DATA_NumSV(_payload)
Definition: gps_skytraq.h:37
#define GPS_SKYTRAQ_ERR_MSG_TOO_LONG
Definition: gps_skytraq.h:60
uint8_t zone
UTM zone number.
uint8_t msg_idx
Definition: gps_skytraq.h:73
#define SKYTRAQ_NAVIGATION_DATA_AEL(_payload)
Definition: gps_skytraq.h:43
uint8_t error_cnt
Definition: gps_skytraq.h:75
#define SKYTRAQ_SYNC1
Definition: gps_skytraq.h:27
#define GOT_LEN1
Definition: gps_skytraq.c:37
float north
in meters
#define GPS_FIX_NONE
Definition: gps.h:40
#define GPS_FIX_2D
Definition: gps.h:41
#define GOT_PAYLOAD
Definition: gps_skytraq.c:40
#define SKYTRAQ_ID_NAVIGATION_DATA
Definition: gps_skytraq.h:34
Device independent GPS code (interface)
#define SKYTRAQ_NAVIGATION_DATA_ECEFVX(_payload)
Definition: gps_skytraq.h:51
void gps_skytraq_read_message(void)
Definition: gps_skytraq.c:61
#define SKYTRAQ_FIX_3D_DGPS
Definition: gps_skytraq.c:47
position in UTM coordinates Units: meters
#define SKYTRAQ_SYNC2
Definition: gps_skytraq.h:28
int32_t alt
in millimeters above WGS84 reference ellipsoid
#define SKYTRAQ_NAVIGATION_DATA_ECEFY(_payload)
Definition: gps_skytraq.h:49
#define SKYTRAQ_NAVIGATION_DATA_ECEFVZ(_payload)
Definition: gps_skytraq.h:53
#define SKYTRAQ_FIX_2D
Definition: gps_skytraq.c:45
#define SKYTRAQ_NAVIGATION_DATA_TOW(_payload)
Definition: gps_skytraq.h:40
#define GPS_SKYTRAQ_ERR_CHECKSUM
Definition: gps_skytraq.h:61
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:65
bool_t msg_available
Definition: gps_skytraq.h:68
#define SKYTRAQ_NAVIGATION_DATA_LAT(_payload)
Definition: gps_skytraq.h:41
uint8_t checksum
Definition: gps_skytraq.h:74
#define TRUE
Definition: imu_chimu.h:144
#define GPS_SKYTRAQ_ERR_OUT_OF_SYNC
Definition: gps_skytraq.h:62
int32_t east
in centimeters
uint16_t len
Definition: gps_skytraq.h:72
unsigned char uint8_t
Definition: types.h:14
int32_t lon
in radians*1e7
#define GOT_SYNC1
Definition: gps_skytraq.c:35
void gps_skytraq_parse(uint8_t c)
Definition: gps_skytraq.c:125
#define UNINIT
Definition: gps_skytraq.c:34
arch independent LED (Light Emitting Diodes) API
struct UtmCoor_i utm_pos
position in UTM (north,east: cm; alt: mm over ellipsoid)
Definition: gps.h:65
#define SKYTRAQ_NAVIGATION_DATA_LON(_payload)
Definition: gps_skytraq.h:42
struct GpsSkytraq gps_skytraq
Definition: gps_skytraq.c:31
#define GOT_LEN2
Definition: gps_skytraq.c:38
float lon
in radians
static struct point c
Definition: discsurvey.c:13
float east
in meters
struct GpsState gps
global GPS state
Definition: gps.c:31
#define SKYTRAQ_NAVIGATION_DATA_ECEFVY(_payload)
Definition: gps_skytraq.h:52
void utm_of_lla_f(struct UtmCoor_f *utm, struct LlaCoor_f *lla)
uint8_t num_sv
number of sat in fix
Definition: gps.h:76