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 : // Swift Navigation SBP GPS driver for ArduPilot. 18 : // Code by Niels Joubert 19 : // 20 : // Swift Binary Protocol format: http://docs.swift-nav.com/ 21 : // 22 : #pragma once 23 : 24 : #include "AP_GPS.h" 25 : #include "GPS_Backend.h" 26 : 27 : #if AP_GPS_SBP_ENABLED 28 : class AP_GPS_SBP : public AP_GPS_Backend 29 : { 30 : public: 31 : AP_GPS_SBP(AP_GPS &_gps, AP_GPS::Params &_params, AP_GPS::GPS_State &_state, AP_HAL::UARTDriver *_port); 32 : 33 0 : AP_GPS_FixType highest_supported_status(void) override { return AP_GPS_FixType::RTK_FIXED; } 34 : 35 : #if HAL_GCS_ENABLED 36 64 : bool supports_mavlink_gps_rtk_message() const override { return true; } 37 : #endif 38 : 39 : // Methods 40 : bool read() override; 41 : 42 : void inject_data(const uint8_t *data, uint16_t len) override; 43 : 44 : static bool _detect(struct SBP_detect_state &state, uint8_t data); 45 : 46 2 : const char *name() const override { return "SBP"; } 47 : 48 : private: 49 : 50 : // ************************************************************************ 51 : // Swift Navigation SBP protocol types and definitions 52 : // ************************************************************************ 53 : 54 : struct sbp_parser_state_t { 55 : enum { 56 : WAITING = 0, 57 : GET_TYPE = 1, 58 : GET_SENDER = 2, 59 : GET_LEN = 3, 60 : GET_MSG = 4, 61 : GET_CRC = 5 62 : } state:8; 63 : uint16_t msg_type; 64 : uint16_t sender_id; 65 : uint16_t crc; 66 : uint8_t msg_len; 67 : uint8_t n_read; 68 : uint8_t msg_buff[256]; 69 : } parser_state; 70 : 71 : static const uint8_t SBP_PREAMBLE = 0x55; 72 : 73 : //Message types supported by this driver 74 : static const uint16_t SBP_STARTUP_MSGTYPE = 0xFF00; 75 : static const uint16_t SBP_HEARTBEAT_MSGTYPE = 0xFFFF; 76 : static const uint16_t SBP_GPS_TIME_MSGTYPE = 0x0100; 77 : static const uint16_t SBP_DOPS_MSGTYPE = 0x0206; 78 : static const uint16_t SBP_POS_ECEF_MSGTYPE = 0x0200; 79 : static const uint16_t SBP_POS_LLH_MSGTYPE = 0x0201; 80 : static const uint16_t SBP_BASELINE_ECEF_MSGTYPE = 0x0202; 81 : static const uint16_t SBP_BASELINE_NED_MSGTYPE = 0x0203; 82 : static const uint16_t SBP_VEL_ECEF_MSGTYPE = 0x0204; 83 : static const uint16_t SBP_VEL_NED_MSGTYPE = 0x0205; 84 : static const uint16_t SBP_TRACKING_STATE_MSGTYPE = 0x0016; 85 : static const uint16_t SBP_IAR_STATE_MSGTYPE = 0x0019; 86 : 87 : // Heartbeat 88 : // struct sbp_heartbeat_t { 89 : // uint32_t flags; //< Status flags (reserved) 90 : // }; // 4 bytes 91 : struct PACKED sbp_heartbeat_t { 92 : bool sys_error : 1; 93 : bool io_error : 1; 94 : bool nap_error : 1; 95 : uint8_t res : 5; 96 : uint8_t protocol_minor : 8; 97 : uint8_t protocol_major : 8; 98 : uint8_t res2 : 7; 99 : bool ext_antenna : 1; 100 : }; // 4 bytes 101 : 102 : // GPS Time 103 : struct PACKED sbp_gps_time_t { 104 : uint16_t wn; //< GPS week number (unit: weeks) 105 : uint32_t tow; //< GPS Time of Week rounded to the nearest ms (unit: ms) 106 : int32_t ns; //< Nanosecond remainder of rounded tow (unit: ns) 107 : uint8_t flags; //< Status flags (reserved) 108 : }; // 11 bytes 109 : 110 : // Dilution of Precision 111 : struct PACKED sbp_dops_t { 112 : uint32_t tow; //< GPS Time of Week (unit: ms) 113 : uint16_t gdop; //< Geometric Dilution of Precision (unit: 0.01) 114 : uint16_t pdop; //< Position Dilution of Precision (unit: 0.01) 115 : uint16_t tdop; //< Time Dilution of Precision (unit: 0.01) 116 : uint16_t hdop; //< Horizontal Dilution of Precision (unit: 0.01) 117 : uint16_t vdop; //< Vertical Dilution of Precision (unit: 0.01) 118 : }; // 14 bytes 119 : 120 : // Geodetic position solution. 121 : struct PACKED sbp_pos_llh_t { 122 : uint32_t tow; //< GPS Time of Week (unit: ms) 123 : double lat; //< Latitude (unit: degrees) 124 : double lon; //< Longitude (unit: degrees) 125 : double height; //< Height (unit: meters) 126 : uint16_t h_accuracy; //< Horizontal position accuracy estimate (unit: mm) 127 : uint16_t v_accuracy; //< Vertical position accuracy estimate (unit: mm) 128 : uint8_t n_sats; //< Number of satellites used in solution 129 : uint8_t flags; //< Status flags 130 : }; // 34 bytes 131 : 132 : // Velocity in NED Velocity in local North East Down (NED) coordinates. 133 : struct PACKED sbp_vel_ned_t { 134 : uint32_t tow; //< GPS Time of Week (unit: ms) 135 : int32_t n; //< Velocity North coordinate (unit: mm/s) 136 : int32_t e; //< Velocity East coordinate (unit: mm/s) 137 : int32_t d; //< Velocity Down coordinate (unit: mm/s) 138 : uint16_t h_accuracy; //< Horizontal velocity accuracy estimate (unit: mm/s) 139 : uint16_t v_accuracy; //< Vertical velocity accuracy estimate (unit: mm/s) 140 : uint8_t n_sats; //< Number of satellites used in solution 141 : uint8_t flags; //< Status flags (reserved) 142 : }; // 22 bytes 143 : 144 : // Activity and Signal-to-Noise data of a tracking channel on the GPS. 145 : struct PACKED sbp_tracking_state_t { 146 : uint8_t state; //< 0 if disabled, 1 if running 147 : uint8_t prn; //< PRN identifier of tracked satellite 148 : float cn0; //< carrier to noise power ratio. 149 : }; 150 : 151 : // Integer Ambiguity Resolution state - how is the RTK resolution doing? 152 : struct PACKED sbp_iar_state_t { 153 : uint32_t num_hypotheses; 154 : }; 155 : 156 : void _sbp_process(); 157 : void _sbp_process_message(); 158 : bool _attempt_state_update(); 159 : 160 : // ************************************************************************ 161 : // Internal Received Messages State 162 : // ************************************************************************ 163 : uint32_t last_heatbeat_received_ms; 164 : uint32_t last_injected_data_ms; 165 : 166 : struct sbp_gps_time_t last_gps_time; 167 : struct sbp_dops_t last_dops; 168 : struct sbp_pos_llh_t last_pos_llh_spp; 169 : struct sbp_pos_llh_t last_pos_llh_rtk; 170 : struct sbp_vel_ned_t last_vel_ned; 171 : uint32_t last_iar_num_hypotheses; 172 : 173 : uint32_t last_full_update_tow; 174 : uint32_t last_full_update_cpu_ms; 175 : 176 : // ************************************************************************ 177 : // Monitoring and Performance Counting 178 : // ************************************************************************ 179 : 180 : uint32_t crc_error_counter; 181 : 182 : // ************************************************************************ 183 : // Logging to AP_Logger 184 : // ************************************************************************ 185 : 186 : void logging_log_full_update(); 187 : }; 188 : #endif
