Line data Source code
1 : /*
2 : This program is free software: you can redistribute it and/or modify
3 : it under the terms of the GNU General Public License as published by
4 : the Free Software Foundation, either version 3 of the License, or
5 : (at your option) any later version.
6 :
7 : This program is distributed in the hope that it will be useful,
8 : but WITHOUT ANY WARRANTY; without even the implied warranty of
9 : MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
10 : GNU General Public License for more details.
11 :
12 : You should have received a copy of the GNU General Public License
13 : along with this program. If not, see <http://www.gnu.org/licenses/>.
14 :
15 :
16 : Author: Francisco Ferreira (some code is copied from sitl_gps.cpp)
17 :
18 : */
19 :
20 : #define AP_MATH_ALLOW_DOUBLE_FUNCTIONS 1
21 :
22 : #include "AP_NMEA_Output.h"
23 :
24 : #if HAL_NMEA_OUTPUT_ENABLED
25 :
26 : #include <AP_Math/definitions.h>
27 : #include <AP_RTC/AP_RTC.h>
28 : #include <AP_GPS/AP_GPS.h>
29 : #include <AP_SerialManager/AP_SerialManager.h>
30 : #include <AP_Common/NMEA.h>
31 : #include <stdio.h>
32 : #include <time.h>
33 : #include <AP_AHRS/AP_AHRS_config.h>
34 :
35 : #if AP_AHRS_ENABLED
36 : #include <AP_AHRS/AP_AHRS.h>
37 : #endif
38 :
39 : #ifndef AP_NMEA_OUTPUT_MESSAGE_ENABLED_DEFAULT
40 : #define AP_NMEA_OUTPUT_MESSAGE_ENABLED_DEFAULT 3 // GPGGA and GPRMC
41 : #endif
42 :
43 : #define AP_NMEA_OUTPUT_INTERVAL_MS_MIN 10
44 : #define AP_NMEA_OUTPUT_INTERVAL_MS_MAX 5000
45 :
46 : extern const AP_HAL::HAL& hal;
47 :
48 : const AP_Param::GroupInfo AP_NMEA_Output::var_info[] = {
49 :
50 : // @Param: RATE_MS
51 : // @DisplayName: NMEA Output rate
52 : // @Description: NMEA Output rate. This controls the interval at which all the enabled NMEA messages are sent. Most NMEA systems expect 100ms (10Hz) or slower.
53 : // @Range: 20 2000
54 : // @Increment: 1
55 : // @Units: ms
56 : // @User: Standard
57 : AP_GROUPINFO("RATE_MS", 1, AP_NMEA_Output, _interval_ms, 100),
58 :
59 : // @Param: MSG_EN
60 : // @DisplayName: Messages Enable bitmask
61 : // @Description: This is a bitmask of enabled NMEA messages. All messages will be sent consecutively at the same rate interval
62 : // @Bitmask: 0:GPGGA,1:GPRMC,2:PASHR
63 : // @User: Standard
64 : AP_GROUPINFO("MSG_EN", 2, AP_NMEA_Output, _message_enable_bitmask, AP_NMEA_OUTPUT_MESSAGE_ENABLED_DEFAULT),
65 :
66 : AP_GROUPEND
67 : };
68 :
69 2086 : void AP_NMEA_Output::init()
70 : {
71 2086 : const AP_SerialManager& sm = AP::serialmanager();
72 :
73 2086 : _num_outputs = 0;
74 2087 : for (uint8_t i = 0; i < ARRAY_SIZE(_uart); i++) {
75 2087 : _uart[i] = sm.find_serial(AP_SerialManager::SerialProtocol_NMEAOutput, i);
76 :
77 2087 : if (_uart[i] == nullptr) {
78 2086 : break;
79 : }
80 1 : _num_outputs++;
81 : }
82 2086 : }
83 :
84 7525129 : void AP_NMEA_Output::update()
85 : {
86 7525129 : if (_num_outputs == 0 || _message_enable_bitmask == 0) {
87 7525053 : return;
88 : }
89 :
90 792 : const uint32_t now_ms = AP_HAL::millis();
91 :
92 792 : if ((now_ms - _last_run_ms) < static_cast<uint32_t>(MAX(_interval_ms.get(), 20))) {
93 634 : return;
94 : }
95 158 : _last_run_ms = now_ms;
96 :
97 : // get time and date
98 : uint64_t time_usec;
99 : #if AP_RTC_ENABLED
100 158 : if (!AP::rtc().get_utc_usec(time_usec)) {
101 82 : return;
102 : }
103 : #else
104 : time_usec = 0;
105 : #endif
106 :
107 76 : uint32_t space_required = 0;
108 :
109 : // not completely accurate, our time includes leap seconds and time_t should be without
110 76 : const time_t time_sec = time_usec / 1000000;
111 76 : struct tm tmd {};
112 76 : struct tm* tm = gmtime_r(&time_sec, &tmd);
113 :
114 : // format time string
115 : char tstring[10];
116 76 : hal.util->snprintf(tstring, sizeof(tstring), "%02u%02u%05.2f", tm->tm_hour, tm->tm_min, tm->tm_sec + (time_usec % 1000000) * 1.0e-6);
117 :
118 76 : Location loc;
119 76 : const auto &gps = AP::gps();
120 76 : const AP_GPS_FixType gps_status = gps.status();
121 :
122 : #if AP_AHRS_ENABLED
123 76 : auto &ahrs = AP::ahrs();
124 : // NOTE: ahrs.get_location() always returns true after having GPS position once because it will be dead-reckoning
125 76 : const bool pos_valid = ahrs.get_location(loc);
126 : #else
127 : const bool pos_valid = (gps_status >= AP_GPS_FixType::FIX_3D);
128 : loc = gps.location();
129 : #endif
130 :
131 : // format latitude
132 : char lat_string[13];
133 76 : double deg = fabs(loc.lat * 1.0e-7f);
134 76 : double min_dec = ((fabs(loc.lat) - (unsigned)deg * 1.0e7)) * 60 * 1.e-7f;
135 76 : hal.util->snprintf(lat_string,
136 : sizeof(lat_string),
137 : "%02u%08.5f,%c",
138 : (unsigned) deg,
139 : min_dec,
140 76 : loc.lat < 0 ? 'S' : 'N');
141 :
142 : // format longitude
143 : char lng_string[14];
144 76 : deg = fabs(loc.lng * 1.0e-7f);
145 76 : min_dec = ((fabs(loc.lng) - (unsigned)deg * 1.0e7)) * 60 * 1.e-7f;
146 76 : hal.util->snprintf(lng_string,
147 : sizeof(lng_string),
148 : "%03u%08.5f,%c",
149 : (unsigned) deg,
150 : min_dec,
151 76 : loc.lng < 0 ? 'W' : 'E');
152 :
153 :
154 : char gga[100];
155 76 : uint16_t gga_length = 0;
156 76 : if ((_message_enable_bitmask.get() & static_cast<int16_t>(Enabled_Messages::GPGGA)) != 0) {
157 : // format GGA message
158 :
159 : // Convert AP_GPS_FixType:
160 : // 0 = NO_GPS
161 : // 1 = NO_FIX
162 : // 2 = GPS_OK_FIX_2D
163 : // 3 = GPS_OK_FIX_3D
164 : // 4 = GPS_OK_FIX_3D_DGPS
165 : // 5 = GPS_OK_FIX_3D_RTK_FLOAT
166 : // 6 = GPS_OK_FIX_3D_RTK_FIXED
167 :
168 : // To NMEA "Fix Quality" per Trimble definition:
169 : // 0: Fix not valid
170 : // 1: GPS fix
171 : // 2: Differential GPS fix (DGNSS), SBAS, OmniSTAR VBS, Beacon, RTX in GVBS mode
172 : // 3: Not applicable
173 : // 4: RTK Fixed, xFill
174 : // 5: RTK Float, OmniSTAR XP/HP, Location RTK, RTX
175 : // 6: INS Dead reckoning
176 : // sources:
177 : // https://orolia.com/manuals/VSP/Content/NC_and_SS/Com/Topics/APPENDIX/NMEA_GGAmess.htm
178 : // http://aprs.gids.nl/nmea/#gga
179 : // https://docs.novatel.com/OEM7/Content/Logs/GPGGA.htm
180 : // https://receiverhelp.trimble.com/alloy-gnss/en-us/NMEA-0183messages_GGA.html <-using this one
181 :
182 : uint8_t fix_quality;
183 76 : switch (gps_status) {
184 0 : default:
185 : case AP_GPS_FixType::NO_GPS:
186 : case AP_GPS_FixType::NONE:
187 : case AP_GPS_FixType::FIX_2D:
188 : // NOTE: ahrs.get_location() always returns pos_valid=true after having GPS position once because it will be dead-reckoning
189 0 : fix_quality = pos_valid ? 6 : 0;
190 0 : break;
191 0 : case AP_GPS_FixType::FIX_3D:
192 0 : fix_quality = 1;
193 0 : break;
194 0 : case AP_GPS_FixType::DGPS:
195 0 : fix_quality = 2;
196 0 : break;
197 0 : case AP_GPS_FixType::RTK_FLOAT:
198 0 : fix_quality = 5;
199 0 : break;
200 76 : case AP_GPS_FixType::RTK_FIXED:
201 76 : fix_quality = 4;
202 76 : break;
203 : }
204 :
205 76 : gga_length = nmea_printf_buffer(gga, sizeof(gga),
206 : "$GPGGA,%s,%s,%s,%01d,%02d,%04.1f,%07.2f,M,0.0,M,,",
207 : tstring,
208 : lat_string,
209 : lng_string,
210 : fix_quality,
211 76 : gps.num_sats(),
212 76 : gps.get_hdop()*0.01,
213 76 : loc.alt * 0.01f);
214 :
215 76 : space_required += gga_length;
216 : }
217 :
218 : char rmc[100];
219 76 : uint16_t rmc_length = 0;
220 76 : if ((_message_enable_bitmask.get() & static_cast<int16_t>(Enabled_Messages::GPRMC)) != 0) {
221 : // format date string
222 : char dstring[7];
223 76 : hal.util->snprintf(dstring, sizeof(dstring), "%02u%02u%02u", tm->tm_mday, tm->tm_mon+1, tm->tm_year % 100);
224 :
225 : // get speed
226 : #if AP_AHRS_ENABLED
227 76 : const Vector2f speed = ahrs.groundspeed_vector();
228 76 : const float speed_knots = speed.length() * M_PER_SEC_TO_KNOTS;
229 76 : const float heading = wrap_360(degrees(atan2f(speed.x, speed.y)));
230 : #else
231 : const float speed_knots = gps.ground_speed() * M_PER_SEC_TO_KNOTS;
232 : const float heading = gps.ground_course();
233 : #endif
234 :
235 : // format RMC message
236 76 : rmc_length = nmea_printf_buffer(rmc, sizeof(rmc),
237 : "$GPRMC,%s,%c,%s,%s,%.2f,%.2f,%s,,",
238 : tstring,
239 : pos_valid ? 'A' : 'V',
240 : lat_string,
241 : lng_string,
242 : speed_knots,
243 : heading,
244 : dstring);
245 :
246 76 : space_required += rmc_length;
247 : }
248 :
249 76 : uint16_t pashr_length = 0;
250 : char pashr[100];
251 : #if AP_AHRS_ENABLED
252 76 : if ((_message_enable_bitmask.get() & static_cast<int16_t>(Enabled_Messages::PASHR)) != 0) {
253 : // get roll, pitch, yaw
254 0 : const float roll_deg = wrap_180(ahrs.get_roll_deg());
255 0 : const float pitch_deg = wrap_180(ahrs.get_pitch_deg());
256 0 : const float yaw_deg = ahrs.get_yaw_deg();
257 0 : const float heave_m = 0; // instantaneous heave in meters
258 0 : const float roll_deg_accuracy = 0; // stddev of roll_deg;
259 0 : const float pitch_deg_accuracy = 0; // stddev of pitch_deg;
260 0 : const float heading_deg_accuracy = 0; // stddev of yaw_deg;
261 :
262 : // GPS Update Quality Flag:
263 : // 0 = no position
264 : // 1 = All non-RTK fixed integer positions
265 : // 2 = RTK fixed integer positions
266 0 : const uint8_t gps_status_flag = (gps_status >= AP_GPS_FixType::RTK_FIXED) ? 2 :
267 : (gps_status >= AP_GPS_FixType::FIX_2D ? 1 : 0);
268 :
269 : // INS Status Flag:
270 : // 0 = All SPAN Pre-Alignment INS Status
271 : // 1 = All SPAN Post-Alignment INS Status
272 0 : const bool ins_status_flag = ahrs.initialised() &&
273 0 : ahrs.healthy() &&
274 0 : (!ahrs.have_inertial_nav() || AP::ins().accel_calibrated_ok_all());
275 :
276 : // format PASHR message
277 0 : pashr_length = nmea_printf_buffer(pashr, sizeof(pashr),
278 : "$PASHR,%s,%.2f,T,%c%.2f,%c%.2f,%c%.2f,%.3f,%.3f,%.3f,%u,%u",
279 : tstring,
280 : yaw_deg, // This is a TRUE NORTH value
281 0 : roll_deg<0? '-':'+', fabs(roll_deg), // always show + or - symbol
282 0 : pitch_deg<0?'-':'+', fabs(pitch_deg), // always show + or - symbol
283 0 : heave_m<0? '-':'+', fabs(heave_m), // always show + or - symbol
284 : roll_deg_accuracy,
285 : pitch_deg_accuracy,
286 : heading_deg_accuracy,
287 : (unsigned)gps_status_flag,
288 : (unsigned)ins_status_flag);
289 :
290 0 : space_required += pashr_length;
291 : }
292 : #endif
293 :
294 : // send to all NMEA output ports
295 152 : for (uint8_t i = 0; i < _num_outputs; i++) {
296 76 : if (_uart[i]->txspace() < space_required) {
297 0 : continue;
298 : }
299 :
300 76 : if (gga_length > 0) {
301 76 : _uart[i]->write(gga);
302 : }
303 :
304 76 : if (rmc_length > 0) {
305 76 : _uart[i]->write(rmc);
306 : }
307 :
308 76 : if (pashr_length > 0) {
309 0 : _uart[i]->write(pashr);
310 : }
311 : }
312 : }
313 :
314 :
315 : #endif // HAL_NMEA_OUTPUT_ENABLED
|
