Line data Source code
1 : /*
2 : SITL handling
3 :
4 : This simulates a GPS on a serial port
5 :
6 : Andrew Tridgell November 2011
7 : */
8 :
9 : #include "SIM_GPS.h"
10 :
11 : #if AP_SIM_GPS_ENABLED
12 :
13 : #include <time.h>
14 : #include <sys/time.h>
15 :
16 : #include <AP_BoardConfig/AP_BoardConfig.h>
17 : #include <AP_HAL/AP_HAL.h>
18 : #include <SITL/SITL.h>
19 : #include <AP_InternalError/AP_InternalError.h>
20 :
21 : #include "SIM_GPS_FILE.h"
22 : #include "SIM_GPS_Trimble.h"
23 : #include "SIM_GPS_MSP.h"
24 : #include "SIM_GPS_NMEA.h"
25 : #include "SIM_GPS_NOVA.h"
26 : #include "SIM_GPS_SBP2.h"
27 : #include "SIM_GPS_SBP.h"
28 : #include "SIM_GPS_UBLOX.h"
29 : #include "SIM_GPS_SBF.h"
30 :
31 : #include <GCS_MAVLink/GCS.h>
32 :
33 : namespace SITL {
34 : // user settable parameters for GNSS sensors
35 : const AP_Param::GroupInfo SIM::GPSParms::var_info[] = {
36 :
37 : // @Param: ENABLE
38 : // @DisplayName: GPS enable
39 : // @Description: Enable simulated GPS
40 : // @Values: 0:Disable, 1:Enable
41 : // @User: Advanced
42 : AP_GROUPINFO_FLAGS("ENABLE", 1, GPSParms, enabled, 0, AP_PARAM_FLAG_ENABLE),
43 :
44 : // @Param: LAG_MS
45 : // @DisplayName: GPS Lag
46 : // @Description: GPS lag
47 : // @Units: ms
48 : // @User: Advanced
49 : AP_GROUPINFO("LAG_MS", 2, GPSParms, delay_ms, 100),
50 :
51 : // @Param: TYPE
52 : // @DisplayName: GPS type
53 : // @Description: Sets the type of simulation used for GPS
54 : // @Values: 0:None, 1:UBlox, 5:NMEA, 6:SBP, 7:File, 8:Nova, 9:SBP2, 11:Trimble, 19:MSP
55 : // @User: Advanced
56 : AP_GROUPINFO("TYPE", 3, GPSParms, type, GPS::Type::UBLOX),
57 :
58 : // @Param: BYTELOS
59 : // @DisplayName: GPS Byteloss
60 : // @Description: Percent of bytes lost from GPS
61 : // @Units: %
62 : // @User: Advanced
63 : AP_GROUPINFO("BYTELOS", 4, GPSParms, byteloss, 0),
64 :
65 : // @Param: NUMSATS
66 : // @DisplayName: GPS Num Satellites
67 : // @Description: Number of satellites GPS has in view
68 : AP_GROUPINFO("NUMSATS", 5, GPSParms, numsats, 10),
69 :
70 : // @Param: GLTCH
71 : // @DisplayName: GPS Glitch
72 : // @Description: Glitch offsets of simulated GPS sensor
73 : // @Vector3Parameter: 1
74 : // @User: Advanced
75 : AP_GROUPINFO("GLTCH", 6, GPSParms, glitch, 0),
76 :
77 : // @Param: HZ
78 : // @DisplayName: GPS Hz
79 : // @Description: GPS Update rate
80 : // @Units: Hz
81 : AP_GROUPINFO("HZ", 7, GPSParms, hertz, 5),
82 :
83 : // @Param: DRFTALT
84 : // @DisplayName: GPS Altitude Drift
85 : // @Description: GPS altitude drift error
86 : // @Units: m
87 : // @User: Advanced
88 : AP_GROUPINFO("DRFTALT", 8, GPSParms, drift_alt, 0),
89 :
90 : // @Param: POS
91 : // @DisplayName: GPS Position
92 : // @Description: GPS antenna phase center position relative to the body frame origin
93 : // @Units: m
94 : // @Vector3Parameter: 1
95 : AP_GROUPINFO("POS", 9, GPSParms, pos_offset, 0),
96 :
97 : // @Param: NOISE
98 : // @DisplayName: GPS vertical noise
99 : // @Description: Amplitude of the GPS vertical position error
100 : // @Units: m
101 : // @User: Advanced
102 : AP_GROUPINFO("NOISE", 10, GPSParms, noise_vertical, 0),
103 :
104 : // @Param: LCKTIME
105 : // @DisplayName: GPS Lock Time
106 : // @Description: Delay in seconds before GPS acquires lock
107 : // @Units: s
108 : // @User: Advanced
109 : AP_GROUPINFO("LCKTIME", 11, GPSParms, lock_time, 0),
110 :
111 : // @Param: ALT_OFS
112 : // @DisplayName: GPS Altitude Offset
113 : // @Description: GPS Altitude Error
114 : // @Units: m
115 : AP_GROUPINFO("ALT_OFS", 12, GPSParms, alt_offset, 0),
116 :
117 : // @Param: HDG
118 : // @DisplayName: GPS Heading
119 : // @Description: Enable GPS output of NMEA heading HDT sentence or UBLOX_RELPOSNED
120 : // @Values: 0:Disabled, 1:Emit HDT, 2:Emit THS, 3:KSXT, 4:Be Moving Baseline Base
121 : // @User: Advanced
122 : AP_GROUPINFO("HDG", 13, GPSParms, hdg_enabled, SIM::GPS_HEADING_NONE),
123 :
124 : // @Param: ACC
125 : // @DisplayName: GPS Accuracy
126 : // @Description: GPS Accuracy
127 : // @User: Advanced
128 : AP_GROUPINFO("ACC", 14, GPSParms, accuracy, 0.3),
129 :
130 : // @Param: VERR
131 : // @DisplayName: GPS Velocity Error
132 : // @Description: GPS Velocity Error Offsets in NED
133 : // @Vector3Parameter: 1
134 : // @User: Advanced
135 : AP_GROUPINFO("VERR", 15, GPSParms, vel_err, 0),
136 :
137 : // @Param: JAM
138 : // @DisplayName: GPS jamming enable
139 : // @Description: Enable simulated GPS jamming
140 : // @User: Advanced
141 : // @Values: 0:Disabled, 1:Enabled
142 : AP_GROUPINFO("JAM", 16, GPSParms, jam, 0),
143 :
144 : // @Param: HDG_OFS
145 : // @DisplayName: GPS heading offset
146 : // @Description: GPS heading offset in degrees. how off the simulated GPS heading is from the actual heading
147 : // @User: Advanced
148 : AP_GROUPINFO("HDG_OFS", 17, GPSParms, heading_offset, 0),
149 :
150 : // @Param: OPTIONS
151 : // @DisplayName: GPS Options
152 : // @Description: GPS Options bitmask
153 : // @Bitmask: 0:UBlox GPS is F9P
154 : // @User: Advanced
155 : AP_GROUPINFO("OPTIONS", 18, GPSParms, options, 0),
156 :
157 : // @Param: FIXTYPE
158 : // @DisplayName: GPS Fix Type
159 : // @Description: Allow setting which fix type (only some GPS's supported); matches AP_GPS_FixType
160 : // @Values: 0:No GPS connected, 1:No Fix, 2:2D Fix, 3:3D Fix, 4:3D DGPS Fix, 5:3D RTK Float, 6:3D RTK Fixed
161 : // @User: Advanced
162 : AP_GROUPINFO("FIXTYPE", 19, GPSParms, fix_type, 6),
163 :
164 : // @Param: HNSE
165 : // @DisplayName: GPS horizontal noise
166 : // @Description: Radius of the GPS horizontal position error in meters
167 : // @Units: m
168 : // @User: Advanced
169 : AP_GROUPINFO("HNSE", 20, GPSParms, noise_horizontal, 0),
170 :
171 : AP_GROUPEND
172 : };
173 : }
174 :
175 : // the number of GPS leap seconds - copied from AP_GPS.h
176 : #define GPS_LEAPSECONDS_MILLIS 18000ULL
177 :
178 : extern const AP_HAL::HAL& hal;
179 :
180 : using namespace SITL;
181 :
182 : // ensure the backend we have allocated matches the one that's configured:
183 14542 : GPS_Backend::GPS_Backend(GPS &_front, uint8_t _instance) :
184 14542 : instance{_instance},
185 14542 : front{_front}
186 : {
187 14542 : _sitl = AP::sitl();
188 :
189 : #if AP_SIM_GPS_ENABLED && AP_SIM_MAX_GPS_SENSORS > 0
190 : // default the first backend to enabled:
191 14542 : if (_instance == 0 && !_sitl->gps[0].enabled.configured()) {
192 9017 : _sitl->gps[0].enabled.set(1);
193 : }
194 : #endif
195 14542 : }
196 :
197 32524434 : ssize_t GPS_Backend::write_to_autopilot(const char *p, size_t size) const
198 : {
199 32524434 : return front.write_to_autopilot(p, size);
200 : }
201 :
202 368788743 : ssize_t GPS_Backend::read_from_autopilot(char *buffer, size_t size) const
203 : {
204 368788743 : return front.read_from_autopilot(buffer, size);
205 : }
206 :
207 14530 : GPS::GPS(uint8_t _instance) :
208 : SerialDevice(8192, 2048),
209 14530 : instance{_instance}
210 : {
211 14530 : }
212 :
213 495658243 : uint32_t GPS::device_baud() const
214 : {
215 495658243 : if (backend == nullptr) {
216 11414 : return 0;
217 : }
218 495646829 : return backend->device_baud();
219 : }
220 :
221 : /*
222 : write some bytes from the simulated GPS
223 : */
224 32524434 : ssize_t GPS::write_to_autopilot(const char *p, size_t size) const
225 : {
226 : // the second GPS instance fails in a different way to the first;
227 : // the first will start sending back 3 satellites, the second just
228 : // stops responding when disabled. This is not necessarily a good
229 : // thing.
230 32524434 : if (instance == 1 && !_sitl->gps[instance].enabled) {
231 15831564 : return -1;
232 : }
233 :
234 16692870 : const float byteloss = _sitl->gps[instance].byteloss;
235 :
236 : // shortcut if we're not doing byteloss:
237 16692870 : if (!is_positive(byteloss)) {
238 16692870 : return SerialDevice::write_to_autopilot(p, size);
239 : }
240 :
241 0 : size_t ret = 0;
242 0 : while (size--) {
243 0 : float r = ((((unsigned)random()) % 1000000)) / 1.0e4;
244 0 : if (r < byteloss) {
245 : // lose the byte
246 0 : p++;
247 0 : continue;
248 : }
249 :
250 0 : const ssize_t pret = SerialDevice::write_to_autopilot(p, 1);
251 0 : if (pret == 0) {
252 : // no space?
253 0 : return ret;
254 : }
255 0 : if (pret != 1) {
256 : // error has occured?
257 0 : return pret;
258 : }
259 0 : ret++;
260 0 : p++;
261 : }
262 :
263 0 : return ret;
264 : }
265 :
266 : /*
267 : get timeval using simulation time
268 : */
269 1588283 : void GPS_Backend::simulation_timeval(struct timeval *tv)
270 : {
271 1588283 : uint64_t now = AP_HAL::micros64();
272 : static uint64_t first_usec;
273 : static struct timeval first_tv;
274 1588283 : if (first_usec == 0) {
275 2093 : first_usec = now;
276 2093 : first_tv.tv_sec = AP::sitl()->start_time_UTC;
277 : }
278 1588283 : *tv = first_tv;
279 1588283 : tv->tv_sec += now / 1000000ULL;
280 1588283 : uint64_t new_usec = tv->tv_usec + (now % 1000000ULL);
281 1588283 : tv->tv_sec += new_usec / 1000000ULL;
282 1588283 : tv->tv_usec = new_usec % 1000000ULL;
283 1588283 : }
284 :
285 : /*
286 : simple simulation of jamming
287 : */
288 151 : void GPS::simulate_jamming(struct GPS_Data &d)
289 : {
290 151 : auto &jam = jamming[instance];
291 151 : const uint32_t now_ms = AP_HAL::millis();
292 151 : if (now_ms - jam.last_jam_ms > 1000) {
293 3 : jam.jam_start_ms = now_ms;
294 3 : jam.latitude = d.latitude;
295 3 : jam.longitude = d.longitude;
296 : }
297 151 : jam.last_jam_ms = now_ms;
298 :
299 : // how often each of the key state variables change during jamming
300 151 : const float vz_change_hz = 0.5;
301 151 : const float vel_change_hz = 0.8;
302 151 : const float pos_change_hz = 1.1;
303 151 : const float sats_change_hz = 3;
304 151 : const float acc_change_hz = 3;
305 :
306 151 : if (now_ms - jam.jam_start_ms < unsigned(1000U+(get_random16()%5000))) {
307 : // total loss of signal for a period at the start is common
308 48 : d.num_sats = 0;
309 48 : d.have_lock = false;
310 : } else {
311 103 : if ((now_ms - jam.last_sats_change_ms)*0.001 > 2*fabsf(rand_float())/sats_change_hz) {
312 53 : jam.last_sats_change_ms = now_ms;
313 53 : d.num_sats = 2 + (get_random16() % 15);
314 53 : if (d.num_sats >= 4) {
315 45 : if (get_random16() % 2 == 0) {
316 18 : d.have_lock = false;
317 : } else {
318 27 : d.have_lock = true;
319 : }
320 : } else {
321 8 : d.have_lock = false;
322 : }
323 : }
324 103 : if ((now_ms - jam.last_vz_change_ms)*0.001 > 2*fabsf(rand_float())/vz_change_hz) {
325 21 : jam.last_vz_change_ms = now_ms;
326 21 : d.speedD = rand_float() * 400;
327 : }
328 103 : if ((now_ms - jam.last_vel_change_ms)*0.001 > 2*fabsf(rand_float())/vel_change_hz) {
329 30 : jam.last_vel_change_ms = now_ms;
330 30 : d.speedN = rand_float() * 400;
331 30 : d.speedE = rand_float() * 400;
332 : }
333 103 : if ((now_ms - jam.last_pos_change_ms)*0.001 > 2*fabsf(rand_float())/pos_change_hz) {
334 32 : jam.last_pos_change_ms = now_ms;
335 32 : jam.latitude += rand_float()*200 * LATLON_TO_M_INV * 1e-7;
336 32 : jam.longitude += rand_float()*200 * LATLON_TO_M_INV * 1e-7;
337 : }
338 103 : if ((now_ms - jam.last_acc_change_ms)*0.001 > 2*fabsf(rand_float())/acc_change_hz) {
339 52 : jam.last_acc_change_ms = now_ms;
340 52 : d.vertical_acc = fabsf(rand_float())*300;
341 52 : d.horizontal_acc = fabsf(rand_float())*300;
342 52 : d.speed_acc = fabsf(rand_float())*50;
343 : }
344 : }
345 :
346 151 : d.latitude = constrain_float(jam.latitude, -90, 90);
347 151 : d.longitude = constrain_float(jam.longitude, -180, 180);
348 151 : }
349 :
350 : /*
351 : return GPS time of week
352 : */
353 1587793 : GPS_Backend::GPS_TOW GPS_Backend::gps_time()
354 : {
355 : GPS_TOW gps_tow;
356 : struct timeval tv;
357 1587793 : simulation_timeval(&tv);
358 1587793 : const uint32_t epoch = 86400*(10*365 + (1980-1969)/4 + 1 + 6 - 2) - (GPS_LEAPSECONDS_MILLIS / 1000ULL);
359 1587793 : uint32_t epoch_seconds = tv.tv_sec - epoch;
360 1587793 : gps_tow.week = epoch_seconds / AP_SEC_PER_WEEK;
361 1587793 : uint32_t t_ms = tv.tv_usec / 1000;
362 : // round time to nearest 200ms
363 1587793 : gps_tow.ms = (epoch_seconds % AP_SEC_PER_WEEK) * AP_MSEC_PER_SEC + ((t_ms/200) * 200);
364 1587793 : return gps_tow;
365 : }
366 :
367 370347508 : void GPS::check_backend_allocation()
368 : {
369 370347508 : const Type configured_type = Type(_sitl->gps[instance].type.get());
370 370347508 : if (allocated_type == configured_type) {
371 370332964 : return;
372 : }
373 :
374 : // mismatch; delete any already-allocated backend:
375 14544 : if (backend != nullptr) {
376 14 : delete backend;
377 14 : backend = nullptr;
378 : }
379 :
380 : // attempt to allocate backend
381 14544 : switch (configured_type) {
382 2 : case Type::NONE:
383 : // no GPS attached
384 2 : break;
385 :
386 : #if AP_SIM_GPS_UBLOX_ENABLED
387 14514 : case Type::UBLOX:
388 14514 : backend = NEW_NOTHROW GPS_UBlox(*this, instance);
389 14514 : break;
390 : #endif
391 :
392 : #if AP_SIM_GPS_NMEA_ENABLED
393 9 : case Type::NMEA:
394 9 : backend = NEW_NOTHROW GPS_NMEA(*this, instance);
395 9 : break;
396 : #endif
397 :
398 : #if AP_SIM_GPS_SBP_ENABLED
399 3 : case Type::SBP:
400 3 : backend = NEW_NOTHROW GPS_SBP(*this, instance);
401 3 : break;
402 : #endif
403 :
404 : #if AP_SIM_GPS_SBP2_ENABLED
405 3 : case Type::SBP2:
406 3 : backend = NEW_NOTHROW GPS_SBP2(*this, instance);
407 3 : break;
408 : #endif
409 :
410 : #if AP_SIM_GPS_NOVA_ENABLED
411 6 : case Type::NOVA:
412 6 : backend = NEW_NOTHROW GPS_NOVA(*this, instance);
413 6 : break;
414 : #endif
415 :
416 : #if AP_SIM_GPS_MSP_ENABLED
417 2 : case Type::MSP:
418 2 : backend = NEW_NOTHROW GPS_MSP(*this, instance);
419 2 : break;
420 : #endif
421 :
422 : #if AP_SIM_GPS_SBF_ENABLED
423 3 : case Type::SBF:
424 3 : backend = NEW_NOTHROW GPS_SBF(*this, instance);
425 3 : break;
426 : #endif
427 :
428 : #if AP_SIM_GPS_TRIMBLE_ENABLED
429 2 : case Type::TRIMBLE:
430 2 : backend = NEW_NOTHROW GPS_Trimble(*this, instance);
431 2 : break;
432 : #endif
433 :
434 : #if AP_SIM_GPS_FILE_ENABLED
435 0 : case Type::FILE:
436 0 : backend = NEW_NOTHROW GPS_FILE(*this, instance);
437 0 : break;
438 : #endif
439 : };
440 :
441 14544 : if (configured_type != Type::NONE && backend == nullptr) {
442 0 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "SIM_GPS: No backend for %u", (unsigned)configured_type);
443 : }
444 14544 : allocated_type = configured_type;
445 : }
446 :
447 : /*
448 : possibly send a new GPS packet
449 : */
450 370347508 : void GPS::update()
451 : {
452 370347508 : if (!init_sitl_pointer()) {
453 368798947 : return;
454 : }
455 :
456 370347508 : check_backend_allocation();
457 370347508 : if (backend == nullptr) {
458 10204 : return;
459 : }
460 :
461 370337304 : double latitude =_sitl->state.latitude;
462 370337304 : double longitude = _sitl->state.longitude;
463 370337304 : float altitude = _sitl->state.altitude;
464 370337304 : const double speedN = _sitl->state.speedN;
465 370337304 : const double speedE = _sitl->state.speedE;
466 370337304 : const double speedD = _sitl->state.speedD;
467 370337304 : const uint32_t now_ms = AP_HAL::millis();
468 :
469 370337304 : if (now_ms < 20000) {
470 : // apply the init offsets for the first 20s. This allows for
471 : // having the origin a long way from the takeoff location,
472 : // which makes testing long flights easier
473 60448623 : latitude += _sitl->gps_init_lat_ofs;
474 60448623 : longitude += _sitl->gps_init_lon_ofs;
475 60448623 : altitude += _sitl->gps_init_alt_ofs;
476 : }
477 :
478 : //Capture current position as basestation location for
479 370351834 : if (!_gps_has_basestation_position &&
480 14530 : now_ms >= _sitl->gps[0].lock_time*1000UL) {
481 14530 : _gps_basestation_data.latitude = latitude;
482 14530 : _gps_basestation_data.longitude = longitude;
483 14530 : _gps_basestation_data.altitude = altitude;
484 14530 : _gps_basestation_data.speedN = speedN;
485 14530 : _gps_basestation_data.speedE = speedE;
486 14530 : _gps_basestation_data.speedD = speedD;
487 14530 : _gps_has_basestation_position = true;
488 : }
489 :
490 370337304 : const auto ¶ms = _sitl->gps[instance];
491 :
492 : // Only let physics run and GPS write at configured GPS rate (default 5Hz).
493 370337304 : if ((now_ms - last_write_update_ms) < (uint32_t)(1000/params.hertz)) {
494 : // Reading runs every iteration.
495 : // Beware- physics don't update every iteration with this approach.
496 : // Currently, none of the drivers rely on quickly updated physics.
497 368788743 : backend->update_read();
498 368788743 : return;
499 : }
500 :
501 1548561 : last_write_update_ms = now_ms;
502 :
503 1548561 : struct GPS_Data d {};
504 :
505 1548561 : d.num_sats = params.numsats;
506 1548561 : d.fix_type = params.fix_type;
507 1548561 : d.latitude = latitude;
508 1548561 : d.longitude = longitude;
509 1548561 : d.yaw_deg = wrap_360(_sitl->state.yawDeg + params.heading_offset);
510 1548561 : d.roll_deg = _sitl->state.rollDeg;
511 1548561 : d.pitch_deg = _sitl->state.pitchDeg;
512 :
513 1548561 : const float gps_wander_angle_rad = now_ms * 0.0005f; // choosing T=12.6 sec arbitrarily
514 : // add an altitude error controlled by a slow sine wave
515 1548561 : d.altitude = altitude + params.noise_vertical * sinf(gps_wander_angle_rad) + params.alt_offset;
516 :
517 : // Add offset to c.g. velocity to get velocity at antenna and add simulated error
518 1548561 : Vector3f velErrorNED = params.vel_err;
519 1548561 : d.speedN = speedN + (velErrorNED.x * rand_float());
520 1548561 : d.speedE = speedE + (velErrorNED.y * rand_float());
521 1548561 : d.speedD = speedD + (velErrorNED.z * rand_float());
522 :
523 : // simulate delayed lock times
524 1548561 : d.have_lock = (params.enabled && now_ms >= params.lock_time*1000UL);
525 :
526 : // fill in accuracies
527 1548561 : d.horizontal_acc = params.accuracy;
528 1548561 : d.vertical_acc = params.accuracy;
529 1548561 : d.speed_acc = params.vel_err.get().xy().length();
530 :
531 1548561 : if (params.drift_alt > 0) {
532 : // add slow altitude drift controlled by a slow sine wave
533 744 : d.altitude += params.drift_alt*sinf(now_ms*0.001f*0.02f);
534 : }
535 :
536 : // correct the latitude, longitude, height and NED velocity for the offset between
537 : // the vehicle c.g. and GPS antenna
538 1548561 : Vector3f posRelOffsetBF = params.pos_offset;
539 1548561 : if (!posRelOffsetBF.is_zero()) {
540 : // get a rotation matrix following DCM conventions (body to earth)
541 9181 : Matrix3f rotmat;
542 9181 : _sitl->state.quaternion.rotation_matrix(rotmat);
543 :
544 : // rotate the antenna offset into the earth frame
545 9181 : Vector3f posRelOffsetEF = rotmat * posRelOffsetBF;
546 :
547 : // Add the offset to the latitude, longitude and height using a spherical earth approximation
548 9181 : double const earth_rad_inv = 1.569612305760477e-7; // use Authalic/Volumetric radius
549 9181 : double lng_scale_factor = earth_rad_inv / cos(radians(d.latitude));
550 9181 : d.latitude += degrees(posRelOffsetEF.x * earth_rad_inv);
551 9181 : d.longitude += degrees(posRelOffsetEF.y * lng_scale_factor);
552 9181 : d.altitude -= posRelOffsetEF.z;
553 :
554 : // calculate a velocity offset due to the antenna position offset and body rotation rate
555 : // note: % operator is overloaded for cross product
556 9181 : Vector3f gyro(radians(_sitl->state.rollRate),
557 9181 : radians(_sitl->state.pitchRate),
558 9181 : radians(_sitl->state.yawRate));
559 9181 : Vector3f velRelOffsetBF = gyro % posRelOffsetBF;
560 :
561 : // rotate the velocity offset into earth frame and add to the c.g. velocity
562 9181 : Vector3f velRelOffsetEF = rotmat * velRelOffsetBF;
563 9181 : d.speedN += velRelOffsetEF.x;
564 9181 : d.speedE += velRelOffsetEF.y;
565 9181 : d.speedD += velRelOffsetEF.z;
566 : }
567 :
568 : // get delayed data
569 1548561 : d.timestamp_ms = now_ms;
570 1548561 : d = interpolate_data(d, params.delay_ms);
571 :
572 : // Applying GPS glitch
573 : // Using first gps glitch
574 1548561 : Vector3f glitch_offsets = params.glitch;
575 : // apply a slow circular horizontal wander of radius noise_horizontal (in metres)
576 1548561 : const double earth_rad_inv = 1.0f/RADIUS_OF_EARTH;
577 1548561 : const float lat_wander_m = params.noise_horizontal * sinf(gps_wander_angle_rad);
578 1548561 : const float lon_wander_m = params.noise_horizontal * cosf(gps_wander_angle_rad);
579 1548561 : const float highest_permissible_lat_deg = 89.0f;
580 1548561 : const double cosine_of_lat = (fabsf(d.latitude) < highest_permissible_lat_deg) ? cos(radians(d.latitude)) : cos(radians(highest_permissible_lat_deg));
581 1548561 : d.latitude += glitch_offsets.x + degrees(lat_wander_m * earth_rad_inv);
582 1548561 : d.longitude += glitch_offsets.y + degrees(lon_wander_m * earth_rad_inv / cosine_of_lat);
583 1548561 : d.altitude += glitch_offsets.z;
584 :
585 1548561 : if (params.jam == 1) {
586 151 : simulate_jamming(d);
587 : }
588 :
589 1548561 : backend->publish(&d);
590 : }
591 :
592 368768976 : void GPS_Backend::update_read()
593 : {
594 : // swallow any config bytes
595 : char c;
596 368768976 : read_from_autopilot(&c, 1);
597 368768976 : }
598 :
599 : /*
600 : get delayed data by interpolation
601 : */
602 1548561 : GPS_Data GPS::interpolate_data(const GPS_Data &d, uint32_t delay_ms)
603 : {
604 1548561 : const uint8_t N = ARRAY_SIZE(_gps_history);
605 1548561 : const uint32_t now_ms = d.timestamp_ms;
606 :
607 : // add in into history array, shifting old elements
608 1548561 : memmove(&_gps_history[1], &_gps_history[0], sizeof(_gps_history[0])*(ARRAY_SIZE(_gps_history)-1));
609 1548561 : _gps_history[0] = d;
610 :
611 1548561 : for (uint8_t i=0; i<N-1; i++) {
612 1548561 : uint32_t dt1 = now_ms - _gps_history[i].timestamp_ms;
613 1548561 : uint32_t dt2 = now_ms - _gps_history[i+1].timestamp_ms;
614 1548561 : if (delay_ms >= dt1 && delay_ms <= dt2) {
615 : // we will interpolate this pair of samples. Start with
616 : // the older sample
617 1548561 : const GPS_Data &s1 = _gps_history[i+1];
618 1548561 : const GPS_Data &s2 = _gps_history[i];
619 1548561 : GPS_Data d2 = s1;
620 1548561 : const float p = (dt2 - delay_ms) / MAX(1,float(dt2 - dt1));
621 1548561 : d2.latitude += p * (s2.latitude - s1.latitude);
622 1548561 : d2.longitude += p * (s2.longitude - s1.longitude);
623 1548561 : d2.altitude += p * (s2.altitude - s1.altitude);
624 1548561 : d2.speedN += p * (s2.speedN - s1.speedN);
625 1548561 : d2.speedE += p * (s2.speedE - s1.speedE);
626 1548561 : d2.speedD += p * (s2.speedD - s1.speedD);
627 1548561 : d2.yaw_deg += p * wrap_180(s2.yaw_deg - s1.yaw_deg);
628 1548561 : return d2;
629 : }
630 : }
631 : // delay is too long, use last sample
632 0 : return _gps_history[N-1];
633 : }
634 :
635 549 : float GPS_Data::ground_track_rad() const
636 : {
637 549 : return atan2f(speedE, speedN);
638 : }
639 :
640 466 : float GPS_Data::speed_2d() const
641 : {
642 466 : const auto velocity = Vector2d{speedN, speedE};
643 466 : return velocity.length();
644 : }
645 :
646 : #endif // AP_SIM_GPS_ENABLED
|
