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 SensAItion serial IMU device
17 : Converts SITL flight dynamics to SensAItion protocol packets
18 : */
19 :
20 : #include "SIM_config.h"
21 :
22 : #if AP_SIM_EAHRS_SENSATION_ENABLED
23 :
24 : #include "SIM_SensAItion.h"
25 : #include <SITL/SITL.h>
26 : #include <AP_HAL/AP_HAL.h>
27 : #include <AP_Math/AP_Math.h>
28 : #include <AP_Baro/AP_Baro.h>
29 : #include <AP_HAL/utility/sparse-endian.h> // Required for put_beXX_ptr
30 : #include <sys/time.h> // Required for struct timeval
31 :
32 : using namespace SITL;
33 :
34 : // Constants
35 : // Gyro: SITL provides deg/s. Driver expects uDeg/s.
36 : // Factor = 1e6. (NOT RAD_TO_UDEG!)
37 : const float GYRO_NOISE_DEG = 0.02f;
38 :
39 : // Baro: SITL provides mhPa
40 : // std 100 mhPa
41 : const float BARO_NOISE_MHPA = 100.0f;
42 :
43 : int sim_log_counter = 0;
44 249900 : static float rand_float_noise()
45 : {
46 249900 : return ((float)rand() / (float)RAND_MAX) * 2.0f - 1.0f;
47 : }
48 : // [CPO FIX] Time Sync Helper
49 : // Anchors the simulation timestamp to the absolute UTC start time of the SITL run.
50 1143 : static void simulation_timeval(struct timeval *tv)
51 : {
52 1143 : uint64_t now = AP_HAL::micros64();
53 : static uint64_t first_usec;
54 : static struct timeval first_tv;
55 1143 : if (first_usec == 0) {
56 3 : first_usec = now;
57 3 : first_tv.tv_sec = AP::sitl()->start_time_UTC;
58 : }
59 1143 : *tv = first_tv;
60 1143 : tv->tv_sec += now / 1000000ULL;
61 1143 : uint64_t new_usec = tv->tv_usec + (now % 1000000ULL);
62 1143 : tv->tv_sec += new_usec / 1000000ULL;
63 1143 : tv->tv_usec = new_usec % 1000000ULL;
64 1143 : }
65 :
66 1143 : static uint32_t get_gps_tow_ms()
67 : {
68 : struct timeval tv;
69 1143 : gettimeofday(&tv, nullptr); // Get host system time (simulated wall clock)
70 :
71 : // Unix Epoch (1970) vs GPS Epoch (1980) offset is ~315964800 seconds
72 : // But simplistic SITL often just needs "seconds since Sunday".
73 : // Unix Epoch was a Thursday.
74 : // +3 days (259200 sec) aligns the modulo to Sunday.
75 : // 18 leap seconds (approx) for current time.
76 :
77 : // However, the most robust way in ArduPilot SITL is to use the
78 : // simulation start time which is aligned to the host clock.
79 :
80 1143 : uint64_t now_us = AP_HAL::micros64();
81 1143 : double sim_time_sec = (double)AP::sitl()->start_time_UTC + (now_us * 1.0e-6);
82 :
83 : // GPS leap seconds (18 as of 2024)
84 : // GPS time is ahead of UTC.
85 1143 : sim_time_sec += 18;
86 :
87 : // Seconds in week
88 1143 : uint32_t seconds_in_week = (uint32_t)sim_time_sec % 604800;
89 1143 : uint32_t ms_part = (now_us / 1000) % 1000;
90 :
91 1143 : return (seconds_in_week * 1000) + ms_part;
92 : }
93 :
94 9 : SensAItion::SensAItion(bool interleaved_mode) : SerialDevice::SerialDevice()
95 : {
96 9 : _interleaved_mode = interleaved_mode;
97 9 : }
98 :
99 385829 : void SensAItion::update(void)
100 : {
101 385829 : int tick1kHz = (AP_HAL::micros() + 500) / 1000;
102 385829 : if (tick1kHz <= _tick) {
103 73439 : return;
104 : }
105 321390 : _tick = tick1kHz;
106 :
107 : char trash_buf[64];
108 321390 : read_from_autopilot(trash_buf, sizeof(trash_buf));
109 :
110 : // [CPO FIX] The "Gentle Start" Delay
111 : // Wait 2 seconds after boot to allow ArduPilot to initialize
112 : // serial ports and parameters before we flood it with data.
113 : // This prevents race conditions during the reboot test.
114 321390 : if (AP_HAL::millis() < 1000) {
115 9000 : return;
116 : }
117 :
118 312390 : if (!init_sitl_pointer()) {
119 0 : return;
120 : }
121 :
122 312390 : const auto &fdm = _sitl->state;
123 :
124 : // 1. Packet 0: IMU
125 312390 : if ((_tick % _periodMessage0) == _phaseMessage0) {
126 62475 : send_packet_0_imu(fdm);
127 : }
128 :
129 : // 2. Interleaved-only Packets
130 312390 : if (_interleaved_mode) {
131 :
132 : // Packet 1: Orientation
133 114160 : if ((_tick % _periodMessage1) == _phaseMessage1) {
134 1140 : send_packet_1_orientation(fdm);
135 : }
136 :
137 : // Packet 2: INS
138 114160 : if ((_tick % _periodMessage2) == _phaseMessage2) {
139 1143 : send_packet_2_ins(fdm);
140 : }
141 : }
142 312390 : flush_packets();
143 : }
144 :
145 62475 : void SensAItion::send_packet_0_imu(const struct sitl_fdm &fdm)
146 : {
147 :
148 : // Convert from SI units to SensAItion units
149 : // Acceleration: m/s² to µg (1e-6 g)
150 62475 : const float gravity = 9.80665f;
151 62475 : int32_t accel_x = (int32_t)(fdm.xAccel / gravity * 1e6);
152 62475 : int32_t accel_y = (int32_t)(fdm.yAccel / gravity * 1e6);
153 62475 : int32_t accel_z = (int32_t)(fdm.zAccel / gravity * 1e6);
154 :
155 : // Angular rates: deg/s to µdeg/s (1e-6 deg/s)
156 62475 : int32_t gyro_x = (int32_t)((fdm.rollRate + (rand_float_noise() * GYRO_NOISE_DEG)) * 1e6);
157 62475 : int32_t gyro_y = (int32_t)((fdm.pitchRate + (rand_float_noise() * GYRO_NOISE_DEG)) * 1e6);
158 62475 : int32_t gyro_z = (int32_t)((fdm.yawRate + (rand_float_noise() * GYRO_NOISE_DEG)) * 1e6);
159 :
160 : // Temperature: special conversion formula
161 62475 : float temp_c = 25.0f;
162 62475 : int16_t temperature = (int16_t)((temp_c - 20.0f) / 0.008f);
163 :
164 : // Magnetometer: Gauss to mGauss
165 62475 : int16_t mag_x = (int16_t)(fdm.bodyMagField.x * 1000);
166 62475 : int16_t mag_y = (int16_t)(fdm.bodyMagField.y * 1000);
167 62475 : int16_t mag_z = (int16_t)(fdm.bodyMagField.z * 1000);
168 :
169 : // Barometer: Calculate pressure from altitude (Pa to 0.1 Pa units)
170 62475 : const float pressure_pa = AP_Baro::get_pressure_for_alt_amsl(fdm.altitude);
171 62475 : int32_t baro = (int32_t)(pressure_pa * 10.0f + rand_float_noise() * BARO_NOISE_MHPA);
172 :
173 : // Periodic status output to verify simulator operation
174 : static uint32_t imu_count = 0;
175 62475 : if (++imu_count % 1000 == 0) {
176 57 : ::printf("SensAItion: IMU packet #%" PRIu32 " sent - accel_x=%" PRId32 " µg, gyro_x=%" PRId32 " µdeg/s\n",
177 : imu_count, accel_x, gyro_x);
178 : }
179 :
180 : // Pack data in big-endian format (36 bytes data)
181 : uint8_t pkt[36];
182 62475 : put_be32_ptr(&pkt[0], accel_x);
183 62475 : put_be32_ptr(&pkt[4], accel_y);
184 62475 : put_be32_ptr(&pkt[8], accel_z);
185 62475 : put_be32_ptr(&pkt[12], gyro_x);
186 62475 : put_be32_ptr(&pkt[16], gyro_y);
187 62475 : put_be32_ptr(&pkt[20], gyro_z);
188 62475 : put_be16_ptr(&pkt[24], temperature);
189 62475 : put_be16_ptr(&pkt[26], mag_x);
190 62475 : put_be16_ptr(&pkt[28], mag_y);
191 62475 : put_be16_ptr(&pkt[30], mag_z);
192 62475 : put_be32_ptr(&pkt[32], baro);
193 :
194 62475 : if (_interleaved_mode) {
195 22831 : write_packet(0x00, pkt, sizeof(pkt));
196 : // --- DETAILED LOGGING (SIM SIDE) ---
197 : // if (sim_log_counter++ % 400 == 0) {
198 : // fprintf(stderr, "[SIM-OUT] IMU Packet (Interleaved Mode: %d)\n", _interleaved_mode);
199 : // fprintf(stderr, " Acc(ug): X=%d Y=%d Z=%d\n", (int)accel_x, (int)accel_y, (int)accel_z);
200 : // fprintf(stderr, " Gyr(ud): X=%d Y=%d Z=%d\n", (int)gyro_x, (int)gyro_y, (int)gyro_z);
201 : // fprintf(stderr, " Mag(mG): X=%d Y=%d Z=%d\n", (int)mag_x, (int)mag_y, (int)mag_z);
202 : // fprintf(stderr, " Bar(0.1Pa): %d | Temp(raw): %d\n", (int)baro, (int)temperature);
203 : // }
204 : } else {
205 39644 : write_legacy_packet(pkt, sizeof(pkt));
206 : //--- DETAILED LOGGING (SIM SIDE) ---
207 : //if (sim_log_counter++ % 400 == 0) {
208 : // fprintf(stderr, "[SIM-OUT] IMU Packet (Legacy Mode: %d)\n", _interleaved_mode);
209 : // fprintf(stderr, " Acc(ug): X=%d Y=%d Z=%d\n", (int)accel_x, (int)accel_y, (int)accel_z);
210 : // fprintf(stderr, " Gyr(ud): X=%d Y=%d Z=%d\n", (int)gyro_x, (int)gyro_y, (int)gyro_z);
211 : // fprintf(stderr, " Mag(mG): X=%d Y=%d Z=%d\n", (int)mag_x, (int)mag_y, (int)mag_z);
212 : // fprintf(stderr, " Bar(0.1Pa): %d | Temp(raw): %d\n", (int)baro, (int)temperature);
213 : //}
214 : }
215 :
216 62475 : }
217 :
218 1140 : void SensAItion::send_packet_1_orientation(const struct sitl_fdm &fdm)
219 : {
220 : // Quaternion scaled by 1e6
221 1140 : int32_t q0 = (int32_t)(fdm.quaternion.q1 * 1.0e6f); // W
222 1140 : int32_t q1 = (int32_t)(fdm.quaternion.q2 * 1.0e6f); // X
223 1140 : int32_t q2 = (int32_t)(fdm.quaternion.q3 * 1.0e6f); // Y
224 1140 : int32_t q3 = (int32_t)(fdm.quaternion.q4 * 1.0e6f); // Z
225 :
226 : uint8_t pkt[16];
227 1140 : put_be32_ptr(&pkt[0], (uint32_t)q0);
228 1140 : put_be32_ptr(&pkt[4], (uint32_t)q1);
229 1140 : put_be32_ptr(&pkt[8], (uint32_t)q2);
230 1140 : put_be32_ptr(&pkt[12], (uint32_t)q3);
231 :
232 : // --- LOGGING PROBE (SIM SIDE) ---
233 : // if (sim_log_counter % 100 == 0) { // Match Parser 1Hz rate
234 : // fprintf(stderr, "[SIM-OUT] AHRS Packet (ID 0x01)\n");
235 : // fprintf(stderr, " Quat(1e-6): W=%d X=%d Y=%d Z=%d\n", q0, q1, q2, q3);
236 : // }
237 :
238 1140 : write_packet(0x01, pkt, sizeof(pkt));
239 1140 : }
240 :
241 : // NOTE: calculate_itow removed in favor of simulation_timeval inline logic
242 :
243 1143 : void SensAItion::send_packet_2_ins(const struct sitl_fdm &fdm)
244 : {
245 : // --- 1. PREPARE DATA ---
246 1143 : uint8_t align_status = 1; // 1 = Aligned
247 1143 : uint8_t gnss1_fix = 3; // 3D Fix
248 1143 : uint8_t gnss2_fix = 3;
249 :
250 1143 : uint32_t num_sats = 0x0C0C0C0C;
251 :
252 : // [CPO FIX] Calculate accurate GPS Time of Week using the SITL wall clock
253 : // Replaces previous calculate_itow()
254 : struct timeval tv;
255 1143 : simulation_timeval(&tv);
256 :
257 : // Convert TV (Seconds+Micros) to GPS Time of Week (ms)
258 : // We assume SITL starts somewhat recently.
259 : // Calculate seconds since Sunday 00:00:00 UTC
260 : //uint32_t seconds_in_week = 604800;
261 : //uint32_t tow_ms = (tv.tv_sec % seconds_in_week) * 1000 + (tv.tv_usec / 1000);
262 1143 : uint32_t itow = get_gps_tow_ms();
263 :
264 : // Position (deg -> 1e-7 deg)
265 1143 : int32_t lat = (int32_t)(fdm.latitude * 1.0e7);
266 1143 : int32_t lon = (int32_t)(fdm.longitude * 1.0e7);
267 :
268 : // Velocity (m/s -> mm/s)
269 1143 : int32_t vel_n = (int32_t)(fdm.speedN * 1000.0f);
270 1143 : int32_t vel_e = (int32_t)(fdm.speedE * 1000.0f);
271 1143 : int32_t vel_d = (int32_t)(fdm.speedD * 1000.0f);
272 :
273 : // Altitude (m -> mm)
274 1143 : int32_t alt_mm = (int32_t)(fdm.altitude * 1000.0);
275 :
276 : // Accuracy (mm, mm/s)
277 1143 : int32_t acc_lat_mm = 100; // 0.1m
278 1143 : int32_t acc_lon_mm = 100; // 0.1m
279 1143 : int32_t acc_vn_mm = 20;
280 1143 : int32_t acc_ve_mm = 20;
281 1143 : int32_t acc_vd_mm = 20;
282 1143 : int32_t acc_vd_pos_mm = 100;
283 1143 : uint32_t err_flags = 0;
284 1143 : uint8_t sensor_valid = 0xFF; // All valid
285 :
286 : // Date
287 1143 : uint16_t year = 2025;
288 1143 : uint16_t month = 12;
289 1143 : uint8_t day = 7;
290 :
291 : // --- 2. PACKING (Big Endian - 69 Bytes) ---
292 : uint8_t pkt[69];
293 :
294 : // Bytes 0-3: Num Sats (4B)
295 1143 : put_be32_ptr(&pkt[0], num_sats);
296 :
297 : // Bytes 4-7: Error Flags (4B)
298 1143 : put_be32_ptr(&pkt[4], err_flags);
299 :
300 : // Byte 8: Sensor Valid (1B)
301 1143 : pkt[8] = sensor_valid;
302 :
303 : // Bytes 9-12: Latitude (4B)
304 1143 : put_be32_ptr(&pkt[9], (uint32_t)lat);
305 :
306 : // Bytes 13-16: Longitude (4B)
307 1143 : put_be32_ptr(&pkt[13], (uint32_t)lon);
308 :
309 : // Bytes 17-28: Velocity N, E, D (12B)
310 1143 : put_be32_ptr(&pkt[17], (uint32_t)vel_n);
311 1143 : put_be32_ptr(&pkt[21], (uint32_t)vel_e);
312 1143 : put_be32_ptr(&pkt[25], (uint32_t)vel_d);
313 :
314 : // Bytes 29-32: Altitude MSL (4B)
315 1143 : put_be32_ptr(&pkt[29], (uint32_t)alt_mm);
316 :
317 : // Byte 33: Alignment Status (1B)
318 1143 : pkt[33] = align_status;
319 :
320 : // Bytes 34-37: Time iTOW (4B)
321 1143 : put_be32_ptr(&pkt[34], itow);
322 :
323 : // 38-39: GNSS Fix
324 1143 : pkt[38] = gnss1_fix;
325 1143 : pkt[39] = gnss2_fix;
326 :
327 : // 40-44: UTC Date/Time
328 1143 : put_be16_ptr(&pkt[40], year);
329 1143 : put_be16_ptr(&pkt[42], month);
330 1143 : pkt[44] = day;
331 :
332 : // 45-68: Accuracy Metrics (mm or mm/s)
333 1143 : put_be32_ptr(&pkt[45], acc_lat_mm);
334 1143 : put_be32_ptr(&pkt[49], acc_lon_mm);
335 1143 : put_be32_ptr(&pkt[53], acc_vn_mm);
336 1143 : put_be32_ptr(&pkt[57], acc_ve_mm);
337 1143 : put_be32_ptr(&pkt[61], acc_vd_mm);
338 1143 : put_be32_ptr(&pkt[65], acc_vd_pos_mm);
339 :
340 : // --- LOGGING PROBE (SIM SIDE) ---
341 : // if (sim_log_counter % 100 == 0) {
342 : // fprintf(stderr, "[SIM-OUT] INS Packet (iTOW: %u)\n", itow);
343 : // fprintf(stderr, " Pos: Lat=%d Lon=%d Alt=%d mm\n", lat, lon, alt_mm);
344 : // fprintf(stderr, " Vel: N=%d E=%d D=%d mm/s\n", vel_n, vel_e, vel_d);
345 : // fprintf(stderr, " Stat: Align=%d Valid=0x%02X\n", align_status, sensor_valid);
346 : // }
347 :
348 :
349 1143 : write_packet(0x02, pkt, sizeof(pkt));
350 1143 : }
351 :
352 :
353 312390 : void SensAItion::flush_packets()
354 : {
355 312390 : if (_buffert_cnt > 0) {
356 62475 : write_to_autopilot((const char *)&_buffert, _buffert_cnt);
357 62475 : _buffert_cnt = 0;
358 : }
359 312390 : }
360 :
361 219388 : void SensAItion::write_to_autopilot_buf(const char *data, int length)
362 : {
363 219388 : memcpy(&_buffert[_buffert_cnt], data, length);
364 219388 : _buffert_cnt += length;
365 219388 : }
366 :
367 25114 : void SensAItion::write_packet(uint8_t msg_id, const uint8_t* payload, uint16_t length)
368 : {
369 25114 : const uint8_t header = 0xFA;
370 25114 : write_to_autopilot_buf((const char *)&header, 1);
371 25114 : write_to_autopilot_buf((const char *)&msg_id, 1);
372 25114 : write_to_autopilot_buf((const char *)payload, length);
373 25114 : uint8_t crc = (uint8_t)calculate_crc(msg_id, payload, length, true);
374 25114 : write_to_autopilot_buf((const char *)&crc, 1);
375 25114 : }
376 :
377 39644 : void SensAItion::write_legacy_packet(const uint8_t* payload, uint16_t length)
378 : {
379 39644 : const uint8_t header = 0xFA;
380 39644 : write_to_autopilot_buf((const char *)&header, 1);
381 39644 : write_to_autopilot_buf((const char *)payload, length);
382 39644 : uint8_t crc = (uint8_t)calculate_crc(0, payload, length, false);
383 39644 : write_to_autopilot_buf((const char *)&crc, 1);
384 39644 : }
385 :
386 64758 : uint16_t SensAItion::calculate_crc(uint8_t msg_id, const uint8_t* payload, uint16_t length, bool use_id)
387 : {
388 64758 : uint8_t crc = 0;
389 64758 : if (use_id) {
390 25114 : crc ^= msg_id;
391 : }
392 2410965 : for (uint16_t i = 0; i < length; i++) {
393 2346207 : crc ^= payload[i];
394 : }
395 64758 : return crc;
396 : }
397 :
398 : #endif // AP_SIM_EAHRS_SENSATION_ENABLED
|
