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_ENABLED
      24             : 
      25             : #include <AP_HAL/AP_HAL.h>
      26             : #include <AP_Param/AP_Param.h>
      27             : #include <AP_Common/Location.h>
      28             : #include <AP_NavEKF/AP_Nav_Common.h>
      29             : #include <AP_GPS/AP_GPS_FixType.h>
      30             : 
      31             : class AP_ExternalAHRS_backend;
      32             : 
      33             : class AP_ExternalAHRS {
      34             : 
      35             : public:
      36             :     friend class AP_ExternalAHRS_backend;
      37             :     friend class AP_ExternalAHRS_SBG;
      38             :     friend class AP_ExternalAHRS_VectorNav;
      39             :     friend class AP_ExternalAHRS_SensAItion;
      40             :     friend class AP_ExternalAHRS_Aeron_plx;
      41             : 
      42             :     AP_ExternalAHRS();
      43             : 
      44             :     void init(void);
      45             : 
      46             :     static const struct AP_Param::GroupInfo var_info[];
      47             : 
      48             :     enum class DevType : uint8_t {
      49             :         None   = 0,
      50             : #if AP_EXTERNAL_AHRS_VECTORNAV_ENABLED
      51             :         VecNav = 1,
      52             : #endif
      53             : #if AP_EXTERNAL_AHRS_MICROSTRAIN5_ENABLED
      54             :         MicroStrain5 = 2,
      55             : #endif
      56             :         // 3 reserved for AdNav
      57             :         // 4 reserved for CINS
      58             : #if AP_EXTERNAL_AHRS_INERTIALLABS_ENABLED
      59             :         InertialLabs = 5,
      60             : #endif
      61             : #if AP_EXTERNAL_AHRS_GSOF_ENABLED
      62             :         // Trimble PX-1 RTX uses the GSOF protocol.
      63             :         GSOF = 6,
      64             : #endif
      65             : #if AP_EXTERNAL_AHRS_MICROSTRAIN7_ENABLED
      66             :         MicroStrain7 = 7,
      67             : #endif
      68             : #if AP_EXTERNAL_AHRS_SBG_ENABLED
      69             :         SBG = 8,
      70             : #endif
      71             :         // 9 reserved for EulerNav
      72             : #if AP_EXTERNAL_AHRS_AERON_PLX_ENABLED
      73             :         Aeron = 10,
      74             : #endif  // AP_EXTERNAL_AHRS_AERON_PLX_ENABLED
      75             : #if AP_EXTERNAL_AHRS_SENSAITION_ENABLED
      76             :         SensAItion = 11,
      77             : #endif
      78             :     };
      79             : 
      80    84741573 :     static AP_ExternalAHRS *get_singleton(void) {
      81    84741573 :         return _singleton;
      82             :     }
      83             : 
      84             :     // expected IMU rate in Hz
      85          18 :     float get_IMU_rate(void) const {
      86          18 :         return rate.get();
      87             :     }
      88             : 
      89             :     // Get model/type name
      90             :     const char* get_name() const;
      91             : 
      92             :     enum class AvailableSensor {
      93             :         GPS = (1U<<0),
      94             :         IMU = (1U<<1),
      95             :         BARO = (1U<<2),
      96             :         COMPASS = (1U<<3),
      97             :     };
      98             : 
      99             :     // get serial port number, -1 for not enabled
     100             :     int8_t get_port(AvailableSensor sensor) const;
     101             : 
     102             :     struct state_t {
     103             :         HAL_Semaphore sem;
     104             : 
     105             :         Vector3f accel;
     106             :         Vector3f gyro;
     107             :         Quaternion quat; // NED
     108             :         Location location;
     109             :         Vector3f velocity;
     110             :         Location origin;
     111             : 
     112             :         bool have_quaternion;
     113             :         bool have_origin;
     114             :         bool have_location;
     115             :         bool have_velocity;
     116             : 
     117             :         uint32_t last_location_update_us;
     118             :     } state;
     119             : 
     120             :     // accessors for AP_AHRS
     121             :     bool enabled() const;
     122             :     bool healthy(void) const;
     123             :     bool initialised(void) const;
     124             :     bool get_quaternion(Quaternion &quat);
     125             :     bool get_origin(Location &loc);
     126             :     bool set_origin(const Location &loc);
     127             :     bool get_location(Location &loc);
     128             :     Vector2f get_groundspeed_vector();
     129             :     bool get_velocity_NED(Vector3f &vel);
     130             :     bool get_speed_down(float &speedD);
     131             :     bool pre_arm_check(char *failure_msg, uint8_t failure_msg_len) const;
     132             :     void get_filter_status(nav_filter_status &status) const;
     133             :     bool get_gyro(Vector3f &gyro);
     134             :     bool get_accel(Vector3f &accel);
     135             :     bool get_variances(float &velVar, float &posVar, float &hgtVar, Vector3f &magVar, float &tasVar) const;
     136             : 
     137             :     // update backend
     138             :     void update();
     139             : 
     140             :     /*
     141             :       structures passed to other subsystems
     142             :      */
     143             :     typedef struct {
     144             :         uint8_t instance;
     145             :         float pressure_pa;
     146             :         float temperature;
     147             :     } baro_data_message_t;
     148             : 
     149             :     typedef struct {
     150             :         Vector3f field; // Magnetic flux density (mgauss)
     151             :     } mag_data_message_t;
     152             : 
     153             :     typedef struct {
     154             :         uint16_t gps_week;
     155             :         uint32_t ms_tow;
     156             :         AP_GPS_FixType  fix_type;
     157             :         uint8_t  satellites_in_view;
     158             :         float horizontal_pos_accuracy;
     159             :         float vertical_pos_accuracy;
     160             :         float horizontal_vel_accuracy;
     161             :         float hdop;
     162             :         float vdop;
     163             :         int32_t  longitude;
     164             :         int32_t  latitude;
     165             :         int32_t  msl_altitude;       // cm
     166             :         float  ned_vel_north;
     167             :         float  ned_vel_east;
     168             :         float  ned_vel_down;
     169             :     } gps_data_message_t;
     170             : 
     171             :     typedef struct {
     172             :         Vector3f accel;
     173             :         Vector3f gyro;
     174             :         float temperature;
     175             :     } ins_data_message_t;
     176             : 
     177             :     typedef struct {
     178             :         float differential_pressure; // Pa
     179             :         float temperature; // degC
     180             :     } airspeed_data_message_t;
     181             : 
     182             :     // set GNSS disable for auxillary function GPS_DISABLE
     183           6 :     void set_gnss_disable(bool disable) {
     184           6 :         gnss_is_disabled = disable;
     185           6 :     }
     186             : 
     187             :     // check if a sensor type is enabled
     188      252513 :     bool has_sensor(AvailableSensor sensor) const {
     189      252513 :         return (uint16_t(sensors.get()) & uint16_t(sensor)) != 0;
     190             :     }
     191             : 
     192             : protected:
     193             : 
     194             :     enum class OPTIONS {
     195             :         VN_UNCOMP_IMU = 1U << 0,
     196             :         SBG_EKF_AS_GNSS = 1U << 1,
     197             :         SENSAITION_INS = 1U << 2,
     198             :     };
     199       17996 :     bool option_is_set(OPTIONS option) const { return (options.get() & int32_t(option)) != 0; }
     200             : 
     201             : private:
     202             :     AP_ExternalAHRS_backend *backend;
     203             : 
     204             :     AP_Enum<DevType> devtype;
     205             :     AP_Int16         rate;
     206             :     AP_Int16         log_rate;
     207             :     AP_Int16         options;
     208             :     AP_Int16         sensors;
     209             : 
     210             :     static AP_ExternalAHRS *_singleton;
     211             : 
     212             :     // set default of EAHRS_SENSORS
     213          39 :     void set_default_sensors(uint16_t _sensors) {
     214          39 :         sensors.set_default(_sensors);
     215          39 :     }
     216             : 
     217             :     uint32_t last_log_ms;
     218             : 
     219             :     // true when user has disabled the GNSS
     220             :     bool gnss_is_disabled;
     221             : };
     222             : 
     223             : namespace AP {
     224             :     AP_ExternalAHRS &externalAHRS();
     225             : };
     226             : 
     227             : #endif  // AP_EXTERNAL_AHRS_ENABLED
     228             :