zrzk
/
ardupilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

Tools: autotest: add test for LORD EAHRS

gps-1.3.1
Davis Schenkenberger 3 years ago committed by Peter Barker
parent
85fbbe0704
commit
3e748d77c9
5 changed files with 87 additions and 76 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 0
      Tools/autotest/ArduPlane_Tests/LordEAHRS/ap1.txt
  2. 7
      Tools/autotest/ArduPlane_Tests/VectorNavEAHRS/ap1.txt
  3. 24
      Tools/autotest/arduplane.py
  4. 109
      libraries/SITL/SIM_LORD.cpp
  5. 23
      libraries/SITL/SIM_LORD.h

0
Tools/autotest/ArduPlane_Tests/ExternalAHRS/ap1.txt → Tools/autotest/ArduPlane_Tests/LordEAHRS/ap1.txt
Unescape View File

7
Tools/autotest/ArduPlane_Tests/VectorNavEAHRS/ap1.txt
Unescape View File

@ -0,0 +1,7 @@ @@ -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

24
Tools/autotest/arduplane.py
Unescape View File

@ -2295,11 +2295,11 @@ class AutoTestPlane(AutoTest): @@ -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): @@ -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): @@ -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",

109
libraries/SITL/SIM_LORD.cpp
Unescape View File

@ -89,42 +89,42 @@ void LORD::send_imu_packet(void) @@ -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) @@ -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) @@ -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) @@ -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) @@ -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);
}
}

23
libraries/SITL/SIM_LORD.h
Unescape View File

@ -1,20 +1,12 @@ @@ -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: @@ -61,4 +53,5 @@ private:
uint64_t start_us;
};
}
}

Write Preview
Loading…
Cancel
Save