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 : ATH- Vinayak Pundhir (VP)
16 : */
17 :
18 : /*
19 : SITL simulation of Aeron Systems PLX3 series INS
20 : */
21 :
22 : #include "SIM_Aeron.h"
23 :
24 : #if AP_SIM_AERON_ENABLED
25 :
26 : #include <AP_Baro/AP_Baro.h>
27 : #include <AP_Math/AP_Math.h>
28 : #include <AP_Math/crc.h>
29 : #include <GCS_MAVLink/GCS.h>
30 :
31 : #include <stdio.h>
32 : #include <string.h>
33 : #include <sys/time.h>
34 :
35 : using namespace SITL;
36 :
37 53 : Aeron::Aeron() :
38 53 : SerialDevice::SerialDevice(2048, 512)
39 : {
40 53 : }
41 :
42 : /*
43 : produce a timeval that advances with the simulation clock
44 : */
45 111409 : void Aeron::simulation_timeval(struct timeval *tv)
46 : {
47 111409 : const uint64_t now = AP_HAL::micros64();
48 : static uint64_t first_usec;
49 : static struct timeval first_tv;
50 111409 : if (first_usec == 0) {
51 53 : first_usec = now;
52 53 : first_tv.tv_sec = AP::sitl()->start_time_UTC;
53 : }
54 111409 : *tv = first_tv;
55 111409 : tv->tv_sec += now / 1000000ULL;
56 111409 : const uint64_t new_usec = tv->tv_usec + (now % 1000000ULL);
57 111409 : tv->tv_sec += new_usec / 1000000ULL;
58 111409 : tv->tv_usec = new_usec % 1000000ULL;
59 111409 : }
60 :
61 : /*
62 : pack the GPS status word consumed by AP_ExternalAHRS_Aeron::format_and_push_gps()
63 :
64 : bits 0..9 : satellites in use (mask 0x000003FF)
65 : bit 10 : fix present (mask 0x00000400)
66 : bits 11..14 : raw fix type (mask 0x00007800)
67 : bits 16..17 : jamming detection level (mask 0x00030000)
68 : bits 18..19 : spoofing detection level (mask 0x000C0000)
69 : */
70 6987 : uint32_t Aeron::build_gps_status(uint16_t num_sats, bool fix_present,
71 : uint8_t fix_raw, uint8_t jam, uint8_t spoof)
72 : {
73 6987 : uint32_t status = uint32_t(num_sats) & 0x3FFu;
74 6987 : if (fix_present) {
75 6987 : status |= (1u << 10);
76 : }
77 6987 : status |= (uint32_t(fix_raw) & 0x0Fu) << 11;
78 6987 : status |= (uint32_t(jam) & 0x03u) << 16;
79 6987 : status |= (uint32_t(spoof) & 0x03u) << 18;
80 6987 : return status;
81 : }
82 :
83 : // Build a complete Aeron frame in a stack buffer and emit it as one
84 : // contiguous write_to_autopilot() call.
85 118418 : void Aeron::send_aeron_packet(PktID prp_id, const void *payload,
86 : uint16_t payload_len)
87 : {
88 : // 4 sync + 2 length + 2 PRP_ID + payload + 1 CRC
89 : uint8_t buf[8 + sizeof(NavPara2Payload) + 1];
90 118418 : const uint16_t total_len = 8 + payload_len; // length field (excludes CRC)
91 118418 : const uint16_t on_wire = total_len + 1; // bytes actually transmitted
92 :
93 : // sanity guard against a future enum getting out of step with the
94 : // structs. Panic rather than silently dropping
95 118418 : if (on_wire > sizeof(buf)) {
96 0 : AP_HAL::panic("Aeron SIM: packet too large for tx buffer (%u > %u)",
97 : unsigned(on_wire), unsigned(sizeof(buf)));
98 : }
99 :
100 : // sync bytes
101 592090 : for (uint8_t index = 0; index < SYNC_COUNT; index++) {
102 473672 : buf[index] = SYNC_BYTE;
103 : }
104 :
105 : // length (BE)
106 118418 : buf[4] = uint8_t(total_len >> 8);
107 118418 : buf[5] = uint8_t(total_len & 0xFF);
108 :
109 : // PRP_ID (BE)
110 118418 : const uint16_t id = uint16_t(prp_id);
111 118418 : buf[6] = uint8_t(id >> 8);
112 118418 : buf[7] = uint8_t(id & 0xFF);
113 :
114 : // payload (LE on both SITL and the MCU)
115 118418 : if (payload_len > 0) {
116 118418 : memcpy(&buf[8], payload, payload_len);
117 : }
118 :
119 : // CRC = XOR over [0 .. total_len-1]
120 118418 : buf[total_len] = crc_xor_of_bytes(buf, total_len);
121 :
122 118418 : write_to_autopilot((const char *)buf, on_wire);
123 118418 : }
124 :
125 : // NAV_PARA1: fused euler/course/velocity/position.
126 34835 : void Aeron::send_nav_para1()
127 : {
128 34835 : const auto &fdm = _sitl->state;
129 :
130 : struct timeval tv;
131 34835 : simulation_timeval(&tv);
132 :
133 : // course over ground [deg], wrapped 0..360
134 34835 : const float track_deg = wrap_360(degrees(atan2f(fdm.speedE, fdm.speedN)));
135 :
136 34835 : const NavPara1Payload p {
137 34835 : uint32_t(tv.tv_sec),
138 34835 : uint32_t(tv.tv_usec),
139 : 0, // ins_status
140 : 0, // hw_status
141 139340 : { float(radians(fdm.rollDeg)), float(radians(fdm.pitchDeg)), float(radians(fdm.yawDeg)) },
142 : track_deg,
143 34835 : { float(fdm.speedN), float(fdm.speedE), float(fdm.speedD) },
144 34835 : { fdm.latitude, fdm.longitude },
145 : // FDM altitude is MSL; PLX3 reports height-above-ellipsoid. With no
146 : // simulated geoid model we treat undulation = 0 (GPS_PARA matches).
147 34835 : float(fdm.altitude)
148 34835 : };
149 :
150 34835 : send_aeron_packet(PktID::NAV_PARA1, &p, sizeof(p));
151 34835 : }
152 :
153 : // NAV_PARA2: quaternion, body-frame velocity, ECEF position.
154 34794 : void Aeron::send_nav_para2()
155 : {
156 34794 : const auto &fdm = _sitl->state;
157 :
158 : struct timeval tv;
159 34794 : simulation_timeval(&tv);
160 :
161 : // rotate NED velocity into body frame
162 34794 : Vector3f v_body { float(fdm.speedN), float(fdm.speedE), float(fdm.speedD) };
163 34794 : fdm.quaternion.earth_to_body(v_body);
164 :
165 : // ECEF via the AP_Math helper
166 34794 : const Vector3d llh { radians(fdm.latitude), radians(fdm.longitude), double(fdm.altitude) };
167 34794 : Vector3d ecef_v;
168 34794 : wgsllh2ecef(llh, ecef_v);
169 :
170 34794 : const NavPara2Payload p {
171 34794 : uint32_t(tv.tv_sec),
172 34794 : uint32_t(tv.tv_usec),
173 : 0, // ins_status
174 : 0, // hw_status
175 : // quaternion (w, x, y, z)
176 34794 : { fdm.quaternion.q1, fdm.quaternion.q2, fdm.quaternion.q3, fdm.quaternion.q4 },
177 34794 : { v_body.x, v_body.y, v_body.z },
178 34794 : { ecef_v.x, ecef_v.y, ecef_v.z },
179 34794 : float(fdm.altitude)
180 34794 : };
181 :
182 34794 : send_aeron_packet(PktID::NAV_PARA2, &p, sizeof(p));
183 34794 : }
184 :
185 : // SENS_PARA: raw IMU / mag / baro. Rate-critical for EAHRS_RATE.
186 34793 : void Aeron::send_sens_para()
187 : {
188 34793 : const auto &fdm = _sitl->state;
189 :
190 : struct timeval tv;
191 34793 : simulation_timeval(&tv);
192 :
193 : // synthesise pressure & temperature from altitude
194 34793 : float pressure_pa = 0.0f;
195 34793 : float temp_K = 0.0f;
196 34793 : AP_Baro::get_pressure_temperature_for_alt_amsl(fdm.altitude, pressure_pa, temp_K);
197 34793 : const float temp_C = KELVIN_TO_C(temp_K);
198 :
199 : const SensParaPayload p {
200 34793 : uint32_t(tv.tv_sec),
201 34793 : uint32_t(tv.tv_usec),
202 : temp_C,
203 : // gyro [deg/s], body frame (FDM rates already deg/s)
204 34793 : { float(fdm.rollRate), float(fdm.pitchRate), float(fdm.yawRate) },
205 : // accel [m/s^2], body frame
206 34793 : { float(fdm.xAccel), float(fdm.yAccel), float(fdm.zAccel) },
207 : // mag [mGauss], body frame (bodyMagField already mGauss)
208 34793 : { float(fdm.bodyMagField.x), float(fdm.bodyMagField.y), float(fdm.bodyMagField.z) },
209 : // euler_rates: small-angle approx; driver ignores these
210 34793 : { float(fdm.rollRate), float(fdm.pitchRate), float(fdm.yawRate) },
211 69586 : pressure_pa + rand_float() * 0.01f,
212 69586 : temp_C + rand_float() * 0.001f,
213 34793 : float(fdm.altitude)
214 34793 : };
215 :
216 34793 : send_aeron_packet(PktID::SENS_PARA, &p, sizeof(p));
217 34793 : }
218 :
219 : /*
220 : GPS_PARA — GNSS solution, status, DOP
221 : */
222 6987 : void Aeron::send_gps_para()
223 : {
224 6987 : const auto &fdm = _sitl->state;
225 :
226 : struct timeval tv;
227 6987 : simulation_timeval(&tv);
228 :
229 6987 : const GpsParaPayload p {
230 6987 : uint32_t(tv.tv_sec),
231 6987 : uint32_t(tv.tv_usec),
232 6987 : build_gps_status(SIM_NUM_SATS, /*fix_present=*/true,
233 : SIM_FIX_RAW, SIM_JAM_STATUS, SIM_SPOOF_LEVEL),
234 6987 : { fdm.latitude, fdm.longitude },
235 6987 : float(fdm.altitude),
236 : 0.0f, // undulation
237 : SIM_PDOP,
238 : SIM_HDOP,
239 : SIM_GDOP
240 6987 : };
241 :
242 6987 : send_aeron_packet(PktID::GPS_PARA, &p, sizeof(p));
243 6987 : }
244 :
245 : // EXTD_GNSS: accuracy metrics + GNSS NED velocity. Drives the accuracy
246 : // fields and GNSS velocity in the constructed GPS message.
247 6987 : void Aeron::send_extd_gnss()
248 : {
249 6987 : const auto &fdm = _sitl->state;
250 :
251 6987 : const ExtdGnssPayload p {
252 : SIM_HPA_M,
253 : SIM_VPA_M,
254 : SIM_HVA_MPS,
255 : SIM_VDOP,
256 6987 : { fdm.speedN, fdm.speedE, fdm.speedD }
257 6987 : };
258 :
259 6987 : send_aeron_packet(PktID::EXTD_GNSS, &p, sizeof(p));
260 6987 : }
261 :
262 : /*
263 : Send Aeron data — drained at every SIMState step.
264 : */
265 835635 : void Aeron::update(void)
266 : {
267 835635 : if (!init_sitl_pointer()) {
268 0 : return;
269 : }
270 :
271 : // 1 kHz tick gate
272 835635 : const uint64_t tick_now = (AP_HAL::micros64() + 500) / 1000;
273 835635 : if (tick_now <= _tick) {
274 139585 : return;
275 : }
276 696050 : _tick = tick_now;
277 :
278 : // Drive the inbound parser. The driver sends EXT_SENSORS_* query
279 : // commands a few seconds after boot; handle_inbound_packet replies
280 : // with the matching presence frame so the driver's response path
281 : // gets exercised in SITL.
282 696050 : process_inbound();
283 :
284 : // Phased emission.
285 696050 : if ((_tick % SENS_PERIOD_MS) == SENS_PHASE_MS) {
286 34793 : send_sens_para();
287 661257 : } else if ((_tick % NAV1_PERIOD_MS) == NAV1_PHASE_MS) {
288 34835 : send_nav_para1();
289 626422 : } else if ((_tick % NAV2_PERIOD_MS) == NAV2_PHASE_MS) {
290 34794 : send_nav_para2();
291 591628 : } else if ((_tick % GPS_PERIOD_MS) == GPS_PHASE_MS) {
292 6987 : send_gps_para();
293 584641 : } else if ((_tick % EXTD_PERIOD_MS) == EXTD_PHASE_MS) {
294 6987 : send_extd_gnss();
295 : }
296 : }
297 :
298 : // Drain whatever the driver has sent us this tick. Each byte feeds a
299 : // minimal state-machine that mirrors the on-wire framing handled by
300 : // AP_ExternalAHRS_Aeron_plx::parse_byte.
301 696050 : void Aeron::process_inbound()
302 : {
303 : char buf[64];
304 : ssize_t num;
305 696062 : while ((num = read_from_autopilot(buf, sizeof(buf))) > 0) {
306 254 : for (ssize_t index = 0; index < num; index++) {
307 242 : handle_inbound_byte(uint8_t(buf[index]));
308 : }
309 : }
310 696050 : }
311 :
312 242 : void Aeron::handle_inbound_byte(uint8_t byte)
313 : {
314 242 : switch (rx_state) {
315 22 : case RxParseState::RESET:
316 22 : rx_write_idx = 0;
317 22 : rx_pkt_len = 0;
318 22 : rx_sync_count = 0;
319 22 : rx_state = RxParseState::SYNC;
320 : FALLTHROUGH;
321 :
322 88 : case RxParseState::SYNC:
323 88 : if (byte == SYNC_BYTE) {
324 88 : rx_buf[rx_write_idx++] = byte;
325 88 : if (++rx_sync_count == SYNC_COUNT) {
326 22 : rx_state = RxParseState::LEN_HIGH;
327 : }
328 : } else {
329 0 : rx_state = RxParseState::RESET;
330 : }
331 88 : return;
332 :
333 22 : case RxParseState::LEN_HIGH:
334 22 : rx_pkt_len = byte;
335 22 : rx_buf[rx_write_idx++] = byte;
336 22 : rx_state = RxParseState::LEN_LOW;
337 22 : return;
338 :
339 22 : case RxParseState::LEN_LOW:
340 22 : rx_buf[rx_write_idx++] = byte;
341 22 : rx_pkt_len = (rx_pkt_len << 8) | byte;
342 22 : if (rx_pkt_len >= 8 && rx_pkt_len < sizeof(rx_buf)) {
343 22 : rx_state = RxParseState::PAYLOAD;
344 : } else {
345 0 : rx_state = RxParseState::RESET;
346 : }
347 22 : return;
348 :
349 88 : case RxParseState::PAYLOAD:
350 88 : rx_buf[rx_write_idx++] = byte;
351 88 : if (rx_write_idx >= rx_pkt_len) {
352 22 : rx_state = RxParseState::CRC_BYTE;
353 : }
354 88 : return;
355 :
356 22 : case RxParseState::CRC_BYTE: {
357 22 : rx_buf[rx_write_idx++] = byte;
358 22 : const bool valid = (byte == crc_xor_of_bytes(rx_buf, rx_pkt_len));
359 22 : if (valid) {
360 22 : const uint16_t prp = (uint16_t(rx_buf[6]) << 8) | rx_buf[7];
361 22 : handle_inbound_packet(prp, &rx_buf[8], rx_pkt_len - 8);
362 : }
363 22 : rx_state = RxParseState::RESET;
364 22 : return;
365 : }
366 : }
367 :
368 0 : rx_state = RxParseState::RESET;
369 0 : return ;
370 : }
371 :
372 : // Dispatch a CRC-valid inbound command. The PLX query format puts the
373 : // target packet ID in the 2-byte payload (big-endian) of a 0x00A1
374 : // command frame; we reply with the matching response packet carrying a
375 : // single byte of presence info.
376 22 : void Aeron::handle_inbound_packet(uint16_t prp_id, const uint8_t *payload, uint16_t payload_len)
377 : {
378 22 : if (prp_id != QUERY_PRP_ID || payload_len < 2) {
379 0 : return;
380 : }
381 :
382 22 : const uint16_t target = (uint16_t(payload[0]) << 8) | payload[1];
383 22 : const uint8_t presence = 1; // SIM always reports the sensor as present
384 :
385 22 : if (target == uint16_t(PktID::EXT_SENSORS_MAG)) {
386 11 : send_aeron_packet(PktID::EXT_SENSORS_MAG, &presence, sizeof(presence));
387 11 : } else if (target == uint16_t(PktID::EXT_SENSORS_ASP)) {
388 11 : send_aeron_packet(PktID::EXT_SENSORS_ASP, &presence, sizeof(presence));
389 : }
390 : }
391 :
392 : #endif // AP_SIM_AERON_ENABLED
|
