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             :    This program is distributed in the hope that it will be useful,
       7             :    but WITHOUT ANY WARRANTY; without even the implied warranty of
       8             :    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
       9             :    GNU General Public License for more details.
      10             :    You should have received a copy of the GNU General Public License
      11             :    along with this program.  If not, see <http://www.gnu.org/licenses/>.
      12             :  */
      13             : /*
      14             :   support for MicroStrain MIP parsing
      15             :  */
      16             : 
      17             : #pragma once
      18             : 
      19             : #include "AP_ExternalAHRS_config.h"
      20             : 
      21             : #if AP_MICROSTRAIN_ENABLED
      22             : 
      23             : #include <AP_GPS/AP_GPS_FixType.h>
      24             : #include <AP_Math/vector3.h>
      25             : #include <AP_Math/quaternion.h>
      26             : 
      27             : class AP_MicroStrain
      28             : {
      29             : public:
      30             : 
      31             : 
      32             : protected:
      33             : 
      34             :     enum class ParseState {
      35             :         WaitingFor_SyncOne,
      36             :         WaitingFor_SyncTwo,
      37             :         WaitingFor_Descriptor,
      38             :         WaitingFor_PayloadLength,
      39             :         WaitingFor_Data,
      40             :         WaitingFor_Checksum
      41             :     };
      42             : 
      43             :     struct {
      44             :         Vector3f accel;
      45             :         Vector3f gyro;
      46             :         Vector3f mag;
      47             :         Quaternion quat;
      48             :         float pressure;
      49             :     } imu_data;
      50             : 
      51             :     static constexpr uint8_t NUM_GNSS_INSTANCES = 2;
      52             : 
      53             :     struct {
      54             :         uint16_t week;
      55             :         uint32_t tow_ms;
      56             :         AP_GPS_FixType fix_type;
      57             :         uint8_t satellites;
      58             :         float horizontal_position_accuracy;
      59             :         float vertical_position_accuracy;
      60             :         float hdop;
      61             :         float vdop;
      62             :         int32_t lon;
      63             :         int32_t lat;
      64             :         int32_t msl_altitude;
      65             :         float ned_velocity_north;
      66             :         float ned_velocity_east;
      67             :         float ned_velocity_down;
      68             :         float speed_accuracy;
      69             :     } gnss_data[NUM_GNSS_INSTANCES];
      70             : 
      71             :     struct {
      72             :         uint16_t state;
      73             :         uint16_t mode;
      74             :         uint16_t flags;
      75             :     } filter_status;
      76             : 
      77             :     struct {
      78             :         uint16_t week;
      79             :         uint32_t tow_ms;
      80             :         // 1-sigma position uncertainty in the NED local-level frame [meters].
      81             :         Vector3f ned_position_uncertainty;
      82             :         int32_t lon;
      83             :         int32_t lat;
      84             :         int32_t hae_altitude;
      85             :         float ned_velocity_north;
      86             :         float ned_velocity_east;
      87             :         float ned_velocity_down;
      88             :         // 1-sigma velocity uncertainties in the NED local-level frame.
      89             :         Vector3f ned_velocity_uncertainty;
      90             :         // 4x1 vector representation of the quaternion describing the orientation of the device with respect to the NED local-level frame.
      91             :         // NED [Qw, Qx, Qy, Qz]
      92             :         Quaternion attitude_quat;
      93             :     } filter_data;
      94             : 
      95             :     enum class DescriptorSet {
      96             :         BaseCommand = 0x01,
      97             :         DMCommand = 0x0C,
      98             :         SystemCommand = 0x7F,
      99             :         IMUData = 0x80,
     100             :         GNSSData = 0x81,
     101             :         FilterData = 0x82,
     102             :         GNSSRecv1 = 0x91,
     103             :         GNSSRecv2 = 0x92
     104             :     };
     105             : 
     106             :     static constexpr uint8_t SYNC_ONE = 0x75;
     107             :     static constexpr uint8_t SYNC_TWO = 0x65;
     108             : 
     109             :     struct MicroStrain_Packet {
     110             :         uint8_t header[4];
     111             :         uint8_t payload[255];
     112             :         uint8_t checksum[2];
     113             : 
     114             :         // Gets the payload length
     115     5174866 :         uint8_t payload_length() const WARN_IF_UNUSED {
     116     5174866 :             return header[3];
     117             :         }
     118             : 
     119             :         // Sets the payload length
     120       33141 :         void payload_length(const uint8_t len) {
     121       33141 :             header[3] = len;
     122       33141 :         }
     123             : 
     124             :         // Gets the descriptor set
     125       35028 :         DescriptorSet descriptor_set() const WARN_IF_UNUSED {
     126       35028 :             return DescriptorSet(header[2]);
     127             :         }
     128             : 
     129             :         // Sets the descriptor set (without validation)
     130       33141 :         void descriptor_set(const uint8_t descriptor_set) {
     131       33141 :             header[2] = descriptor_set;
     132       33141 :         }
     133             :     };
     134             : 
     135             :     struct {
     136             :         MicroStrain_Packet packet;
     137             :         AP_MicroStrain::ParseState state;
     138             :         uint8_t index;
     139             :     } message_in;
     140             : 
     141             :     uint32_t last_imu_pkt;
     142             :     uint32_t last_gps_pkt;
     143             :     uint32_t last_filter_pkt;
     144             : 
     145             :     // Handle a single byte.
     146             :     // If the byte matches a descriptor, it returns true and that type should be handled.
     147             :     bool handle_byte(const uint8_t b, DescriptorSet& descriptor);
     148             :     // Returns true if the fletcher checksum for the packet is valid, else false.
     149             :     static bool valid_packet(const MicroStrain_Packet &packet);
     150             :     // Calls the correct functions based on the packet descriptor of the packet
     151             :     DescriptorSet handle_packet(const MicroStrain_Packet &packet);
     152             :     // Collects data from an imu packet into `imu_data`
     153             :     void handle_imu(const MicroStrain_Packet &packet);
     154             :     // Collects data from a gnss packet into `gnss_data`
     155             :     void handle_gnss(const MicroStrain_Packet &packet);
     156             :     void handle_filter(const MicroStrain_Packet &packet);
     157             :     static Vector3f populate_vector3f(const uint8_t* data, uint8_t offset);
     158             :     static Quaternion populate_quaternion(const uint8_t* data, uint8_t offset);
     159             :     // Depending on the descriptor, the data corresponds to a different GNSS instance.
     160             :     static bool get_gnss_instance(const DescriptorSet& descriptor, uint8_t& instance);
     161             : };
     162             : 
     163             : #endif // AP_MICROSTRAIN_ENABLED