From 3e748d77c92e556a982b0347ae69a149d155bff1 Mon Sep 17 00:00:00 2001 From: Davis Schenkenberger Date: Thu, 30 Sep 2021 02:35:51 -0600 Subject: [PATCH] Tools: autotest: add test for LORD EAHRS --- .../{ExternalAHRS => LordEAHRS}/ap1.txt | 0 .../ArduPlane_Tests/VectorNavEAHRS/ap1.txt | 7 ++ Tools/autotest/arduplane.py | 24 ++-- libraries/SITL/SIM_LORD.cpp | 109 +++++++++--------- libraries/SITL/SIM_LORD.h | 23 ++-- 5 files changed, 87 insertions(+), 76 deletions(-) rename Tools/autotest/ArduPlane_Tests/{ExternalAHRS => LordEAHRS}/ap1.txt (100%) create mode 100644 Tools/autotest/ArduPlane_Tests/VectorNavEAHRS/ap1.txt diff --git a/Tools/autotest/ArduPlane_Tests/ExternalAHRS/ap1.txt b/Tools/autotest/ArduPlane_Tests/LordEAHRS/ap1.txt similarity index 100% rename from Tools/autotest/ArduPlane_Tests/ExternalAHRS/ap1.txt rename to Tools/autotest/ArduPlane_Tests/LordEAHRS/ap1.txt diff --git a/Tools/autotest/ArduPlane_Tests/VectorNavEAHRS/ap1.txt b/Tools/autotest/ArduPlane_Tests/VectorNavEAHRS/ap1.txt new file mode 100644 index 0000000000..ab2be0d1a4 --- /dev/null +++ b/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 diff --git a/Tools/autotest/arduplane.py b/Tools/autotest/arduplane.py index 97ce49206e..50deb3bfb3 100644 --- a/Tools/autotest/arduplane.py +++ b/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", - "Test external AHRS support", - self.fly_external_AHRS), + ("VectorNavEAHRS", + "Test VectorNav EAHRS support", + self.test_vectornav), + + ("LordEAHRS", + "Test LORD Microstrain EAHRS support", + self.test_lord), ("Deadreckoning", "Test deadreckoning support", diff --git a/libraries/SITL/SIM_LORD.cpp b/libraries/SITL/SIM_LORD.cpp index 1ec12bc835..af26dd4763 100644 --- a/libraries/SITL/SIM_LORD.cpp +++ b/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[1] = 0x65; - packet.header[2] = 0x80; + packet.header[0] = 0x75; // Sync One + packet.header[1] = 0x65; // Sync Two + packet.header[2] = 0x80; // INS Descriptor // Add ambient pressure field - packet.payload[packet.payload_size++] = 0x06; - packet.payload[packet.payload_size++] = 0x17; + packet.payload[packet.payload_size++] = 0x06; // Ambient Pressure Field Size + 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++] = 0x06; + packet.payload[packet.payload_size++] = 0x0E; // Scaled Magnetometer Field Size + 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 - packet.payload[packet.payload_size++] = 0x0E; - packet.payload[packet.payload_size++] = 0x04; + // Add scaled accelerometer field + packet.payload[packet.payload_size++] = 0x0E; // Scaled Accelerometer Field Size + 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++] = 0x05; + packet.payload[packet.payload_size++] = 0x0E; // Scaled Gyro Field Size + 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++] = 0x0A; + packet.payload[packet.payload_size++] = 0x12; // CF Quaternion Field Size + 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,39 +144,39 @@ void LORD::send_gnss_packet(void) struct timeval tv; simulation_timeval(&tv); - packet.header[0] = 0x75; - packet.header[1] = 0x65; - packet.header[2] = 0x81; + packet.header[0] = 0x75; // Sync One + packet.header[1] = 0x65; // Sync Two + packet.header[2] = 0x81; // GNSS Descriptor - // Add ambient GPS Time - packet.payload[packet.payload_size++] = 0x0E; - packet.payload[packet.payload_size++] = 0x09; + // Add GPS Time + packet.payload[packet.payload_size++] = 0x0E; // GPS Time Field Size + 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++] = 0x0B; + packet.payload[packet.payload_size++] = 0x08; // GNSS Fix Field Size + 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++] = 0x03; + packet.payload[packet.payload_size++] = 0x2C; // GNSS LLH Field Size + 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 put_double(packet, fdm.altitude); put_float(packet, 0.5f); // Horizontal accuracy put_float(packet, 0.5f); // Vertical accuracy - put_int(packet, 31); // Valid flags + put_int(packet, 31); // Valid flags // Add DOP Data - packet.payload[packet.payload_size++] = 0x20; - packet.payload[packet.payload_size++] = 0x07; + packet.payload[packet.payload_size++] = 0x20; // DOP Field Size + packet.payload[packet.payload_size++] = 0x07; // Descriptor put_float(packet, 0); // GDOP put_float(packet, 0); // PDOP put_float(packet, 0); // HDOP @@ -187,17 +187,17 @@ 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++] = 0x05; + packet.payload[packet.payload_size++] = 0x24; // GNSS NED Velocity Field Size + packet.payload[packet.payload_size++] = 0x05; // Descriptor put_float(packet, fdm.speedN); put_float(packet, fdm.speedE); put_float(packet, fdm.speedD); - put_float(packet, 0); //speed - unused - put_float(packet, 0); //ground speed - unused - put_float(packet, 0); //heading - unused - put_float(packet, 0.25f); //speed accuracy - put_float(packet, 0); //heading accuracy - unused - put_int(packet, 31); //valid flags + put_float(packet, 0); //speed - unused + put_float(packet, 0); //ground speed - unused + put_float(packet, 0); //heading - unused + put_float(packet, 0.25f); //speed accuracy + put_float(packet, 0); //heading accuracy - unused + put_int(packet, 31); //valid flags packet.header[3] = packet.payload_size; @@ -213,39 +213,39 @@ void LORD::send_filter_packet(void) struct timeval tv; simulation_timeval(&tv); - packet.header[0] = 0x75; - packet.header[1] = 0x65; - packet.header[2] = 0x82; + packet.header[0] = 0x75; // Sync One + packet.header[1] = 0x65; // Sync Two + packet.header[2] = 0x82; // Filter Descriptor - // Add ambient Filter Time - packet.payload[packet.payload_size++] = 0x0E; - packet.payload[packet.payload_size++] = 0x11; + // Add Filter Time + packet.payload[packet.payload_size++] = 0x0E; // Filter Time Field Size + 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++] = 0x02; + packet.payload[packet.payload_size++] = 0x10; // GNSS Velocity Field Size + 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++] = 0x01; + packet.payload[packet.payload_size++] = 0x1C; // Filter LLH Field Size + 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 + put_double(packet, 0); // Height above ellipsoid - unused + put_int(packet, 0x0001); // Valid flags - packet.payload[packet.payload_size++] = 0x08; - packet.payload[packet.payload_size++] = 0x10; - put_int(packet, 0x02); // Filter state - put_int(packet, 0x03); // Dynamics mode - put_int(packet, 0); // Filter flags + // Add Filter State + packet.payload[packet.payload_size++] = 0x08; // Filter State Field Size + packet.payload[packet.payload_size++] = 0x10; // Descriptor + put_int(packet, 0x02); // Filter state (Running, Solution Valid) + put_int(packet, 0x03); // Dynamics mode (Airborne) + put_int(packet, 0); // Filter flags (None, no warnings) packet.header[3] = packet.payload_size; @@ -303,4 +303,5 @@ 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); -} \ No newline at end of file +} + diff --git a/libraries/SITL/SIM_LORD.h b/libraries/SITL/SIM_LORD.h index 4b0e3bc048..d32001a503 100644 --- a/libraries/SITL/SIM_LORD.h +++ b/libraries/SITL/SIM_LORD.h @@ -1,20 +1,12 @@ -// -// Created by asa on 9/23/21. -// +// Created by Asa Davis and Davis Schenkenberger on 23rd September 21. //usage: //PARAMS: -// AHRS_EKF_TYPE = 11 -// EAHRS_TYPE = 2 -// SERIAL4_PROTOCOL = 36 -// 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 - +// param set AHRS_EKF_TYPE 11 +// param set EAHRS_TYPE 2 +// param set SERIAL4_PROTOCOL 36 +// param set SERIAL4_BAUD 115 +// sim_vehicle.py -v ArduPlane -D --console --map -A --uartE=sim:LORD #pragma once #include "SIM_Aircraft.h" @@ -61,4 +53,5 @@ private: uint64_t start_us; }; -} \ No newline at end of file +} +