Line data Source code
1 : #include "SIM_config.h"
2 :
3 : #if AP_SIM_GPS_UBLOX_ENABLED
4 :
5 : #include "SIM_GPS_UBLOX.h"
6 :
7 : #include <SITL/SITL.h>
8 :
9 : using namespace SITL;
10 :
11 : /*
12 : send a UBLOX GPS message
13 : */
14 10838897 : void GPS_UBlox::send_ubx(uint8_t msgid, uint8_t *buf, uint16_t size)
15 : {
16 10838897 : const uint8_t PREAMBLE1 = 0xb5;
17 10838897 : const uint8_t PREAMBLE2 = 0x62;
18 10838897 : const uint8_t CLASS_NAV = 0x1;
19 : uint8_t hdr[6], chk[2];
20 10838897 : hdr[0] = PREAMBLE1;
21 10838897 : hdr[1] = PREAMBLE2;
22 10838897 : hdr[2] = CLASS_NAV;
23 10838897 : hdr[3] = msgid;
24 10838897 : hdr[4] = size & 0xFF;
25 10838897 : hdr[5] = size >> 8;
26 10838897 : chk[0] = chk[1] = hdr[2];
27 10838897 : chk[1] += (chk[0] += hdr[3]);
28 10838897 : chk[1] += (chk[0] += hdr[4]);
29 10838897 : chk[1] += (chk[0] += hdr[5]);
30 583816831 : for (uint16_t i=0; i<size; i++) {
31 572977934 : chk[1] += (chk[0] += buf[i]);
32 : }
33 10838897 : write_to_autopilot((char*)hdr, sizeof(hdr));
34 10838897 : write_to_autopilot((char*)buf, size);
35 10838897 : write_to_autopilot((char*)chk, sizeof(chk));
36 10838897 : }
37 :
38 2906 : void GPS_UBlox::update_relposned(ubx_nav_relposned &relposned, uint32_t tow_ms, float yaw_deg)
39 : {
40 2906 : Vector3f ant1_pos { NaNf, NaNf, NaNf };
41 :
42 : // find our partner:
43 2910 : for (uint8_t i=0; i<ARRAY_SIZE(_sitl->gps); i++) {
44 2909 : if (i == instance) {
45 : // this shouldn't matter as our heading type should never be base
46 1 : continue;
47 : }
48 2908 : if (_sitl->gps[i].hdg_enabled != SITL::SIM::GPS_HEADING_BASE) {
49 3 : continue;
50 : }
51 2905 : ant1_pos = _sitl->gps[i].pos_offset.get();
52 2905 : break;
53 : }
54 2906 : if (ant1_pos.is_nan()) {
55 1 : return;
56 : }
57 :
58 2905 : const Vector3f ant2_pos = _sitl->gps[instance].pos_offset.get();
59 2905 : Vector3f rel_antenna_pos = ant2_pos - ant1_pos;
60 2905 : Matrix3f rot;
61 : // project attitude back using gyros to get antenna orientation at time of GPS sample
62 2905 : Vector3f gyro(radians(_sitl->state.rollRate),
63 2905 : radians(_sitl->state.pitchRate),
64 2905 : radians(_sitl->state.yawRate));
65 2905 : rot.from_euler(radians(_sitl->state.rollDeg), radians(_sitl->state.pitchDeg), radians(yaw_deg));
66 2905 : const float lag = _sitl->gps[instance].delay_ms * 0.001;
67 2905 : rot.rotate(gyro * (-lag));
68 2905 : rel_antenna_pos = rot * rel_antenna_pos;
69 2905 : relposned.version = 1;
70 2905 : relposned.iTOW = tow_ms;
71 2905 : relposned.relPosN = rel_antenna_pos.x * 100;
72 2905 : relposned.relPosE = rel_antenna_pos.y * 100;
73 2905 : relposned.relPosD = rel_antenna_pos.z * 100;
74 2905 : relposned.relPosLength = rel_antenna_pos.length() * 100;
75 2905 : relposned.relPosHeading = degrees(Vector2f(rel_antenna_pos.x, rel_antenna_pos.y).angle()) * 1.0e5;
76 2905 : relposned.flags = gnssFixOK | diffSoln | carrSolnFixed | isMoving | relPosValid | relPosHeadingValid;
77 : }
78 :
79 : /*
80 : send a new set of GPS UBLOX packets
81 : */
82 1547605 : void GPS_UBlox::publish(const GPS_Data *d)
83 : {
84 : struct PACKED ubx_nav_posllh {
85 : uint32_t time; // GPS msToW
86 : int32_t longitude;
87 : int32_t latitude;
88 : int32_t altitude_ellipsoid;
89 : int32_t altitude_msl;
90 : uint32_t horizontal_accuracy;
91 : uint32_t vertical_accuracy;
92 1547605 : } pos {};
93 : struct PACKED ubx_nav_status {
94 : uint32_t time; // GPS msToW
95 : uint8_t fix_type;
96 : uint8_t fix_status;
97 : uint8_t differential_status;
98 : uint8_t res;
99 : uint32_t time_to_first_fix;
100 : uint32_t uptime; // milliseconds
101 1547605 : } status {};
102 : struct PACKED ubx_nav_velned {
103 : uint32_t time; // GPS msToW
104 : int32_t ned_north;
105 : int32_t ned_east;
106 : int32_t ned_down;
107 : uint32_t speed_3d;
108 : uint32_t speed_2d;
109 : int32_t heading_2d;
110 : uint32_t speed_accuracy;
111 : uint32_t heading_accuracy;
112 1547605 : } velned {};
113 : struct PACKED ubx_nav_solution {
114 : uint32_t time;
115 : int32_t time_nsec;
116 : int16_t week;
117 : uint8_t fix_type;
118 : uint8_t fix_status;
119 : int32_t ecef_x;
120 : int32_t ecef_y;
121 : int32_t ecef_z;
122 : uint32_t position_accuracy_3d;
123 : int32_t ecef_x_velocity;
124 : int32_t ecef_y_velocity;
125 : int32_t ecef_z_velocity;
126 : uint32_t speed_accuracy;
127 : uint16_t position_DOP;
128 : uint8_t res;
129 : uint8_t satellites;
130 : uint32_t res2;
131 1547605 : } sol {};
132 : struct PACKED ubx_nav_dop {
133 : uint32_t time; // GPS msToW
134 : uint16_t gDOP;
135 : uint16_t pDOP;
136 : uint16_t tDOP;
137 : uint16_t vDOP;
138 : uint16_t hDOP;
139 : uint16_t nDOP;
140 : uint16_t eDOP;
141 1547605 : } dop {};
142 : struct PACKED ubx_nav_pvt {
143 : uint32_t itow;
144 : uint16_t year;
145 : uint8_t month, day, hour, min, sec;
146 : uint8_t valid;
147 : uint32_t t_acc;
148 : int32_t nano;
149 : uint8_t fix_type;
150 : uint8_t flags;
151 : uint8_t flags2;
152 : uint8_t num_sv;
153 : int32_t lon, lat;
154 : int32_t height, h_msl;
155 : uint32_t h_acc, v_acc;
156 : int32_t velN, velE, velD, gspeed;
157 : int32_t head_mot;
158 : uint32_t s_acc;
159 : uint32_t head_acc;
160 : uint16_t p_dop;
161 : uint8_t reserved1[6];
162 : uint32_t headVeh;
163 : uint8_t reserved2[4];
164 1547605 : } pvt {};
165 : struct PACKED ubx_nav_timegps {
166 : uint32_t itow;
167 : int32_t ftow;
168 : uint16_t week;
169 : int8_t leapS;
170 : uint8_t valid; // leapsvalid | weekvalid | tow valid;
171 : uint32_t tAcc;
172 1547605 : } timegps {};
173 1547605 : const uint8_t SV_COUNT = 10;
174 : struct PACKED ubx_nav_svinfo {
175 : uint32_t itow;
176 : uint8_t numCh;
177 : uint8_t globalFlags;
178 : uint8_t reserved1[2];
179 : // repeated block
180 : struct PACKED svinfo_sv {
181 : uint8_t chn;
182 : uint8_t svid;
183 : uint8_t flags;
184 : uint8_t quality;
185 : uint8_t cno;
186 : int8_t elev;
187 : int16_t azim;
188 : int32_t prRes;
189 : } sv[SV_COUNT];
190 1547605 : } svinfo {};
191 1547605 : ubx_nav_relposned relposned {};
192 1547605 : const uint8_t MSG_POSLLH = 0x2;
193 1547605 : const uint8_t MSG_STATUS = 0x3;
194 1547605 : const uint8_t MSG_DOP = 0x4;
195 1547605 : const uint8_t MSG_VELNED = 0x12;
196 1547605 : const uint8_t MSG_SOL = 0x6;
197 1547605 : const uint8_t MSG_PVT = 0x7;
198 1547605 : const uint8_t MSG_TIMEGPS = 0x20;
199 1547605 : const uint8_t MSG_SVINFO = 0x30;
200 1547605 : const uint8_t MSG_RELPOSNED = 0x3c;
201 :
202 : // Note: This variable is actually a bug but removing it causes some CI failures.
203 : // Keep this here for now pending further investigation.
204 1547605 : uint32_t _next_nav_sv_info_time = 0;
205 :
206 1547605 : const auto gps_tow = gps_time();
207 :
208 1547605 : pos.time = gps_tow.ms;
209 1547605 : pos.longitude = d->longitude * 1.0e7;
210 1547605 : pos.latitude = d->latitude * 1.0e7;
211 1547605 : pos.altitude_ellipsoid = d->altitude * 1000.0f;
212 1547605 : pos.altitude_msl = d->altitude * 1000.0f;
213 1547605 : pos.horizontal_accuracy = d->horizontal_acc*1000;
214 1547605 : pos.vertical_accuracy = d->vertical_acc*1000;
215 :
216 1547605 : status.time = gps_tow.ms;
217 1547605 : status.fix_type = d->have_lock?3:0;
218 1547605 : status.fix_status = d->have_lock?1:0;
219 1547605 : status.differential_status = 0;
220 1547605 : status.res = 0;
221 1547605 : status.time_to_first_fix = 0;
222 1547605 : status.uptime = AP_HAL::millis();
223 :
224 1547605 : velned.time = gps_tow.ms;
225 1547605 : velned.ned_north = 100.0f * d->speedN;
226 1547605 : velned.ned_east = 100.0f * d->speedE;
227 1547605 : velned.ned_down = 100.0f * d->speedD;
228 1547605 : velned.speed_2d = norm(d->speedN, d->speedE) * 100;
229 1547605 : velned.speed_3d = norm(d->speedN, d->speedE, d->speedD) * 100;
230 1547605 : velned.heading_2d = degrees(atan2f(d->speedE, d->speedN)) * 100000.0f;
231 1547605 : if (velned.heading_2d < 0.0f) {
232 667877 : velned.heading_2d += 360.0f * 100000.0f;
233 : }
234 1547605 : velned.speed_accuracy = d->speed_acc * 100; // m/s -> cm/s
235 1547605 : velned.heading_accuracy = 4;
236 :
237 1547605 : memset(&sol, 0, sizeof(sol));
238 1547605 : if (d->have_lock) {
239 773298 : sol.satellites = d->num_sats;
240 773298 : switch(d->fix_type) {
241 : // https://content.u-blox.com/sites/default/files/products/documents/u-blox8-M8_ReceiverDescrProtSpec_UBX-13003221.pdf?utm_content=UBX-13003221
242 : // See page 377, 32.17.17 UBX-NAV-PVT
243 22 : case 0: // no gps - don't publish data.
244 22 : return;
245 3 : case 1: // no lock
246 3 : pvt.flags = 0b00000000;
247 3 : sol.fix_type = 0;
248 3 : break;
249 3 : case 2: // 2d
250 3 : sol.fix_type = 2;
251 3 : pvt.flags = 0b00000001;
252 3 : break;
253 3 : case 3: // 3d
254 3 : sol.fix_type = 3;
255 3 : pvt.flags = 0b00000001;
256 3 : break;
257 3 : case 4: // dgps
258 3 : sol.fix_type = 3;
259 3 : pvt.flags = 0b00000011;
260 3 : break;
261 3 : case 5: // rtk float
262 3 : sol.fix_type = 3;
263 3 : pvt.flags = 0b01000011;
264 3 : break;
265 773261 : case 6: // rtk fixed
266 773261 : sol.fix_type = 3;
267 773261 : pvt.flags = 0b10000011;
268 773261 : break;
269 : }
270 : } else {
271 774307 : sol.fix_type = 0;
272 774307 : pvt.flags = 0b00000000;
273 774307 : sol.satellites = 3;
274 : }
275 1547583 : sol.fix_status = 221;
276 1547583 : sol.time = gps_tow.ms;
277 1547583 : sol.week = gps_tow.week;
278 :
279 1547583 : dop.time = gps_tow.ms;
280 1547583 : dop.gDOP = 65535;
281 1547583 : dop.pDOP = 65535;
282 1547583 : dop.tDOP = 65535;
283 1547583 : dop.vDOP = 200;
284 1547583 : dop.hDOP = 121;
285 1547583 : dop.nDOP = 65535;
286 1547583 : dop.eDOP = 65535;
287 :
288 1547583 : pvt.itow = gps_tow.ms;
289 1547583 : pvt.year = 0;
290 1547583 : pvt.month = 0;
291 1547583 : pvt.day = 0;
292 1547583 : pvt.hour = 0;
293 1547583 : pvt.min = 0;
294 1547583 : pvt.sec = 0;
295 1547583 : pvt.valid = 0; // invalid utc date
296 1547583 : pvt.t_acc = 0;
297 1547583 : pvt.nano = 0;
298 1547583 : pvt.fix_type = sol.fix_type;
299 1547583 : pvt.flags2 =0;
300 1547583 : pvt.num_sv = d->have_lock ? d->num_sats : 3;
301 1547583 : pvt.lon = d->longitude * 1.0e7;
302 1547583 : pvt.lat = d->latitude * 1.0e7;
303 1547583 : pvt.height = d->altitude * 1000.0f;
304 1547583 : pvt.h_msl = d->altitude * 1000.0f;
305 1547583 : pvt.h_acc = d->horizontal_acc * 1000;
306 1547583 : pvt.v_acc = d->vertical_acc * 1000;
307 1547583 : pvt.velN = 1000.0f * d->speedN;
308 1547583 : pvt.velE = 1000.0f * d->speedE;
309 1547583 : pvt.velD = 1000.0f * d->speedD;
310 1547583 : pvt.gspeed = norm(d->speedN, d->speedE) * 1000;
311 1547583 : pvt.head_mot = degrees(atan2f(d->speedE, d->speedN)) * 1.0e5;
312 1547583 : pvt.s_acc = velned.speed_accuracy;
313 1547583 : pvt.head_acc = 38 * 1.0e5;
314 1547583 : pvt.p_dop = 65535;
315 1547583 : memset(pvt.reserved1, '\0', ARRAY_SIZE(pvt.reserved1));
316 1547583 : pvt.headVeh = 0;
317 1547583 : memset(pvt.reserved2, '\0', ARRAY_SIZE(pvt.reserved2));
318 :
319 1547583 : timegps.itow = gps_tow.ms;
320 1547583 : timegps.ftow = 0; // we don't simulate fractional ns
321 1547583 : timegps.week = gps_tow.week;
322 1547583 : timegps.leapS = 0;
323 1547583 : timegps.valid = d->have_lock ? 0x03 : 0x00; // tow valid|week valid
324 1547583 : timegps.tAcc = 0;
325 :
326 1547583 : switch (_sitl->gps[instance].hdg_enabled) {
327 1544677 : case SITL::SIM::GPS_HEADING_NONE:
328 : case SITL::SIM::GPS_HEADING_BASE:
329 1544677 : break;
330 2906 : case SITL::SIM::GPS_HEADING_THS:
331 : case SITL::SIM::GPS_HEADING_KSXT:
332 : case SITL::SIM::GPS_HEADING_HDT:
333 2906 : update_relposned(relposned, gps_tow.ms, d->yaw_deg);
334 2906 : break;
335 : }
336 :
337 1547583 : send_ubx(MSG_POSLLH, (uint8_t*)&pos, sizeof(pos));
338 1547583 : send_ubx(MSG_STATUS, (uint8_t*)&status, sizeof(status));
339 1547583 : send_ubx(MSG_VELNED, (uint8_t*)&velned, sizeof(velned));
340 1547583 : const bool is_f9p = (_sitl->gps[instance].options & static_cast<int32_t>(SITL::SIM::GPSOptions::UBX_IS_F9P)) != 0;
341 :
342 1547583 : if (is_f9p) {
343 0 : const uint32_t now_ms = AP_HAL::millis();
344 0 : if ((int32_t)(now_ms - _next_timegps_send_ms) >= 0) {
345 0 : _next_timegps_send_ms = now_ms + 1000;
346 0 : send_ubx(MSG_TIMEGPS, (uint8_t*)&timegps, sizeof(timegps));
347 : }
348 : } else {
349 : // F9P and later use TIMEGPS to set week number, older u-blox use SOL
350 1547583 : send_ubx(MSG_SOL, (uint8_t*)&sol, sizeof(sol));
351 : }
352 1547583 : send_ubx(MSG_DOP, (uint8_t*)&dop, sizeof(dop));
353 1547583 : send_ubx(MSG_PVT, (uint8_t*)&pvt, sizeof(pvt));
354 1547583 : if (_sitl->gps[instance].hdg_enabled > SITL::SIM::GPS_HEADING_NONE) {
355 5816 : send_ubx(MSG_RELPOSNED, (uint8_t*)&relposned, sizeof(relposned));
356 : }
357 :
358 1547583 : if (gps_tow.ms > _next_nav_sv_info_time) {
359 1547583 : svinfo.itow = gps_tow.ms;
360 1547583 : svinfo.numCh = 32;
361 1547583 : svinfo.globalFlags = 4; // u-blox 8/M8
362 : // fill in the SV's with some data even though firmware does not currently use it
363 : // note that this is not using num_sats as we aren't dynamically creating this to match
364 17023413 : for (uint8_t i = 0; i < SV_COUNT; i++) {
365 15475830 : svinfo.sv[i].chn = i;
366 15475830 : svinfo.sv[i].svid = i;
367 15475830 : svinfo.sv[i].flags = (i < d->num_sats) ? 0x7 : 0x6; // sv used, diff correction data, orbit information
368 15475830 : svinfo.sv[i].quality = 7; // code and carrier lock and time synchronized
369 15475830 : svinfo.sv[i].cno = MAX(20, 30 - i);
370 15475830 : svinfo.sv[i].elev = MAX(30, 90 - i);
371 15475830 : svinfo.sv[i].azim = i;
372 : // not bothering to fill in prRes
373 : }
374 1547583 : send_ubx(MSG_SVINFO, (uint8_t*)&svinfo, sizeof(svinfo));
375 1547583 : _next_nav_sv_info_time = gps_tow.ms + 10000; // 10 second delay
376 : }
377 : }
378 :
379 : #endif // AP_SIM_GPS_UBLOX_ENABLED
|
