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 Trimble PX-1 GNSS-RTX-INS.
      15             :  */
      16             : 
      17             : 
      18             : #pragma once
      19             : 
      20             : #include "AP_ExternalAHRS_config.h"
      21             : 
      22             : #if AP_EXTERNAL_AHRS_GSOF_ENABLED
      23             : 
      24             : #include "AP_ExternalAHRS_backend.h"
      25             : #include <AP_HAL/AP_HAL.h>
      26             : #include <AP_GSOF/AP_GSOF.h>
      27             : 
      28             : class AP_ExternalAHRS_GSOF: public AP_ExternalAHRS_backend, public AP_GSOF
      29             : {
      30             : public:
      31             : 
      32             :     AP_ExternalAHRS_GSOF(AP_ExternalAHRS *frontend, AP_ExternalAHRS::state_t &state);
      33             : 
      34             :     // Get model/type name
      35             :     const char* get_name() const override;
      36             : 
      37             :     // get serial port number, -1 for not enabled
      38             :     int8_t get_port() 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             : 
      48             :     // check for new data
      49           0 :     void update() override
      50             :     {
      51             :         // This driver update runs in a dedicated thread, skip this.
      52           0 :     };
      53             : 
      54             : protected:
      55             : 
      56           0 :     uint8_t num_gps_sensors(void) const override
      57             :     {
      58           0 :         return 1;
      59             :     }
      60             : 
      61             : private:
      62             : 
      63             :     void update_thread();
      64             :     void check_initialise_state();
      65             :     bool times_healthy() const;
      66             :     bool filter_healthy() const;
      67             :     void post_filter() const;
      68             : 
      69             :     AP_HAL::UARTDriver *uart;
      70             :     int8_t port_num;
      71             :     uint32_t baudrate;
      72             : 
      73             :     HAL_Semaphore sem;
      74             : 
      75             :     // Used to monitor initialization state.
      76             :     bool last_init_state;
      77             : 
      78             :     // The last time we received a message from the GNSS
      79             :     // containing AP_GSOF::POS_TIME.
      80             :     uint32_t last_pos_time_ms;
      81             :     // The last time we received a message from the GNSS
      82             :     // containing AP_GSOF::INS_FULL_NAV.
      83             :     uint32_t last_ins_full_nav_ms;
      84             :     // The last time we received a message from the GNSS
      85             :     // containing AP_GSOF::INS_RMS.
      86             :     uint32_t last_ins_rms_ms;
      87             :     // The last time we received a message from the GNSS
      88             :     // containing AP_GSOF::LLH_MSL.
      89             :     uint32_t last_llh_msl_ms;
      90             : 
      91             :     // The last calculated undulation, which is computed only at the rate both inputs are received.
      92             :     // The undulation is necessary to convert the INS full nav solution from ellipsoid to MSL altitude.
      93             :     double undulation;
      94             : 
      95             :     AP_ExternalAHRS::gps_data_message_t gps_data;
      96             : 
      97             : };
      98             : 
      99             : #endif // AP_EXTERNAL_AHRS_GSOF_ENABLED