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_SBP2_ENABLED 28 : class AP_GPS_SBP2 : public AP_GPS_Backend 29 : { 30 : public: 31 : AP_GPS_SBP2(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 : // Methods 36 : bool read() override; 37 : 38 : void inject_data(const uint8_t *data, uint16_t len) override; 39 : 40 : static bool _detect(struct SBP2_detect_state &state, uint8_t data); 41 : 42 2 : const char *name() const override { return "SBP2"; } 43 : 44 : private: 45 : 46 : // ************************************************************************ 47 : // Swift Navigation SBP protocol types and definitions 48 : // ************************************************************************ 49 : 50 : struct sbp_parser_state_t { 51 : enum { 52 : WAITING = 0, 53 : GET_TYPE = 1, 54 : GET_SENDER = 2, 55 : GET_LEN = 3, 56 : GET_MSG = 4, 57 : GET_CRC = 5 58 : } state:8; 59 : uint16_t msg_type; 60 : uint16_t sender_id; 61 : uint16_t crc; 62 : uint8_t msg_len; 63 : uint8_t n_read; 64 : uint8_t msg_buff[256]; 65 : } parser_state; 66 : 67 : static const uint8_t SBP_PREAMBLE = 0x55; 68 : 69 : // Message types supported by this driver 70 : static const uint16_t SBP_STARTUP_MSGTYPE = 0xFF00; 71 : static const uint16_t SBP_HEARTBEAT_MSGTYPE = 0xFFFF; 72 : static const uint16_t SBP_GPS_TIME_MSGTYPE = 0x0102; 73 : static const uint16_t SBP_DOPS_MSGTYPE = 0x0208; 74 : static const uint16_t SBP_POS_ECEF_MSGTYPE = 0x0209; 75 : static const uint16_t SBP_POS_LLH_MSGTYPE = 0x020A; 76 : static const uint16_t SBP_BASELINE_ECEF_MSGTYPE = 0x020B; 77 : static const uint16_t SBP_BASELINE_NED_MSGTYPE = 0x020C; 78 : static const uint16_t SBP_VEL_ECEF_MSGTYPE = 0x020D; 79 : static const uint16_t SBP_VEL_NED_MSGTYPE = 0x020E; 80 : static const uint16_t SBP_TRACKING_STATE_MSGTYPE = 0x0013; 81 : static const uint16_t SBP_IAR_STATE_MSGTYPE = 0x0019; 82 : static const uint16_t SBP_EXT_EVENT_MSGTYPE = 0x0101; 83 : 84 : // Heartbeat 85 : struct PACKED sbp_heartbeat_t { 86 : bool sys_error : 1; 87 : bool io_error : 1; 88 : bool nap_error : 1; 89 : uint8_t res : 5; 90 : uint8_t protocol_minor : 8; 91 : uint8_t protocol_major : 8; 92 : uint8_t res2 : 6; 93 : bool ext_antenna_short : 1; 94 : bool ext_antenna : 1; 95 : }; // 4 bytes 96 : 97 : // GPS Time 98 : struct PACKED sbp_gps_time_t { 99 : uint16_t wn; //< GPS week number (unit: weeks) 100 : uint32_t tow; //< GPS Time of Week rounded to the nearest ms (unit: ms) 101 : int32_t ns; //< Nanosecond remainder of rounded tow (unit: ns) 102 : struct PACKED flags { 103 : uint8_t time_src:3; //< Fix mode (0: invalid, 1: GNSS Solution, 2: Propagated 104 : uint8_t res:5; //< Reserved 105 : } flags; 106 : }; // 11 bytes 107 : 108 : // Dilution of Precision 109 : struct PACKED sbp_dops_t { 110 : uint32_t tow; //< GPS Time of Week (unit: ms) 111 : uint16_t gdop; //< Geometric Dilution of Precision (unit: 0.01) 112 : uint16_t pdop; //< Position Dilution of Precision (unit: 0.01) 113 : uint16_t tdop; //< Time Dilution of Precision (unit: 0.01) 114 : uint16_t hdop; //< Horizontal Dilution of Precision (unit: 0.01) 115 : uint16_t vdop; //< Vertical Dilution of Precision (unit: 0.01) 116 : struct PACKED flags { 117 : uint8_t fix_mode:3; //< Fix mode (0: invalid, 1: SPP, 2: DGNSS, 3: Float RTX, 4: Fixed RTX, 5: Undefined, 6: SBAS Position 118 : uint8_t res:4; //< Reserved 119 : bool raim_repair:1; //< Solution from RAIM? 120 : } flags; 121 : }; // 15 bytes 122 : 123 : // Geodetic position solution. 124 : struct PACKED sbp_pos_llh_t { 125 : uint32_t tow; //< GPS Time of Week (unit: ms) 126 : double lat; //< Latitude (unit: degrees) 127 : double lon; //< Longitude (unit: degrees) 128 : double height; //< Height (unit: meters) 129 : uint16_t h_accuracy; //< Horizontal position accuracy estimate (unit: mm) 130 : uint16_t v_accuracy; //< Vertical position accuracy estimate (unit: mm) 131 : uint8_t n_sats; //< Number of satellites used in solution 132 : struct PACKED flags { 133 : uint8_t fix_mode:3; //< Fix mode (0: invalid, 1: SPP, 2: DGNSS, 3: Float RTX, 4: Fixed RTX, 5: Dead Reckoning, 6: SBAS Position 134 : uint8_t ins_mode:2; //< Inertial navigation mode (0: none, 1: INS used) 135 : uint8_t res:3; //< Reserved 136 : } flags; 137 : }; // 34 bytes 138 : 139 : // Velocity in NED Velocity in local North East Down (NED) coordinates. 140 : struct PACKED sbp_vel_ned_t { 141 : uint32_t tow; //< GPS Time of Week (unit: ms) 142 : int32_t n; //< Velocity North coordinate (unit: mm/s) 143 : int32_t e; //< Velocity East coordinate (unit: mm/s) 144 : int32_t d; //< Velocity Down coordinate (unit: mm/s) 145 : uint16_t h_accuracy; //< Horizontal velocity accuracy estimate (unit: mm/s) 146 : uint16_t v_accuracy; //< Vertical velocity accuracy estimate (unit: mm/s) 147 : uint8_t n_sats; //< Number of satellites used in solution 148 : struct PACKED flags { 149 : uint8_t vel_mode:3; //< Velocity mode (0: Invalid, 1: Measured Doppler derived, 2: Computed Doppler derived, 3: Dead reckoning) 150 : uint8_t ins_mode:2; //< Inertial navigation mode (0: none, 1: INS used) 151 : uint8_t res:3; //< Reserved 152 : } flags; 153 : }; // 22 bytes 154 : 155 : // Messages reporting accurately-timestamped external events, e.g. camera shutter time. 156 : struct PACKED sbp_ext_event_t { 157 : uint16_t wn; //< GPS week number (unit: weeks) 158 : uint32_t tow; //< GPS Time of Week (unit: ms) 159 : int32_t ns_residual; //< Nanosecond residual of millisecond-rounded TOW (ranges from -500000 to 500000) 160 : struct PACKED flags { 161 : uint8_t level:1; //< New level of pin values (0: Low (falling edge), 1: High (rising edge)) 162 : uint8_t quality:1; //< Time quality values (0: Unknown - don't have nav solution, 1: Good (< 1 microsecond)) 163 : uint8_t res:6; //< Reserved 164 : } flags; 165 : uint8_t pin; //< Pin number (0-9) 166 : }; // 12 bytes 167 : 168 : void _sbp_process(); 169 : void _sbp_process_message(); 170 : bool _attempt_state_update(); 171 : 172 : // ************************************************************************ 173 : // Internal Received Messages State 174 : // ************************************************************************ 175 : uint32_t last_heartbeat_received_ms; 176 : uint32_t last_injected_data_ms; 177 : 178 : struct sbp_heartbeat_t last_heartbeat; 179 : struct sbp_gps_time_t last_gps_time; 180 : struct sbp_dops_t last_dops; 181 : struct sbp_pos_llh_t last_pos_llh; 182 : struct sbp_vel_ned_t last_vel_ned; 183 : struct sbp_ext_event_t last_event; 184 : 185 : uint32_t last_full_update_tow; 186 : uint16_t last_full_update_wn; 187 : 188 : // ************************************************************************ 189 : // Monitoring and Performance Counting 190 : // ************************************************************************ 191 : 192 : uint32_t crc_error_counter; 193 : 194 : // ************************************************************************ 195 : // Logging to AP_Logger 196 : // ************************************************************************ 197 : 198 : void logging_log_full_update(); 199 : void logging_ext_event(); 200 : 201 : int32_t distMod(int32_t tow1_ms, int32_t tow2_ms, int32_t mod); 202 : 203 : }; 204 : #endif
