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 : // Emlid Reach Binary (ERB) GPS driver for ArduPilot. 18 : // ERB protocol: http://files.emlid.com/ERB.pdf 19 : 20 : #pragma once 21 : 22 : #include "AP_GPS.h" 23 : #include "GPS_Backend.h" 24 : 25 : #if AP_GPS_ERB_ENABLED 26 : class AP_GPS_ERB : public AP_GPS_Backend 27 : { 28 : public: 29 : 30 : using AP_GPS_Backend::AP_GPS_Backend; 31 : 32 : // Methods 33 : bool read() override; 34 : 35 0 : AP_GPS_FixType highest_supported_status(void) override { return AP_GPS_FixType::RTK_FIXED; } 36 : 37 : #if HAL_GCS_ENABLED 38 0 : bool supports_mavlink_gps_rtk_message() const override { return true; } 39 : #endif 40 : 41 : static bool _detect(struct ERB_detect_state &state, uint8_t data); 42 : 43 0 : const char *name() const override { return "ERB"; } 44 : 45 : private: 46 : struct PACKED erb_header { 47 : uint8_t preamble1; 48 : uint8_t preamble2; 49 : uint8_t msg_id; 50 : uint16_t length; 51 : }; 52 : struct PACKED erb_ver { 53 : uint32_t time; ///< GPS time of week of the navigation epoch [ms] 54 : uint8_t ver_high; 55 : uint8_t ver_medium; 56 : uint8_t ver_low; 57 : }; 58 : struct PACKED erb_pos { 59 : uint32_t time; ///< GPS time of week of the navigation epoch [ms] 60 : double longitude; 61 : double latitude; 62 : double altitude_ellipsoid; ///< Height above ellipsoid [m] 63 : double altitude_msl; ///< Height above mean sea level [m] 64 : uint32_t horizontal_accuracy; ///< Horizontal accuracy estimate [mm] 65 : uint32_t vertical_accuracy; ///< Vertical accuracy estimate [mm] 66 : }; 67 : struct PACKED erb_stat { 68 : uint32_t time; ///< GPS time of week of the navigation epoch [ms] 69 : uint16_t week; 70 : uint8_t fix_type; ///< see erb_fix_type enum 71 : uint8_t fix_status; 72 : uint8_t satellites; 73 : }; 74 : struct PACKED erb_dops { 75 : uint32_t time; ///< GPS time of week of the navigation epoch [ms] 76 : uint16_t gDOP; ///< Geometric DOP 77 : uint16_t pDOP; ///< Position DOP 78 : uint16_t vDOP; ///< Vertical DOP 79 : uint16_t hDOP; ///< Horizontal DOP 80 : }; 81 : struct PACKED erb_vel { 82 : uint32_t time; ///< GPS time of week of the navigation epoch [ms] 83 : int32_t vel_north; ///< North velocity component [cm/s] 84 : int32_t vel_east; ///< East velocity component [cm/s] 85 : int32_t vel_down; ///< Down velocity component [cm/s] 86 : uint32_t speed_2d; ///< Ground speed (2-D) [cm/s] 87 : int32_t heading_2d; ///< Heading of motion 2-D [1e5 deg] 88 : uint32_t speed_accuracy; ///< Speed accuracy Estimate [cm/s] 89 : }; 90 : struct PACKED erb_rtk { 91 : uint8_t base_num_sats; ///< Current number of satellites used for RTK calculation 92 : uint16_t age_cs; ///< Age of the corrections in centiseconds (0 when no corrections, 0xFFFF indicates overflow) 93 : int32_t baseline_N_mm; ///< distance between base and rover along the north axis in millimeters 94 : int32_t baseline_E_mm; ///< distance between base and rover along the east axis in millimeters 95 : int32_t baseline_D_mm; ///< distance between base and rover along the down axis in millimeters 96 : uint16_t ar_ratio; ///< AR ratio multiplied by 10 97 : uint16_t base_week_number; ///< GPS Week Number of last baseline 98 : uint32_t base_time_week_ms; ///< GPS Time of Week of last baseline in milliseconds 99 : }; 100 : 101 : // Receive buffer 102 : union PACKED { 103 0 : DEFINE_BYTE_ARRAY_METHODS 104 : erb_ver ver; 105 : erb_pos pos; 106 : erb_stat stat; 107 : erb_dops dops; 108 : erb_vel vel; 109 : erb_rtk rtk; 110 : } _buffer; 111 : 112 : enum erb_protocol_bytes { 113 : PREAMBLE1 = 0x45, 114 : PREAMBLE2 = 0x52, 115 : MSG_VER = 0x01, 116 : MSG_POS = 0x02, 117 : MSG_STAT = 0x03, 118 : MSG_DOPS = 0x04, 119 : MSG_VEL = 0x05, 120 : MSG_RTK = 0x07, 121 : }; 122 : 123 : enum erb_fix_type { 124 : FIX_NONE = 0x00, 125 : FIX_SINGLE = 0x01, 126 : FIX_FLOAT = 0x02, 127 : FIX_FIX = 0x03, 128 : }; 129 : 130 : // Packet checksum accumulators 131 : uint8_t _ck_a; 132 : uint8_t _ck_b; 133 : 134 : // State machine state 135 : uint8_t _step; 136 : uint8_t _msg_id; 137 : uint16_t _payload_length; 138 : uint16_t _payload_counter; 139 : 140 : // 8 bit count of fix messages processed, used for periodic processing 141 : uint8_t _fix_count; 142 : 143 : uint32_t _last_pos_time; 144 : uint32_t _last_vel_time; 145 : 146 : // do we have new position information? 147 : bool _new_position:1; 148 : // do we have new speed information? 149 : bool _new_speed:1; 150 : 151 : // Buffer parse & GPS state update 152 : bool _parse_gps(); 153 : 154 : // used to update fix between status and position packets 155 : AP_GPS_FixType next_fix = AP_GPS_FixType::NONE; 156 : }; 157 : #endif
