mirror of https://github.com/ArduPilot/ardupilot
Tools: autotest: add test for LORD EAHRS
This commit is contained in:
Davis Schenkenberger
Peter Barker
parent
85fbbe0704
commit
3e748d77c9
|
|
@ -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
|
||||
|
|
@ -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",
|
||||
|
|
|
|||
|
|
@ -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,28 +144,28 @@ 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
|
||||
|
|
@ -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++] = 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,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++] = 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);
|
||||
|
|
@ -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
|
||||
|
||||
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;
|
||||
|
||||
|
|
@ -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);
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -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
|
||||
// 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
|
||||
|
||||
//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:
|
|||
};
|
||||
|
||||
}
|
||||
|
||||
|
|
|
|||
Loading…
Reference in New Issue