Line data    Source code

       1             : /*
       2             :    This program is free software: you can redistribute it and/or modify
       3             :    it under the terms of the GNU General Public License as published by
       4             :    the Free Software Foundation, either version 3 of the License, or
       5             :    (at your option) any later version.
       6             : 
       7             :    This program is distributed in the hope that it will be useful,
       8             :    but WITHOUT ANY WARRANTY; without even the implied warranty of
       9             :    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
      10             :    GNU General Public License for more details.
      11             : 
      12             :    You should have received a copy of the GNU General Public License
      13             :    along with this program.  If not, see <http://www.gnu.org/licenses/>.
      14             :  */
      15             : /*
      16             :     Simulate MicroStrain 7-series GNSS-INS devices
      17             : 
      18             :     Usage:
      19             :     PARAMS:
      20             :         param set AHRS_EKF_TYPE 11
      21             :         param set EAHRS_TYPE 7
      22             :         param set SERIAL3_PROTOCOL 36
      23             :         param set SERIAL3_BAUD 115
      24             :     sim_vehicle.py -v Plane -A "--serial3=sim:MicroStrain7" --console --map -DG
      25             : */
      26             : 
      27             : #include "SIM_MicroStrain.h"
      28             : 
      29             : using namespace SITL;
      30             : 
      31         723 : void MicroStrain7::send_gnss_packet(void)
      32             : {
      33         723 :     const auto &fdm = _sitl->state;
      34             : 
      35         723 :     constexpr uint8_t descriptors[2] = {0x91, 0x92};
      36        2169 :     for (uint8_t i = 0; i < ARRAY_SIZE(descriptors); i++) {
      37        1446 :         MicroStrain_Packet packet;
      38             : 
      39             :         struct timeval tv;
      40        1446 :         simulation_timeval(&tv);
      41             : 
      42        1446 :         packet.header[0] = 0x75; // Sync One
      43        1446 :         packet.header[1] = 0x65; // Sync Two
      44        1446 :         packet.header[2] = descriptors[i]; // GNSS Descriptor
      45             : 
      46             :         // Add GPS Timestamp
      47             :         // https://s3.amazonaws.com/files.microstrain.com/GQ7+User+Manual/external_content/dcp/Data/shared_data/data/mip_field_shared_gps_timestamp.htm
      48        1446 :         packet.payload[packet.payload_size++] = 0x0E; // GPS Time Field Size
      49        1446 :         packet.payload[packet.payload_size++] = 0xD3; // Descriptor
      50        1446 :         put_double(packet, (double) tv.tv_sec);
      51        1446 :         put_int(packet, tv.tv_usec / (AP_MSEC_PER_WEEK * 1000000ULL));
      52        1446 :         put_int(packet, 0);
      53             : 
      54             :         // Add GNSS Fix Information
      55             :         // https://s3.amazonaws.com/files.microstrain.com/GQ7+User+Manual/external_content/dcp/Data/gnss_recv_1/data/mip_field_gnss_fix_info.htm
      56        1446 :         packet.payload[packet.payload_size++] =  0x08; // GNSS Fix Field Size
      57        1446 :         packet.payload[packet.payload_size++] = 0x0B; // Descriptor
      58        1446 :         packet.payload[packet.payload_size++] = 0x00; // Fix type FIX_3D
      59        1446 :         packet.payload[packet.payload_size++] = 19; // Sat count
      60        1446 :         put_int(packet, 0); // Fix flags
      61        1446 :         put_int(packet, 0); // Valid flags
      62             : 
      63             :         // Add GNSS LLH position
      64             :         // https://s3.amazonaws.com/files.microstrain.com/GQ7+User+Manual/external_content/dcp/Data/gnss_recv_1/data/mip_field_gnss_llh_pos.htm
      65        1446 :         packet.payload[packet.payload_size++] = 0x2C; // GNSS LLH Field Size
      66        1446 :         packet.payload[packet.payload_size++] = 0x03; // Descriptor
      67        1446 :         put_double(packet, fdm.latitude);
      68        1446 :         put_double(packet, fdm.longitude);
      69        1446 :         put_double(packet, 0);   // Height above ellipsoid - unused
      70        1446 :         put_double(packet, fdm.altitude);
      71        1446 :         put_float(packet, 0.5f); // Horizontal accuracy
      72        1446 :         put_float(packet, 0.5f); // Vertical accuracy
      73        1446 :         put_int(packet, 31); // Valid flags
      74             : 
      75             :         // Add DOP Data
      76             :         // https://s3.amazonaws.com/files.microstrain.com/GQ7+User+Manual/external_content/dcp/Data/gnss_recv_1/data/mip_field_gnss_dop.htm
      77        1446 :         packet.payload[packet.payload_size++] = 0x20; // DOP Field Size
      78        1446 :         packet.payload[packet.payload_size++] = 0x07; // Descriptor
      79        1446 :         put_float(packet, 0); // GDOP
      80        1446 :         put_float(packet, 0); // PDOP
      81        1446 :         put_float(packet, 0); // HDOP
      82        1446 :         put_float(packet, 0); // VDOP
      83        1446 :         put_float(packet, 0); // TDOP
      84        1446 :         put_float(packet, 0); // NDOP
      85        1446 :         put_float(packet, 0); // EDOP
      86        1446 :         put_int(packet, 127);
      87             : 
      88             :         // Add GNSS NED velocity
      89        1446 :         packet.payload[packet.payload_size++] = 0x24; // GNSS NED Velocity Field Size
      90        1446 :         packet.payload[packet.payload_size++] = 0x05; // Descriptor
      91        1446 :         put_float(packet, fdm.speedN);
      92        1446 :         put_float(packet, fdm.speedE);
      93        1446 :         put_float(packet, fdm.speedD);
      94        1446 :         put_float(packet, 0); //speed - unused
      95        1446 :         put_float(packet, 0); //ground speed - unused
      96        1446 :         put_float(packet, 0); //heading - unused
      97        1446 :         put_float(packet, 0.25f); //speed accuracy
      98        1446 :         put_float(packet, 0); //heading accuracy - unused
      99        1446 :         put_int(packet, 31); //valid flags
     100             : 
     101        1446 :         packet.header[3] = packet.payload_size;
     102             : 
     103        1446 :         send_packet(packet);
     104             :     }
     105             : 
     106         723 : }
     107             : 
     108        9049 : void MicroStrain7::send_filter_packet(void)
     109             : {
     110        9049 :     const auto &fdm = _sitl->state;
     111        9049 :     MicroStrain_Packet packet;
     112             : 
     113             :     struct timeval tv;
     114        9049 :     simulation_timeval(&tv);
     115             : 
     116        9049 :     packet.header[0] = 0x75; // Sync One
     117        9049 :     packet.header[1] = 0x65; // Sync Two
     118        9049 :     packet.header[2] = 0x82; // Filter Descriptor
     119             : 
     120             :     // Add GPS Timestamp Shared Data
     121        9049 :     packet.payload[packet.payload_size++] = 0x0E; // GPS Timestamp Field Size
     122        9049 :     packet.payload[packet.payload_size++] = 0xD3; // Descriptor
     123        9049 :     put_double(packet, (double) tv.tv_usec / 1e6);
     124        9049 :     put_int(packet, tv.tv_usec / (AP_MSEC_PER_WEEK * 1000000ULL));
     125        9049 :     put_int(packet, 0x0001);
     126             : 
     127             :     // Add GNSS Filter velocity
     128        9049 :     packet.payload[packet.payload_size++] = 0x10; // GNSS Velocity Field Size
     129        9049 :     packet.payload[packet.payload_size++] = 0x02; // Descriptor
     130        9049 :     put_float(packet, fdm.speedN);
     131        9049 :     put_float(packet, fdm.speedE);
     132        9049 :     put_float(packet, fdm.speedD);
     133        9049 :     put_int(packet, 0x0001);
     134             : 
     135             :     // Add Filter LLH position
     136        9049 :     packet.payload[packet.payload_size++] = 0x1C; // Filter LLH Field Size
     137        9049 :     packet.payload[packet.payload_size++] = 0x01; // Descriptor
     138        9049 :     put_double(packet, fdm.latitude);
     139        9049 :     put_double(packet, fdm.longitude);
     140        9049 :     put_double(packet, 0); // Height above ellipsoid - unused
     141        9049 :     put_int(packet, 0x0001); // Valid flags
     142             : 
     143             :     // Add Filter State
     144             :     // https://s3.amazonaws.com/files.microstrain.com/GQ7+User+Manual/external_content/dcp/Data/filter_data/data/mip_field_filter_status.htm
     145        9049 :     packet.payload[packet.payload_size++] = 0x08; // Filter State Field Size
     146        9049 :     packet.payload[packet.payload_size++] = 0x10; // Descriptor
     147        9049 :     put_int(packet, 0x04); // Filter state (GQ7_FULL_NAV)
     148        9049 :     put_int(packet, 0x03); // Dynamics mode (Airborne)
     149        9049 :     put_int(packet, 0); // Filter flags (None, no warnings)
     150             : 
     151             :     // Add Attitude Quaternion
     152             :     // https://s3.amazonaws.com/files.microstrain.com/GQ7+User+Manual/external_content/dcp/Data/filter_data/data/mip_field_filter_attitude_quaternion.htm
     153        9049 :     packet.payload[packet.payload_size++] = 0x14; // Attitude Quaternion Field Size
     154        9049 :     packet.payload[packet.payload_size++] = 0x03; // Descriptor
     155        9049 :     put_float(packet, fdm.quaternion.q1);
     156        9049 :     put_float(packet, fdm.quaternion.q2);
     157        9049 :     put_float(packet, fdm.quaternion.q3);
     158        9049 :     put_float(packet, fdm.quaternion.q4);
     159        9049 :     put_int(packet, 0x0001); // Valid flags
     160             : 
     161        9049 :     packet.header[3] = packet.payload_size;
     162             : 
     163        9049 :     send_packet(packet);
     164        9049 : }