Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-21 15:23:57 -04:00
Code Issues Packages Projects Releases Wiki Activity

SITL: Switch to using GPS Timestamp for 5 series driver

Browse Source 
* The old descriptor is deprecated

Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com>
This commit is contained in:
Ryan Friedman 2023-11-20 19:17:20 -07:00 committed by Andrew Tridgell
parent bcd11701fa
commit 1ae13cd66d
3 changed files with 207 additions and 40 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

56
libraries/SITL/SIM_MicroStrain.cpp
View File

@ -13,7 +13,7 @@
along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
/*
simulate LORD MicroStrain serial device
simulate MicroStrain GNSS-INS devices
*/
#include "SIM_MicroStrain.h"
#include <stdio.h>
@ -24,15 +24,12 @@
using namespace SITL;
MicroStrain5::MicroStrain5() :SerialDevice::SerialDevice()
MicroStrain::MicroStrain() :SerialDevice::SerialDevice()
{
}
/*
get timeval using simulation time
*/
static void simulation_timeval(struct timeval *tv)
void MicroStrain::simulation_timeval(struct timeval *tv)
{
uint64_t now = AP_HAL::micros64();
static uint64_t first_usec;
@ -48,7 +45,7 @@ static void simulation_timeval(struct timeval *tv)
tv->tv_usec = new_usec % 1000000ULL;
}
void MicroStrain5::generate_checksum(MicroStrain_Packet& packet)
void MicroStrain::generate_checksum(MicroStrain_Packet& packet)
{
uint8_t checksumByte1 = 0;
uint8_t checksumByte2 = 0;
@ -67,7 +64,7 @@ void MicroStrain5::generate_checksum(MicroStrain_Packet& packet)
packet.checksum[1] = checksumByte2;
}
void MicroStrain5::send_packet(MicroStrain_Packet packet)
void MicroStrain::send_packet(MicroStrain_Packet packet)
{
generate_checksum(packet);
@ -77,7 +74,7 @@ void MicroStrain5::send_packet(MicroStrain_Packet packet)
}
void MicroStrain5::send_imu_packet(void)
void MicroStrain::send_imu_packet(void)
{
const auto &fdm = _sitl->state;
MicroStrain_Packet packet;
@ -135,7 +132,6 @@ void MicroStrain5::send_imu_packet(void)
send_packet(packet);
}
void MicroStrain5::send_gnss_packet(void)
{
const auto &fdm = _sitl->state;
@ -148,9 +144,9 @@ void MicroStrain5::send_gnss_packet(void)
packet.header[1] = 0x65; // Sync Two
packet.header[2] = 0x81; // GNSS Descriptor
// Add GPS Time
// Add GPS Timestamp
packet.payload[packet.payload_size++] = 0x0E; // GPS Time Field Size
packet.payload[packet.payload_size++] = 0x09; // Descriptor
packet.payload[packet.payload_size++] = 0xD3; // Descriptor
put_double(packet, (double) tv.tv_sec);
put_int(packet, tv.tv_usec / (AP_MSEC_PER_WEEK * 1000000ULL));
put_int(packet, 0);
@ -205,6 +201,8 @@ void MicroStrain5::send_gnss_packet(void)
send_packet(packet);
}
void MicroStrain5::send_filter_packet(void)
{
const auto &fdm = _sitl->state;
@ -217,9 +215,9 @@ void MicroStrain5::send_filter_packet(void)
packet.header[1] = 0x65; // Sync Two
packet.header[2] = 0x82; // Filter Descriptor
// Add Filter Time
packet.payload[packet.payload_size++] = 0x0E; // Filter Time Field Size
packet.payload[packet.payload_size++] = 0x11; // Descriptor
// Add GPS Timestamp Shared Data
packet.payload[packet.payload_size++] = 0x0E; // GPS Timestamp Field Size
packet.payload[packet.payload_size++] = 0xD3; // Descriptor
put_double(packet, (double) tv.tv_usec / 1e6);
put_int(packet, tv.tv_usec / (AP_MSEC_PER_WEEK * 1000000ULL));
put_int(packet, 0x0001);
@ -256,34 +254,34 @@ void MicroStrain5::send_filter_packet(void)
/*
send MicroStrain data
*/
void MicroStrain5::update(void)
void MicroStrain::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 ms_between_imu_packets = 40;
uint32_t ms_between_gnss_packets = 500;
uint32_t ms_between_filter_packets = 40;
uint32_t now = AP_HAL::micros();
if (now - last_imu_pkt_us >= us_between_imu_packets) {
last_imu_pkt_us = now;
uint32_t now = AP_HAL::millis();
if (now - last_imu_pkt_ms >= ms_between_imu_packets) {
last_imu_pkt_ms = now;
send_imu_packet();
}
if (now - last_gnss_pkt_us >= us_between_gnss_packets) {
last_gnss_pkt_us = now;
if (now - last_gnss_pkt_ms >= ms_between_gnss_packets) {
last_gnss_pkt_ms = now;
send_gnss_packet();
}
if (now - last_filter_pkt_us >= us_between_filter_packets) {
last_filter_pkt_us = now;
if (now - last_filter_pkt_ms >= ms_between_filter_packets) {
last_filter_pkt_ms = now;
send_filter_packet();
}
}
void MicroStrain5::put_float(MicroStrain_Packet &packet, float f)
void MicroStrain::put_float(MicroStrain_Packet &packet, float f)
{
uint32_t fbits = 0;
memcpy(&fbits, &f, sizeof(fbits));
@ -291,7 +289,7 @@ void MicroStrain5::put_float(MicroStrain_Packet &packet, float f)
packet.payload_size += sizeof(float);
}
void MicroStrain5::put_double(MicroStrain_Packet &packet, double d)
void MicroStrain::put_double(MicroStrain_Packet &packet, double d)
{
uint64_t dbits = 0;
memcpy(&dbits, &d, sizeof(dbits));
@ -299,7 +297,7 @@ void MicroStrain5::put_double(MicroStrain_Packet &packet, double d)
packet.payload_size += sizeof(double);
}
void MicroStrain5::put_int(MicroStrain_Packet &packet, uint16_t t)
void MicroStrain::put_int(MicroStrain_Packet &packet, uint16_t t)
{
put_be16_ptr(&packet.payload[packet.payload_size], t);
packet.payload_size += sizeof(uint16_t);

44
libraries/SITL/SIM_MicroStrain.h
View File

@ -4,9 +4,9 @@
//PARAMS:
// 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:MicroStrain
// param set SERIAL3_PROTOCOL 36
// param set SERIAL3_BAUD 115
// sim_vehicle.py -v Plane -A "--serial3=sim:MicroStrain7" --console --map -DG
#pragma once
#include "SIM_Aircraft.h"
@ -17,16 +17,17 @@
namespace SITL
{
class MicroStrain5 : public SerialDevice
class MicroStrain : public SerialDevice
{
// This class implements the common MicroStrain driver support.
public:
MicroStrain5();
MicroStrain();
// update state
void update(void);
private:
protected:
struct MicroStrain_Packet {
uint8_t header[4];
uint8_t payload[256];
@ -35,23 +36,44 @@ private:
size_t payload_size = 0;
};
uint32_t last_imu_pkt_us;
uint32_t last_gnss_pkt_us;
uint32_t last_filter_pkt_us;
uint32_t last_imu_pkt_ms;
uint32_t last_gnss_pkt_ms;
uint32_t last_filter_pkt_ms;
void generate_checksum(MicroStrain_Packet&);
void send_packet(MicroStrain_Packet);
void send_imu_packet();
void send_gnss_packet();
void send_filter_packet();
virtual void send_gnss_packet() = 0;
virtual void send_filter_packet() = 0;
void put_float(MicroStrain_Packet&, float);
void put_double(MicroStrain_Packet&, double);
void put_int(MicroStrain_Packet&, uint16_t);
// get timeval using simulation time
static void simulation_timeval(struct timeval *tv);
uint64_t start_us;
};
class MicroStrain5 : public MicroStrain
{
// This is a specialization for the 3DM-GX5-GNSS/INS
private:
void send_gnss_packet() override;
void send_filter_packet() override;
};
class MicroStrain7 : public MicroStrain
{
// This is a specialization for the 3DM-GQ7-GNSS/INS
private:
void send_gnss_packet() override;
void send_filter_packet() override;
};
}

147
libraries/SITL/SIM_MicroStrain7.cpp Normal file
View File

@ -0,0 +1,147 @@
/*
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 MicroStrain 7-series GNSS-INS devices
*/
#include "SIM_MicroStrain.h"
using namespace SITL;
void MicroStrain7::send_gnss_packet(void)
{
const auto &fdm = _sitl->state;
constexpr uint8_t descriptors[2] = {0x91, 0x92};
for (uint8_t i = 0; i < ARRAY_SIZE(descriptors); i++) {
MicroStrain_Packet packet;
struct timeval tv;
simulation_timeval(&tv);
packet.header[0] = 0x75; // Sync One
packet.header[1] = 0x65; // Sync Two
packet.header[2] = descriptors[i]; // GNSS Descriptor
// Add GPS Timestamp
// https://s3.amazonaws.com/files.microstrain.com/GQ7+User+Manual/external_content/dcp/Data/shared_data/data/mip_field_shared_gps_timestamp.htm
packet.payload[packet.payload_size++] = 0x0E; // GPS Time Field Size
packet.payload[packet.payload_size++] = 0xD3; // 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
// https://s3.amazonaws.com/files.microstrain.com/GQ7+User+Manual/external_content/dcp/Data/gnss_recv_1/data/mip_field_gnss_fix_info.htm
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 FIX_3D
packet.payload[packet.payload_size++] = 19; // Sat count
put_int(packet, 0); // Fix flags
put_int(packet, 0); // Valid flags
// Add GNSS LLH position
// https://s3.amazonaws.com/files.microstrain.com/GQ7+User+Manual/external_content/dcp/Data/gnss_recv_1/data/mip_field_gnss_llh_pos.htm
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
// Add DOP Data
// https://s3.amazonaws.com/files.microstrain.com/GQ7+User+Manual/external_content/dcp/Data/gnss_recv_1/data/mip_field_gnss_dop.htm
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
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; // 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
packet.header[3] = packet.payload_size;
send_packet(packet);
}
}
void MicroStrain7::send_filter_packet(void)
{
const auto &fdm = _sitl->state;
MicroStrain_Packet packet;
struct timeval tv;
simulation_timeval(&tv);
packet.header[0] = 0x75; // Sync One
packet.header[1] = 0x65; // Sync Two
packet.header[2] = 0x82; // Filter Descriptor
// Add GPS Timestamp Shared Data
packet.payload[packet.payload_size++] = 0x0E; // GPS Timestamp Field Size
packet.payload[packet.payload_size++] = 0xD3; // 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; // 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; // 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
// Add Filter State
// https://s3.amazonaws.com/files.microstrain.com/GQ7+User+Manual/external_content/dcp/Data/filter_data/data/mip_field_filter_status.htm
packet.payload[packet.payload_size++] = 0x08; // Filter State Field Size
packet.payload[packet.payload_size++] = 0x10; // Descriptor
put_int(packet, 0x04); // Filter state (GQ7_FULL_NAV)
put_int(packet, 0x03); // Dynamics mode (Airborne)
put_int(packet, 0); // Filter flags (None, no warnings)
packet.header[3] = packet.payload_size;
send_packet(packet);
}