SITL: added InertialLabs simulator

libraries/SITL/SIM_GPS.h

@@ -74,13 +74,6 @@ public:
     virtual void update_read();
     // writing fix information to autopilot (e.g.)
     virtual void publish(const GPS_Data *d) = 0;
-    
-protected:
-
-    uint8_t instance;
-    GPS &front;
-
-    class SIM *_sitl;
 
     struct GPS_TOW {
         // Number of weeks since midnight 5-6 January 1980
@@ -90,6 +83,14 @@ protected:
     };
 
     static GPS_TOW gps_time();
+
+protected:
+
+    uint8_t instance;
+    GPS &front;
+
+    class SIM *_sitl;
+
     static void simulation_timeval(struct timeval *tv);
 };
 

libraries/SITL/SIM_InertialLabs.cpp

@@ -0,0 +1,124 @@
+/*
+   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 <http://www.gnu.org/licenses/>.
+ */
+/*
+  simulate InertialLabs external AHRS
+*/
+#include "SIM_InertialLabs.h"
+#include <GCS_MAVLink/GCS.h>
+#include "SIM_GPS.h"
+
+using namespace SITL;
+
+InertialLabs::InertialLabs() : SerialDevice::SerialDevice()
+{
+}
+
+void InertialLabs::send_packet(void)
+{
+    const auto &fdm = _sitl->state;
+
+    pkt.msg_len = sizeof(pkt)-2;
+
+    pkt.accel_data_hr.x = (fdm.yAccel * 1.0e6)/GRAVITY_MSS;
+    pkt.accel_data_hr.y = (fdm.xAccel * 1.0e6)/GRAVITY_MSS;
+    pkt.accel_data_hr.z = (-fdm.zAccel * 1.0e6)/GRAVITY_MSS;
+
+    pkt.gyro_data_hr.y = fdm.rollRate*1.0e5;
+    pkt.gyro_data_hr.x = fdm.pitchRate*1.0e5;
+    pkt.gyro_data_hr.z = -fdm.yawRate*1.0e5;
+
+    float sigma, delta, theta;
+    AP_Baro::SimpleAtmosphere((fdm.altitude+rand_float()*0.25) * 0.001, sigma, delta, theta);
+    pkt.baro_data.pressure_pa2 = SSL_AIR_PRESSURE * delta * 0.5;
+    pkt.baro_data.baro_alt = fdm.altitude;
+    pkt.temperature = KELVIN_TO_C(SSL_AIR_TEMPERATURE * theta);
+
+    pkt.mag_data.x = (fdm.bodyMagField.y / NTESLA_TO_MGAUSS)*0.1;
+    pkt.mag_data.y = (fdm.bodyMagField.x / NTESLA_TO_MGAUSS)*0.1;
+    pkt.mag_data.z = (-fdm.bodyMagField.z / NTESLA_TO_MGAUSS)*0.1;
+
+    pkt.orientation_angles.roll = fdm.rollDeg*100;
+    pkt.orientation_angles.pitch = fdm.pitchDeg*100;
+    pkt.orientation_angles.yaw = fdm.yawDeg*100;
+
+    pkt.velocity.x = fdm.speedE*100;
+    pkt.velocity.y = fdm.speedN*100;
+    pkt.velocity.z = -fdm.speedD*100;
+
+    pkt.position.lat = fdm.latitude*1e7;
+    pkt.position.lon = fdm.longitude*1e7;
+    pkt.position.alt = fdm.altitude*1e2;
+
+    pkt.kf_vel_covariance.x = 10;
+    pkt.kf_vel_covariance.z = 10;
+    pkt.kf_vel_covariance.z = 10;
+
+    pkt.kf_pos_covariance.x = 20;
+    pkt.kf_pos_covariance.z = 20;
+    pkt.kf_pos_covariance.z = 20;
+
+    const auto gps_tow = GPS_Backend::gps_time();
+
+    pkt.gps_ins_time_ms = gps_tow.ms;
+
+    pkt.gnss_new_data = 0;
+
+    if (packets_sent % gps_frequency == 0) {
+        // update GPS data at 5Hz
+        pkt.gps_week = gps_tow.week;
+        pkt.gnss_pos_timestamp = gps_tow.ms;
+        pkt.gnss_new_data = 3;
+        pkt.gps_position.lat = pkt.position.lat;
+        pkt.gps_position.lon = pkt.position.lon;
+        pkt.gps_position.alt = pkt.position.alt;
+
+        pkt.num_sats = 32;
+        pkt.gnss_vel_track.hor_speed = norm(fdm.speedN,fdm.speedE)*100;
+        Vector2d track{fdm.speedN,fdm.speedE};
+        pkt.gnss_vel_track.track_over_ground = wrap_360(degrees(track.angle()))*100;
+        pkt.gnss_vel_track.ver_speed = -fdm.speedD*100;
+
+        pkt.gnss_extended_info.fix_type = 2;
+    }
+
+    pkt.differential_pressure = 0.5*sq(fdm.airspeed+fabsf(rand_float()*0.25))*1.0e4;
+    pkt.supply_voltage = 12.3*100;
+    pkt.temperature = 23.4*10;
+
+    const uint8_t *buffer = (const uint8_t *)&pkt;
+    pkt.crc = crc_sum_of_bytes_16(&buffer[2], sizeof(pkt)-4);
+
+    write_to_autopilot((char *)&pkt, sizeof(pkt));
+
+    packets_sent++;
+}
+
+
+/*
+  send InertialLabs data
+ */
+void InertialLabs::update(void)
+{
+    if (!init_sitl_pointer()) {
+        return;
+    }
+    const uint32_t us_between_packets = 5000; // 200Hz
+    const uint32_t now = AP_HAL::micros();
+    if (now - last_pkt_us >= us_between_packets) {
+        last_pkt_us = now;
+        send_packet();
+    }
+
+}

libraries/SITL/SIM_InertialLabs.h

@@ -0,0 +1,123 @@
+//usage:
+//PARAMS:
+// param set AHRS_EKF_TYPE 11
+// param set EAHRS_TYPE 5
+// param set SERIAL4_PROTOCOL 36
+// param set SERIAL4_BAUD 460800
+// sim_vehicle.py -v ArduPlane -D --console --map -A --uartE=sim:ILabs
+#pragma once
+
+#include "SIM_Aircraft.h"
+
+#include <SITL/SITL.h>
+#include "SIM_SerialDevice.h"
+
+namespace SITL
+{
+
+class InertialLabs : public SerialDevice
+{
+public:
+
+    InertialLabs();
+
+    // update state
+    void update(void);
+
+private:
+    void send_packet(void);
+
+    struct PACKED vec3_16_t {
+        int16_t x,y,z;
+    };
+    struct PACKED vec3_32_t {
+        int32_t x,y,z;
+    };
+    struct PACKED vec3_u8_t {
+        uint8_t x,y,z;
+    };
+    struct PACKED vec3_u16_t {
+        uint16_t x,y,z;
+    };
+
+    struct gnss_extended_info_t {
+        uint8_t fix_type;
+        uint8_t spoofing_status;
+    };
+
+    struct gnss_info_short_t {
+        uint8_t info1;
+        uint8_t info2;
+    };
+
+    struct PACKED ILabsPacket {
+        uint16_t magic = 0x55AA;
+        uint8_t msg_type = 1;
+        uint8_t msg_id = 0x95;
+        uint16_t msg_len; // total packet length-2
+
+        // send Table4, 27 messages
+        uint8_t num_messages = 27;
+        uint8_t messages[27] = {
+            0x01, 0x3C, 0x23, 0x21, 0x25, 0x24, 0x07, 0x12, 0x10, 0x58, 0x57, 0x53, 0x4a,
+            0x3b, 0x30, 0x32, 0x3e, 0x36, 0x41, 0xc0, 0x28, 0x86, 0x8a, 0x8d, 0x50,
+            0x52, 0x5a
+        };
+        uint32_t gps_ins_time_ms; // ms since start of GPS week for IMU data
+        uint16_t gps_week;
+        vec3_32_t accel_data_hr; // g * 1e6
+        vec3_32_t gyro_data_hr; // deg/s * 1e5
+        struct PACKED {
+            uint16_t pressure_pa2; // Pascals/2
+            int32_t baro_alt; // meters*100
+        } baro_data;
+        vec3_16_t mag_data; // nT/10
+        struct PACKED {
+            int16_t yaw; // deg*100
+            int16_t pitch; // deg*100
+            int16_t roll; // deg*100
+        } orientation_angles; // 321 euler order
+        vec3_32_t velocity; // m/s * 100
+        struct PACKED {
+            int32_t lat; // deg*1e7
+            int32_t lon; // deg*1e7
+            int32_t alt; // m*100, AMSL
+        } position;
+        vec3_u8_t kf_vel_covariance; // mm/s
+        vec3_u16_t kf_pos_covariance;
+        uint16_t unit_status;
+        gnss_extended_info_t gnss_extended_info;
+        uint8_t num_sats;
+        struct PACKED {
+            int32_t lat; // deg*1e7
+            int32_t lon; // deg*1e7
+            int32_t alt; // m*100, AMSL
+        } gps_position;
+        struct PACKED {
+            int32_t hor_speed; // m/s*100
+            uint16_t track_over_ground; // deg*100
+            int32_t ver_speed; // m/s*100
+        } gnss_vel_track;
+        uint32_t gnss_pos_timestamp; // ms
+        gnss_info_short_t gnss_info_short;
+        uint8_t gnss_new_data;
+        uint8_t gnss_jam_status;
+        int32_t differential_pressure; // mbar*1e4
+        int16_t true_airspeed; // m/s*100
+        vec3_16_t wind_speed; // m/s*100
+        uint16_t air_data_status;
+        uint16_t supply_voltage; // V*100
+        int16_t temperature; // degC*10
+        uint16_t unit_status2;
+        uint16_t crc;
+    } pkt;
+
+    uint32_t last_pkt_us;
+    const uint16_t pkt_rate_hz = 200;
+    const uint16_t gps_rate_hz = 10;
+    const uint16_t gps_frequency = pkt_rate_hz / gps_rate_hz;
+    uint32_t packets_sent;
+};
+
+}
+