Tools: autotest: add test for LORD EAHRS

Tools/autotest/ArduPlane_Tests/VectorNavEAHRS/ap1.txt

@@ -0,0 +1,7 @@
+QGC WPL 110
+0	0	0	16	0.000000	0.000000	0.000000	0.000000	-35.363262	149.165237	584.090027	1
+1	0	3	22	0.000000	0.000000	0.000000	0.000000	-35.361553	149.163956	20.000000	1
+2	0	3	16	0.000000	0.000000	0.000000	0.000000	-35.364540	149.162857	50.000000	1
+3	0	3	16	0.000000	0.000000	0.000000	0.000000	-35.367333	149.163164	28.000000	1
+4	0	3	16	0.000000	0.000000	0.000000	0.000000	-35.366814	149.165878	28.000000	1
+5	0	3	21	0.000000	0.000000	0.000000	1.000000	-35.362947	149.165179	0.000000	1

Tools/autotest/arduplane.py

@@ -2295,11 +2295,11 @@ class AutoTestPlane(AutoTest):
         self.set_parameter("WP_LOITER_RAD", default_rad)
         self.fly_home_land_and_disarm(240)
 
-    def fly_external_AHRS(self):
+    def fly_external_AHRS(self, sim, eahrs_type, mission):
         """Fly with external AHRS (VectorNav)"""
-        self.customise_SITL_commandline(["--uartE=sim:VectorNav"])
+        self.customise_SITL_commandline(["--uartE=sim:%s" % sim])
 
-        self.set_parameter("EAHRS_TYPE", 1)
+        self.set_parameter("EAHRS_TYPE", eahrs_type)
         self.set_parameter("SERIAL4_PROTOCOL", 36)
         self.set_parameter("SERIAL4_BAUD", 230400)
         self.set_parameter("GPS_TYPE", 21)
@@ -2313,7 +2313,13 @@ class AutoTestPlane(AutoTest):
 
         self.wait_ready_to_arm()
         self.arm_vehicle()
-        self.fly_mission("ap1.txt")
+        self.fly_mission(mission)
+
+    def test_vectornav(self):
+        self.fly_external_AHRS("VectorNav", 1, "ap1.txt")
+
+    def test_lord(self):
+        self.fly_external_AHRS("LORD", 2, "ap1.txt")
 
     def get_accelvec(self, m):
         return Vector3(m.xacc, m.yacc, m.zacc) * 0.001 * 9.81
@@ -3319,9 +3325,13 @@ class AutoTestPlane(AutoTest):
              "Test terrain following in loiter",
              self.test_loiter_terrain),
 
-            ("ExternalAHRS",
+            ("VectorNavEAHRS",
-             "Test external AHRS support",
+             "Test VectorNav EAHRS support",
-             self.fly_external_AHRS),
+             self.test_vectornav),
+
+            ("LordEAHRS",
+             "Test LORD Microstrain EAHRS support",
+             self.test_lord),
 
             ("Deadreckoning",
              "Test deadreckoning support",

libraries/SITL/SIM_LORD.cpp

@@ -89,42 +89,42 @@ void LORD::send_imu_packet(void)
         start_us = tv.tv_usec * 1000;
     }
 
-    packet.header[0] = 0x75;
+    packet.header[0] = 0x75; // Sync One
-    packet.header[1] = 0x65;
+    packet.header[1] = 0x65; // Sync Two
-    packet.header[2] = 0x80;
+    packet.header[2] = 0x80; // INS Descriptor
 
     // Add ambient pressure field
-    packet.payload[packet.payload_size++] = 0x06;
+    packet.payload[packet.payload_size++] = 0x06; // Ambient Pressure Field Size
-    packet.payload[packet.payload_size++] = 0x17;
+    packet.payload[packet.payload_size++] = 0x17; // Descriptor
     float sigma, delta, theta;
     AP_Baro::SimpleAtmosphere(fdm.altitude * 0.001f, sigma, delta, theta);
     put_float(packet, SSL_AIR_PRESSURE * delta * 0.001 + rand_float() * 0.1);
 
     // Add scaled magnetometer field
-    packet.payload[packet.payload_size++] = 0x0E;
+    packet.payload[packet.payload_size++] = 0x0E; // Scaled Magnetometer Field Size
-    packet.payload[packet.payload_size++] = 0x06;
+    packet.payload[packet.payload_size++] = 0x06; // Descriptor
     put_float(packet, fdm.bodyMagField.x*0.001);
     put_float(packet, fdm.bodyMagField.y*0.001);
     put_float(packet, fdm.bodyMagField.z*0.001);
 
-    // Add scaled acceletometer field
+    // Add scaled accelerometer field
-    packet.payload[packet.payload_size++] = 0x0E;
+    packet.payload[packet.payload_size++] = 0x0E; // Scaled Accelerometer Field Size
-    packet.payload[packet.payload_size++] = 0x04;
+    packet.payload[packet.payload_size++] = 0x04; // Descriptor
     put_float(packet, fdm.xAccel / GRAVITY_MSS);
     put_float(packet, fdm.yAccel / GRAVITY_MSS);
     put_float(packet, fdm.zAccel / GRAVITY_MSS);
 
     // Add scaled gyro field
     const float gyro_noise = 0.05;
-    packet.payload[packet.payload_size++] = 0x0E;
+    packet.payload[packet.payload_size++] = 0x0E; // Scaled Gyro Field Size
-    packet.payload[packet.payload_size++] = 0x05;
+    packet.payload[packet.payload_size++] = 0x05; // Descriptor
     put_float(packet, radians(fdm.rollRate + rand_float() * gyro_noise));
     put_float(packet, radians(fdm.pitchRate + rand_float() * gyro_noise));
     put_float(packet, radians(fdm.yawRate + rand_float() * gyro_noise));
 
     // Add CF Quaternion field
-    packet.payload[packet.payload_size++] = 0x12;
+    packet.payload[packet.payload_size++] = 0x12; // CF Quaternion Field Size
-    packet.payload[packet.payload_size++] = 0x0A;
+    packet.payload[packet.payload_size++] = 0x0A; // Descriptor
     put_float(packet, fdm.quaternion.q1);
     put_float(packet, fdm.quaternion.q2);
     put_float(packet, fdm.quaternion.q3);
@@ -144,28 +144,28 @@ void LORD::send_gnss_packet(void)
     struct timeval tv;
     simulation_timeval(&tv);
 
-    packet.header[0] = 0x75;
+    packet.header[0] = 0x75; // Sync One
-    packet.header[1] = 0x65;
+    packet.header[1] = 0x65; // Sync Two
-    packet.header[2] = 0x81;
+    packet.header[2] = 0x81; // GNSS Descriptor
 
-    // Add ambient GPS Time
+    // Add GPS Time
-    packet.payload[packet.payload_size++] = 0x0E;
+    packet.payload[packet.payload_size++] = 0x0E; // GPS Time Field Size
-    packet.payload[packet.payload_size++] = 0x09;
+    packet.payload[packet.payload_size++] = 0x09; // Descriptor
     put_double(packet, (double) tv.tv_sec);
     put_int(packet, tv.tv_usec / (AP_MSEC_PER_WEEK * 1000000ULL));
     put_int(packet, 0);
 
     // Add GNSS Fix Information
-    packet.payload[packet.payload_size++] =  0x08;
+    packet.payload[packet.payload_size++] =  0x08; // GNSS Fix Field Size
-    packet.payload[packet.payload_size++] = 0x0B;
+    packet.payload[packet.payload_size++] = 0x0B; // Descriptor
     packet.payload[packet.payload_size++] = 0x00; // Fix type
     packet.payload[packet.payload_size++] = 19; // Sat count
     put_int(packet, 0); // Fix flags
     put_int(packet, 0); // Valid flags
 
     // Add GNSS LLH position
-    packet.payload[packet.payload_size++] = 0x2C;
+    packet.payload[packet.payload_size++] = 0x2C; // GNSS LLH Field Size
-    packet.payload[packet.payload_size++] = 0x03;
+    packet.payload[packet.payload_size++] = 0x03; // Descriptor
     put_double(packet, fdm.latitude);
     put_double(packet, fdm.longitude);
     put_double(packet, 0);   // Height above ellipsoid - unused
@@ -175,8 +175,8 @@ void LORD::send_gnss_packet(void)
     put_int(packet, 31); // Valid flags
 
     // Add DOP Data
-    packet.payload[packet.payload_size++] = 0x20;
+    packet.payload[packet.payload_size++] = 0x20; // DOP Field Size
-    packet.payload[packet.payload_size++] = 0x07;
+    packet.payload[packet.payload_size++] = 0x07; // Descriptor
     put_float(packet, 0); // GDOP
     put_float(packet, 0); // PDOP
     put_float(packet, 0); // HDOP
@@ -187,8 +187,8 @@ void LORD::send_gnss_packet(void)
     put_int(packet, 127);
 
     // Add GNSS NED velocity
-    packet.payload[packet.payload_size++] = 0x24;
+    packet.payload[packet.payload_size++] = 0x24; // GNSS NED Velocity Field Size
-    packet.payload[packet.payload_size++] = 0x05;
+    packet.payload[packet.payload_size++] = 0x05; // Descriptor
     put_float(packet, fdm.speedN);
     put_float(packet, fdm.speedE);
     put_float(packet, fdm.speedD);
@@ -213,39 +213,39 @@ void LORD::send_filter_packet(void)
     struct timeval tv;
     simulation_timeval(&tv);
 
-    packet.header[0] = 0x75;
+    packet.header[0] = 0x75; // Sync One
-    packet.header[1] = 0x65;
+    packet.header[1] = 0x65; // Sync Two
-    packet.header[2] = 0x82;
+    packet.header[2] = 0x82; // Filter Descriptor
 
-    // Add ambient Filter Time
+    // Add Filter Time
-    packet.payload[packet.payload_size++] = 0x0E;
+    packet.payload[packet.payload_size++] = 0x0E; // Filter Time Field Size
-    packet.payload[packet.payload_size++] = 0x11;
+    packet.payload[packet.payload_size++] = 0x11; // Descriptor
     put_double(packet, (double) tv.tv_usec / 1e6);
     put_int(packet, tv.tv_usec / (AP_MSEC_PER_WEEK * 1000000ULL));
     put_int(packet, 0x0001);
 
     // Add GNSS Filter velocity
-    packet.payload[packet.payload_size++] = 0x10;
+    packet.payload[packet.payload_size++] = 0x10; // GNSS Velocity Field Size
-    packet.payload[packet.payload_size++] = 0x02;
+    packet.payload[packet.payload_size++] = 0x02; // Descriptor
     put_float(packet, fdm.speedN);
     put_float(packet, fdm.speedE);
     put_float(packet, fdm.speedD);
     put_int(packet, 0x0001);
 
-
     // Add Filter LLH position
-    packet.payload[packet.payload_size++] = 0x1C;
+    packet.payload[packet.payload_size++] = 0x1C; // Filter LLH Field Size
-    packet.payload[packet.payload_size++] = 0x01;
+    packet.payload[packet.payload_size++] = 0x01; // Descriptor
     put_double(packet, fdm.latitude);
     put_double(packet, fdm.longitude);
     put_double(packet, 0); // Height above ellipsoid - unused
     put_int(packet, 0x0001); // Valid flags
 
-    packet.payload[packet.payload_size++] = 0x08;
+    // Add Filter State
-    packet.payload[packet.payload_size++] = 0x10;
+    packet.payload[packet.payload_size++] = 0x08; // Filter State Field Size
-    put_int(packet, 0x02); // Filter state
+    packet.payload[packet.payload_size++] = 0x10; // Descriptor
-    put_int(packet, 0x03); // Dynamics mode
+    put_int(packet, 0x02); // Filter state (Running, Solution Valid)
-    put_int(packet, 0); // Filter flags
+    put_int(packet, 0x03); // Dynamics mode (Airborne)
+    put_int(packet, 0); // Filter flags (None, no warnings)
 
     packet.header[3] = packet.payload_size;
 
@@ -304,3 +304,4 @@ void LORD::put_int(LORD_Packet &packet, uint16_t t)
     put_be16_ptr(&packet.payload[packet.payload_size], t);
     packet.payload_size += sizeof(uint16_t);
 }
+

libraries/SITL/SIM_LORD.h

@@ -1,20 +1,12 @@
-//
+// Created by Asa Davis and Davis Schenkenberger on 23rd September 21.
-// Created by asa on 9/23/21.
-//
 
 //usage:
 //PARAMS:
-//  AHRS_EKF_TYPE = 11
+// param set AHRS_EKF_TYPE 11
-//  EAHRS_TYPE = 2
+// param set EAHRS_TYPE 2
-//  SERIAL4_PROTOCOL = 36
+// param set SERIAL4_PROTOCOL 36
-//  SERIAL4_BAUD = 115
+// param set SERIAL4_BAUD 115
 // sim_vehicle.py -v ArduPlane -D --console --map -A --uartE=sim:LORD
-
-//debugging:
-//echo 0 | sudo tee /proc/sys/kernel/yama/ptrace_scope
-//sim_vehicle.py -v ArduPlane -D --console --map -A --uartE=sim:LORD
-//then just attach to process and set breakpoints in AP_InertialSensor_SITL to look at raw stat variables
-
 #pragma once
 
 #include "SIM_Aircraft.h"
@@ -62,3 +54,4 @@ private:
 };
 
 }
+