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             : /*
      17             :   SensAItion Protocol Parser
      18             :   Handles binary protocol parsing (state machine, packet validation, checksums)
      19             :   Separated from ArduPilot state management for testability
      20             : */
      21             : 
      22             : #pragma once
      23             : 
      24             : #include <AP_HAL/AP_HAL.h>
      25             : #include <AP_Math/AP_Math.h>
      26             : #include <AP_Common/Location.h>
      27             : 
      28             : class AP_ExternalAHRS_SensAItion_Parser
      29             : {
      30             : public:
      31             :     // Configuration Mode defined in EAHRS_OPTIONS (Bit 1)
      32             :     enum class ConfigMode {
      33             :         IMU = 0,            // IMU Only Mode (No ID byte, fixed 38 byte packet)
      34             :         INTERLEAVED_INS = 1 // Interleaved Mode (Header -> ID -> Payload)
      35             :     };
      36             : 
      37             :     // Type of data contained in a decoded Measurement
      38             :     enum class MeasurementType {
      39             :         UNINITIALIZED = 0,
      40             :         IMU,
      41             :         AHRS, // Maps to Packet 1 (Orientation)
      42             :         INS   // Maps to Packet 2 (Navigation)
      43             :     };
      44             : 
      45             :     // Public Constants (Available to Driver)
      46             :     static const uint16_t MAX_PACKET_SIZE = 1024;
      47             :     static const uint8_t HEADER_BYTE = 0xFA;
      48             : 
      49             :     // Container for decoded data passed to Backend
      50             :     struct Measurement {
      51             :         MeasurementType type = MeasurementType::UNINITIALIZED;
      52             :         uint64_t timestamp_us;
      53             : 
      54             :         // Packet 0 (IMU) Data
      55             :         Vector3f acceleration_mss;
      56             :         Vector3f angular_velocity_rads;
      57             :         Vector3f magnetic_field_mgauss;
      58             :         float temperature_degc;
      59             :         float air_pressure_p;
      60             : 
      61             :         // Packet 1 (Orientation) Data
      62             :         Quaternion orientation;
      63             : 
      64             :         // Packet 2 (INS) Data
      65             :         Location location;          // Lat/Lon/Alt
      66             :         Vector3f velocity_ned;      // North/East/Down (m/s)
      67             : 
      68             :         // Accuracy Metrics (Vectors as requested)
      69             :         // ArduPilot often uses float for horiz/vert, but Vector3f is more flexible
      70             :         // if the sensor provides 3-axis accuracy.
      71             :         // Based on your config (AccLat, AccLon, AccPosD), we have 3 components.
      72             :         Vector3f pos_accuracy;      // North/East/Down (m)
      73             :         Vector3f vel_accuracy;      // North/East/Down (m/s)
      74             : 
      75             :         // Status & Health Flags
      76             :         uint8_t alignment_status;   // 1 = Align OK
      77             :         uint8_t gnss1_fix;
      78             :         uint8_t gnss2_fix;
      79             : 
      80             :         uint8_t num_sats_gnss1;
      81             :         uint8_t num_sats_gnss2;
      82             : 
      83             :         // Time
      84             :         uint32_t time_itow_ms;      // GNSS time of week (ms)
      85             :         uint16_t gps_week;          // Calculated Week Number
      86             : 
      87             :         uint32_t error_flags;       // Bitmask from sensor
      88             :         uint8_t sensor_valid;       // Validity bitmask
      89             :     };
      90             : 
      91             :     // Constructor
      92             :     AP_ExternalAHRS_SensAItion_Parser(ConfigMode mode);
      93             : 
      94             :     // Parse at most 'data_size' bytes from 'data', return # of bytes parsed.
      95             :     // If we parsed less than 'data_size' bytes, 'meas' contains a measurement,
      96             :     // otherwise it contains a valid measurement OR is uninitialized.
      97             :     size_t parse_stream(const uint8_t* data, size_t data_size, Measurement& meas);
      98             : 
      99             :     // Number of parsed full length buffers that did not contain a valid packet
     100           8 :     uint32_t get_parse_errors() const
     101             :     {
     102           8 :         return parse_errors;
     103             :     }
     104             : 
     105             :     // Number of valid packets received during object lifetime
     106          16 :     uint32_t get_valid_packets() const
     107             :     {
     108          16 :         return valid_packets;
     109             :     }
     110             : 
     111             : private:
     112             :     // Payload Sizes (Excluding Header, ID, CRC)
     113             :     static const uint8_t PAYLOAD_SIZE_IMU = 36;  // Packet 0
     114             :     static const uint8_t PAYLOAD_SIZE_QUAT = 16; // Packet 1
     115             :     static const uint8_t PAYLOAD_SIZE_INS = 69;  // Packet 2
     116             : 
     117             :     // Packet IDs (Interleaved Mode)
     118             :     enum class PacketID : uint8_t {
     119             :         IMU = 0x00,
     120             :         AHRS = 0x01,
     121             :         INS = 0x02,
     122             :         UNKNOWN = 0xFF
     123             :     };
     124             : 
     125             :     // Internal State Machine
     126             :     enum class ParseState {
     127             :         WAITING_HEADER,
     128             :         WAITING_ID,
     129             :         COLLECTING_PAYLOAD
     130             :     };
     131             : 
     132             :     // Core Logic
     133             :     void reset_parser(); // Does not reset packet and parse error counters
     134             :     bool parse_single_byte(uint8_t byte); // Return true if it was the last byte of a valid packet
     135             :     void handle_invalid_packet(); // Robust error recovery (memmove)
     136             :     bool buffer_contains_valid_packet() const;
     137             :     bool validate_checksum() const;
     138             : 
     139             :     // Decoders
     140             :     void decode_packet(Measurement& measurement);
     141             :     void decode_imu(const uint8_t* payload, Measurement& measurement);
     142             :     void decode_ahrs(const uint8_t* payload, Measurement& measurement);
     143             :     void decode_ins(const uint8_t* payload, Measurement& measurement);
     144             : 
     145             :     //Helpers
     146             :     uint16_t calculate_gps_week(uint16_t year, uint8_t month, uint8_t day);
     147             : 
     148             :     // Members
     149             :     ConfigMode config_mode;
     150             :     ParseState parse_state;
     151             :     PacketID current_packet_id;
     152             : 
     153             :     uint8_t packet_buffer[MAX_PACKET_SIZE];
     154             :     size_t packet_buffer_len;
     155             :     size_t target_payload_len;
     156             : 
     157             :     uint32_t valid_packets;
     158             :     uint32_t parse_errors;
     159             : };