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 GQ7 serially connected AHRS Systems
      15             :  */
      16             : 
      17             : #pragma once
      18             : 
      19             : #include "AP_ExternalAHRS_config.h"
      20             : 
      21             : #if AP_EXTERNAL_AHRS_MICROSTRAIN7_ENABLED
      22             : 
      23             : #include "AP_ExternalAHRS_backend.h"
      24             : #include <AP_GPS/AP_GPS.h>
      25             : #include <AP_HAL/AP_HAL.h>
      26             : #include "MicroStrain_common.h"
      27             : 
      28             : class AP_ExternalAHRS_MicroStrain7: public AP_ExternalAHRS_backend, public AP_MicroStrain
      29             : {
      30             : public:
      31             : 
      32             :     AP_ExternalAHRS_MicroStrain7(AP_ExternalAHRS *frontend, AP_ExternalAHRS::state_t &state);
      33             : 
      34             :     // get serial port number, -1 for not enabled
      35             :     int8_t get_port(void) const override;
      36             : 
      37             :     // Get model/type name
      38             :     const char* get_name() const override;
      39             : 
      40             :     // accessors for AP_AHRS
      41             :     bool healthy(void) const override;
      42             :     bool initialised(void) const override;
      43             :     bool pre_arm_check(char *failure_msg, uint8_t failure_msg_len) const override;
      44             :     void get_filter_status(nav_filter_status &status) const override;
      45             :     bool get_variances(float &velVar, float &posVar, float &hgtVar, Vector3f &magVar, float &tasVar) const override;
      46             : 
      47             :     // check for new data
      48       16791 :     void update() override
      49             :     {
      50       16791 :         build_packet();
      51       16791 :     };
      52             : 
      53             : protected:
      54             : 
      55          79 :     uint8_t num_gps_sensors(void) const override
      56             :     {
      57          79 :         return AP_MicroStrain::NUM_GNSS_INSTANCES;
      58             :     }
      59             : 
      60             : private:
      61             : 
      62             :     // GQ7 Filter States
      63             :     // https://s3.amazonaws.com/files.microstrain.com/GQ7+User+Manual/external_content/dcp/Data/filter_data/data/mip_field_filter_status.htm
      64             :     enum class FilterState {
      65             :         GQ7_INIT = 0x01,
      66             :         GQ7_VERT_GYRO = 0x02,
      67             :         GQ7_AHRS = 0x03,
      68             :         GQ7_FULL_NAV = 0x04
      69             :     };
      70             : 
      71             :     uint32_t baudrate;
      72             :     int8_t port_num;
      73             :     bool port_open = false;
      74             : 
      75             : 
      76             : 
      77             :     void build_packet();
      78             : 
      79             :     void post_imu() const;
      80             :     void post_gnss() const;
      81             :     void post_filter() const;
      82             : 
      83             :     void update_thread();
      84             :     void check_initialise_state();
      85             : 
      86             :     // Returns true when data is not stale.
      87             :     bool times_healthy() const;
      88             : 
      89             :     // Returns true when the filter is currently healthy.
      90             :     bool filter_healthy() const;
      91             : 
      92             :     // Only some of the fix types satisfy a healthy filter.
      93             :     // GQ7_VERT_GYRO is NOT considered healthy for now.
      94             :     // This may be vehicle-dependent in the future.
      95             :     static bool filter_state_healthy(FilterState state) WARN_IF_UNUSED;
      96             : 
      97             :     AP_HAL::UARTDriver *uart;
      98             :     HAL_Semaphore sem;
      99             : 
     100             :     // Used to monitor initialization state.
     101             :     bool last_init_state = false;
     102             : 
     103             : };
     104             : 
     105             : #endif // AP_EXTERNAL_AHRS_MICROSTRAIN7_ENABLED