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             : #pragma once
      17             : 
      18             : #include "AP_ExternalAHRS_config.h"
      19             : 
      20             : #if AP_EXTERNAL_AHRS_SENSAITION_ENABLED
      21             : 
      22             : #include "AP_ExternalAHRS_backend.h"
      23             : #include "AP_ExternalAHRS_SensAItion_Parser.h"
      24             : 
      25             : /*
      26             : This class is the interface to Kebni's SensAItion range of inertial navigation
      27             : sensors, which can provide raw sensor data and/or a sensor fusion solution.
      28             : */
      29             : class AP_ExternalAHRS_SensAItion : public AP_ExternalAHRS_backend
      30             : {
      31             : public:
      32             :     AP_ExternalAHRS_SensAItion(AP_ExternalAHRS *frontend, AP_ExternalAHRS::state_t &_state);
      33             : 
      34             :     // Hardware Identification
      35             :     int8_t get_port() const override;
      36             :     const char* get_name() const override;
      37             : 
      38             :     // Health & Status Interface
      39             :     bool healthy() const override;
      40             :     bool initialised() const override;
      41             :     bool pre_arm_check(char *failure_msg, uint8_t failure_msg_len) const override;
      42             :     void get_filter_status(nav_filter_status &status) const override;
      43             :     bool get_variances(float &velVar, float &posVar, float &hgtVar, Vector3f &magVar, float &tasVar) const override;
      44             : 
      45             :     // GPS Interface
      46             :     uint8_t num_gps_sensors() const override;
      47             : 
      48             :     // Main Loop
      49       87165 :     void update() override
      50             :     {
      51       87165 :         check_uart();
      52       87165 :     }
      53             : 
      54             : private:
      55             :     mutable HAL_Semaphore sem_handle;
      56             : 
      57             :     // The member variables below are accessed both from our own
      58             :     // thread and the main thread and should be protected
      59             :     // by sem_handle!
      60             :     // =======================================================
      61             :     AP_ExternalAHRS::ins_data_message_t ins;
      62             :     AP_ExternalAHRS::mag_data_message_t mag;
      63             :     AP_ExternalAHRS::baro_data_message_t baro;
      64             :     AP_ExternalAHRS::gps_data_message_t gps;
      65             :     AP_ExternalAHRS_SensAItion_Parser parser;
      66             : 
      67             :     // UART
      68             :     AP_HAL::UARTDriver *uart;
      69             :     uint8_t buffer[AP_ExternalAHRS_SensAItion_Parser::MAX_PACKET_SIZE];
      70             : 
      71             :     bool setup_complete;
      72             : 
      73             :     // End of member variables protected by sem_handle
      74             :     // =======================================================
      75             : 
      76             :     // These states are accessed by the public accessor functions,
      77             :     // so to avoid slowing them down by waiting for sem_handle,
      78             :     // we use the included semaphore to protect access to them.
      79             :     // USAGE:
      80             :     // - If taking both semaphores, always take sem_handle first!
      81             :     // - Hold this semaphore only while accessing the driver state
      82             :     //   to minimize the waiting time for it!
      83             :     struct {
      84             :         mutable HAL_Semaphore semaphore;
      85             : 
      86             :         uint32_t last_imu_pkt_ms;
      87             :         uint32_t last_ins_pkt_ms; // Only used in INS mode
      88             :         uint32_t last_quat_pkt_ms; // Only used in INS mode
      89             :         uint32_t last_baro_update_ms;
      90             : 
      91             :         // Last known values from INS packet (Packet 2)
      92             :         uint8_t  last_alignment_status;
      93             :         uint8_t  last_gnss1_fix;
      94             :         uint8_t  last_gnss2_fix;
      95             :         uint8_t  last_sensor_valid;
      96             :         float    last_h_pos_quality = 999.9f;
      97             :         float    last_v_pos_quality = 999.9f;
      98             :         float    last_vel_quality = 999.9f;
      99             :         uint32_t last_error_flags;
     100             :     } driver_state;
     101             : 
     102             :     // Read-only after construction, do not need semaphore
     103             :     bool ins_mode_enabled = false;
     104             :     uint32_t baudrate = 460800;
     105             :     int8_t port_num = -1;
     106             : 
     107             :     void update_thread();
     108             :     bool check_uart();
     109             : 
     110             :     void handle_imu(const AP_ExternalAHRS_SensAItion_Parser::Measurement& meas, uint32_t now_ms);
     111             :     void handle_ahrs(const AP_ExternalAHRS_SensAItion_Parser::Measurement& meas, uint32_t now_ms);
     112             :     void handle_ins(const AP_ExternalAHRS_SensAItion_Parser::Measurement& meas, uint32_t now_ms);
     113             : };
     114             : #endif  // AP_EXTERNAL_AHRS_SENSAITION_ENABLED