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

SITL: add simulator for LORD Microstrain serial EAHRS

gps-1.3.1
Asa Davis 3 years ago committed by Peter Barker
parent
795e7e53bf
commit
85fbbe0704
4 changed files with 389 additions and 0 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. 15
      libraries/AP_HAL_SITL/SITL_State.cpp
  2. 4
      libraries/AP_HAL_SITL/SITL_State.h
  3. 306
      libraries/SITL/SIM_LORD.cpp
  4. 64
      libraries/SITL/SIM_LORD.h

15
libraries/AP_HAL_SITL/SITL_State.cpp
Unescape View File

@ -379,6 +379,12 @@ int SITL_State::sim_fd(const char *name, const char *arg) @@ -379,6 +379,12 @@ int SITL_State::sim_fd(const char *name, const char *arg)
}
vectornav = new SITL::VectorNav();
return vectornav->fd();
} else if (streq(name, "LORD")) {
if (lord != nullptr) {
AP_HAL::panic("Only one LORD at a time");
}
lord = new SITL::LORD();
return lord->fd();
} else if (streq(name, "AIS")) {
if (ais != nullptr) {
AP_HAL::panic("Only one AIS at a time");
@ -507,6 +513,11 @@ int SITL_State::sim_fd_write(const char *name) @@ -507,6 +513,11 @@ int SITL_State::sim_fd_write(const char *name)
AP_HAL::panic("No VectorNav created");
}
return vectornav->write_fd();
} else if (streq(name, "LORD")) {
if (lord == nullptr) {
AP_HAL::panic("No LORD created");
}
return lord->write_fd();
} else if (streq(name, "AIS")) {
if (ais == nullptr) {
AP_HAL::panic("No AIS created");
@ -732,6 +743,10 @@ void SITL_State::_fdm_input_local(void) @@ -732,6 +743,10 @@ void SITL_State::_fdm_input_local(void)
vectornav->update();
}
if (lord != nullptr) {
lord->update();
}
if (ais != nullptr) {
ais->update();
}

4
libraries/AP_HAL_SITL/SITL_State.h
Unescape View File

@ -44,6 +44,7 @@ @@ -44,6 +44,7 @@
#include <SITL/SIM_RF_MAVLink.h>
#include <SITL/SIM_RF_GYUS42v2.h>
#include <SITL/SIM_VectorNav.h>
#include <SITL/SIM_LORD.h>
#include <SITL/SIM_AIS.h>
#include <SITL/SIM_Frsky_D.h>
@ -310,6 +311,9 @@ private: @@ -310,6 +311,9 @@ private:
// simulated VectorNav system:
SITL::VectorNav *vectornav;
// simulated LORD Microstrain system
SITL::LORD *lord;
// Ride along instances via JSON SITL backend
SITL::JSON_Master ride_along;

306
libraries/SITL/SIM_LORD.cpp
Unescape View File

@ -0,0 +1,306 @@ @@ -0,0 +1,306 @@
/*
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 LORD Microstrain serial device
*/
#include "SIM_LORD.h"
#include <stdio.h>
#include <unistd.h>
#include <fcntl.h>
#include <AP_HAL/utility/sparse-endian.h>
#include <GCS_MAVLink/GCS.h>
using namespace SITL;
LORD::LORD() :SerialDevice::SerialDevice()
{
}
/*
get timeval using simulation time
*/
static void simulation_timeval(struct timeval *tv)
{
uint64_t now = AP_HAL::micros64();
static uint64_t first_usec;
static struct timeval first_tv;
if (first_usec == 0) {
first_usec = now;
first_tv.tv_sec = AP::sitl()->start_time_UTC;
}
*tv = first_tv;
tv->tv_sec += now / 1000000ULL;
uint64_t new_usec = tv->tv_usec + (now % 1000000ULL);
tv->tv_sec += new_usec / 1000000ULL;
tv->tv_usec = new_usec % 1000000ULL;
}
void LORD::generate_checksum(LORD_Packet& packet)
{
uint8_t checksumByte1 = 0;
uint8_t checksumByte2 = 0;
for (int i = 0; i < 4; i++) {
checksumByte1 += packet.header[i];
checksumByte2 += checksumByte1;
}
for (int i = 0; i < packet.header[3]; i++) {
checksumByte1 += packet.payload[i];
checksumByte2 += checksumByte1;
}
packet.checksum[0] = checksumByte1;
packet.checksum[1] = checksumByte2;
}
void LORD::send_packet(LORD_Packet packet)
{
generate_checksum(packet);
write_to_autopilot((char *)&packet.header, sizeof(packet.header));
write_to_autopilot((char *)&packet.payload, packet.payload_size);
write_to_autopilot((char *)&packet.checksum, sizeof(packet.checksum));
}
void LORD::send_imu_packet(void)
{
const auto &fdm = _sitl->state;
LORD_Packet packet;
struct timeval tv;
simulation_timeval(&tv);
if (start_us == 0) {
start_us = tv.tv_usec * 1000;
}
packet.header[0] = 0x75;
packet.header[1] = 0x65;
packet.header[2] = 0x80;
// Add ambient pressure field
packet.payload[packet.payload_size++] = 0x06;
packet.payload[packet.payload_size++] = 0x17;
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;
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;
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;
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;
put_float(packet, fdm.quaternion.q1);
put_float(packet, fdm.quaternion.q2);
put_float(packet, fdm.quaternion.q3);
put_float(packet, fdm.quaternion.q4);
packet.header[3] = packet.payload_size;
send_packet(packet);
}
void LORD::send_gnss_packet(void)
{
const auto &fdm = _sitl->state;
LORD_Packet packet;
struct timeval tv;
simulation_timeval(&tv);
packet.header[0] = 0x75;
packet.header[1] = 0x65;
packet.header[2] = 0x81;
// Add ambient GPS Time
packet.payload[packet.payload_size++] = 0x0E;
packet.payload[packet.payload_size++] = 0x09;
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++] = 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;
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
// Add DOP Data
packet.payload[packet.payload_size++] = 0x20;
packet.payload[packet.payload_size++] = 0x07;
put_float(packet, 0); // GDOP
put_float(packet, 0); // PDOP
put_float(packet, 0); // HDOP
put_float(packet, 0); // VDOP
put_float(packet, 0); // TDOP
put_float(packet, 0); // NDOP
put_float(packet, 0); // EDOP
put_int(packet, 127);
// Add GNSS NED velocity
packet.payload[packet.payload_size++] = 0x24;
packet.payload[packet.payload_size++] = 0x05;
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
packet.header[3] = packet.payload_size;
send_packet(packet);
}
void LORD::send_filter_packet(void)
{
const auto &fdm = _sitl->state;
LORD_Packet packet;
struct timeval tv;
simulation_timeval(&tv);
packet.header[0] = 0x75;
packet.header[1] = 0x65;
packet.header[2] = 0x82;
// Add ambient Filter Time
packet.payload[packet.payload_size++] = 0x0E;
packet.payload[packet.payload_size++] = 0x11;
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;
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;
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
packet.header[3] = packet.payload_size;
send_packet(packet);
}
/*
send LORD data
*/
void LORD::update(void)
{
if (!init_sitl_pointer()) {
return;
}
uint32_t us_between_imu_packets = 20000;
uint32_t us_between_gnss_packets = 250000;
uint32_t us_between_filter_packets = 100000;
uint32_t now = AP_HAL::micros();
if (now - last_imu_pkt_us >= us_between_imu_packets) {
last_imu_pkt_us = now;
send_imu_packet();
}
if (now - last_gnss_pkt_us >= us_between_gnss_packets) {
last_gnss_pkt_us = now;
send_gnss_packet();
}
if (now - last_filter_pkt_us >= us_between_filter_packets) {
last_filter_pkt_us = now;
send_filter_packet();
}
}
void LORD::put_float(LORD_Packet &packet, float f)
{
uint32_t fbits = 0;
memcpy(&fbits, &f, sizeof(fbits));
put_be32_ptr(&packet.payload[packet.payload_size], fbits);
packet.payload_size += sizeof(float);
}
void LORD::put_double(LORD_Packet &packet, double d)
{
uint64_t dbits = 0;
memcpy(&dbits, &d, sizeof(dbits));
put_be64_ptr(&packet.payload[packet.payload_size], dbits);
packet.payload_size += sizeof(double);
}
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);
}

64
libraries/SITL/SIM_LORD.h
Unescape View File

@ -0,0 +1,64 @@ @@ -0,0 +1,64 @@
//
// Created by asa on 9/23/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
#pragma once
#include "SIM_Aircraft.h"
#include <SITL/SITL.h>
#include "SIM_SerialDevice.h"
namespace SITL
{
class LORD : public SerialDevice
{
public:
LORD();
// update state
void update(void);
private:
struct LORD_Packet {
uint8_t header[4];
uint8_t payload[256];
uint8_t checksum[2];
size_t payload_size = 0;
};
uint32_t last_imu_pkt_us;
uint32_t last_gnss_pkt_us;
uint32_t last_filter_pkt_us;
void generate_checksum(LORD_Packet&);
void send_packet(LORD_Packet);
void send_imu_packet();
void send_gnss_packet();
void send_filter_packet();
void put_float(LORD_Packet&, float);
void put_double(LORD_Packet&, double);
void put_int(LORD_Packet&, uint16_t);
uint64_t start_us;
};
}
Write Preview
Loading…
Cancel
Save