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             : /*
      17             :   GPS driver backend class
      18             :  */
      19             : #pragma once
      20             : 
      21             : #include "AP_GPS_config.h"
      22             : 
      23             : #if AP_GPS_ENABLED
      24             : 
      25             : #include <GCS_MAVLink/GCS_MAVLink.h>
      26             : #include <GCS_MAVLink/GCS_config.h>
      27             : #include <AP_RTC/JitterCorrection.h>
      28             : #include "AP_GPS.h"
      29             : #include "AP_GPS_config.h"
      30             : 
      31             : #ifndef AP_GPS_DEBUG_LOGGING_ENABLED
      32             : // enable this to log all bytes from the GPS. Also needs a call to
      33             : // log_data() in each backend
      34             : #define AP_GPS_DEBUG_LOGGING_ENABLED 0
      35             : #endif
      36             : 
      37             : #ifndef AP_GPS_MB_MIN_LAG
      38             : #define AP_GPS_MB_MIN_LAG 0.05f
      39             : #endif
      40             : 
      41             : #ifndef AP_GPS_MB_MAX_LAG
      42             : #define AP_GPS_MB_MAX_LAG 0.25f
      43             : #endif
      44             : 
      45             : #if AP_GPS_DEBUG_LOGGING_ENABLED
      46             : #include <AP_HAL/utility/RingBuffer.h>
      47             : #endif
      48             : 
      49             : class AP_GPS_Backend
      50             : {
      51             : public:
      52             :     AP_GPS_Backend(AP_GPS &_gps, AP_GPS::Params &_params, AP_GPS::GPS_State &_state, AP_HAL::UARTDriver *_port);
      53             : 
      54             :     // we declare a virtual destructor so that GPS drivers can
      55             :     // override with a custom destructor if need be.
      56           6 :     virtual ~AP_GPS_Backend(void) {}
      57             : 
      58             :     // The read() method is the only one needed in each driver. It
      59             :     // should return true when the backend has successfully received a
      60             :     // valid packet from the GPS.
      61             :     virtual bool read() = 0;
      62             : 
      63             :     // Highest status supported by this GPS. 
      64             :     // Allows external system to identify type of receiver connected.
      65           0 :     virtual AP_GPS_FixType highest_supported_status(void) { return AP_GPS_FixType::FIX_3D; }
      66             : 
      67      141698 :     virtual bool is_configured(void) const { return true; }
      68             : 
      69             :     virtual void inject_data(const uint8_t *data, uint16_t len);
      70             : 
      71             : #if HAL_GCS_ENABLED
      72             :     //MAVLink methods
      73     1054794 :     virtual bool supports_mavlink_gps_rtk_message() const { return false; }
      74             : #if AP_GPS_GPS_RTK_SENDING_ENABLED || AP_GPS_GPS2_RTK_SENDING_ENABLED
      75             :     virtual void send_mavlink_gps_rtk(mavlink_channel_t chan);
      76             : #endif
      77           0 :     virtual void handle_msg(const mavlink_message_t &msg) { return ; }
      78             : #endif
      79             : 
      80           0 :     virtual void broadcast_configuration_failure_reason(void) const { return ; }
      81             : 
      82             : #if HAL_MSP_GPS_ENABLED
      83           0 :     virtual void handle_msp(const MSP::msp_gps_data_message_t &pkt) { return; }
      84             : #endif
      85             : #if AP_EXTERNAL_AHRS_ENABLED
      86           0 :     virtual void handle_external(const AP_ExternalAHRS::gps_data_message_t &pkt) { return; }
      87             : #endif
      88             :     
      89             :     // driver specific lag, returns true if the driver is confident in the provided lag
      90      127302 :     virtual bool get_lag(float &lag) const { lag = 0.2f; return true; }
      91             : 
      92             :     // driver specific health, returns true if the driver is healthy
      93       12801 :     virtual bool is_healthy(void) const { return true; }
      94             :     // returns true if the GPS is doing any logging it is expected to
      95           0 :     virtual bool logging_healthy(void) const { return true; }
      96             : 
      97             :     virtual const char *name() const = 0;
      98             : 
      99             :     void broadcast_gps_type() const;
     100             : #if HAL_LOGGING_ENABLED
     101             :     virtual void Write_AP_Logger_Log_Startup_messages() const;
     102             : #endif
     103             : 
     104         990 :     virtual bool prepare_for_arming(void) { return true; }
     105             : 
     106             :     // optional support for retrieving RTCMv3 data from a moving baseline base
     107           0 :     virtual bool get_RTCMV3(const uint8_t *&bytes, uint16_t &len) { return false; }
     108           0 :     virtual void clear_RTCMV3(void) {};
     109             : 
     110           0 :     virtual bool get_error_codes(uint32_t &error_codes) const { return false; }
     111             : 
     112             :     // return iTOW of last message, or zero if not supported
     113             :     uint32_t get_last_itow_ms(void) const;
     114             : 
     115             :     // check if an option is set
     116      695846 :     bool option_set(const AP_GPS::DriverOptions option) const {
     117      695846 :         return gps.option_set(option);
     118             :     }
     119             : 
     120             :     // Convert BCD date (DDMMYY) and time (MTK19 millisecond form) to GPS week and time
     121             :     static void BCD_to_gps_time(uint32_t bcd_date, uint32_t bcd_time_ms, uint16_t& gps_week, uint32_t& gps_time_ms);
     122             : 
     123             : protected:
     124             :     AP_HAL::UARTDriver *port;           ///< UART we are attached to
     125             :     AP_GPS &gps;                        ///< access to frontend (for parameters)
     126             :     AP_GPS::GPS_State &state;           ///< public state for this instance
     127             :     AP_GPS::Params &params;
     128             : 
     129             :     uint64_t _last_pps_time_us;
     130             :     JitterCorrection jitter_correction;
     131             :     uint32_t _last_itow_ms;
     132             :     bool _have_itow;
     133             : 
     134             :     /*
     135             :       fill in 3D velocity from 2D components
     136             :      */
     137             :     void fill_3d_velocity(void);
     138             : 
     139             :     /*
     140             :       fill ground course and speed from velocity
     141             :      */
     142             :     void velocity_to_speed_course(AP_GPS::GPS_State &s);
     143             : 
     144             :     /*
     145             :        fill in time_week_ms and time_week from BCD date and time components
     146             :        assumes MTK19 millisecond form of bcd_time
     147             :     */
     148         403 :     void make_gps_time(uint32_t bcd_date, uint32_t bcd_milliseconds)
     149             :     {
     150         403 :         BCD_to_gps_time(bcd_date, bcd_milliseconds, state.time_week, state.time_week_ms);
     151         403 :     }
     152             : 
     153             :     void _detection_message(char *buffer, uint8_t buflen) const;
     154             : 
     155             :     bool should_log() const;
     156             : 
     157             :     /*
     158             :       set a timestamp based on arrival time on uart at current byte,
     159             :       assuming the message started nbytes ago
     160             :      */
     161             :     void set_uart_timestamp(uint16_t nbytes);
     162             : 
     163             :     void check_new_itow(uint32_t itow, uint32_t msg_length);
     164             : 
     165             : #if GPS_MOVING_BASELINE
     166             :     bool calculate_moving_base_yaw(const float reported_heading_deg, const float reported_distance, const float reported_D);
     167             :     bool calculate_moving_base_yaw(AP_GPS::GPS_State &interim_state, const float reported_heading_deg, const float reported_distance, const float reported_D);
     168             : #endif //GPS_MOVING_BASELINE
     169             : 
     170             :     // get GPS type, for subtype config
     171       54982 :     AP_GPS::GPS_Type get_type() const {
     172       54982 :         return gps.get_type(state.instance);
     173             :     }
     174             : 
     175        4642 :     virtual void set_pps_desired_freq(uint8_t freq) {}
     176             : 
     177             : #if AP_GPS_DEBUG_LOGGING_ENABLED
     178             :     // log some data for debugging
     179             :     void log_data(const uint8_t *data, uint16_t length);
     180             : #endif
     181             : 
     182             :     // set alt in location, honouring GPS driver option for ellipsoid height
     183             :     void set_alt_amsl_cm(AP_GPS::GPS_State &_state, int32_t alt_amsl_cm);
     184             : 
     185             : private:
     186             :     // itow from previous message
     187             :     uint64_t _pseudo_itow;
     188             :     int32_t _pseudo_itow_delta_ms;
     189             :     uint32_t _last_ms;
     190             :     uint32_t _rate_ms;
     191             :     uint32_t _last_rate_ms;
     192             :     uint16_t _rate_counter;
     193             : 
     194             : #if AP_GPS_DEBUG_LOGGING_ENABLED
     195             :     // support raw GPS logging
     196             :     static struct loginfo {
     197             :         int fd = -1;
     198             :         ByteBuffer buf{16000};
     199             :     } logging[2];
     200             :     static bool log_thread_created;
     201             :     static void logging_loop(void);
     202             :     void logging_start(void);
     203             : #endif
     204             : 
     205             : };
     206             : 
     207             : #endif  // AP_GPS_ENABLED