Paparazzi UAS v7.1_unstable
Paparazzi is a free software Unmanned Aircraft System.
Loading...
Searching...
No Matches
gps_nmea_send.c
Go to the documentation of this file.
1/*
2 * Copyright (C) 2025 Jean-Baptiste FORESTIER <jean-baptiste.forestier@enac.fr>
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, see
18 * <http://www.gnu.org/licenses/>.
19 */
26
27
28
30#include "mcu_periph/uart.h"
31#include "generated/airframe.h"
32#include "state.h" // State interface for rotation compensation
33#include "modules/gps/gps.h"
34
35#include <string.h>
36#include <stdio.h>
37#include <stdint.h>
38#include <stdlib.h>
39#include <math.h>
40#include <inttypes.h>
41
42// ** Declaration ** //
44
45static void recover_gps_data(void);
46static void build_nmea_sentence(void);
47static uint8_t nmea_checksum(const char *sentence, int length);
48static void nmea_convert_deg_to_DDMM(double deg, char *buf, int is_lat);
49static void gps_nmea_get_system_date_str(char *buf_date, size_t buf_date_size, char *buf_time, size_t buf_time_size);
50static void nmea_send(const char *payload, int payload_length);
51
52
57{
58 gps_nmea_send.error_init = false;
59
60 gps_nmea_send.msg.lat = 0;
61 gps_nmea_send.msg.lon = 0;
62 gps_nmea_send.msg.num_sv = 0;
63 gps_nmea_send.msg.pdop = 0;
64 gps_nmea_send.msg.hmsl = 0;
65 gps_nmea_send.msg.vground = 0;
66 gps_nmea_send.msg.course = 0;
67}
68
77
78
83{
84#if GPS_NMEA_SEND_USE_STATE_DATA
85 struct LlaCoor_f *lla = stateGetPositionLla_f();
86 gps_nmea_send.msg.lat = (double)gps->lat;
87 gps_nmea_send.msg.lon = (double)gps->lon;
91#else // Default, use unfiltered GPS data
92 gps_nmea_send.msg.lat = (double)gps.lla_pos.lat / 1e7;
93 gps_nmea_send.msg.lon = (double)gps.lla_pos.lon / 1e7;
94 gps_nmea_send.msg.hmsl = (float)gps.hmsl / 1000.f; //in meters
95 gps_nmea_send.msg.vground = (float)gps.gspeed / 100.f; // in meters
96 gps_nmea_send.msg.course = (float)DegOfRad(gps.course / 1e7);
97#endif
98 gps_nmea_send.msg.num_sv = gps.num_sv;
99 gps_nmea_send.msg.pdop = gps.pdop;
100}
101
106{
107 // Latitude and Longitude
108 char lat_buf[16], lon_buf[16], gga[120], rmc[120], date_sys[6], time_sys[6];
109 int len_gga = 0;
110 int len_rmc = 0;
111 uint8_t cs;
112 nmea_convert_deg_to_DDMM(gps_nmea_send.msg.lat, lat_buf, 1);
113 nmea_convert_deg_to_DDMM(gps_nmea_send.msg.lon, lon_buf, 0);
114 char lat_hemi = (gps_nmea_send.msg.lat >= 0) ? 'N' : 'S';
115 char lon_hemi = (gps_nmea_send.msg.lon >= 0) ? 'E' : 'W';
116
117 for (int i = 0; i < 120; i++) { gga[i] = '\0'; rmc[i] = '\0'; }
118
119 if (gps.fix >= GPS_FIX_3D) {
120 gps_nmea_get_system_date_str(date_sys, sizeof(date_sys) + 1, time_sys, sizeof(time_sys) + 1);
121
122 // -------------------------------
123 // GPGGA Frame
124 // -------------------------------
125 sprintf(gga,
126 "$GPGGA,%s,%s,%c,%s,%c,1,%02u,%04u,%09.3f,M,0,M,,",
127 time_sys, lat_buf, lat_hemi, lon_buf, lon_hemi,
128 gps_nmea_send.msg.num_sv, gps_nmea_send.msg.pdop,
129 gps_nmea_send.msg.hmsl);
130 len_gga = 7; // $GPGGA, = 7
131 len_gga += 7; // time_nmea, = 7
132 len_gga += 10; // lat_buf = 9 + comma
133 len_gga += 2; // lat_hemi = N,
134 len_gga += 11; // lon_buf = 10 + comma
135 len_gga += 2; // lon_hemi = W,
136 len_gga += 2; // GPS Quality indicator GPS fix = 1,
137 len_gga += 3; // number of sats = 08,
138 len_gga += 5; // position dilution of precision scaled by 100 = 1020,
139 len_gga += 10; // height above mean sea level (MSL) in m = 02945.127,
140 len_gga += 2; // height above mean sea level unit = M,
141 len_gga += 2; // Ellipsoid to geoid distance set 0, don't where to find it = 0,
142 len_gga += 2; // Distance above unit = M,
143 len_gga += 1; // Empty = ,
144
145 cs = nmea_checksum(gga, len_gga);
146 sprintf(gga + len_gga, "*%02X\r\n", cs);
147 len_gga += 5; // * + Checksum + \r\n = *4F\r\n
148
149 nmea_send(gga, len_gga);
150
151 // -------------------------------
152 // GPRMC Frame
153 // -------------------------------
154 sprintf(rmc,
155 "$GPRMC,%s,A,%s,%c,%s,%c,%05.1f,%05.1f,%s,000.0,W",
156 time_sys, lat_buf, lat_hemi, lon_buf, lon_hemi,
157 gps_nmea_send.msg.vground, gps_nmea_send.msg.course, date_sys);
158 len_rmc = 7; // $GPRMC, = 7
159 len_rmc += 7; // time_nmea, = 7
160 len_rmc += 2; // Status A=active or V=void = A,
161 len_rmc += 10; // lat_buf = 9 + comma
162 len_rmc += 2; // lat_hemi = N,
163 len_rmc += 11; // lon_buf = 10 + comma
164 len_rmc += 2; // lon_hemi = W,
165 len_rmc += 6; // vground = 050.4,
166 len_rmc += 6; // track angle course = 050.4,
167 len_rmc += 7; // Date = 230394,
168 len_rmc += 7; // Magnetic variation, in degrees = 003.1,W,
169
170 cs = nmea_checksum(rmc, len_rmc);
171 sprintf(rmc + len_rmc, "*%02X\r\n", cs);
172 len_rmc += 5; // * + Checksum + \r\n = *4F\r\n
173
174 nmea_send(rmc, len_rmc);
175
176 } else {
177 //If no fix, empty GGA
178 sprintf(gga, "$GPGGA,,,,,,0,00,99.99,,,,,,*68\r\n");
179 nmea_send(gga, 33);
180
181 //If no fix, empty RMC
182 sprintf(rmc, "$GPRMC,,V,,,,,,,,,,*53\r\n");
183 nmea_send(rmc, 24);
184 }
185}
186
190uint8_t nmea_checksum(const char *sentence, int length)
191{
192 uint8_t cs = 0;
193 for (int i = 0; i < length; i++) {
194 cs ^= (uint8_t)sentence[i];
195 }
196 return cs;
197}
198
203void nmea_convert_deg_to_DDMM(double deg, char *buf, int is_lat)
204{
205 double abs_deg = fabs(deg);
206 int d = (int)abs_deg;
207 double minutes = (abs_deg - d) * 60.0;
208
209 if (is_lat) {
210 // 2 integers for degrees 0 to 90 without sign => %02d | 2 integers for minutes, 1 for point, 4 for minutes decimals
211 sprintf(buf, "%02d%07.4f", d, minutes);
212 } else {
213 // 3 integers for degrees 0 to 180 without sign => %03d | 2 integers for minutes, 1 for point, 4 for minutes decimals
214 sprintf(buf, "%03d%07.4f", d, minutes);
215 }
216}
217
221void gps_nmea_get_system_date_str(char *buf_date, size_t buf_date_size, char *buf_time, size_t buf_time_size)
222{
223 // days from 1970-01-01
224 const int64_t gps_epoch_days = 3657;
225 // Total time gps
226 uint64_t gps_sec = (uint64_t)gps.week * 7 * 24 * 3600 + gps.tow / 1000;
227 // Day gps + gps epoch
228 int64_t days = gps_epoch_days + gps_sec / 86400;
229
230 // Algo to get year month day from Week and tow
231 // Shift day count so that day 0 corresponds to the Gregorian epoch (0000-03-01)
232 days += 719468;
233 // Determine the 400-year Gregorian cycle, One Gregorian era = 400 years = 146097 days.
234 int64_t era = (days >= 0 ? days : days - 146096) / 146097;
235 // Day Of Era: number of days elapsed within the current 400-year cycle.
236 uint64_t doe = (uint64_t)(days - era * 146097);
237 // Year Of Era: compute the year number inside the cycle, correcting for leap years (every 4 years, except centuries, except 400-year multiples).
238 uint64_t yoe = (doe - doe / 1460 + doe / 36524 - doe / 146096) / 365;
239 // Convert "year in era" to an absolute Gregorian year.
240 int64_t y = (int64_t)yoe + era * 400;
241 // Day Of Year: subtract all full days from completed years to get day index within the year.
242 uint64_t doy = doe - (365 * yoe + yoe / 4 - yoe / 100);
243 // Month part: day-of-year into a month index in a calendar where March = 0 and February = 11
244 uint64_t mp = (5 * doy + 2) / 153;
245 // Compute the day-of-month
246 uint64_t d = doy - (153 * mp + 2) / 5 + 1;
247 // Convert shifted month index back to standard month 0-12
248 uint64_t m = mp + (mp < 10 ? 3 : -9);
249 y += (m <= 2);
250
251 // Hours / minutes / secondes
252 uint32_t sec_of_day = gps_sec % 86400;
253 uint8_t hour = sec_of_day / 3600;
254 uint8_t min = (sec_of_day % 3600) / 60;
255 uint8_t sec = sec_of_day % 60;
256
257 // Format : DDMMYY
258 snprintf(buf_date, buf_date_size, "%02d%02d%02d",
259 (uint8_t)d,
260 (uint8_t)m,
261 (uint8_t)(y % 100));
262
263 // Format : HHMMSS
264 snprintf(buf_time, buf_time_size, "%02d%02d%02d",
265 hour,
266 min,
267 sec);
268}
269
273void nmea_send(const char *payload, int payload_length)
274{
275 uart_put_buffer(&NMEA_SEND_UART, 0, (const uint8_t *)payload, payload_length);
276}
277
struct GpsState gps
global GPS state
Definition gps.c:74
Device independent GPS code (interface).
#define GPS_FIX_3D
3D GPS fix
Definition gps.h:44
static uint8_t nmea_checksum(const char *sentence, int length)
Function to calculate checksum.
static void recover_gps_data(void)
Function to recover data available onboard from GPS.
static void gps_nmea_get_system_date_str(char *buf_date, size_t buf_date_size, char *buf_time, size_t buf_time_size)
Get Date from system.
struct Gps_Nmea_Send gps_nmea_send
static void nmea_send(const char *payload, int payload_length)
Function to send payload to UART.
void gps_nmea_send_init(void)
Initialization function.
static void nmea_convert_deg_to_DDMM(double deg, char *buf, int is_lat)
Function to convert lat long in DDLL.MMMM Format Lat and long don't have the same length.
static void build_nmea_sentence(void)
Function to build sentence for NMEA Protocol.
void gps_nmea_send_periodic(void)
Periodic function to send data.
module used to send GPS data over a Tawaki UART for extern instrument using NMEA protocol Exemple of ...
static struct LlaCoor_f * stateGetPositionLla_f(void)
Get position in LLA coordinates (float).
Definition state.h:868
float stateGetHmslOrigin_f(void)
Get the HMSL of the frame origin (float).
Definition state.c:210
static struct EnuCoor_f * stateGetPositionEnu_f(void)
Get position in local ENU coordinates (float).
Definition state.h:859
static float stateGetHorizontalSpeedNorm_f(void)
Get norm of horizontal ground speed (float).
Definition state.h:1087
static float stateGetHorizontalSpeedDir_f(void)
Get dir of horizontal ground speed (float).
Definition state.h:1096
float z
in meters
vector in Latitude, Longitude and Altitude
#define NormCourseRad(x)
Normalize a rad angle between 0 and 2*PI.
Definition navigation.h:159
API to get/set the generic vehicle states.
void WEAK uart_put_buffer(struct uart_periph *p, long fd, const uint8_t *data, uint16_t len)
Definition uart.c:161
arch independent UART (Universal Asynchronous Receiver/Transmitter) API
unsigned int uint32_t
Typedef defining 32 bit unsigned int type.
unsigned long long uint64_t
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.