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 : simulate VectorNav serial AHRS
17 : */
18 :
19 : #include "SIM_VectorNav.h"
20 : #include <stdio.h>
21 : #include <sys/time.h>
22 : #include <unistd.h>
23 : #include <fcntl.h>
24 : #include <AP_Common/NMEA.h>
25 :
26 : using namespace SITL;
27 :
28 20 : VectorNav::VectorNav(VNModel VN_Name) :
29 20 : SerialDevice::SerialDevice()
30 : {
31 20 : model = VN_Name;
32 20 : }
33 :
34 : struct PACKED VN_IMU_packet_sim {
35 : static constexpr uint8_t header[]{0x01, 0x21, 0x07};
36 : uint64_t timeStartup;
37 : float gyro[3];
38 : float accel[3];
39 : float uncompAccel[3];
40 : float uncompAngRate[3];
41 : float mag[3];
42 : float temp;
43 : float pressure;
44 : };
45 : constexpr uint8_t VN_IMU_packet_sim::header[];
46 :
47 : struct PACKED VN_INS_ekf_packet_sim {
48 : static constexpr uint8_t header[]{0x31, 0x01, 0x00, 0x06, 0x01, 0x13, 0x06};
49 : uint64_t timeStartup;
50 : float ypr[3];
51 : float quaternion[4];
52 : float yprU[3];
53 : uint16_t insStatus;
54 : double posLla[3];
55 : float velNed[3];
56 : float posU;
57 : float velU;
58 : };
59 : constexpr uint8_t VN_INS_ekf_packet_sim::header[];
60 :
61 : struct PACKED VN_INS_gnss_packet_sim {
62 : static constexpr uint8_t header[]{0x49, 0x03, 0x00, 0xB8, 0x26, 0x18, 0x00};
63 : uint64_t timeStartup;
64 : uint64_t timeGps;
65 : uint8_t numSats1;
66 : uint8_t fix1;
67 : double posLla1[3];
68 : float velNed1[3];
69 : float posU1[3];
70 : float velU1;
71 : float dop1[7];
72 : uint8_t numSats2;
73 : uint8_t fix2;
74 : };
75 : constexpr uint8_t VN_INS_gnss_packet_sim::header[];
76 :
77 : struct PACKED VN_AHRS_ekf_packet_sim {
78 : static constexpr uint8_t header[]{0x11, 0x01, 0x00, 0x06, 0x01};
79 : uint64_t timeStartup;
80 : float ypr[3];
81 : float quaternion[4];
82 : float yprU[3];
83 : };
84 : constexpr uint8_t VN_AHRS_ekf_packet_sim::header[];
85 :
86 : /*
87 : get timeval using simulation time
88 : */
89 2056 : static void simulation_timeval(struct timeval *tv)
90 : {
91 2056 : uint64_t now = AP_HAL::micros64();
92 : static uint64_t first_usec;
93 : static struct timeval first_tv;
94 2056 : if (first_usec == 0) {
95 20 : first_usec = now;
96 20 : first_tv.tv_sec = AP::sitl()->start_time_UTC;
97 : }
98 2056 : *tv = first_tv;
99 2056 : tv->tv_sec += now / 1000000ULL;
100 2056 : uint64_t new_usec = tv->tv_usec + (now % 1000000ULL);
101 2056 : tv->tv_sec += new_usec / 1000000ULL;
102 2056 : tv->tv_usec = new_usec % 1000000ULL;
103 2056 : }
104 :
105 19920 : void VectorNav::send_imu_packet(void)
106 : {
107 19920 : const auto &fdm = _sitl->state;
108 :
109 19920 : struct VN_IMU_packet_sim pkt {};
110 :
111 19920 : pkt.timeStartup = AP_HAL::micros() * 1e3;
112 :
113 :
114 19920 : const float gyro_noise = 0.05;
115 :
116 19920 : pkt.gyro[0] = radians(fdm.rollRate + rand_float() * gyro_noise);
117 19920 : pkt.gyro[1] = radians(fdm.pitchRate + rand_float() * gyro_noise);
118 19920 : pkt.gyro[2] = radians(fdm.yawRate + rand_float() * gyro_noise);
119 :
120 19920 : pkt.accel[0] = fdm.xAccel;
121 19920 : pkt.accel[1] = fdm.yAccel;
122 19920 : pkt.accel[2] = fdm.zAccel;
123 :
124 19920 : pkt.uncompAccel[0] = fdm.xAccel;
125 19920 : pkt.uncompAccel[1] = fdm.yAccel;
126 19920 : pkt.uncompAccel[2] = fdm.zAccel;
127 :
128 19920 : pkt.uncompAngRate[0] = radians(fdm.rollRate + gyro_noise * rand_float());
129 19920 : pkt.uncompAngRate[1] = radians(fdm.pitchRate + gyro_noise * rand_float());
130 19920 : pkt.uncompAngRate[2] = radians(fdm.yawRate + gyro_noise * rand_float());
131 :
132 19920 : pkt.mag[0] = fdm.bodyMagField.x*0.001;
133 19920 : pkt.mag[1] = fdm.bodyMagField.y*0.001;
134 19920 : pkt.mag[2] = fdm.bodyMagField.z*0.001;
135 :
136 19920 : pkt.temp = AP_Baro::get_temperatureC_for_alt_amsl(fdm.altitude);
137 :
138 19920 : const float pressure_Pa = AP_Baro::get_pressure_for_alt_amsl(fdm.altitude);
139 19920 : pkt.pressure = pressure_Pa*0.001 + rand_float() * 0.01;
140 :
141 19920 : const uint8_t sync_byte = 0xFA;
142 19920 : write_to_autopilot((const char *)&sync_byte, 1);
143 19920 : write_to_autopilot((const char *)&VN_IMU_packet_sim::header, sizeof(VN_IMU_packet_sim::header));
144 19920 : write_to_autopilot((const char *)&pkt, sizeof(pkt));
145 :
146 19920 : uint16_t crc = crc16_ccitt(&VN_IMU_packet_sim::header[0], sizeof(VN_IMU_packet_sim::header), 0);
147 19920 : crc = crc16_ccitt((const uint8_t *)&pkt, sizeof(pkt), crc);
148 : uint16_t crc2;
149 19920 : swab(&crc, &crc2, 2);
150 :
151 19920 : write_to_autopilot((const char *)&crc2, sizeof(crc2));
152 19920 : }
153 :
154 19920 : void VectorNav::send_ins_ekf_packet(void)
155 : {
156 19920 : const auto &fdm = _sitl->state;
157 :
158 19920 : struct VN_INS_ekf_packet_sim pkt {};
159 :
160 19920 : pkt.timeStartup = AP_HAL::micros() * 1e3;
161 :
162 19920 : pkt.ypr[0] = fdm.yawDeg;
163 19920 : pkt.ypr[1] = fdm.pitchDeg;
164 19920 : pkt.ypr[2] = fdm.rollDeg;
165 :
166 19920 : pkt.quaternion[0] = fdm.quaternion.q2;
167 19920 : pkt.quaternion[1] = fdm.quaternion.q3;
168 19920 : pkt.quaternion[2] = fdm.quaternion.q4;
169 19920 : pkt.quaternion[3] = fdm.quaternion.q1;
170 :
171 19920 : pkt.yprU[0] = 0.03;
172 19920 : pkt.yprU[1] = 0.03;
173 19920 : pkt.yprU[2] = 0.15;
174 :
175 19920 : pkt.insStatus = 0x0306;
176 :
177 19920 : pkt.posLla[0] = fdm.latitude;
178 19920 : pkt.posLla[1] = fdm.longitude;
179 19920 : pkt.posLla[2] = fdm.altitude;
180 19920 : pkt.velNed[0] = fdm.speedN;
181 19920 : pkt.velNed[1] = fdm.speedE;
182 19920 : pkt.velNed[2] = fdm.speedD;
183 19920 : pkt.posU = 0.5;
184 19920 : pkt.velU = 0.25;
185 :
186 19920 : const uint8_t sync_byte = 0xFA;
187 19920 : write_to_autopilot((const char *)&sync_byte, 1);
188 19920 : write_to_autopilot((const char *)&VN_INS_ekf_packet_sim::header, sizeof(VN_INS_ekf_packet_sim::header));
189 19920 : write_to_autopilot((const char *)&pkt, sizeof(pkt));
190 :
191 19920 : uint16_t crc = crc16_ccitt(&VN_INS_ekf_packet_sim::header[0], sizeof(VN_INS_ekf_packet_sim::header), 0);
192 19920 : crc = crc16_ccitt((const uint8_t *)&pkt, sizeof(pkt), crc);
193 :
194 : uint16_t crc2;
195 19920 : swab(&crc, &crc2, 2);
196 :
197 19920 : write_to_autopilot((const char *)&crc2, sizeof(crc2));
198 19920 : }
199 :
200 2056 : void VectorNav::send_ins_gnss_packet(void)
201 : {
202 2056 : const auto &fdm = _sitl->state;
203 :
204 2056 : struct VN_INS_gnss_packet_sim pkt {};
205 :
206 2056 : pkt.timeStartup = AP_HAL::micros() * 1e3;
207 :
208 : struct timeval tv;
209 2056 : simulation_timeval(&tv);
210 :
211 2056 : pkt.timeGps = tv.tv_usec * 1000ULL;
212 :
213 2056 : pkt.numSats1 = 19;
214 2056 : pkt.fix1 = 3;
215 2056 : pkt.posLla1[0] = fdm.latitude;
216 2056 : pkt.posLla1[1] = fdm.longitude;
217 2056 : pkt.posLla1[2] = fdm.altitude;
218 2056 : pkt.velNed1[0] = fdm.speedN;
219 2056 : pkt.velNed1[1] = fdm.speedE;
220 2056 : pkt.velNed1[2] = fdm.speedD;
221 :
222 2056 : pkt.posU1[0] = 1;
223 2056 : pkt.posU1[1] = 1;
224 2056 : pkt.posU1[2] = 1.5;
225 :
226 2056 : pkt.velU1 = 0.05;
227 : // pkt.dop1 =
228 2056 : pkt.numSats2 = 18;
229 2056 : pkt.fix2 = 3;
230 :
231 2056 : const uint8_t sync_byte = 0xFA;
232 2056 : write_to_autopilot((const char *)&sync_byte, 1);
233 2056 : write_to_autopilot((const char *)&VN_INS_gnss_packet_sim::header, sizeof(VN_INS_gnss_packet_sim::header));
234 2056 : write_to_autopilot((const char *)&pkt, sizeof(pkt));
235 :
236 2056 : uint16_t crc = crc16_ccitt(&VN_INS_gnss_packet_sim::header[0], sizeof(VN_INS_gnss_packet_sim::header), 0);
237 2056 : crc = crc16_ccitt((const uint8_t *)&pkt, sizeof(pkt), crc);
238 :
239 : uint16_t crc2;
240 2056 : swab(&crc, &crc2, 2);
241 :
242 2056 : write_to_autopilot((const char *)&crc2, sizeof(crc2));
243 2056 : }
244 :
245 0 : void VectorNav::send_ahrs_packet(void)
246 : {
247 0 : const auto &fdm = _sitl->state;
248 :
249 0 : struct VN_AHRS_ekf_packet_sim pkt{};
250 0 : pkt.timeStartup = AP_HAL::micros() * 1e3;
251 :
252 0 : pkt.ypr[0] = fdm.yawDeg;
253 0 : pkt.ypr[1] = fdm.pitchDeg;
254 0 : pkt.ypr[2] = fdm.rollDeg;
255 :
256 0 : pkt.quaternion[0] = fdm.quaternion.q2;
257 0 : pkt.quaternion[1] = fdm.quaternion.q3;
258 0 : pkt.quaternion[2] = fdm.quaternion.q4;
259 0 : pkt.quaternion[3] = fdm.quaternion.q1;
260 :
261 0 : pkt.yprU[0] = 0.03;
262 0 : pkt.yprU[1] = 0.03;
263 0 : pkt.yprU[2] = 0.15;
264 :
265 0 : const uint8_t sync = 0xFA;
266 0 : write_to_autopilot((const char*)&sync, 1);
267 0 : write_to_autopilot((const char*)VN_AHRS_ekf_packet_sim::header,
268 : sizeof(VN_AHRS_ekf_packet_sim::header));
269 0 : write_to_autopilot((const char*)&pkt, sizeof(pkt));
270 :
271 0 : uint16_t crc = crc16_ccitt(VN_AHRS_ekf_packet_sim::header,
272 0 : sizeof(VN_AHRS_ekf_packet_sim::header), 0);
273 0 : crc = crc16_ccitt((const uint8_t*)&pkt, sizeof(pkt), crc);
274 :
275 : uint16_t crc_swapped;
276 0 : swab(&crc, &crc_swapped, 2);
277 :
278 0 : write_to_autopilot((const char*)&crc_swapped, sizeof(crc_swapped));
279 0 : }
280 :
281 12 : void VectorNav::nmea_printf(const char *fmt, ...)
282 : {
283 : va_list ap;
284 :
285 12 : va_start(ap, fmt);
286 12 : char *s = nmea_vaprintf(fmt, ap);
287 12 : va_end(ap);
288 12 : if (s != nullptr) {
289 12 : write_to_autopilot((const char*)s, strlen(s));
290 12 : free(s);
291 : }
292 12 : }
293 :
294 : /*
295 : send VectorNav data
296 : */
297 498127 : void VectorNav::update(void)
298 : {
299 498127 : if (!init_sitl_pointer()) {
300 498031 : return;
301 : }
302 :
303 498127 : uint32_t now = AP_HAL::micros();
304 498127 : if (now - last_imu_pkt_us >= 20000) {
305 19920 : last_imu_pkt_us = now;
306 19920 : send_imu_packet();
307 : }
308 :
309 498127 : if (model == VNModel::VN300) {
310 498127 : if (now - last_ekf_pkt_us >= 20000) {
311 19920 : last_ekf_pkt_us = now;
312 19920 : send_ins_ekf_packet();
313 : }
314 498127 : if (now - last_gnss_pkt_us >= 200000) {
315 2056 : last_gnss_pkt_us = now;
316 2056 : send_ins_gnss_packet();
317 : }
318 : } else {
319 0 : if (now - last_ahrs_pkt_us >= 20000) {
320 0 : last_ahrs_pkt_us = now;
321 0 : send_ahrs_packet();
322 : }
323 : }
324 :
325 : char receive_buf[50];
326 498127 : ssize_t n = read_from_autopilot(&receive_buf[0], ARRAY_SIZE(receive_buf));
327 498127 : if (n <= 0) {
328 498031 : return;
329 : }
330 :
331 : // avoid parsing the NMEA stream here by making assumptions about
332 : // how we receive configuration strings. Generally we can just
333 : // echo back the configuration string to make the driver happy.
334 96 : if (n >= 9) {
335 : // intercept device-version query, respond with simulated version:
336 96 : const char *ver_query_string = "$VNRRG,01";
337 96 : if (strncmp(receive_buf, ver_query_string, strlen(ver_query_string)) == 0) {
338 12 : if (model == VNModel::VN100) {
339 0 : nmea_printf("$VNRRG,01,VN-100-SITL");
340 : } else {
341 12 : nmea_printf("$VNRRG,01,VN-300-SITL");
342 : }
343 : // consume the query so we don't "respond" twice:
344 12 : memmove(&receive_buf[0], &receive_buf[strlen(ver_query_string)], n - strlen(ver_query_string));
345 12 : n -= strlen(ver_query_string);
346 : }
347 : }
348 96 : write_to_autopilot(receive_buf, n);
349 : }
350 :
|
