SITL: Switch to using GPS Timestamp for 5 series driver

* The old descriptor is deprecated

Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com>

libraries/SITL/SIM_MicroStrain.cpp

@@ -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()
 {
 }
 
 
-/*
+void MicroStrain::simulation_timeval(struct timeval *tv)
-  get timeval using simulation time
- */
-static void 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
+    // Add GPS Timestamp Shared Data
-    packet.payload[packet.payload_size++] = 0x0E; // Filter Time Field Size
+    packet.payload[packet.payload_size++] = 0x0E; // GPS Timestamp Field Size
-    packet.payload[packet.payload_size++] = 0x11; // Descriptor
+    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 ms_between_imu_packets = 40;
-    uint32_t us_between_gnss_packets = 250000;
+    uint32_t ms_between_gnss_packets = 500;
-    uint32_t us_between_filter_packets = 100000;
+    uint32_t ms_between_filter_packets = 40;
 
-    uint32_t now = AP_HAL::micros();
+    uint32_t now = AP_HAL::millis();
-    if (now - last_imu_pkt_us >= us_between_imu_packets) {
+    if (now - last_imu_pkt_ms >= ms_between_imu_packets) {
-        last_imu_pkt_us = now;
+        last_imu_pkt_ms = now;
         send_imu_packet();
     }
 
-    if (now - last_gnss_pkt_us >= us_between_gnss_packets) {
+    if (now - last_gnss_pkt_ms >= ms_between_gnss_packets) {
-        last_gnss_pkt_us = now;
+        last_gnss_pkt_ms = now;
         send_gnss_packet();
     }
 
-    if (now - last_filter_pkt_us >= us_between_filter_packets) {
+    if (now - last_filter_pkt_ms >= ms_between_filter_packets) {
-        last_filter_pkt_us = now;
+        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);

libraries/SITL/SIM_MicroStrain.h

@@ -4,9 +4,9 @@
 //PARAMS:
 // param set AHRS_EKF_TYPE 11
 // param set EAHRS_TYPE 2
-// param set SERIAL4_PROTOCOL 36
+// param set SERIAL3_PROTOCOL 36
-// param set SERIAL4_BAUD 115
+// param set SERIAL3_BAUD 115
-// sim_vehicle.py -v ArduPlane -D --console --map -A --uartE=sim:MicroStrain
+// 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_imu_pkt_ms;
-    uint32_t last_gnss_pkt_us;
+    uint32_t last_gnss_pkt_ms;
-    uint32_t last_filter_pkt_us;
+    uint32_t last_filter_pkt_ms;
 
     void generate_checksum(MicroStrain_Packet&);
 
     void send_packet(MicroStrain_Packet);
     void send_imu_packet();
-    void send_gnss_packet();
+    virtual void send_gnss_packet() = 0;
-    void send_filter_packet();
+    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;
+
+};
+
 }
 

libraries/SITL/SIM_MicroStrain7.cpp

@@ -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);
+}