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             :   support for serial connected AHRS systems
      17             :  */
      18             : 
      19             : #pragma once
      20             : 
      21             : #include "AP_ExternalAHRS_config.h"
      22             : 
      23             : #if AP_EXTERNAL_AHRS_VECTORNAV_ENABLED
      24             : 
      25             : #include "AP_ExternalAHRS_backend.h"
      26             : 
      27             : class AP_ExternalAHRS_VectorNav : public AP_ExternalAHRS_backend {
      28             : 
      29             : public:
      30             :     AP_ExternalAHRS_VectorNav(AP_ExternalAHRS *frontend, AP_ExternalAHRS::state_t &state);
      31             : 
      32             :     // get serial port number, -1 for not enabled
      33             :     int8_t get_port(void) const override;
      34             : 
      35             :     // accessors for AP_AHRS
      36             :     bool healthy(void) const override;
      37             :     bool initialised(void) const override;
      38             :     bool pre_arm_check(char *failure_msg, uint8_t failure_msg_len) const override;
      39             :     void get_filter_status(nav_filter_status &status) const override;
      40             :     bool get_variances(float &velVar, float &posVar, float &hgtVar, Vector3f &magVar, float &tasVar) const override;
      41             : 
      42             :     // check for new data
      43      333305 :     void update() override {
      44      333305 :         check_uart();
      45      333305 :     }
      46             : 
      47             :     // Get model/type name
      48             :     const char* get_name() const override;
      49             : 
      50             : protected:
      51             : 
      52          60 :     uint8_t num_gps_sensors(void) const override {
      53          60 :         return 1;
      54             :     }
      55             : private:
      56             :     AP_HAL::UARTDriver *uart;
      57             :     int8_t port_num;
      58             :     bool setup_complete;
      59             :     uint32_t baudrate;
      60             : 
      61             :     void update_thread();
      62             :     bool check_uart();
      63             : 
      64             :     void initialize();
      65             : 
      66             :     void run_command(const char *fmt, ...);
      67             : 
      68             :     struct VNAT;
      69             :     void write_vnat(const VNAT& data_to_log) const;
      70             :     void process_imu_packet(const uint8_t *b);
      71             :     void process_ahrs_ekf_packet(const uint8_t *b);
      72             :     void process_ins_ekf_packet(const uint8_t *b);
      73             :     void process_ins_gnss_packet(const uint8_t *b);
      74             : 
      75             : 
      76             :     uint8_t *pktbuf;
      77             :     uint16_t pktoffset;
      78             :     uint16_t bufsize;
      79             : 
      80             :     struct VN_imu_packet *latest_imu_packet;
      81             :     struct VN_INS_ekf_packet *latest_ins_ekf_packet;
      82             :     struct VN_INS_gnss_packet *latest_ins_gnss_packet;
      83             : 
      84             :     uint32_t last_pkt1_ms;
      85             :     uint32_t last_pkt2_ms;
      86             :     uint32_t last_pkt3_ms;
      87             : 
      88             :     enum class TYPE {
      89             :         VN_INS,  // Full INS mode, requiring GNSS. Used by VN-2X0 and VN-3X0
      90             :         VN_AHRS,  // IMU-only mode, used by VN-1X0
      91             :     } type;
      92             : 
      93             :     bool has_dual_gnss = false;
      94             : 
      95             :     char model_name[20];
      96             : 
      97             :     char message_to_send[50];
      98             :     // NMEA parsing for setup
      99             :     bool decode(char c);
     100             :     bool decode_latest_term();
     101             :     struct NMEA_parser {
     102             :         char term[20];            // buffer for the current term within the current sentence
     103             :         uint8_t term_offset;      // offset within the _term buffer where the next character should be placed
     104             :         uint8_t term_number;      // term index within the current sentence
     105             :         uint8_t checksum;         // checksum accumulator
     106             :         bool term_is_checksum;    // current term is the checksum
     107             :         bool sentence_valid;      // is current sentence valid so far
     108             :         bool sentence_done;       // true if this sentence has already been decoded
     109             :         bool error_response;      // true if received a VNERR response
     110             :     } nmea;
     111             : };
     112             : 
     113             : #endif  // AP_EXTERNAL_AHRS_VECTORNAV_ENABLED