From b77dcd5fe6d9d7431989b1dfd125a96eaa274a55 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Magnus=20Lindh=C3=A9?= Date: Tue, 23 Dec 2025 14:28:00 +0100 Subject: [PATCH] SITL: Add support for Kebni SensAItion as External AHRS --- libraries/SITL/SIM_SensAItion.cpp | 398 ++++++++++++++++++++++++++++++ libraries/SITL/SIM_SensAItion.h | 67 +++++ libraries/SITL/SIM_config.h | 3 + 3 files changed, 468 insertions(+) create mode 100644 libraries/SITL/SIM_SensAItion.cpp create mode 100644 libraries/SITL/SIM_SensAItion.h diff --git a/libraries/SITL/SIM_SensAItion.cpp b/libraries/SITL/SIM_SensAItion.cpp new file mode 100644 index 00000000000..b55bd74ff06 --- /dev/null +++ b/libraries/SITL/SIM_SensAItion.cpp @@ -0,0 +1,398 @@ +/* + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . +*/ +/* + Simulate SensAItion serial IMU device + Converts SITL flight dynamics to SensAItion protocol packets +*/ + +#include "SIM_config.h" + +#if AP_SIM_EAHRS_SENSATION_ENABLED + +#include "SIM_SensAItion.h" +#include +#include +#include +#include +#include // Required for put_beXX_ptr +#include // Required for struct timeval + +using namespace SITL; + +// Constants +// Gyro: SITL provides deg/s. Driver expects uDeg/s. +// Factor = 1e6. (NOT RAD_TO_UDEG!) +const float GYRO_NOISE_DEG = 0.02f; + +// Baro: SITL provides mhPa +// std 100 mhPa +const float BARO_NOISE_MHPA = 100.0f; + +int sim_log_counter = 0; +static float rand_float_noise() +{ + return ((float)rand() / (float)RAND_MAX) * 2.0f - 1.0f; +} +// [CPO FIX] Time Sync Helper +// Anchors the simulation timestamp to the absolute UTC start time of the SITL run. +static void simulation_timeval(struct timeval *tv) +{ + uint64_t now = AP_HAL::micros64(); + static uint64_t first_usec; + static struct timeval first_tv; + if (first_usec == 0) { + first_usec = now; + first_tv.tv_sec = AP::sitl()->start_time_UTC; + } + *tv = first_tv; + tv->tv_sec += now / 1000000ULL; + uint64_t new_usec = tv->tv_usec + (now % 1000000ULL); + tv->tv_sec += new_usec / 1000000ULL; + tv->tv_usec = new_usec % 1000000ULL; +} + +static uint32_t get_gps_tow_ms() +{ + struct timeval tv; + gettimeofday(&tv, nullptr); // Get host system time (simulated wall clock) + + // Unix Epoch (1970) vs GPS Epoch (1980) offset is ~315964800 seconds + // But simplistic SITL often just needs "seconds since Sunday". + // Unix Epoch was a Thursday. + // +3 days (259200 sec) aligns the modulo to Sunday. + // 18 leap seconds (approx) for current time. + + // However, the most robust way in ArduPilot SITL is to use the + // simulation start time which is aligned to the host clock. + + uint64_t now_us = AP_HAL::micros64(); + double sim_time_sec = (double)AP::sitl()->start_time_UTC + (now_us * 1.0e-6); + + // GPS leap seconds (18 as of 2024) + // GPS time is ahead of UTC. + sim_time_sec += 18; + + // Seconds in week + uint32_t seconds_in_week = (uint32_t)sim_time_sec % 604800; + uint32_t ms_part = (now_us / 1000) % 1000; + + return (seconds_in_week * 1000) + ms_part; +} + +SensAItion::SensAItion(bool interleaved_mode) : SerialDevice::SerialDevice() +{ + _interleaved_mode = interleaved_mode; +} + +void SensAItion::update(void) +{ + int tick1kHz = (AP_HAL::micros() + 500) / 1000; + if (tick1kHz <= _tick) { + return; + } + _tick = tick1kHz; + + char trash_buf[64]; + read_from_autopilot(trash_buf, sizeof(trash_buf)); + + // [CPO FIX] The "Gentle Start" Delay + // Wait 2 seconds after boot to allow ArduPilot to initialize + // serial ports and parameters before we flood it with data. + // This prevents race conditions during the reboot test. + if (AP_HAL::millis() < 1000) { + return; + } + + if (!init_sitl_pointer()) { + return; + } + + const auto &fdm = _sitl->state; + + // 1. Packet 0: IMU + if ((_tick % _periodMessage0) == _phaseMessage0) { + send_packet_0_imu(fdm); + } + + // 2. Interleaved-only Packets + if (_interleaved_mode) { + + // Packet 1: Orientation + if ((_tick % _periodMessage1) == _phaseMessage1) { + send_packet_1_orientation(fdm); + } + + // Packet 2: INS + if ((_tick % _periodMessage2) == _phaseMessage2) { + send_packet_2_ins(fdm); + } + } + flush_packets(); +} + +void SensAItion::send_packet_0_imu(const struct sitl_fdm &fdm) +{ + + // Convert from SI units to SensAItion units + // Acceleration: m/s² to µg (1e-6 g) + const float gravity = 9.80665f; + int32_t accel_x = (int32_t)(fdm.xAccel / gravity * 1e6); + int32_t accel_y = (int32_t)(fdm.yAccel / gravity * 1e6); + int32_t accel_z = (int32_t)(fdm.zAccel / gravity * 1e6); + + // Angular rates: deg/s to µdeg/s (1e-6 deg/s) + int32_t gyro_x = (int32_t)((fdm.rollRate + (rand_float_noise() * GYRO_NOISE_DEG)) * 1e6); + int32_t gyro_y = (int32_t)((fdm.pitchRate + (rand_float_noise() * GYRO_NOISE_DEG)) * 1e6); + int32_t gyro_z = (int32_t)((fdm.yawRate + (rand_float_noise() * GYRO_NOISE_DEG)) * 1e6); + + // Temperature: special conversion formula + float temp_c = 25.0f; + int16_t temperature = (int16_t)((temp_c - 20.0f) / 0.008f); + + // Magnetometer: Gauss to mGauss + int16_t mag_x = (int16_t)(fdm.bodyMagField.x * 1000); + int16_t mag_y = (int16_t)(fdm.bodyMagField.y * 1000); + int16_t mag_z = (int16_t)(fdm.bodyMagField.z * 1000); + + // Barometer: Calculate pressure from altitude (Pa to 0.1 Pa units) + const float pressure_pa = AP_Baro::get_pressure_for_alt_amsl(fdm.altitude); + int32_t baro = (int32_t)(pressure_pa * 10.0f + rand_float_noise() * BARO_NOISE_MHPA); + + // Periodic status output to verify simulator operation + static uint32_t imu_count = 0; + if (++imu_count % 1000 == 0) { + ::printf("SensAItion: IMU packet #%" PRIu32 " sent - accel_x=%" PRId32 " µg, gyro_x=%" PRId32 " µdeg/s\n", + imu_count, accel_x, gyro_x); + } + + // Pack data in big-endian format (36 bytes data) + uint8_t pkt[36]; + put_be32_ptr(&pkt[0], accel_x); + put_be32_ptr(&pkt[4], accel_y); + put_be32_ptr(&pkt[8], accel_z); + put_be32_ptr(&pkt[12], gyro_x); + put_be32_ptr(&pkt[16], gyro_y); + put_be32_ptr(&pkt[20], gyro_z); + put_be16_ptr(&pkt[24], temperature); + put_be16_ptr(&pkt[26], mag_x); + put_be16_ptr(&pkt[28], mag_y); + put_be16_ptr(&pkt[30], mag_z); + put_be32_ptr(&pkt[32], baro); + + if (_interleaved_mode) { + write_packet(0x00, pkt, sizeof(pkt)); + // --- DETAILED LOGGING (SIM SIDE) --- + // if (sim_log_counter++ % 400 == 0) { + // fprintf(stderr, "[SIM-OUT] IMU Packet (Interleaved Mode: %d)\n", _interleaved_mode); + // fprintf(stderr, " Acc(ug): X=%d Y=%d Z=%d\n", (int)accel_x, (int)accel_y, (int)accel_z); + // fprintf(stderr, " Gyr(ud): X=%d Y=%d Z=%d\n", (int)gyro_x, (int)gyro_y, (int)gyro_z); + // fprintf(stderr, " Mag(mG): X=%d Y=%d Z=%d\n", (int)mag_x, (int)mag_y, (int)mag_z); + // fprintf(stderr, " Bar(0.1Pa): %d | Temp(raw): %d\n", (int)baro, (int)temperature); + // } + } else { + write_legacy_packet(pkt, sizeof(pkt)); + //--- DETAILED LOGGING (SIM SIDE) --- + //if (sim_log_counter++ % 400 == 0) { + // fprintf(stderr, "[SIM-OUT] IMU Packet (Legacy Mode: %d)\n", _interleaved_mode); + // fprintf(stderr, " Acc(ug): X=%d Y=%d Z=%d\n", (int)accel_x, (int)accel_y, (int)accel_z); + // fprintf(stderr, " Gyr(ud): X=%d Y=%d Z=%d\n", (int)gyro_x, (int)gyro_y, (int)gyro_z); + // fprintf(stderr, " Mag(mG): X=%d Y=%d Z=%d\n", (int)mag_x, (int)mag_y, (int)mag_z); + // fprintf(stderr, " Bar(0.1Pa): %d | Temp(raw): %d\n", (int)baro, (int)temperature); + //} + } + +} + +void SensAItion::send_packet_1_orientation(const struct sitl_fdm &fdm) +{ + // Quaternion scaled by 1e6 + int32_t q0 = (int32_t)(fdm.quaternion.q1 * 1.0e6f); // W + int32_t q1 = (int32_t)(fdm.quaternion.q2 * 1.0e6f); // X + int32_t q2 = (int32_t)(fdm.quaternion.q3 * 1.0e6f); // Y + int32_t q3 = (int32_t)(fdm.quaternion.q4 * 1.0e6f); // Z + + uint8_t pkt[16]; + put_be32_ptr(&pkt[0], (uint32_t)q0); + put_be32_ptr(&pkt[4], (uint32_t)q1); + put_be32_ptr(&pkt[8], (uint32_t)q2); + put_be32_ptr(&pkt[12], (uint32_t)q3); + + // --- LOGGING PROBE (SIM SIDE) --- + // if (sim_log_counter % 100 == 0) { // Match Parser 1Hz rate + // fprintf(stderr, "[SIM-OUT] AHRS Packet (ID 0x01)\n"); + // fprintf(stderr, " Quat(1e-6): W=%d X=%d Y=%d Z=%d\n", q0, q1, q2, q3); + // } + + write_packet(0x01, pkt, sizeof(pkt)); +} + +// NOTE: calculate_itow removed in favor of simulation_timeval inline logic + +void SensAItion::send_packet_2_ins(const struct sitl_fdm &fdm) +{ + // --- 1. PREPARE DATA --- + uint8_t align_status = 1; // 1 = Aligned + uint8_t gnss1_fix = 3; // 3D Fix + uint8_t gnss2_fix = 3; + + uint32_t num_sats = 0x0C0C0C0C; + + // [CPO FIX] Calculate accurate GPS Time of Week using the SITL wall clock + // Replaces previous calculate_itow() + struct timeval tv; + simulation_timeval(&tv); + + // Convert TV (Seconds+Micros) to GPS Time of Week (ms) + // We assume SITL starts somewhat recently. + // Calculate seconds since Sunday 00:00:00 UTC + //uint32_t seconds_in_week = 604800; + //uint32_t tow_ms = (tv.tv_sec % seconds_in_week) * 1000 + (tv.tv_usec / 1000); + uint32_t itow = get_gps_tow_ms(); + + // Position (deg -> 1e-7 deg) + int32_t lat = (int32_t)(fdm.latitude * 1.0e7); + int32_t lon = (int32_t)(fdm.longitude * 1.0e7); + + // Velocity (m/s -> mm/s) + int32_t vel_n = (int32_t)(fdm.speedN * 1000.0f); + int32_t vel_e = (int32_t)(fdm.speedE * 1000.0f); + int32_t vel_d = (int32_t)(fdm.speedD * 1000.0f); + + // Altitude (m -> mm) + int32_t alt_mm = (int32_t)(fdm.altitude * 1000.0); + + // Accuracy (mm, mm/s) + int32_t acc_lat_mm = 100; // 0.1m + int32_t acc_lon_mm = 100; // 0.1m + int32_t acc_vn_mm = 20; + int32_t acc_ve_mm = 20; + int32_t acc_vd_mm = 20; + int32_t acc_vd_pos_mm = 100; + uint32_t err_flags = 0; + uint8_t sensor_valid = 0xFF; // All valid + + // Date + uint16_t year = 2025; + uint16_t month = 12; + uint8_t day = 7; + + // --- 2. PACKING (Big Endian - 69 Bytes) --- + uint8_t pkt[69]; + + // Bytes 0-3: Num Sats (4B) + put_be32_ptr(&pkt[0], num_sats); + + // Bytes 4-7: Error Flags (4B) + put_be32_ptr(&pkt[4], err_flags); + + // Byte 8: Sensor Valid (1B) + pkt[8] = sensor_valid; + + // Bytes 9-12: Latitude (4B) + put_be32_ptr(&pkt[9], (uint32_t)lat); + + // Bytes 13-16: Longitude (4B) + put_be32_ptr(&pkt[13], (uint32_t)lon); + + // Bytes 17-28: Velocity N, E, D (12B) + put_be32_ptr(&pkt[17], (uint32_t)vel_n); + put_be32_ptr(&pkt[21], (uint32_t)vel_e); + put_be32_ptr(&pkt[25], (uint32_t)vel_d); + + // Bytes 29-32: Altitude MSL (4B) + put_be32_ptr(&pkt[29], (uint32_t)alt_mm); + + // Byte 33: Alignment Status (1B) + pkt[33] = align_status; + + // Bytes 34-37: Time iTOW (4B) + put_be32_ptr(&pkt[34], itow); + + // 38-39: GNSS Fix + pkt[38] = gnss1_fix; + pkt[39] = gnss2_fix; + + // 40-44: UTC Date/Time + put_be16_ptr(&pkt[40], year); + put_be16_ptr(&pkt[42], month); + pkt[44] = day; + + // 45-68: Accuracy Metrics (mm or mm/s) + put_be32_ptr(&pkt[45], acc_lat_mm); + put_be32_ptr(&pkt[49], acc_lon_mm); + put_be32_ptr(&pkt[53], acc_vn_mm); + put_be32_ptr(&pkt[57], acc_ve_mm); + put_be32_ptr(&pkt[61], acc_vd_mm); + put_be32_ptr(&pkt[65], acc_vd_pos_mm); + + // --- LOGGING PROBE (SIM SIDE) --- + // if (sim_log_counter % 100 == 0) { + // fprintf(stderr, "[SIM-OUT] INS Packet (iTOW: %u)\n", itow); + // fprintf(stderr, " Pos: Lat=%d Lon=%d Alt=%d mm\n", lat, lon, alt_mm); + // fprintf(stderr, " Vel: N=%d E=%d D=%d mm/s\n", vel_n, vel_e, vel_d); + // fprintf(stderr, " Stat: Align=%d Valid=0x%02X\n", align_status, sensor_valid); + // } + + + write_packet(0x02, pkt, sizeof(pkt)); +} + + +void SensAItion::flush_packets() +{ + if (_buffert_cnt > 0) { + write_to_autopilot((const char *)&_buffert, _buffert_cnt); + _buffert_cnt = 0; + } +} + +void SensAItion::write_to_autopilot_buf(const char *data, int length) +{ + memcpy(&_buffert[_buffert_cnt], data, length); + _buffert_cnt += length; +} + +void SensAItion::write_packet(uint8_t msg_id, const uint8_t* payload, uint16_t length) +{ + const uint8_t header = 0xFA; + write_to_autopilot_buf((const char *)&header, 1); + write_to_autopilot_buf((const char *)&msg_id, 1); + write_to_autopilot_buf((const char *)payload, length); + uint8_t crc = (uint8_t)calculate_crc(msg_id, payload, length, true); + write_to_autopilot_buf((const char *)&crc, 1); +} + +void SensAItion::write_legacy_packet(const uint8_t* payload, uint16_t length) +{ + const uint8_t header = 0xFA; + write_to_autopilot_buf((const char *)&header, 1); + write_to_autopilot_buf((const char *)payload, length); + uint8_t crc = (uint8_t)calculate_crc(0, payload, length, false); + write_to_autopilot_buf((const char *)&crc, 1); +} + +uint16_t SensAItion::calculate_crc(uint8_t msg_id, const uint8_t* payload, uint16_t length, bool use_id) +{ + uint8_t crc = 0; + if (use_id) { + crc ^= msg_id; + } + for (uint16_t i = 0; i < length; i++) { + crc ^= payload[i]; + } + return crc; +} + +#endif // AP_SIM_EAHRS_SENSATION_ENABLED \ No newline at end of file diff --git a/libraries/SITL/SIM_SensAItion.h b/libraries/SITL/SIM_SensAItion.h new file mode 100644 index 00000000000..6f95d440e47 --- /dev/null +++ b/libraries/SITL/SIM_SensAItion.h @@ -0,0 +1,67 @@ +/* + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . +*/ +/* + Simulate SensAItion serial IMU/AHRS device + Generates high-rate sensor data for ArduPilot testing + +*/ + +#pragma once + +#include "SIM_SerialDevice.h" +#include + +namespace SITL +{ + +class SensAItion : public SerialDevice +{ +public: + SensAItion(bool interleved_mode); + void update(void); + +private: + int _tick = 0; + int _periodMessage0 = 5; + int _phaseMessage0 = 0; + int _periodMessage1 = 100; + int _phaseMessage1 = 0; + int _periodMessage2 = 100; + int _phaseMessage2 = 5; + + uint8_t _buffert[512]; + int _buffert_cnt = 0; + + + void send_packet_0_imu(const struct sitl_fdm &fdm); + void send_packet_1_orientation(const struct sitl_fdm &fdm); + void send_packet_2_ins(const struct sitl_fdm &fdm); + uint32_t calculate_itow(uint64_t now_us, uint32_t start_time_utc); + + + void flush_packets(); + void write_to_autopilot_buf(const char *data, int length); + + void write_packet(uint8_t msg_id, const uint8_t* payload, uint16_t length); + void write_legacy_packet(const uint8_t* payload, uint16_t length); + uint16_t calculate_crc(uint8_t msg_id, const uint8_t* payload, uint16_t length, bool use_id); + + uint32_t last_update_us = 0; + uint32_t tick_count = 0; + + bool _interleaved_mode = false; // Now mutable, Shall be driven by EAHRS_OPTIONS. TODO +}; + +} // namespace SITL diff --git a/libraries/SITL/SIM_config.h b/libraries/SITL/SIM_config.h index 3af5337ff1e..aee22e4a2da 100644 --- a/libraries/SITL/SIM_config.h +++ b/libraries/SITL/SIM_config.h @@ -183,6 +183,9 @@ #define AP_SIM_GPS_UBLOX_ENABLED AP_SIM_GPS_BACKEND_DEFAULT_ENABLED #endif +#ifndef AP_SIM_EAHRS_SENSATION_ENABLED +#define AP_SIM_EAHRS_SENSATION_ENABLED (CONFIG_HAL_BOARD == HAL_BOARD_SITL) +#endif // simulated compass support; currently only in SITL, not SimOnHW: #ifndef AP_SIM_COMPASS_BACKEND_DEFAULT_ENABLED