├── .editorconfig ├── .github └── workflows │ └── linux_debug.yml ├── .gitignore ├── CMakeLists.txt ├── LICENSE.md ├── README.md ├── gps-parser-test.cpp └── src ├── ashtech.cpp ├── ashtech.h ├── base_station.h ├── crc.cpp ├── crc.h ├── emlid_reach.cpp ├── emlid_reach.h ├── femtomes.cpp ├── femtomes.h ├── gps_helper.cpp ├── gps_helper.h ├── mtk.cpp ├── mtk.h ├── nmea.cpp ├── nmea.h ├── rtcm.cpp ├── rtcm.h ├── sbf.cpp ├── sbf.h ├── ubx.cpp ├── ubx.h ├── unicore.cpp └── unicore.h /.editorconfig: -------------------------------------------------------------------------------- 1 | root = true 2 | 3 | [*] 4 | insert_final_newline = false 5 | 6 | [{*.{c,cpp,cc,h,hpp},CMakeLists.txt}] 7 | indent_style = tab 8 | tab_width = 8 9 | # Not in the official standard, but supported by many editors 10 | max_line_length = 120 11 | -------------------------------------------------------------------------------- /.github/workflows/linux_debug.yml: -------------------------------------------------------------------------------- 1 | name: Linux Test 2 | 3 | on: 4 | push: 5 | branches: 6 | - main 7 | pull_request: 8 | 9 | jobs: 10 | build: 11 | runs-on: ubuntu-latest 12 | 13 | strategy: 14 | matrix: 15 | BuildType: [Debug, Release] 16 | 17 | defaults: 18 | run: 19 | shell: bash 20 | 21 | steps: 22 | - name: Checkout repo 23 | uses: actions/checkout@v4 24 | with: 25 | fetch-tags: true 26 | fetch-depth: 0 27 | 28 | - uses: lukka/get-cmake@latest 29 | 30 | - uses: seanmiddleditch/gha-setup-ninja@v5 31 | 32 | - run: mkdir ${{ runner.temp }}/build 33 | 34 | - name: Configure 35 | working-directory: ${{ runner.temp }}/build 36 | run: cmake -S ${{ github.workspace }} -B . -G Ninja -DCMAKE_BUILD_TYPE=${{ matrix.BuildType }} 37 | 38 | - name: Build 39 | working-directory: ${{ runner.temp }}/build 40 | run: cmake --build . --target all --config ${{ matrix.BuildType }} 41 | 42 | - name: Install 43 | working-directory: ${{ runner.temp }}/build 44 | run: cmake --install . --config ${{ matrix.BuildType }} 45 | 46 | - name: Run unit tests 47 | working-directory: ${{ runner.temp }}/build 48 | run: ctest 49 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | build 2 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.16) 2 | 3 | project(gps-parser-test CXX) 4 | 5 | set(CMAKE_CXX_STANDARD 17) 6 | set(CMAKE_CXX_STANDARD_REQUIRED ON) 7 | 8 | add_executable(gps-parser-test 9 | gps-parser-test.cpp 10 | src/unicore.cpp 11 | src/crc.cpp 12 | ) 13 | 14 | target_compile_options(gps-parser-test 15 | PRIVATE 16 | -Wall 17 | -Wextra 18 | -Wconversion 19 | -Wpedantic 20 | ) 21 | 22 | target_include_directories(gps-parser-test 23 | PRIVATE 24 | src/ 25 | ) 26 | 27 | include(CTest) 28 | enable_testing() 29 | 30 | add_test(NAME GPSParserTest COMMAND gps-parser-test) 31 | -------------------------------------------------------------------------------- /LICENSE.md: -------------------------------------------------------------------------------- 1 | Copyright (c) 2016, PX4 Pro Drone Autopilot 2 | All rights reserved. 3 | 4 | Redistribution and use in source and binary forms, with or without 5 | modification, are permitted provided that the following conditions are met: 6 | 7 | * Redistributions of source code must retain the above copyright notice, this 8 | list of conditions and the following disclaimer. 9 | 10 | * Redistributions in binary form must reproduce the above copyright notice, 11 | this list of conditions and the following disclaimer in the documentation 12 | and/or other materials provided with the distribution. 13 | 14 | * Neither the name of GpsDrivers nor the names of its 15 | contributors may be used to endorse or promote products derived from 16 | this software without specific prior written permission. 17 | 18 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 19 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 21 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 22 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 23 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 24 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 25 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 26 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 27 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 28 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # GPS Drivers 2 | 3 | ## Overview 4 | 5 | This repository contains user-space gps drivers, used as a submodule in 6 | [PX4-Autopilot](https://github.com/PX4/PX4-Autopilot) and 7 | [QGroundControl](https://github.com/mavlink/qgroundcontrol). 8 | 9 | All platform-specific stuff is done via a callback function and a 10 | `definitions.h` header file. 11 | 12 | In order for the project to build, `definitions.h` must include definitions for `sensor_gnss_relative_s`, `sensor_gps_s` and `satellite_info_s`. 13 | For example, check the implementation in [PX4 Autopilot](https://github.com/PX4/PX4-Autopilot/blob/master/src/drivers/gps/definitions.h) or [QGroundControl](https://github.com/mavlink/qgroundcontrol/blob/master/src/GPS/definitions.h). 14 | 15 | 16 | ## Parser tests 17 | 18 | To test parsers, build and run the cmake project: 19 | 20 | ``` 21 | cmake -Bbuild -H. 22 | cmake --build build && build/gps-parser-test 23 | ``` 24 | -------------------------------------------------------------------------------- /gps-parser-test.cpp: -------------------------------------------------------------------------------- 1 | #include "unicore.h" 2 | #include 3 | #include 4 | 5 | void test_empty() 6 | { 7 | const char str[] = " "; 8 | 9 | UnicoreParser unicore_parser; 10 | 11 | for (unsigned i = 0; i < sizeof(str); ++i) { 12 | auto result = unicore_parser.parseChar(str[i]); 13 | assert(result == UnicoreParser::Result::None); 14 | } 15 | } 16 | 17 | void test_garbage() 18 | { 19 | const char str[] = "#GARBAGE,BLA"; 20 | 21 | UnicoreParser unicore_parser; 22 | 23 | for (unsigned i = 0; i < sizeof(str); ++i) { 24 | auto result = unicore_parser.parseChar(str[i]); 25 | assert(result == UnicoreParser::Result::None); 26 | } 27 | } 28 | 29 | void test_too_long() 30 | { 31 | const char str[] = 32 | "#UNIHEADINGA,89,GPS,FINE,2251,168052600,0,0,18,11;SOL_COMPUTED," 33 | "NARROW_INT,0.3718,67.0255,-0.7974,0.0000,0.8065,3.3818,\"999\"," 34 | "31,21,21,18,3,01,3,f3,1234567890,1234567890,1234567890,1234567890," 35 | "1234567890,1234567890,1234567890,1234567890,1234567890,1234567890," 36 | "1234567890,1234567890,1234567890,1234567890,1234567890,1234567890," 37 | "1234567890,1234567890,1234567890,1234567890,1234567890,1234567890," 38 | "1234567890,1234567890,1234567890,1234567890,1234567890,1234567890," 39 | "1234567890,1234567890,1234567890,1234567890,1234567890,1234567890," 40 | "1234567890,1234567890,1234567890,1234567890,1234567890,1234567890," 41 | "1234567890,1234567890,1234567890,1234567890,1234567890,1234567890," 42 | "1234567890,1234567890,1234567890,1234567890,1234567890,1234567890," 43 | "1234567890,ffffffff"; 44 | 45 | UnicoreParser unicore_parser; 46 | 47 | for (unsigned i = 0; i < sizeof(str); ++i) { 48 | auto result = unicore_parser.parseChar(str[i]); 49 | assert(result == UnicoreParser::Result::None); 50 | } 51 | } 52 | 53 | void test_uniheadinga_wrong_crc() 54 | { 55 | const char str[] = 56 | "#UNIHEADINGA,89,GPS,FINE,2251,168052600,0,0,18,11;SOL_COMPUTED,NARROW_INT," 57 | "0.3718,67.0255,-0.7974,0.0000,0.8065,3.3818,\"999\",31,21,21,18,3,01,3,f3*" 58 | "ffffffff"; 59 | 60 | UnicoreParser unicore_parser; 61 | 62 | for (unsigned i = 0; i < sizeof(str); ++i) { 63 | auto result = unicore_parser.parseChar(str[i]); 64 | 65 | if (result == UnicoreParser::Result::WrongCrc) { 66 | return; 67 | } 68 | } 69 | 70 | assert(false); 71 | } 72 | 73 | 74 | void test_uniheadinga() 75 | { 76 | const char str[] = 77 | "#UNIHEADINGA,89,GPS,FINE,2251,168052600,0,0,18,11;SOL_COMPUTED,NARROW_INT,0.3718,67.0255,-0.7974,0.0000,0.8065,3.3818,\"999\",31,21,21,18,3,01,3,f3*ece5bb07"; 78 | 79 | UnicoreParser unicore_parser; 80 | 81 | for (unsigned i = 0; i < sizeof(str); ++i) { 82 | auto result = unicore_parser.parseChar(str[i]); 83 | 84 | if (result == UnicoreParser::Result::GotHeading) { 85 | assert(unicore_parser.heading().heading_deg == 67.0255f); 86 | assert(unicore_parser.heading().baseline_m == 0.3718f); 87 | return; 88 | } 89 | } 90 | 91 | assert(false); 92 | } 93 | 94 | void test_uniheadinga_twice() 95 | { 96 | const char str[] = 97 | "#UNIHEADINGA,89,GPS,FINE,2251,168052600,0,0,18,11;SOL_COMPUTED,NARROW_INT,0.3718,67.0255,-0.7974,0.0000,0.8065,3.3818,\"999\",31,21,21,18,3,01,3,f3*ece5bb07" 98 | "#UNIHEADINGA,88,GPS,FINE,2251,175391000,0,0,18,13;SOL_COMPUTED,NARROW_INT,0.3500,88.4714,-1.5969,0.0000,1.0175,2.8484,\"999\",28,18,18,12,3,01,3,f3*2888f737"; 99 | 100 | UnicoreParser unicore_parser; 101 | 102 | unsigned num_parsed = 0; 103 | 104 | for (unsigned i = 0; i < sizeof(str); ++i) { 105 | auto result = unicore_parser.parseChar(str[i]); 106 | 107 | if (result == UnicoreParser::Result::GotHeading) { 108 | ++num_parsed; 109 | } 110 | } 111 | 112 | assert(num_parsed == 2); 113 | } 114 | 115 | void test_agrica() 116 | { 117 | const char str[] = 118 | "#AGRICA,68,GPS,FINE,2063,454587000,0,0,18,38;GNSS,236,19,7,26,6,16,9,4,4,12,10," 119 | "9,306.7191,10724.0176,-16.4796,0.0089,0.0070,0.0181,67.9651,29.3584,0.0000,0.003,0.003,0.001,-0.002,0.021," 120 | "0.039,0.025,40.07896719907,116.23652055432,67.3108,-2160482.7849,4383625.2350,4084735.7632,0.0140,0.0125,0.0296,0.0107," 121 | "0.0198,0.0128,40.07627310896,116.11079363322,65.3740,0.00000000000,0.00000000000,0.0000,454587000,38.000,16.723207,-9.406086,0.000000,0.000000,8,0,0,0*e9402e02"; 122 | 123 | UnicoreParser unicore_parser; 124 | 125 | for (unsigned i = 0; i < sizeof(str); ++i) { 126 | auto result = unicore_parser.parseChar(str[i]); 127 | 128 | if (result == UnicoreParser::Result::GotAgrica) { 129 | assert(unicore_parser.agrica().velocity_m_s == 0.003f); 130 | assert(unicore_parser.agrica().velocity_north_m_s == 0.003f); 131 | assert(unicore_parser.agrica().velocity_east_m_s == 0.001f); 132 | assert(unicore_parser.agrica().velocity_up_m_s == -0.002f); 133 | assert(unicore_parser.agrica().stddev_velocity_north_m_s == 0.021f); 134 | assert(unicore_parser.agrica().stddev_velocity_east_m_s == 0.039f); 135 | assert(unicore_parser.agrica().stddev_velocity_up_m_s == 0.025f); 136 | return; 137 | } 138 | } 139 | 140 | assert(false); 141 | } 142 | 143 | void test_unicore() 144 | { 145 | test_empty(); 146 | test_garbage(); 147 | test_too_long(); 148 | test_uniheadinga_wrong_crc(); 149 | test_uniheadinga(); 150 | test_uniheadinga_twice(); 151 | test_agrica(); 152 | } 153 | 154 | int main(int, char **) 155 | { 156 | test_unicore(); 157 | 158 | return 0; 159 | } -------------------------------------------------------------------------------- /src/ashtech.h: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * 3 | * Copyright (C) 2013. All rights reserved. 4 | * Author: Boriskin Aleksey 5 | * Kistanov Alexander 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * 1. Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * 2. Redistributions in binary form must reproduce the above copyright 14 | * notice, this list of conditions and the following disclaimer in 15 | * the documentation and/or other materials provided with the 16 | * distribution. 17 | * 3. Neither the name PX4 nor the names of its contributors may be 18 | * used to endorse or promote products derived from this software 19 | * without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS 28 | * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 29 | * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | * 34 | ****************************************************************************/ 35 | 36 | /** @file ASHTECH protocol definitions */ 37 | 38 | #pragma once 39 | 40 | #include "base_station.h" 41 | 42 | #include 43 | 44 | class RTCMParsing; 45 | 46 | #define ASHTECH_RECV_BUFFER_SIZE 512 47 | #define ASH_RESPONSE_TIMEOUT 200 // ms, timeout for waiting for a response 48 | 49 | class GPSDriverAshtech : public GPSBaseStationSupport 50 | { 51 | public: 52 | /** 53 | * @param heading_offset heading offset in radians [-pi, pi]. It is substracted from the measurement. 54 | */ 55 | GPSDriverAshtech(GPSCallbackPtr callback, void *callback_user, sensor_gps_s *gps_position, 56 | satellite_info_s *satellite_info, float heading_offset = 0.f); 57 | 58 | virtual ~GPSDriverAshtech(); 59 | 60 | int configure(unsigned &baudrate, const GPSConfig &config) override; 61 | 62 | int receive(unsigned timeout) override; 63 | 64 | private: 65 | enum class AshtechBoard { 66 | trimble_mb_two, 67 | other 68 | }; 69 | 70 | enum class NMEACommand { 71 | Acked, // Command that returns a (N)Ack 72 | PRT, // port config 73 | RID, // board identification 74 | RECEIPT// board identification 75 | }; 76 | 77 | enum class NMEACommandState { 78 | idle, 79 | waiting, 80 | nack, 81 | received 82 | }; 83 | 84 | enum class NMEADecodeState { 85 | uninit, 86 | got_sync1, 87 | got_asteriks, 88 | got_first_cs_byte 89 | }; 90 | 91 | /** 92 | * enable output of correction output 93 | */ 94 | void activateCorrectionOutput(); 95 | 96 | void activateRTCMOutput(bool reduce_update_rate); 97 | 98 | void decodeInit(void); 99 | 100 | int handleMessage(int len); 101 | 102 | int parseChar(uint8_t b); 103 | 104 | /** 105 | * receive data for at least the specified amount of time 106 | */ 107 | void receiveWait(unsigned timeout_min); 108 | 109 | void sendSurveyInStatusUpdate(bool active, bool valid, double latitude = static_cast(NAN), 110 | double longitude = static_cast(NAN), float altitude = NAN); 111 | 112 | /** 113 | * Write a command and wait for a (N)Ack 114 | * @return 0 on success, <0 otherwise 115 | */ 116 | int writeAckedCommand(const void *buf, int buf_length, unsigned timeout); 117 | 118 | int waitForReply(NMEACommand command, const unsigned timeout); 119 | 120 | bool _correction_output_activated{false}; 121 | bool _configure_done{false}; 122 | bool _got_pashr_pos_message{false}; /**< If we got a PASHR,POS message, we will ignore GGA messages */ 123 | 124 | char _port{'A'}; /**< port we are connected to (e.g. 'A') */ 125 | 126 | uint8_t _rx_buffer[ASHTECH_RECV_BUFFER_SIZE]; 127 | uint16_t _rx_buffer_bytes{}; 128 | uint64_t _last_timestamp_time{0}; 129 | 130 | float _heading_offset; 131 | 132 | gps_abstime _survey_in_start{0}; 133 | 134 | sensor_gps_s *_gps_position {nullptr}; 135 | 136 | satellite_info_s *_satellite_info {nullptr}; 137 | 138 | AshtechBoard _board{AshtechBoard::other}; /**< board we are connected to */ 139 | 140 | NMEACommand _waiting_for_command; 141 | 142 | NMEACommandState _command_state{NMEACommandState::idle}; 143 | 144 | NMEADecodeState _decode_state{NMEADecodeState::uninit}; 145 | 146 | OutputMode _output_mode{OutputMode::GPS}; 147 | 148 | RTCMParsing *_rtcm_parsing{nullptr}; 149 | }; 150 | 151 | -------------------------------------------------------------------------------- /src/base_station.h: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * 3 | * Copyright (c) 2018 PX4 Development Team. All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions 7 | * are met: 8 | * 9 | * 1. Redistributions of source code must retain the above copyright 10 | * notice, this list of conditions and the following disclaimer. 11 | * 2. Redistributions in binary form must reproduce the above copyright 12 | * notice, this list of conditions and the following disclaimer in 13 | * the documentation and/or other materials provided with the 14 | * distribution. 15 | * 3. Neither the name PX4 nor the names of its contributors may be 16 | * used to endorse or promote products derived from this software 17 | * without specific prior written permission. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 20 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 21 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 22 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 23 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 24 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 25 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS 26 | * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 27 | * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 28 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 29 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 30 | * POSSIBILITY OF SUCH DAMAGE. 31 | * 32 | ****************************************************************************/ 33 | 34 | /** 35 | * @file base_station.h 36 | * 37 | * @author Beat Küng 38 | * 39 | */ 40 | 41 | #pragma once 42 | 43 | #include "gps_helper.h" 44 | 45 | /** 46 | * @class GPSBaseStationSupport 47 | * GPS driver base class with Base Station Support 48 | */ 49 | class GPSBaseStationSupport : public GPSHelper 50 | { 51 | public: 52 | GPSBaseStationSupport(GPSCallbackPtr callback, void *callback_user) 53 | : GPSHelper(callback, callback_user) {} 54 | 55 | virtual ~GPSBaseStationSupport() = default; 56 | 57 | /** 58 | * set survey-in specs for RTK base station setup (for finding an accurate base station position 59 | * by averaging the position measurements over time). 60 | * @param survey_in_acc_limit minimum accuracy in 0.1mm 61 | * @param survey_in_min_dur minimum duration in seconds 62 | */ 63 | void setSurveyInSpecs(uint32_t survey_in_acc_limit, uint32_t survey_in_min_dur) 64 | { 65 | _base_settings.type = BaseSettingsType::survey_in; 66 | _base_settings.settings.survey_in.acc_limit = survey_in_acc_limit; 67 | _base_settings.settings.survey_in.min_dur = survey_in_min_dur; 68 | } 69 | 70 | /** 71 | * Set a fixed base station position. This can be used if the base position is already known to 72 | * avoid doing a survey-in. 73 | * @param latitude [deg] 74 | * @param longitude [deg] 75 | * @param altitude [m] 76 | * @param position_accuracy 3D position accuracy (set to 0 if unknown) [mm] 77 | */ 78 | void setBasePosition(double latitude, double longitude, float altitude, float position_accuracy) 79 | { 80 | _base_settings.type = BaseSettingsType::fixed_position; 81 | _base_settings.settings.fixed_position.latitude = latitude; 82 | _base_settings.settings.fixed_position.longitude = longitude; 83 | _base_settings.settings.fixed_position.altitude = altitude; 84 | _base_settings.settings.fixed_position.position_accuracy = position_accuracy; 85 | } 86 | 87 | protected: 88 | 89 | enum class BaseSettingsType : uint8_t { 90 | survey_in, 91 | fixed_position 92 | }; 93 | struct SurveyInSettings { 94 | uint32_t acc_limit; 95 | uint32_t min_dur; 96 | }; 97 | struct FixedPositionSettings { 98 | double latitude; 99 | double longitude; 100 | float altitude; 101 | float position_accuracy; 102 | }; 103 | struct BaseSettings { 104 | BaseSettingsType type; 105 | union { 106 | SurveyInSettings survey_in; 107 | FixedPositionSettings fixed_position; 108 | } settings; 109 | }; 110 | BaseSettings _base_settings; 111 | }; 112 | 113 | -------------------------------------------------------------------------------- /src/crc.cpp: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * 3 | * Copyright (c) 2023 PX4 Development Team. All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions 7 | * are met: 8 | * 9 | * 1. Redistributions of source code must retain the above copyright 10 | * notice, this list of conditions and the following disclaimer. 11 | * 2. Redistributions in binary form must reproduce the above copyright 12 | * notice, this list of conditions and the following disclaimer in 13 | * the documentation and/or other materials provided with the 14 | * distribution. 15 | * 3. Neither the name PX4 nor the names of its contributors may be 16 | * used to endorse or promote products derived from this software 17 | * without specific prior written permission. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 20 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 21 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 22 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 23 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 24 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 25 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS 26 | * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 27 | * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 28 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 29 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 30 | * POSSIBILITY OF SUCH DAMAGE. 31 | * 32 | ****************************************************************************/ 33 | 34 | #include "crc.h" 35 | 36 | // According to https://en.wikipedia.org/wiki/Cyclic_redundancy_check 37 | // 38 | // the CRC is based on the CRC-32 ISO 3309 / ANSI X3.66 39 | // implementetation, and the polynomial is supplied as "Reversed". 40 | // 41 | // The CRC algorithm was extracted from the Femtomes driver. 42 | // 43 | 44 | static constexpr uint32_t CRC32_POLYNOMIAL = 0xEDB88320; 45 | 46 | static uint32_t crc32Value(uint32_t crc) 47 | { 48 | for (int i = 8 ; i > 0; i--) { 49 | if (crc & 1) { 50 | crc = (crc >> 1) ^ CRC32_POLYNOMIAL; 51 | 52 | } else { 53 | crc >>= 1; 54 | } 55 | } 56 | 57 | return crc; 58 | } 59 | 60 | uint32_t 61 | calculateCRC32(uint32_t length, uint8_t *buffer, uint32_t crc) 62 | { 63 | while (length-- != 0) { 64 | crc = ((crc >> 8) & 0x00FFFFFFL) ^ (crc32Value(((uint32_t) crc ^ *buffer++) & 0xff)); 65 | } 66 | 67 | return crc; 68 | } 69 | -------------------------------------------------------------------------------- /src/crc.h: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * 3 | * Copyright (c) 2023 PX4 Development Team. All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions 7 | * are met: 8 | * 9 | * 1. Redistributions of source code must retain the above copyright 10 | * notice, this list of conditions and the following disclaimer. 11 | * 2. Redistributions in binary form must reproduce the above copyright 12 | * notice, this list of conditions and the following disclaimer in 13 | * the documentation and/or other materials provided with the 14 | * distribution. 15 | * 3. Neither the name PX4 nor the names of its contributors may be 16 | * used to endorse or promote products derived from this software 17 | * without specific prior written permission. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 20 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 21 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 22 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 23 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 24 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 25 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS 26 | * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 27 | * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 28 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 29 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 30 | * POSSIBILITY OF SUCH DAMAGE. 31 | * 32 | ****************************************************************************/ 33 | 34 | #pragma once 35 | 36 | #include 37 | 38 | uint32_t calculateCRC32(uint32_t length, uint8_t *buffer, uint32_t crc); 39 | -------------------------------------------------------------------------------- /src/emlid_reach.cpp: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * 3 | * Copyright (c) 2012-2019 PX4 Development Team. All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions 7 | * are met: 8 | * 9 | * 1. Redistributions of source code must retain the above copyright 10 | * notice, this list of conditions and the following disclaimer. 11 | * 2. Redistributions in binary form must reproduce the above copyright 12 | * notice, this list of conditions and the following disclaimer in 13 | * the documentation and/or other materials provided with the 14 | * distribution. 15 | * 3. Neither the name PX4 nor the names of its contributors may be 16 | * used to endorse or promote products derived from this software 17 | * without specific prior written permission. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 20 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 21 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 22 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 23 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 24 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 25 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS 26 | * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 27 | * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 28 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 29 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 30 | * POSSIBILITY OF SUCH DAMAGE. 31 | * 32 | ****************************************************************************/ 33 | 34 | /** 35 | * @file emlid_reach.cpp 36 | * 37 | * @author Bastien Auneau 38 | */ 39 | 40 | 41 | #include "emlid_reach.h" 42 | 43 | #include 44 | #include 45 | #include 46 | #include 47 | #include 48 | #include 49 | 50 | #include 51 | 52 | //// ERB 53 | // 'E' Ox45 | 'R' Ox52 | ID | LENGTH (2 Bytes little endian) | payload | CHECKSUM (2B) 54 | #define ERB_SYNC_1 0x45 // E 55 | #define ERB_SYNC_2 0x52 // R 56 | 57 | #define ERB_ID_VERSION 0x01 58 | #define ERB_ID_GEODIC_POSITION 0x02 59 | #define ERB_ID_NAV_STATUS 0x03 60 | #define ERB_ID_DOPS 0x04 61 | #define ERB_ID_VELOCITY_NED 0x05 62 | #define ERB_ID_SPACE_INFO 0x06 // not used, reduces stack usage 63 | #define ERB_ID_RTK 0x07 // RTK, TODO 64 | 65 | 66 | #define EMLID_UNUSED(x) (void)x; 67 | 68 | #define GPS_PI 3.141592653589793238462643383280f 69 | 70 | #define AUTO_DETECT_MAX_READ_SENTENCE 5 // if more detect succeed 71 | 72 | 73 | GPSDriverEmlidReach::GPSDriverEmlidReach(GPSCallbackPtr callback, void *callback_user, 74 | sensor_gps_s *gps_position, satellite_info_s *satellite_info) : 75 | GPSHelper(callback, callback_user), 76 | _gps_position(gps_position), _satellite_info(satellite_info) 77 | {} 78 | 79 | 80 | int 81 | GPSDriverEmlidReach::configure(unsigned &baudrate, const GPSConfig &config) 82 | { 83 | // TODO RTK 84 | if (config.output_mode != OutputMode::GPS) { 85 | GPS_WARN("EMLIDREACH: Unsupported Output Mode %i", (int)config.output_mode); 86 | return -1; 87 | } 88 | 89 | unsigned baud_allowed[] {57600, 115200, 230400}; 90 | 91 | for (unsigned i = 0; i < sizeof(baud_allowed) / sizeof(baud_allowed[0]); i++) { 92 | if (baudrate > 0 && baudrate != baud_allowed[i]) { 93 | continue; 94 | } 95 | 96 | _sentence_cnt = 0; 97 | _erb_decode_state = ERB_State::init; 98 | _erb_payload_len = 0; 99 | _erb_buff_cnt = 0; 100 | 101 | if (GPSHelper::setBaudrate(baud_allowed[i]) != 0) { 102 | continue; 103 | } 104 | 105 | if (! testConnection()) { 106 | continue; 107 | } 108 | 109 | baudrate = baud_allowed[i]; 110 | return 0; 111 | } 112 | 113 | return -1; 114 | } 115 | 116 | 117 | bool 118 | GPSDriverEmlidReach::testConnection() 119 | { 120 | _testing_connection = true; 121 | receive(500); 122 | _testing_connection = false; 123 | return _sentence_cnt >= AUTO_DETECT_MAX_READ_SENTENCE; 124 | } 125 | 126 | 127 | int 128 | GPSDriverEmlidReach::receive(unsigned timeout) 129 | { 130 | uint8_t read_buff[GPS_READ_BUFFER_SIZE]; 131 | unsigned read_buff_len = 0; 132 | uint8_t read_ind = 0; 133 | int return_status = 0; 134 | 135 | gps_abstime time_started = gps_absolute_time(); 136 | 137 | while (true) { 138 | // read from serial, timeout may be truncated further in read() 139 | read_buff_len = read(read_buff, sizeof(read_buff), timeout); 140 | 141 | if (read_buff_len > 0) { 142 | read_ind = 0; 143 | } 144 | 145 | // process data in buffer return by read() 146 | while (read_ind < read_buff_len) { 147 | int ret = 0; 148 | ret = erbParseChar(read_buff[read_ind++]); 149 | 150 | if (ret > 0) { 151 | _sentence_cnt ++; 152 | 153 | // when testig connection, we care about syntax not semantic 154 | if (! _testing_connection) { 155 | return_status = handleErbSentence(); 156 | } 157 | } 158 | } 159 | 160 | if (return_status > 0) { 161 | return return_status; 162 | } 163 | 164 | /* abort after timeout if no useful packets received */ 165 | if (time_started + timeout * 1000 < gps_absolute_time()) { 166 | // device timed out 167 | return -1; 168 | } 169 | } 170 | 171 | return -1; 172 | } 173 | 174 | 175 | //// ERB 176 | 177 | int 178 | GPSDriverEmlidReach::erbParseChar(uint8_t b) 179 | { 180 | int ret = 0; 181 | uint8_t *buff_ptr = (uint8_t *)&_erb_buff; 182 | 183 | switch (_erb_decode_state) { 184 | case ERB_State::init: 185 | if (b == ERB_SYNC_1) { 186 | _erb_buff_cnt = 0; 187 | buff_ptr[_erb_buff_cnt ++] = b; 188 | _erb_decode_state = ERB_State::got_sync_1; 189 | } 190 | 191 | break; 192 | 193 | case ERB_State::got_sync_1: 194 | if (b == ERB_SYNC_2) { 195 | buff_ptr[_erb_buff_cnt ++] = b; 196 | _erb_decode_state = ERB_State::got_sync_2; 197 | 198 | } else { 199 | _erb_decode_state = ERB_State::init; 200 | } 201 | 202 | break; 203 | 204 | case ERB_State::got_sync_2: 205 | if (b >= ERB_ID_VERSION && b <= ERB_ID_RTK) { 206 | buff_ptr[_erb_buff_cnt ++] = b; 207 | _erb_decode_state = ERB_State::got_id; 208 | 209 | if (b == ERB_ID_SPACE_INFO || b == ERB_ID_RTK) { 210 | //ignore those 211 | _erb_decode_state = ERB_State::init; 212 | } 213 | 214 | } else { 215 | _erb_decode_state = ERB_State::init; 216 | } 217 | 218 | break; 219 | 220 | case ERB_State::got_id: 221 | buff_ptr[_erb_buff_cnt ++] = b; 222 | _erb_decode_state = ERB_State::got_len_1; 223 | _erb_payload_len = b; 224 | break; 225 | 226 | case ERB_State::got_len_1: 227 | buff_ptr[_erb_buff_cnt ++] = b; 228 | _erb_decode_state = ERB_State::got_len_2; 229 | _erb_payload_len = (b << 8) | _erb_payload_len; 230 | break; 231 | 232 | case ERB_State::got_len_2: 233 | if (_erb_buff_cnt > ERB_SENTENCE_MAX_LEN - sizeof(erb_checksum_t)) { 234 | _erb_decode_state = ERB_State::init; 235 | 236 | } else { 237 | buff_ptr[_erb_buff_cnt ++] = b; 238 | } 239 | 240 | if (_erb_buff_cnt >= _erb_payload_len + static_cast(ERB_HEADER_LEN)) { 241 | _erb_decode_state = ERB_State::got_payload; 242 | } 243 | 244 | break; 245 | 246 | case ERB_State::got_payload: 247 | _erb_checksum.ck_a = b; 248 | _erb_decode_state = ERB_State::got_CK_A; 249 | break; 250 | 251 | case ERB_State::got_CK_A: 252 | _erb_checksum.ck_b = b; 253 | 254 | uint8_t cka = 0, ckb = 0; 255 | 256 | for (unsigned i = 2; i < _erb_payload_len + static_cast(ERB_HEADER_LEN); i++) { 257 | cka += buff_ptr[i]; 258 | ckb += cka; 259 | } 260 | 261 | if (cka == _erb_checksum.ck_a && ckb == _erb_checksum.ck_b) { 262 | ret = 1; 263 | 264 | } else { 265 | ret = 0; 266 | } 267 | 268 | _erb_decode_state = ERB_State::init; 269 | break; 270 | } 271 | 272 | return ret; 273 | } 274 | 275 | int 276 | GPSDriverEmlidReach::handleErbSentence() 277 | { 278 | int ret = 0; 279 | 280 | if (_erb_buff.header.id == ERB_ID_VERSION) { 281 | //GPS_INFO("EMLIDREACH: ERB version: %d.%d.%d", _buff[ERB_HEADER_LEN + 4], _buff[ERB_HEADER_LEN + 5], _buff[ERB_HEADER_LEN + 6]); 282 | 283 | } else if (_erb_buff.header.id == ERB_ID_GEODIC_POSITION) { 284 | 285 | _gps_position->timestamp = gps_absolute_time(); 286 | 287 | _last_POS_timeGPS = _erb_buff.payload.geodic_position.timeGPS; 288 | _gps_position->longitude_deg = _erb_buff.payload.geodic_position.longitude; 289 | _gps_position->latitude_deg = _erb_buff.payload.geodic_position.latitude; 290 | _gps_position->altitude_ellipsoid_m = _erb_buff.payload.geodic_position.altElipsoid; 291 | _gps_position->altitude_msl_m = _erb_buff.payload.geodic_position.altMeanSeaLevel; 292 | 293 | _rate_count_lat_lon++; 294 | 295 | _gps_position->eph = static_cast(_erb_buff.payload.geodic_position.accHorizontal) * 1e-3f; 296 | _gps_position->epv = static_cast(_erb_buff.payload.geodic_position.accVertical) * 1e-3f; 297 | 298 | _gps_position->vel_ned_valid = false; 299 | _gps_position->hdop = _hdop; 300 | _gps_position->vdop = _vdop; 301 | _gps_position->satellites_used = _satellites_used; 302 | _gps_position->fix_type = _fix_type; 303 | 304 | _POS_received = true; 305 | 306 | } else if (_erb_buff.header.id == ERB_ID_NAV_STATUS) { 307 | 308 | _fix_type = _erb_buff.payload.navigation_status.fixType; 309 | _fix_status = _erb_buff.payload.navigation_status.fixStatus; 310 | _satellites_used = _erb_buff.payload.navigation_status.numSatUsed; 311 | 312 | if (_fix_type == 0) { 313 | // no Fix 314 | _fix_type = 0; 315 | 316 | } else if (_fix_type == 1) { 317 | // Single 318 | _fix_type = (_satellites_used > 4) ? 3 : 2; 319 | 320 | } else if (_fix_type == 2) { 321 | // RTK Float 322 | _fix_type = 5; 323 | 324 | } else if (_fix_type == 3) { 325 | // RTK Fix 326 | _fix_type = 6; 327 | 328 | } else { 329 | _fix_type = 0; 330 | } 331 | 332 | 333 | } else if (_erb_buff.header.id == ERB_ID_DOPS) { 334 | 335 | _hdop = static_cast(_erb_buff.payload.dop.dopHorizontal) / 100.0f; 336 | _vdop = static_cast(_erb_buff.payload.dop.dopVertical) / 100.0f; 337 | 338 | } else if (_erb_buff.header.id == ERB_ID_VELOCITY_NED) { 339 | 340 | _last_VEL_timeGPS = _erb_buff.payload.ned_velocity.timeGPS; 341 | 342 | _gps_position->vel_n_m_s = static_cast(_erb_buff.payload.ned_velocity.velN) / 100.0f; 343 | _gps_position->vel_e_m_s = static_cast(_erb_buff.payload.ned_velocity.velE) / 100.0f; 344 | _gps_position->vel_d_m_s = static_cast(_erb_buff.payload.ned_velocity.velD) / 100.0f; 345 | 346 | _gps_position->vel_m_s = static_cast(_erb_buff.payload.ned_velocity.speed) / 100.0f; 347 | _gps_position->cog_rad = (static_cast(_erb_buff.payload.ned_velocity.heading) / 1e5f) * GPS_PI / 180.0f; 348 | 349 | _gps_position->s_variance_m_s = static_cast(_erb_buff.payload.ned_velocity.speedAccuracy) / 100.0f; 350 | 351 | _gps_position->vel_ned_valid = true; 352 | _rate_count_vel++; 353 | 354 | _VEL_received = true; 355 | 356 | } else if (_erb_buff.header.id == ERB_ID_SPACE_INFO) { 357 | 358 | } else if (_erb_buff.header.id == ERB_ID_RTK) { 359 | 360 | } else { 361 | //GPS_WARN("EMLIDREACH: ERB ID not known: %d", _erb_buff.header.id); 362 | } 363 | 364 | // Emlid doc: "If position and velocity are valid, it takes value 0x01, else it takes 0x00" 365 | if (_fix_status == 1 366 | && _POS_received && _VEL_received 367 | && _last_POS_timeGPS == _last_VEL_timeGPS) { 368 | ret = 1; 369 | _POS_received = false; 370 | _VEL_received = false; 371 | } 372 | 373 | return ret; 374 | } 375 | 376 | 377 | -------------------------------------------------------------------------------- /src/emlid_reach.h: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * 3 | * Copyright (c) 2012-2019 PX4 Development Team. All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions 7 | * are met: 8 | * 9 | * 1. Redistributions of source code must retain the above copyright 10 | * notice, this list of conditions and the following disclaimer. 11 | * 2. Redistributions in binary form must reproduce the above copyright 12 | * notice, this list of conditions and the following disclaimer in 13 | * the documentation and/or other materials provided with the 14 | * distribution. 15 | * 3. Neither the name PX4 nor the names of its contributors may be 16 | * used to endorse or promote products derived from this software 17 | * without specific prior written permission. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 20 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 21 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 22 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 23 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 24 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 25 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS 26 | * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 27 | * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 28 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 29 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 30 | * POSSIBILITY OF SUCH DAMAGE. 31 | * 32 | ****************************************************************************/ 33 | 34 | /** 35 | * @file emlid_reach.h 36 | * 37 | * @author Bastien Auneau 38 | */ 39 | 40 | #pragma once 41 | 42 | #include "gps_helper.h" 43 | 44 | // Emlid documentation 45 | // https://docs.emlid.com/reachm-plus/ 46 | // https://files.emlid.com/ERB.pdf 47 | 48 | #define ERB_HEADER_LEN 5 49 | // Not using ERB_ID_SPACE_INFO, so use a smaller buff length 50 | #define ERB_SENTENCE_MAX_LEN (sizeof(erb_message_t)) 51 | 52 | #define MAX_CONST(a, b) ((a>b) ? a : b) 53 | 54 | // Emlid ERB message definition 55 | #pragma pack(push, 1) 56 | 57 | typedef struct { 58 | uint8_t sync1; 59 | uint8_t sync2; 60 | uint8_t id; 61 | uint16_t length; 62 | } erb_header_t; 63 | 64 | typedef struct { 65 | uint8_t ck_a; 66 | uint8_t ck_b; 67 | } erb_checksum_t; 68 | 69 | typedef struct { 70 | uint32_t timeGPS; 71 | uint8_t verH; 72 | uint8_t verM; 73 | uint8_t verL; 74 | erb_checksum_t checksum; 75 | } erb_version_t; 76 | 77 | typedef struct { 78 | uint32_t timeGPS; 79 | double longitude; 80 | double latitude; 81 | double altElipsoid; 82 | double altMeanSeaLevel; 83 | uint32_t accHorizontal; 84 | uint32_t accVertical; 85 | erb_checksum_t checksum; 86 | } erb_geodic_position_t; 87 | 88 | typedef struct { 89 | uint32_t timeGPS; 90 | uint16_t weekGPS; 91 | uint8_t fixType; 92 | uint8_t fixStatus; 93 | uint8_t numSatUsed; 94 | erb_checksum_t checksum; 95 | } erb_navigation_status_t; 96 | 97 | typedef struct { 98 | uint32_t timeGPS; 99 | uint16_t dopGeometric; 100 | uint16_t dopPosition; 101 | uint16_t dopVertical; 102 | uint16_t dopHorizontal; 103 | erb_checksum_t checksum; 104 | } erb_dop_t; 105 | 106 | typedef struct { 107 | uint32_t timeGPS; 108 | int32_t velN; 109 | int32_t velE; 110 | int32_t velD; 111 | uint32_t speed; 112 | int32_t heading; 113 | uint32_t speedAccuracy; 114 | erb_checksum_t checksum; 115 | } erb_ned_velocity_t; 116 | 117 | typedef union { 118 | erb_version_t version; 119 | erb_geodic_position_t geodic_position; 120 | erb_navigation_status_t navigation_status; 121 | erb_dop_t dop; 122 | erb_ned_velocity_t ned_velocity; 123 | } erb_payload_t; 124 | 125 | typedef struct { 126 | erb_header_t header; 127 | erb_payload_t payload; 128 | } erb_message_t; 129 | 130 | #pragma pack(pop) 131 | 132 | 133 | /** 134 | * Driver class for Emlid Reach 135 | * Populates caller provided sensor_gps_s 136 | * Some ERB messages are cached and correlated by timestamp before publishing it 137 | */ 138 | class GPSDriverEmlidReach : public GPSHelper 139 | { 140 | public: 141 | GPSDriverEmlidReach(GPSCallbackPtr callback, void *callback_user, 142 | sensor_gps_s *gps_position, 143 | satellite_info_s *satellite_info 144 | ); 145 | 146 | virtual ~GPSDriverEmlidReach() = default; 147 | 148 | int receive(unsigned timeout) override; 149 | int configure(unsigned &baudrate, const GPSConfig &config) override; 150 | 151 | private: 152 | 153 | enum class ERB_State { 154 | init = 0, 155 | got_sync_1, // E 156 | got_sync_2, // R 157 | got_id, 158 | got_len_1, 159 | got_len_2, 160 | got_payload, 161 | got_CK_A 162 | }; 163 | 164 | 165 | /** NMEA parser state machine */ 166 | ERB_State _erb_decode_state; 167 | 168 | /** Buffer used by parser to build ERB sentences */ 169 | erb_message_t _erb_buff{}; 170 | uint16_t _erb_buff_cnt{}; 171 | 172 | /** Buffer used by parser to build ERB checksum */ 173 | erb_checksum_t _erb_checksum{}; 174 | uint8_t _erb_checksum_cnt{}; 175 | 176 | /** Pointer provided by caller, ie gps.cpp */ 177 | sensor_gps_s *_gps_position {nullptr}; 178 | /** Pointer provided by caller, gps.cpp */ 179 | satellite_info_s *_satellite_info {nullptr}; 180 | 181 | bool _testing_connection{false}; 182 | /** counts decoded sentence when testing connection */ 183 | unsigned _sentence_cnt{0}; 184 | 185 | uint16_t _erb_payload_len{0}; 186 | 187 | uint32_t _last_POS_timeGPS{0}; 188 | uint32_t _last_VEL_timeGPS{0}; 189 | bool _POS_received{false}; 190 | bool _VEL_received{false}; 191 | 192 | 193 | ///// ERB messages caches ///// 194 | uint8_t _fix_type{0}; 195 | uint8_t _fix_status{0}; 196 | uint8_t _satellites_used{0}; 197 | float _hdop{0}; 198 | float _vdop{0}; 199 | 200 | 201 | /** Feed ERB parser with received bytes from serial 202 | * @return len of decoded message, 0 if not completed, -1 if error 203 | */ 204 | int erbParseChar(uint8_t b); 205 | 206 | /** ERB sentence into sensor_gps_s or satellite_info_s, to be used by GPSHelper 207 | * @return 1 if gps_position updated, 2 for satellite_info_s (can be bit OR), 0 for nothing 208 | */ 209 | int handleErbSentence(); 210 | 211 | void computeNedVelocity(); 212 | 213 | bool testConnection(); 214 | 215 | }; 216 | 217 | -------------------------------------------------------------------------------- /src/femtomes.cpp: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * 3 | * Copyright (c) 2012-2018 PX4 Development Team. All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions 7 | * are met: 8 | * 9 | * 1. Redistributions of source code must retain the above copyright 10 | * notice, this list of conditions and the following disclaimer. 11 | * 2. Redistributions in binary form must reproduce the above copyright 12 | * notice, this list of conditions and the following disclaimer in 13 | * the documentation and/or other materials provided with the 14 | * distribution. 15 | * 3. Neither the name PX4 nor the names of its contributors may be 16 | * used to endorse or promote products derived from this software 17 | * without specific prior written permission. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 20 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 21 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 22 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 23 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 24 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 25 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS 26 | * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 27 | * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 28 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 29 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 30 | * POSSIBILITY OF SUCH DAMAGE. 31 | * 32 | ****************************************************************************/ 33 | 34 | #include 35 | #include 36 | #include 37 | #include 38 | #include 39 | #include 40 | 41 | #include "femtomes.h" 42 | #include "rtcm.h" 43 | #include "crc.h" 44 | 45 | 46 | /* ms, timeout for waiting for a response*/ 47 | #define FEMTO_RESPONSE_TIMEOUT 200 48 | 49 | #define FEMTO_MSG_MAX_LENGTH 256 50 | /* Femtomes ID for UAV output message */ 51 | #define FEMTO_MSG_ID_UAVGPS 8001 52 | #define FEMTO_MSG_ID_RTCM3 784 53 | #define FEMTO_MSG_ID_GPGGA 218 54 | #define FEMTO_MSG_ID_UAVSTATUS 8017 55 | 56 | /* Femto uavgps message frame premble 3 bytes*/ 57 | #define FEMTO_PREAMBLE1 0xaa 58 | #define FEMTO_PREAMBLE2 0x44 59 | #define FEMTO_PREAMBLE3 0x12 60 | 61 | #define MIN(X,Y) ((X) < (Y) ? (X) : (Y)) 62 | #define FEMTO_UNUSED(x) (void)x; 63 | 64 | #if defined _FMTOMES_DEBUG 65 | #define FEMTO_INFO(...) {GPS_INFO(__VA_ARGS__);} 66 | #define FEMTO_DEBUG(...) {GPS_WARN(__VA_ARGS__);} 67 | #define FEMTO_ERR(...) {GPS_ERR(__VA_ARGS__);} 68 | #else 69 | #define FEMTO_INFO(...) {(void)0;} 70 | #define FEMTO_DEBUG(...) {(void)0;} 71 | #define FEMTO_ERR(...) {GPS_WARN(__VA_ARGS__);} 72 | #endif 73 | 74 | 75 | GPSDriverFemto::GPSDriverFemto(GPSCallbackPtr callback, void *callback_user, 76 | struct sensor_gps_s *gps_position, 77 | satellite_info_s *satellite_info, 78 | float heading_offset) : 79 | GPSBaseStationSupport(callback, callback_user), 80 | _gps_position(gps_position), 81 | _satellite_info(satellite_info), 82 | _heading_offset(heading_offset) 83 | { 84 | decodeInit(); 85 | } 86 | 87 | GPSDriverFemto::~GPSDriverFemto() 88 | { 89 | delete _rtcm_parsing; 90 | } 91 | 92 | int GPSDriverFemto::handleMessage(int len) 93 | { 94 | int ret = 0; 95 | uint16_t messageid = _femto_msg.header.femto_header.messageid; 96 | 97 | if (messageid == FEMTO_MSG_ID_UAVGPS) { /**< uavgpsB*/ 98 | memcpy(&_femto_uav_gps, _femto_msg.data, sizeof(femto_uav_gps_t)); 99 | 100 | _gps_position->time_utc_usec = _femto_uav_gps.time_utc_usec; 101 | _gps_position->latitude_deg = _femto_uav_gps.lat / 1e7; 102 | _gps_position->longitude_deg = _femto_uav_gps.lon / 1e7; 103 | _gps_position->altitude_msl_m = _femto_uav_gps.alt / 1e3; 104 | _gps_position->altitude_ellipsoid_m = _femto_uav_gps.alt_ellipsoid / 1e3; 105 | _gps_position->s_variance_m_s = _femto_uav_gps.s_variance_m_s; 106 | _gps_position->c_variance_rad = _femto_uav_gps.c_variance_rad; 107 | _gps_position->eph = _femto_uav_gps.eph; 108 | _gps_position->epv = _femto_uav_gps.epv; 109 | _gps_position->hdop = _femto_uav_gps.hdop; 110 | _gps_position->vdop = _femto_uav_gps.vdop; 111 | _gps_position->noise_per_ms = _femto_uav_gps.noise_per_ms; 112 | _gps_position->jamming_indicator = _femto_uav_gps.jamming_indicator; 113 | _gps_position->vel_m_s = _femto_uav_gps.vel_m_s; 114 | _gps_position->vel_n_m_s = _femto_uav_gps.vel_n_m_s; 115 | _gps_position->vel_e_m_s = _femto_uav_gps.vel_e_m_s; 116 | _gps_position->vel_d_m_s = _femto_uav_gps.vel_d_m_s; 117 | _gps_position->cog_rad = _femto_uav_gps.cog_rad; 118 | _gps_position->timestamp_time_relative = _femto_uav_gps.timestamp_time_relative; 119 | _gps_position->fix_type = _femto_uav_gps.fix_type; 120 | _gps_position->vel_ned_valid = _femto_uav_gps.vel_ned_valid; 121 | _gps_position->satellites_used = _femto_uav_gps.satellites_used; 122 | 123 | if (_femto_uav_gps.heading_type == 6) { 124 | float heading = _femto_uav_gps.heading; 125 | heading *= M_PI_F / 180.0f; // deg to rad, now in range [0, 2pi] 126 | heading -= _heading_offset; // range: [-pi, 3pi] 127 | 128 | if (heading > M_PI_F) { 129 | heading -= 2.f * M_PI_F; // final range is [-pi, pi] 130 | } 131 | 132 | _gps_position->heading = heading; 133 | 134 | } else { 135 | _gps_position->heading = NAN; 136 | } 137 | 138 | _gps_position->timestamp = gps_absolute_time(); 139 | 140 | ret = 1; 141 | 142 | } else if (_satellite_info && messageid == FEMTO_MSG_ID_UAVSTATUS) { /**< set satellite info */ 143 | const femto_uav_status_t *uav_status = (const femto_uav_status_t *)_femto_msg.data; 144 | 145 | _satellite_info->count = MIN(uav_status->sat_number, satellite_info_s::SAT_INFO_MAX_SATELLITES); 146 | 147 | for (int8_t i = 0; i < _satellite_info->count; i++) { 148 | _satellite_info->svid[i] = uav_status->sat_status[i].svid; 149 | _satellite_info->used[i] = 1; 150 | _satellite_info->elevation[i] = uav_status->sat_status[i].ele; 151 | _satellite_info->azimuth[i] = uav_status->sat_status[i].azi; 152 | _satellite_info->snr[i] = uav_status->sat_status[i].cn0; 153 | _satellite_info->prn[i] = uav_status->sat_status[i].system_id; 154 | } 155 | 156 | ret = 2; 157 | 158 | } else if (OutputMode::RTCM == _output_mode 159 | && messageid == FEMTO_MSG_ID_GPGGA 160 | && (memcmp(_femto_msg.data + 3, "GGA,", 3) == 0)) { /**< GPGGA only used in base station, for survey-in */ 161 | int uiCalcComma = 0; 162 | 163 | for (int i = 0 ; i < len; i++) { 164 | if (_femto_msg.data[i] == ',') { uiCalcComma++; } 165 | } 166 | 167 | if (uiCalcComma == 14) { 168 | char *bufptr = (char *)(_femto_msg.data + 6); 169 | char *endp = nullptr; 170 | double ashtech_time = 0.0, lat = 0.0, lon = 0.0, alt = 0.0; 171 | int num_of_sv = 0, fix_quality = 0; 172 | double hdop = 99.9; 173 | char ns = '?', ew = '?'; 174 | 175 | if (bufptr && *(++bufptr) != ',') { ashtech_time = strtod(bufptr, &endp); bufptr = endp; } 176 | 177 | if (bufptr && *(++bufptr) != ',') { lat = strtod(bufptr, &endp); bufptr = endp; } 178 | 179 | if (bufptr && *(++bufptr) != ',') { ns = *(bufptr++); } 180 | 181 | if (bufptr && *(++bufptr) != ',') { lon = strtod(bufptr, &endp); bufptr = endp; } 182 | 183 | if (bufptr && *(++bufptr) != ',') { ew = *(bufptr++); } 184 | 185 | if (bufptr && *(++bufptr) != ',') { fix_quality = strtol(bufptr, &endp, 10); bufptr = endp; } 186 | 187 | if (bufptr && *(++bufptr) != ',') { num_of_sv = strtol(bufptr, &endp, 10); bufptr = endp; } 188 | 189 | if (bufptr && *(++bufptr) != ',') { hdop = strtod(bufptr, &endp); bufptr = endp; } 190 | 191 | if (bufptr && *(++bufptr) != ',') { alt = strtod(bufptr, &endp); bufptr = endp; } 192 | 193 | if (ns == 'S') { 194 | lat = -lat; 195 | } 196 | 197 | if (ew == 'W') { 198 | lon = -lon; 199 | } 200 | 201 | FEMTO_UNUSED(ashtech_time) 202 | FEMTO_UNUSED(hdop) 203 | 204 | if (!_correction_output_activated && 7 == fix_quality) { 205 | _survey_in_start = 0; /**< finished survey-in */ 206 | 207 | lat = (int(lat * 0.01) + (lat * 0.01 - int(lat * 0.01)) * 100.0 / 60.0) * 10000000; 208 | lon = (int(lon * 0.01) + (lon * 0.01 - int(lon * 0.01)) * 100.0 / 60.0) * 10000000; 209 | alt = alt * 1000; 210 | 211 | sendSurveyInStatusUpdate(false, true, lat, lon, (float)alt); 212 | activateRTCMOutput(); 213 | } 214 | 215 | if (_satellite_info) { 216 | _satellite_info->count = (uint8_t)num_of_sv; /**< base station satellite count */ 217 | } 218 | 219 | ret = 2; 220 | } 221 | } 222 | 223 | // handle survey-in status update 224 | if (_survey_in_start != 0) { 225 | const gps_abstime now = gps_absolute_time(); 226 | uint32_t survey_in_duration = (now - _survey_in_start) / 1000000; 227 | 228 | if (survey_in_duration != _base_settings.settings.survey_in.min_dur) { 229 | _base_settings.settings.survey_in.min_dur = survey_in_duration; 230 | sendSurveyInStatusUpdate(true, false); 231 | } 232 | } 233 | 234 | return ret; 235 | } 236 | 237 | void GPSDriverFemto::receiveWait(unsigned timeout_min) 238 | { 239 | gps_abstime time_started = gps_absolute_time(); 240 | 241 | while (gps_absolute_time() < time_started + timeout_min * 1000) { 242 | receive(timeout_min); 243 | } 244 | 245 | } 246 | 247 | int GPSDriverFemto::receive(unsigned timeout) 248 | { 249 | uint8_t buf[GPS_READ_BUFFER_SIZE]; 250 | 251 | /* timeout additional to poll */ 252 | uint64_t time_started = gps_absolute_time(); 253 | int j = 0; 254 | int bytes_count = 0; 255 | 256 | while (true) { 257 | 258 | /* pass received bytes to the packet decoder */ 259 | while (j < bytes_count) { 260 | int l = 0; 261 | 262 | if ((l = parseChar(buf[j])) > 0) { 263 | /* return to configure during configuration or to the gps driver during normal work 264 | * if a packet has arrived */ 265 | int ret = handleMessage(l); 266 | 267 | if (ret > 0) { 268 | _decode_state = FemtoDecodeState::pream_ble1; 269 | return ret; 270 | } 271 | } 272 | 273 | j++; 274 | } 275 | 276 | /* everything is read */ 277 | j = bytes_count = 0; 278 | 279 | /* then poll or read for new data */ 280 | int ret = read(buf, sizeof(buf), timeout * 2); 281 | 282 | if (ret < 0) { 283 | /* something went wrong when polling */ 284 | return -1; 285 | 286 | } else if (ret == 0) { 287 | /* Timeout while polling or just nothing read if reading, let's 288 | * stay here, and use timeout below. */ 289 | 290 | } else { 291 | /* if we have new data from GPS, go handle it */ 292 | bytes_count = ret; 293 | } 294 | 295 | /* in case we get crap from GPS or time out */ 296 | if (time_started + timeout * 1000 * 4 < gps_absolute_time()) { 297 | FEMTO_DEBUG("Femto: timeout\n"); 298 | return -1; 299 | } 300 | } 301 | } 302 | 303 | #define HEXDIGIT_CHAR(d) ((char)((d) + (((d) < 0xA) ? '0' : 'A'-0xA))) 304 | 305 | int GPSDriverFemto::parseChar(uint8_t temp) 306 | { 307 | int iRet = 0; 308 | 309 | if (_rtcm_parsing) { 310 | if (_rtcm_parsing->addByte(temp)) { 311 | FEMTO_DEBUG("Femto: got RTCM message with length %i", (int)_rtcm_parsing->messageLength()) 312 | gotRTCMMessage(_rtcm_parsing->message(), _rtcm_parsing->messageLength()); 313 | decodeInit(); 314 | _rtcm_parsing->reset(); 315 | return iRet; 316 | } 317 | } 318 | 319 | if (_output_mode == OutputMode::GPS) { 320 | 321 | switch (_decode_state) { 322 | case FemtoDecodeState::pream_ble1: 323 | if (temp == FEMTO_PREAMBLE1) { 324 | _decode_state = FemtoDecodeState::pream_ble2; 325 | _femto_msg.read = 0; 326 | } 327 | 328 | break; 329 | 330 | case FemtoDecodeState::pream_ble2: 331 | if (temp == FEMTO_PREAMBLE2) { 332 | _decode_state = FemtoDecodeState::pream_ble3; 333 | 334 | } else { 335 | _decode_state = FemtoDecodeState::pream_ble1; 336 | } 337 | 338 | break; 339 | 340 | case FemtoDecodeState::pream_ble3: 341 | if (temp == FEMTO_PREAMBLE3) { 342 | _decode_state = FemtoDecodeState::head_length; 343 | 344 | } else { 345 | _decode_state = FemtoDecodeState::pream_ble1; 346 | } 347 | 348 | break; 349 | 350 | case FemtoDecodeState::head_length: 351 | _femto_msg.header.data[0] = FEMTO_PREAMBLE1; 352 | _femto_msg.header.data[1] = FEMTO_PREAMBLE2; 353 | _femto_msg.header.data[2] = FEMTO_PREAMBLE3; 354 | _femto_msg.header.data[3] = temp; 355 | _femto_msg.header.femto_header.headerlength = temp; 356 | _decode_state = FemtoDecodeState::head_data; 357 | _femto_msg.read = 4; 358 | break; 359 | 360 | case FemtoDecodeState::head_data: 361 | if (_femto_msg.read >= sizeof(_femto_msg.header.data)) { 362 | _decode_state = FemtoDecodeState::pream_ble1; 363 | break; 364 | } 365 | 366 | _femto_msg.header.data[_femto_msg.read] = temp; 367 | _femto_msg.read++; 368 | 369 | if (_femto_msg.read >= _femto_msg.header.femto_header.headerlength) { 370 | _decode_state = FemtoDecodeState::data; 371 | } 372 | 373 | break; 374 | 375 | case FemtoDecodeState::data: 376 | if (_femto_msg.read >= FEMTO_MSG_MAX_LENGTH) { 377 | _decode_state = FemtoDecodeState::pream_ble1; 378 | break; 379 | } 380 | 381 | _femto_msg.data[_femto_msg.read - _femto_msg.header.femto_header.headerlength] = temp; 382 | _femto_msg.read++; 383 | 384 | if (_femto_msg.read >= (_femto_msg.header.femto_header.messagelength + _femto_msg.header.femto_header.headerlength)) { 385 | _decode_state = FemtoDecodeState::crc1; 386 | } 387 | 388 | break; 389 | 390 | case FemtoDecodeState::crc1: 391 | _femto_msg.crc = (uint32_t)(temp << 0); 392 | _decode_state = FemtoDecodeState::crc2; 393 | break; 394 | 395 | case FemtoDecodeState::crc2: 396 | _femto_msg.crc += (uint32_t)(temp << 8); 397 | _decode_state = FemtoDecodeState::crc3; 398 | break; 399 | 400 | case FemtoDecodeState::crc3: 401 | _femto_msg.crc += (uint32_t)(temp << 16); 402 | _decode_state = FemtoDecodeState::crc4; 403 | break; 404 | 405 | case FemtoDecodeState::crc4: { 406 | _femto_msg.crc += (uint32_t)(temp << 24); 407 | _decode_state = FemtoDecodeState::pream_ble1; 408 | 409 | uint32_t crc = calculateCRC32((uint32_t)_femto_msg.header.femto_header.headerlength, 410 | (uint8_t *)&_femto_msg.header.data, (uint32_t)0); 411 | crc = calculateCRC32((uint32_t)_femto_msg.header.femto_header.messagelength, (uint8_t *)&_femto_msg.data[0], crc); 412 | 413 | if (_femto_msg.crc == crc) { 414 | iRet = _femto_msg.read; 415 | 416 | } else { 417 | FEMTO_DEBUG("Femto: data packet is bad"); 418 | } 419 | } 420 | break; 421 | 422 | default: 423 | break; 424 | } 425 | 426 | } else { /**< RTCM mode */ 427 | 428 | switch (_decode_state) { 429 | case FemtoDecodeState::pream_ble1: 430 | if (temp == '$') { 431 | _decode_state = FemtoDecodeState::pream_nmea_got_sync1; 432 | _femto_msg.read = 0; 433 | _femto_msg.data[_femto_msg.read++] = temp; 434 | } 435 | 436 | break; 437 | 438 | case FemtoDecodeState::pream_nmea_got_sync1: 439 | if (temp == '$') { 440 | _decode_state = FemtoDecodeState::pream_nmea_got_sync1; 441 | _femto_msg.read = 0; 442 | 443 | } else if (temp == '*') { 444 | _decode_state = FemtoDecodeState::pream_nmea_got_asteriks; 445 | 446 | } 447 | 448 | if (_femto_msg.read >= (sizeof(_femto_msg.data) - 5)) { 449 | FEMTO_DEBUG("buffer overflow") 450 | _decode_state = FemtoDecodeState::pream_ble1; 451 | _femto_msg.read = 0; 452 | 453 | } else { 454 | _femto_msg.data[_femto_msg.read++] = temp; 455 | } 456 | 457 | break; 458 | 459 | case FemtoDecodeState::pream_nmea_got_asteriks: 460 | _femto_msg.data[_femto_msg.read++] = temp; 461 | _decode_state = FemtoDecodeState::pream_nmea_got_first_cs_byte; 462 | break; 463 | 464 | case FemtoDecodeState::pream_nmea_got_first_cs_byte: { 465 | _femto_msg.data[_femto_msg.read++] = temp; 466 | uint8_t checksum = 0; 467 | uint8_t *buffer = _femto_msg.data + 1; 468 | uint8_t *bufend = _femto_msg.data + _femto_msg.read - 3; 469 | 470 | for (; buffer < bufend; buffer++) { 471 | checksum ^= *buffer; 472 | } 473 | 474 | if ((HEXDIGIT_CHAR(checksum >> 4) == *(_femto_msg.data + _femto_msg.read - 2)) && 475 | (HEXDIGIT_CHAR(checksum & 0x0F) == *(_femto_msg.data + _femto_msg.read - 1))) { 476 | iRet = _femto_msg.read; 477 | _femto_msg.header.femto_header.messageid = FEMTO_MSG_ID_GPGGA; 478 | FEMTO_DEBUG("Femto: got NMEA message with length %i", _femto_msg.read) 479 | 480 | if (_rtcm_parsing) { 481 | _rtcm_parsing->reset(); 482 | } 483 | } 484 | 485 | decodeInit(); 486 | break; 487 | } 488 | 489 | default: 490 | break; 491 | } 492 | 493 | } 494 | 495 | return iRet; 496 | } 497 | 498 | void GPSDriverFemto::decodeInit() 499 | { 500 | _decode_state = FemtoDecodeState::pream_ble1; 501 | } 502 | 503 | int GPSDriverFemto::writeAckedCommandFemto(const char *command, const char *reply, const unsigned int timeout) 504 | { 505 | /**< write command*/ 506 | write(command, strlen(command)); 507 | /**< wait for reply*/ 508 | uint8_t buf[GPS_READ_BUFFER_SIZE]; 509 | gps_abstime time_started = gps_absolute_time(); 510 | 511 | while (time_started + timeout * 2000 > gps_absolute_time()) { 512 | int ret = read(buf, sizeof(buf), timeout); 513 | buf[sizeof(buf) - 1] = 0; 514 | 515 | if (ret > 0 && strstr((char *)buf, reply) != nullptr) { 516 | FEMTO_DEBUG("Femto: command reply success: %s", command); 517 | return 0; 518 | } 519 | } 520 | 521 | return -1; 522 | } 523 | 524 | int GPSDriverFemto::configure(unsigned &baudrate, const GPSConfig &config) 525 | { 526 | FEMTO_DEBUG("Femto: configure gps driver") 527 | 528 | if (config.output_mode != OutputMode::GPS && config.output_mode != OutputMode::RTCM) { 529 | FEMTO_DEBUG("Femto: Unsupported Output Mode %i", (int)config.output_mode); 530 | return -1; 531 | } 532 | 533 | _output_mode = config.output_mode; 534 | _configure_done = false; 535 | _correction_output_activated = false; 536 | /** Try different baudrates (115200 is the default for Femtomes) and request the baudrate that we want. */ 537 | const unsigned baudrates_to_try[] = {115200}; 538 | bool success = false; 539 | 540 | unsigned test_baudrate = 0; 541 | 542 | for (unsigned int baud_i = 0; !success && baud_i < sizeof(baudrates_to_try) / sizeof(baudrates_to_try[0]); baud_i++) { 543 | test_baudrate = baudrates_to_try[baud_i]; 544 | 545 | if (baudrate > 0 && baudrate != test_baudrate) { 546 | continue; /**< skip to next baudrate*/ 547 | } 548 | 549 | setBaudrate(test_baudrate); 550 | 551 | FEMTO_DEBUG("Femto: baudrate set to %i", test_baudrate); 552 | 553 | for (int run = 0; run < 2; ++run) { /** try several times*/ 554 | if (writeAckedCommandFemto("UNLOGALL THISPORT\r\n", "reset(); 611 | } 612 | 613 | if (_output_mode == OutputMode::GPS) { 614 | if (writeAckedCommandFemto("LOG UAVGPSB 0.1\r\n", "= 0 && len < (int)(sizeof(buffer))) { 687 | if (writeAckedCommandFemto(buffer, "FIX OK", FEMTO_RESPONSE_TIMEOUT) == 0) { 688 | FEMTO_DEBUG("Femto: command %s success", buffer) 689 | activateRTCMOutput(); 690 | sendSurveyInStatusUpdate(false, true, settings.latitude, settings.longitude, settings.altitude); 691 | 692 | if (writeAckedCommandFemto("LOG GPGGA 1 \r\n", " 5 | * 6 | * Redistribution and use in source and binary forms, with or without 7 | * modification, are permitted provided that the following conditions 8 | * are met: 9 | * 10 | * 1. Redistributions of source code must retain the above copyright 11 | * notice, this list of conditions and the following disclaimer. 12 | * 2. Redistributions in binary form must reproduce the above copyright 13 | * notice, this list of conditions and the following disclaimer in 14 | * the documentation and/or other materials provided with the 15 | * distribution. 16 | * 3. Neither the name PX4 nor the names of its contributors may be 17 | * used to endorse or promote products derived from this software 18 | * without specific prior written permission. 19 | * 20 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 21 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 22 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 23 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 24 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 25 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 26 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS 27 | * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 28 | * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 29 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 30 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 31 | * POSSIBILITY OF SUCH DAMAGE. 32 | * 33 | ****************************************************************************/ 34 | 35 | /** @file Femtomes protocol definitions */ 36 | #pragma once 37 | 38 | #include "base_station.h" 39 | 40 | class RTCMParsing; 41 | 42 | /*** femtomes protocol binary message and payload definitions ***/ 43 | #pragma pack(push, 1) 44 | 45 | /** 46 | * femto_uav_gps_t struct need to be packed 47 | */ 48 | typedef struct { 49 | uint64_t time_utc_usec; /** Timestamp (microseconds, UTC), this is the timestamp which comes from the gps module. It might be unavailable right after cold start, indicated by a value of 0*/ 50 | int32_t lat; /** Latitude in 1E-7 degrees*/ 51 | int32_t lon; /** Longitude in 1E-7 degrees*/ 52 | int32_t alt; /** Altitude in 1E-3 meters above MSL, (millimetres)*/ 53 | int32_t alt_ellipsoid; /** Altitude in 1E-3 meters bove Ellipsoid, (millimetres)*/ 54 | float s_variance_m_s; /** GPS speed accuracy estimate, (metres/sec)*/ 55 | float c_variance_rad; /** GPS course accuracy estimate, (radians)*/ 56 | float eph; /** GPS horizontal position accuracy (metres)*/ 57 | float epv; /** GPS vertical position accuracy (metres)*/ 58 | float hdop; /** Horizontal dilution of precision*/ 59 | float vdop; /** Vertical dilution of precision*/ 60 | int32_t noise_per_ms; /** GPS noise per millisecond*/ 61 | int32_t jamming_indicator; /** indicates jamming is occurring*/ 62 | float vel_m_s; /** GPS ground speed, (metres/sec)*/ 63 | float vel_n_m_s; /** GPS North velocity, (metres/sec)*/ 64 | float vel_e_m_s; /** GPS East velocity, (metres/sec)*/ 65 | float vel_d_m_s; /** GPS Down velocity, (metres/sec)*/ 66 | float cog_rad; /** Course over ground (NOT heading, but direction of movement), -PI..PI, (radians)*/ 67 | int32_t timestamp_time_relative;/** timestamp + timestamp_time_relative = Time of the UTC timestamp since system start, (microseconds)*/ 68 | float heading; /** heading angle of XYZ body frame rel to NED. Set to NaN if not available and updated (used for dual antenna GPS), (rad, [-PI, PI])*/ 69 | uint8_t fix_type; /** 0-1: no fix, 2: 2D fix, 3: 3D fix, 4: RTCM code differential, 5: Real-Time Kinematic, float, 6: Real-Time Kinematic, fixed, 8: Extrapolated. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix.*/ 70 | bool vel_ned_valid; /** True if NED velocity is valid*/ 71 | uint8_t satellites_used; /** Number of satellites used*/ 72 | uint8_t heading_type; /**< 0 invalid,5 for float,6 for fix*/ 73 | } femto_uav_gps_t; 74 | 75 | /** 76 | * femto_msg_header_t is femto data header 77 | */ 78 | typedef struct { 79 | uint8_t preamble[3]; /**< Frame header preamble 0xaa 0x44 0x12 */ 80 | uint8_t headerlength; /**< Frame header length ,from the beginning 0xaa */ 81 | uint16_t messageid; /**< Frame message id ,example the FEMTO_MSG_ID_UAVGPS 8001*/ 82 | uint8_t messagetype; /**< Frame message id type */ 83 | uint8_t portaddr; /**< Frame message port address */ 84 | uint16_t messagelength; /**< Frame message data length,from the beginning headerlength+1,end headerlength + messagelength*/ 85 | uint16_t sequence; 86 | uint8_t idletime; /**< Frame message idle module time */ 87 | uint8_t timestatus; 88 | uint16_t week; 89 | uint32_t tow; 90 | uint32_t recvstatus; 91 | uint16_t resv; 92 | uint16_t recvswver; 93 | } femto_msg_header_t; 94 | 95 | /** 96 | * uav status data 97 | */ 98 | typedef struct { 99 | int32_t master_ant_status; 100 | int32_t slave_ant_status; 101 | uint16_t master_ant_power; 102 | uint16_t slave_ant_power; 103 | uint32_t jam_status; 104 | uint32_t spoofing_status; 105 | uint16_t reserved16_1; 106 | uint16_t diff_age; /**< in unit of second*/ 107 | uint32_t base_id; 108 | uint32_t reserved32_1; 109 | uint32_t reserved32_2; 110 | uint32_t sat_number; 111 | struct femto_uav_sat_status_data_t { 112 | uint8_t svid; 113 | uint8_t system_id; 114 | uint8_t cn0; /**< in unit of dB-Hz*/ 115 | uint8_t ele; /**< in unit of degree*/ 116 | uint16_t azi; /**< in unit of degree*/ 117 | uint16_t status; 118 | } sat_status[64]; /**< uav status of all satellites */ 119 | } femto_uav_status_t; 120 | 121 | /** 122 | * Analysis Femto uavgps frame header 123 | */ 124 | typedef union { 125 | femto_msg_header_t femto_header; 126 | uint8_t data[28]; 127 | } msg_header_t; 128 | 129 | /** 130 | * receive Femto complete uavgps frame 131 | */ 132 | typedef struct { 133 | uint8_t data[600]; /**< receive Frame message content */ 134 | uint32_t crc; /**< receive Frame message crc 4 bytes */ 135 | msg_header_t header; /**< receive Frame message header */ 136 | uint16_t read; /**< receive Frame message read bytes count */ 137 | } femto_msg_t; 138 | 139 | #pragma pack(pop) 140 | /*** END OF femtomes protocol binary message and payload definitions ***/ 141 | 142 | enum class FemtoDecodeState { 143 | pream_ble1, /**< Frame header preamble first byte 0xaa */ 144 | pream_ble2, /**< Frame header preamble second byte 0x44 */ 145 | pream_ble3, /**< Frame header preamble third byte 0x12 */ 146 | head_length, /**< Frame header length */ 147 | head_data, /**< Frame header data */ 148 | data, /**< Frame data */ 149 | crc1, /**< Frame crc1 */ 150 | crc2, /**< Frame crc2 */ 151 | crc3, /**< Frame crc3 */ 152 | crc4, /**< Frame crc4 */ 153 | 154 | pream_nmea_got_sync1, /**< NMEA Frame '$' */ 155 | pream_nmea_got_asteriks, /**< NMEA Frame '*' */ 156 | pream_nmea_got_first_cs_byte, /**< NMEA Frame cs first byte */ 157 | }; 158 | 159 | class GPSDriverFemto : public GPSBaseStationSupport 160 | { 161 | public: 162 | /** 163 | * @param heading_offset heading offset in radians [-pi, pi]. It is substracted from the measurement. 164 | */ 165 | GPSDriverFemto(GPSCallbackPtr callback, void *callback_user, struct sensor_gps_s *gps_position, 166 | satellite_info_s *satellite_info = nullptr, 167 | float heading_offset = 0.f); 168 | virtual ~GPSDriverFemto(); 169 | 170 | int receive(unsigned timeout) override; 171 | int configure(unsigned &baudrate, const GPSConfig &config) override; 172 | 173 | private: 174 | 175 | /** 176 | * when Constructor is work, initialize parameters 177 | */ 178 | void decodeInit(void); 179 | 180 | /** 181 | * check the message if whether is 8001,memcpy data to _gps_position 182 | */ 183 | int handleMessage(int len); 184 | 185 | /** 186 | * analysis frame data from buf[] to _femto_msg and check the frame is suceess or not 187 | */ 188 | int parseChar(uint8_t b); 189 | 190 | /** 191 | * Write a command and wait for a (N)Ack 192 | * @return 0 on success, <0 otherwise 193 | */ 194 | int writeAckedCommandFemto(const char *command, const char *reply, const unsigned timeout); 195 | 196 | /** 197 | * receive data for at least the specified amount of time 198 | */ 199 | void receiveWait(unsigned timeout_min); 200 | 201 | /** 202 | * enable output of correction output 203 | */ 204 | void activateCorrectionOutput(); 205 | 206 | /** 207 | * enable output of rtcm 208 | */ 209 | void activateRTCMOutput(); 210 | 211 | /** 212 | * update survery in status of QGC RTK GPS 213 | */ 214 | void sendSurveyInStatusUpdate(bool active, bool valid, double latitude = (double)NAN, 215 | double longitude = (double)NAN, float altitude = NAN); 216 | 217 | 218 | struct sensor_gps_s *_gps_position {nullptr}; 219 | FemtoDecodeState _decode_state{FemtoDecodeState::pream_ble1}; 220 | femto_uav_gps_t _femto_uav_gps; 221 | femto_msg_t _femto_msg; 222 | satellite_info_s *_satellite_info{nullptr}; 223 | float _heading_offset; 224 | 225 | RTCMParsing *_rtcm_parsing{nullptr}; 226 | OutputMode _output_mode{OutputMode::GPS}; 227 | bool _configure_done{false}; 228 | bool _correction_output_activated{false}; 229 | 230 | gps_abstime _survey_in_start{0}; 231 | }; 232 | -------------------------------------------------------------------------------- /src/gps_helper.cpp: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * 3 | * Copyright (c) 2012-2018 PX4 Development Team. All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions 7 | * are met: 8 | * 9 | * 1. Redistributions of source code must retain the above copyright 10 | * notice, this list of conditions and the following disclaimer. 11 | * 2. Redistributions in binary form must reproduce the above copyright 12 | * notice, this list of conditions and the following disclaimer in 13 | * the documentation and/or other materials provided with the 14 | * distribution. 15 | * 3. Neither the name PX4 nor the names of its contributors may be 16 | * used to endorse or promote products derived from this software 17 | * without specific prior written permission. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 20 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 21 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 22 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 23 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 24 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 25 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS 26 | * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 27 | * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 28 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 29 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 30 | * POSSIBILITY OF SUCH DAMAGE. 31 | * 32 | ****************************************************************************/ 33 | 34 | #include "gps_helper.h" 35 | #include 36 | 37 | #ifndef M_PI 38 | #define M_PI 3.141592653589793238462643383280 39 | #endif 40 | 41 | 42 | /** 43 | * @file gps_helper.cpp 44 | * 45 | * @author Thomas Gubler 46 | * @author Julian Oes 47 | */ 48 | 49 | GPSHelper::GPSHelper(GPSCallbackPtr callback, void *callback_user) : 50 | _callback(callback), 51 | _callback_user(callback_user) 52 | { 53 | } 54 | 55 | void 56 | GPSHelper::resetUpdateRates() 57 | { 58 | _rate_count_vel = 0; 59 | _rate_count_lat_lon = 0; 60 | _interval_rate_start = gps_absolute_time(); 61 | } 62 | 63 | void 64 | GPSHelper::storeUpdateRates() 65 | { 66 | _rate_vel = _rate_count_vel / (((float)(gps_absolute_time() - _interval_rate_start)) / 1000000.0f); 67 | _rate_lat_lon = _rate_count_lat_lon / (((float)(gps_absolute_time() - _interval_rate_start)) / 1000000.0f); 68 | } 69 | 70 | void GPSHelper::ECEF2lla(double ecef_x, double ecef_y, double ecef_z, double &latitude, double &longitude, 71 | float &altitude) 72 | { 73 | // WGS84 ellipsoid constants 74 | constexpr double a = 6378137.; // radius 75 | constexpr double e = 8.1819190842622e-2; // eccentricity 76 | 77 | constexpr double asq = a * a; 78 | constexpr double esq = e * e; 79 | 80 | double x = ecef_x; 81 | double y = ecef_y; 82 | double z = ecef_z; 83 | 84 | double b = sqrt(asq * (1. - esq)); 85 | double bsq = b * b; 86 | double ep = sqrt((asq - bsq) / bsq); 87 | double p = sqrt(x * x + y * y); 88 | double th = atan2(a * z, b * p); 89 | 90 | longitude = atan2(y, x); 91 | double sin_th = sin(th); 92 | double cos_th = cos(th); 93 | latitude = atan2(z + ep * ep * b * sin_th * sin_th * sin_th, p - esq * a * cos_th * cos_th * cos_th); 94 | double sin_lat = sin(latitude); 95 | double N = a / sqrt(1. - esq * sin_lat * sin_lat); 96 | altitude = (float)(p / cos(latitude) - N); 97 | 98 | // rad to deg 99 | longitude *= 180. / M_PI; 100 | latitude *= 180. / M_PI; 101 | 102 | // correction for altitude near poles left out. 103 | } 104 | -------------------------------------------------------------------------------- /src/gps_helper.h: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * 3 | * Copyright (c) 2012-2014 PX4 Development Team. All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions 7 | * are met: 8 | * 9 | * 1. Redistributions of source code must retain the above copyright 10 | * notice, this list of conditions and the following disclaimer. 11 | * 2. Redistributions in binary form must reproduce the above copyright 12 | * notice, this list of conditions and the following disclaimer in 13 | * the documentation and/or other materials provided with the 14 | * distribution. 15 | * 3. Neither the name PX4 nor the names of its contributors may be 16 | * used to endorse or promote products derived from this software 17 | * without specific prior written permission. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 20 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 21 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 22 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 23 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 24 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 25 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS 26 | * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 27 | * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 28 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 29 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 30 | * POSSIBILITY OF SUCH DAMAGE. 31 | * 32 | ****************************************************************************/ 33 | 34 | /** 35 | * @file gps_helper.h 36 | * @author Thomas Gubler 37 | * @author Julian Oes 38 | */ 39 | 40 | #pragma once 41 | 42 | #include 43 | #include 44 | 45 | #ifndef GPS_DEFINITIONS_HEADER 46 | #define GPS_DEFINITIONS_HEADER "../../definitions.h" 47 | #endif 48 | 49 | #include GPS_DEFINITIONS_HEADER 50 | 51 | #ifndef GPS_READ_BUFFER_SIZE 52 | #define GPS_READ_BUFFER_SIZE 150 ///< buffer size for the read() call. Messages can be longer than that. 53 | #endif 54 | 55 | #ifndef M_PI_F 56 | # define M_PI_F 3.14159265358979323846f 57 | #endif 58 | 59 | enum class GPSCallbackType { 60 | /** 61 | * Read data from device. This is a blocking operation with a timeout. 62 | * data1: points to a buffer to be written to. The first sizeof(int) bytes contain the 63 | * timeout in ms when calling the method. 64 | * data2: buffer length in bytes. Less bytes than this can be read. 65 | * return: num read bytes, 0 on timeout (the method can actually also return 0 before 66 | * the timeout happens). 67 | */ 68 | readDeviceData = 0, 69 | 70 | /** 71 | * Write data to device 72 | * data1: data to be written 73 | * data2: number of bytes to write 74 | * return: num written bytes 75 | */ 76 | writeDeviceData, 77 | 78 | /** 79 | * set Baudrate 80 | * data1: ignored 81 | * data2: baudrate 82 | * return: 0 on success 83 | */ 84 | setBaudrate, 85 | 86 | /** 87 | * Got an RTCM message from the device. 88 | * data1: pointer to the message 89 | * data2: message length 90 | * return: ignored 91 | */ 92 | gotRTCMMessage, 93 | 94 | /** 95 | * Got a relative position message from the device. 96 | * data1: pointer to the message 97 | * data2: message length 98 | * return: ignored 99 | */ 100 | gotRelativePositionMessage, 101 | 102 | /** 103 | * message about current survey-in status 104 | * data1: points to a SurveyInStatus struct 105 | * data2: ignored 106 | * return: ignored 107 | */ 108 | surveyInStatus, 109 | 110 | /** 111 | * can be used to set the current clock accurately 112 | * data1: pointer to a timespec struct 113 | * data2: ignored 114 | * return: ignored 115 | */ 116 | setClock, 117 | }; 118 | 119 | enum class GPSRestartType { 120 | None, 121 | 122 | /** 123 | * In hot start mode, the receiver was powered down only for a short time (4 hours or less), 124 | * so that its ephemeris is still valid. Since the receiver doesn't need to download ephemeris 125 | * again, this is the fastest startup method. 126 | */ 127 | Hot, 128 | 129 | /** 130 | * In warm start mode, the receiver has approximate information for time, position, and coarse 131 | * satellite position data (Almanac). In this mode, after power-up, the receiver normally needs 132 | * to download ephemeris before it can calculate position and velocity data. 133 | */ 134 | Warm, 135 | 136 | /** 137 | * In cold start mode, the receiver has no information from the last position at startup. 138 | * Therefore, the receiver must search the full time and frequency space, and all possible 139 | * satellite numbers. If a satellite signal is found, it is tracked to decode the ephemeris, 140 | * whereas the other channels continue to search satellites. Once there is a sufficient number 141 | * of satellites with valid ephemeris, the receiver can calculate position and velocity data. 142 | */ 143 | Cold 144 | }; 145 | 146 | /** Callback function for platform-specific stuff. 147 | * data1 and data2 depend on type and user is the custom user-supplied argument. 148 | * @return <0 on error, >=0 on success (depending on type) 149 | */ 150 | typedef int (*GPSCallbackPtr)(GPSCallbackType type, void *data1, int data2, void *user); 151 | 152 | 153 | struct SurveyInStatus { 154 | double latitude; /**< NAN if unknown/not set [deg] */ 155 | double longitude; /**< NAN if unknown/not set [deg] */ 156 | float altitude; /**< NAN if unknown/not set [m] */ 157 | uint32_t mean_accuracy; /**< [mm] */ 158 | uint32_t duration; /**< [s] */ 159 | uint8_t flags; /**< bit 0: valid, bit 1: active */ 160 | }; 161 | 162 | // TODO: this number seems wrong 163 | #define GPS_EPOCH_SECS ((time_t)1234567890ULL) 164 | 165 | class GPSHelper 166 | { 167 | public: 168 | enum class OutputMode : uint8_t { 169 | GPS = 0, ///< normal GPS output 170 | GPSAndRTCM, ///< normal GPS+RTCM output 171 | RTCM ///< request RTCM output. This is used for (fixed position) base stations 172 | }; 173 | 174 | enum class Interface : uint8_t { 175 | UART = 0, 176 | SPI 177 | }; 178 | 179 | /** 180 | * Bitmask for GPS_1_GNSS and GPS_2_GNSS 181 | * No bits set should keep the receiver's default config 182 | */ 183 | enum class GNSSSystemsMask : int32_t { 184 | RECEIVER_DEFAULTS = 0, 185 | ENABLE_GPS = 1 << 0, 186 | ENABLE_SBAS = 1 << 1, 187 | ENABLE_GALILEO = 1 << 2, 188 | ENABLE_BEIDOU = 1 << 3, 189 | ENABLE_GLONASS = 1 << 4, 190 | ENABLE_NAVIC = 1 << 5 191 | }; 192 | 193 | enum class InterfaceProtocolsMask : int32_t { 194 | ALL_DISABLED = 0, 195 | I2C_IN_PROT_UBX = 1 << 0, 196 | I2C_IN_PROT_NMEA = 1 << 1, 197 | I2C_IN_PROT_RTCM3X = 1 << 2, 198 | I2C_OUT_PROT_UBX = 1 << 3, 199 | I2C_OUT_PROT_NMEA = 1 << 4, 200 | I2C_OUT_PROT_RTCM3X = 1 << 5 201 | }; 202 | 203 | struct GPSConfig { 204 | OutputMode output_mode; 205 | GNSSSystemsMask gnss_systems; 206 | InterfaceProtocolsMask interface_protocols; 207 | }; 208 | 209 | 210 | GPSHelper(GPSCallbackPtr callback, void *callback_user); 211 | virtual ~GPSHelper() = default; 212 | 213 | /** 214 | * configure the device 215 | * @param baud Input and output parameter: if set to 0, the baudrate will be automatically detected and set to 216 | * the detected baudrate. If not 0, a fixed baudrate is used. 217 | * @param config GPS Config 218 | * @return 0 on success, <0 otherwise 219 | */ 220 | virtual int configure(unsigned &baud, const GPSConfig &config) = 0; 221 | 222 | /** 223 | * receive & handle new data from the device 224 | * @param timeout [ms] 225 | * @return <0 on error, otherwise a bitset: 226 | * bit 0 set: got gps position update 227 | * bit 1 set: got satellite info update 228 | */ 229 | virtual int receive(unsigned timeout) = 0; 230 | 231 | /** 232 | * Reset GPS device 233 | * @param restart_type 234 | * @return <0 failure 235 | * -1 not implemented 236 | * 0 success 237 | */ 238 | virtual int reset(GPSRestartType restart_type) { (void)restart_type; return -1; } 239 | 240 | float getPositionUpdateRate() { return _rate_lat_lon; } 241 | float getVelocityUpdateRate() { return _rate_vel; } 242 | void resetUpdateRates(); 243 | void storeUpdateRates(); 244 | 245 | /** 246 | * Allow a driver to disable RTCM injection 247 | */ 248 | virtual bool shouldInjectRTCM() { return true; } 249 | 250 | protected: 251 | 252 | /** 253 | * read from device 254 | * @param buf: pointer to read buffer 255 | * @param buf_length: size of read buffer 256 | * @param timeout: timeout in ms 257 | * @return: 0 for nothing read, or poll timed out 258 | * < 0 for error 259 | * > 0 number of bytes read 260 | */ 261 | int read(uint8_t *buf, int buf_length, int timeout) 262 | { 263 | memcpy(buf, &timeout, sizeof(timeout)); 264 | return _callback(GPSCallbackType::readDeviceData, buf, buf_length, _callback_user); 265 | } 266 | 267 | /** 268 | * write to the device 269 | * @param buf 270 | * @param buf_length 271 | * @return num written bytes, -1 on error 272 | */ 273 | int write(const void *buf, int buf_length) 274 | { 275 | return _callback(GPSCallbackType::writeDeviceData, (void *)buf, buf_length, _callback_user); 276 | } 277 | 278 | /** 279 | * set the Baudrate 280 | * @param baudrate 281 | * @return 0 on success, <0 otherwise 282 | */ 283 | int setBaudrate(int baudrate) 284 | { 285 | return _callback(GPSCallbackType::setBaudrate, nullptr, baudrate, _callback_user); 286 | } 287 | 288 | void surveyInStatus(SurveyInStatus &status) 289 | { 290 | _callback(GPSCallbackType::surveyInStatus, &status, 0, _callback_user); 291 | } 292 | 293 | /** got an RTCM message from the device */ 294 | void gotRTCMMessage(uint8_t *buf, int buf_length) 295 | { 296 | _callback(GPSCallbackType::gotRTCMMessage, buf, buf_length, _callback_user); 297 | } 298 | 299 | /** got a relative position message from the device */ 300 | void gotRelativePositionMessage(sensor_gnss_relative_s &gnss_relative) 301 | { 302 | _callback(GPSCallbackType::gotRelativePositionMessage, &gnss_relative, sizeof(sensor_gnss_relative_s), _callback_user); 303 | } 304 | 305 | void setClock(timespec &t) 306 | { 307 | _callback(GPSCallbackType::setClock, &t, 0, _callback_user); 308 | } 309 | 310 | /** 311 | * Convert an ECEF (Earth Centered Earth Fixed) coordinate to LLA WGS84 (Lat, Lon, Alt). 312 | * Ported from: https://stackoverflow.com/a/25428344 313 | * @param ecef_x ECEF X-coordinate [m] 314 | * @param ecef_y ECEF Y-coordinate [m] 315 | * @param ecef_z ECEF Z-coordinate [m] 316 | * @param latitude [deg] 317 | * @param longitude [deg] 318 | * @param altitude [m] 319 | */ 320 | static void ECEF2lla(double ecef_x, double ecef_y, double ecef_z, double &latitude, double &longitude, float &altitude); 321 | 322 | GPSCallbackPtr _callback{nullptr}; 323 | void *_callback_user{}; 324 | 325 | uint8_t _rate_count_lat_lon{}; 326 | uint8_t _rate_count_vel{}; 327 | 328 | float _rate_lat_lon{0.0f}; 329 | float _rate_vel{0.0f}; 330 | 331 | uint64_t _interval_rate_start{0}; 332 | }; 333 | 334 | inline bool operator&(GPSHelper::GNSSSystemsMask a, GPSHelper::GNSSSystemsMask b) 335 | { 336 | return static_cast(a) & static_cast(b); 337 | } 338 | 339 | inline bool operator&(GPSHelper::InterfaceProtocolsMask a, GPSHelper::InterfaceProtocolsMask b) 340 | { 341 | return static_cast(a) & static_cast(b); 342 | } 343 | -------------------------------------------------------------------------------- /src/mtk.cpp: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * 3 | * Copyright (c) 2012-2015 PX4 Development Team. All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions 7 | * are met: 8 | * 9 | * 1. Redistributions of source code must retain the above copyright 10 | * notice, this list of conditions and the following disclaimer. 11 | * 2. Redistributions in binary form must reproduce the above copyright 12 | * notice, this list of conditions and the following disclaimer in 13 | * the documentation and/or other materials provided with the 14 | * distribution. 15 | * 3. Neither the name PX4 nor the names of its contributors may be 16 | * used to endorse or promote products derived from this software 17 | * without specific prior written permission. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 20 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 21 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 22 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 23 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 24 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 25 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS 26 | * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 27 | * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 28 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 29 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 30 | * POSSIBILITY OF SUCH DAMAGE. 31 | * 32 | ****************************************************************************/ 33 | 34 | /** 35 | * @file mtk.cpp 36 | * 37 | * @author Thomas Gubler 38 | * @author Julian Oes 39 | */ 40 | 41 | #include "mtk.h" 42 | 43 | #include 44 | #include 45 | #include 46 | #include 47 | #include 48 | 49 | 50 | GPSDriverMTK::GPSDriverMTK(GPSCallbackPtr callback, void *callback_user, sensor_gps_s *gps_position) : 51 | GPSHelper(callback, callback_user), 52 | _gps_position(gps_position) 53 | { 54 | decodeInit(); 55 | } 56 | 57 | int 58 | GPSDriverMTK::configure(unsigned &baudrate, const GPSConfig &config) 59 | { 60 | if (config.output_mode != OutputMode::GPS) { 61 | GPS_WARN("MTK: Unsupported Output Mode %i", (int)config.output_mode); 62 | return -1; 63 | } 64 | 65 | if (baudrate > 0 && baudrate != MTK_BAUDRATE) { 66 | return -1; 67 | } 68 | 69 | /* set baudrate first */ 70 | if (GPSHelper::setBaudrate(MTK_BAUDRATE) != 0) { 71 | return -1; 72 | } 73 | 74 | baudrate = MTK_BAUDRATE; 75 | 76 | /* Write config messages, don't wait for an answer */ 77 | if (strlen(MTK_OUTPUT_5HZ) != write(MTK_OUTPUT_5HZ, strlen(MTK_OUTPUT_5HZ))) { 78 | goto errout; 79 | } 80 | 81 | gps_usleep(10000); 82 | 83 | if (strlen(MTK_SET_BINARY) != write(MTK_SET_BINARY, strlen(MTK_SET_BINARY))) { 84 | goto errout; 85 | } 86 | 87 | gps_usleep(10000); 88 | 89 | if (strlen(MTK_SBAS_ON) != write(MTK_SBAS_ON, strlen(MTK_SBAS_ON))) { 90 | goto errout; 91 | } 92 | 93 | gps_usleep(10000); 94 | 95 | if (strlen(MTK_WAAS_ON) != write(MTK_WAAS_ON, strlen(MTK_WAAS_ON))) { 96 | goto errout; 97 | } 98 | 99 | gps_usleep(10000); 100 | 101 | if (strlen(MTK_NAVTHRES_OFF) != write(MTK_NAVTHRES_OFF, strlen(MTK_NAVTHRES_OFF))) { 102 | goto errout; 103 | } 104 | 105 | return 0; 106 | 107 | errout: 108 | GPS_WARN("mtk: config write failed"); 109 | return -1; 110 | } 111 | 112 | int 113 | GPSDriverMTK::receive(unsigned timeout) 114 | { 115 | uint8_t buf[GPS_READ_BUFFER_SIZE]; 116 | gps_mtk_packet_t packet{}; 117 | 118 | /* timeout additional to poll */ 119 | gps_abstime time_started = gps_absolute_time(); 120 | 121 | int j = 0; 122 | 123 | while (true) { 124 | 125 | int ret = read(buf, sizeof(buf), timeout); 126 | 127 | if (ret > 0) { 128 | /* first read whatever is left */ 129 | if (j < ret) { 130 | /* pass received bytes to the packet decoder */ 131 | while (j < ret) { 132 | if (parseChar(buf[j], packet) > 0) { 133 | handleMessage(packet); 134 | return 1; 135 | } 136 | 137 | j++; 138 | } 139 | 140 | /* everything is read */ 141 | j = 0; 142 | } 143 | 144 | } else { 145 | gps_usleep(20000); 146 | } 147 | 148 | /* in case we keep trying but only get crap from GPS */ 149 | if (time_started + timeout * 1000 < gps_absolute_time()) { 150 | return -1; 151 | } 152 | } 153 | } 154 | 155 | void 156 | GPSDriverMTK::decodeInit() 157 | { 158 | _rx_ck_a = 0; 159 | _rx_ck_b = 0; 160 | _rx_count = 0; 161 | _decode_state = MTK_DECODE_UNINIT; 162 | } 163 | int 164 | GPSDriverMTK::parseChar(uint8_t b, gps_mtk_packet_t &packet) 165 | { 166 | int ret = 0; 167 | 168 | if (_decode_state == MTK_DECODE_UNINIT) { 169 | 170 | if (b == MTK_SYNC1_V16) { 171 | _decode_state = MTK_DECODE_GOT_CK_A; 172 | _mtk_revision = 16; 173 | 174 | } else if (b == MTK_SYNC1_V19) { 175 | _decode_state = MTK_DECODE_GOT_CK_A; 176 | _mtk_revision = 19; 177 | } 178 | 179 | } else if (_decode_state == MTK_DECODE_GOT_CK_A) { 180 | if (b == MTK_SYNC2) { 181 | _decode_state = MTK_DECODE_GOT_CK_B; 182 | 183 | } else { 184 | // Second start symbol was wrong, reset state machine 185 | decodeInit(); 186 | } 187 | 188 | } else if (_decode_state == MTK_DECODE_GOT_CK_B) { 189 | // Add to checksum 190 | if (_rx_count < 33) { 191 | addByteToChecksum(b); 192 | } 193 | 194 | // Fill packet buffer 195 | ((uint8_t *)(&packet))[_rx_count] = b; 196 | _rx_count++; 197 | 198 | /* Packet size minus checksum, XXX ? */ 199 | if (_rx_count >= sizeof(packet)) { 200 | /* Compare checksum */ 201 | if (_rx_ck_a == packet.ck_a && _rx_ck_b == packet.ck_b) { 202 | ret = 1; 203 | 204 | } else { 205 | ret = -1; 206 | } 207 | 208 | // Reset state machine to decode next packet 209 | decodeInit(); 210 | } 211 | } 212 | 213 | return ret; 214 | } 215 | 216 | void 217 | GPSDriverMTK::handleMessage(gps_mtk_packet_t &packet) 218 | { 219 | if (_mtk_revision == 16) { 220 | _gps_position->latitude_deg = packet.latitude / 1e6; // from degrees*1e6 to degrees 221 | _gps_position->longitude_deg = packet.longitude / 1e6; // from degrees*1e6 to degrees 222 | 223 | } else if (_mtk_revision == 19) { 224 | _gps_position->latitude_deg = packet.latitude / 1e7; // from degrees*1e7 to degrees 225 | _gps_position->longitude_deg = packet.longitude / 1e7; // from degrees*1e7 to degrees 226 | 227 | } else { 228 | GPS_WARN("mtk: unknown revision"); 229 | _gps_position->latitude_deg = 0.0; 230 | _gps_position->longitude_deg = 0.0; 231 | 232 | // Indicate this data is not usable and bail out 233 | _gps_position->eph = 1000.0f; 234 | _gps_position->epv = 1000.0f; 235 | _gps_position->fix_type = 0; 236 | return; 237 | } 238 | 239 | _gps_position->altitude_msl_m = packet.msl_altitude / 100.0; // from cm to meters 240 | _gps_position->fix_type = packet.fix_type; 241 | _gps_position->eph = packet.hdop / 100.0f; // from cm to m 242 | _gps_position->epv = _gps_position->eph; // unknown in mtk custom mode, so we cheat with eph 243 | _gps_position->hdop = packet.hdop / 100.0f; 244 | _gps_position->vdop = _gps_position->hdop; 245 | _gps_position->vel_m_s = ((float)packet.ground_speed) / 100.0f; // from cm/s to m/s 246 | _gps_position->cog_rad = ((float)packet.heading) * M_DEG_TO_RAD_F * 1e-2f; //from deg *100 to rad 247 | _gps_position->satellites_used = packet.satellites; 248 | 249 | #ifndef NO_MKTIME 250 | /* convert time and date information to unix timestamp */ 251 | struct tm timeinfo = {}; 252 | uint32_t timeinfo_conversion_temp; 253 | 254 | timeinfo.tm_mday = packet.date / 10000; 255 | timeinfo_conversion_temp = packet.date - timeinfo.tm_mday * 10000; 256 | timeinfo.tm_mon = (timeinfo_conversion_temp / 100) - 1; 257 | timeinfo.tm_year = (timeinfo_conversion_temp - (timeinfo.tm_mon + 1) * 100) + 100; 258 | 259 | timeinfo.tm_hour = (packet.utc_time / 10000000); 260 | timeinfo_conversion_temp = packet.utc_time - timeinfo.tm_hour * 10000000; 261 | timeinfo.tm_min = timeinfo_conversion_temp / 100000; 262 | timeinfo_conversion_temp -= timeinfo.tm_min * 100000; 263 | timeinfo.tm_sec = timeinfo_conversion_temp / 1000; 264 | timeinfo_conversion_temp -= timeinfo.tm_sec * 1000; 265 | 266 | timeinfo.tm_isdst = 0; 267 | 268 | 269 | time_t epoch = mktime(&timeinfo); 270 | 271 | if (epoch > GPS_EPOCH_SECS) { 272 | // FMUv2+ boards have a hardware RTC, but GPS helps us to configure it 273 | // and control its drift. Since we rely on the HRT for our monotonic 274 | // clock, updating it from time to time is safe. 275 | 276 | timespec ts{}; 277 | ts.tv_sec = epoch; 278 | ts.tv_nsec = timeinfo_conversion_temp * 1000000ULL; 279 | 280 | setClock(ts); 281 | 282 | _gps_position->time_utc_usec = static_cast(epoch) * 1000000ULL; 283 | _gps_position->time_utc_usec += timeinfo_conversion_temp * 1000ULL; 284 | 285 | } else { 286 | _gps_position->time_utc_usec = 0; 287 | } 288 | 289 | #else 290 | _gps_position->time_utc_usec = 0; 291 | #endif 292 | 293 | _gps_position->timestamp = gps_absolute_time(); 294 | _gps_position->timestamp_time_relative = 0; 295 | 296 | // Position and velocity update always at the same time 297 | _rate_count_vel++; 298 | _rate_count_lat_lon++; 299 | } 300 | 301 | void 302 | GPSDriverMTK::addByteToChecksum(uint8_t b) 303 | { 304 | _rx_ck_a = _rx_ck_a + b; 305 | _rx_ck_b = _rx_ck_b + _rx_ck_a; 306 | } 307 | -------------------------------------------------------------------------------- /src/mtk.h: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * 3 | * Copyright (c) 2012-2016 PX4 Development Team. All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions 7 | * are met: 8 | * 9 | * 1. Redistributions of source code must retain the above copyright 10 | * notice, this list of conditions and the following disclaimer. 11 | * 2. Redistributions in binary form must reproduce the above copyright 12 | * notice, this list of conditions and the following disclaimer in 13 | * the documentation and/or other materials provided with the 14 | * distribution. 15 | * 3. Neither the name PX4 nor the names of its contributors may be 16 | * used to endorse or promote products derived from this software 17 | * without specific prior written permission. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 20 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 21 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 22 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 23 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 24 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 25 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS 26 | * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 27 | * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 28 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 29 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 30 | * POSSIBILITY OF SUCH DAMAGE. 31 | * 32 | ****************************************************************************/ 33 | 34 | /** 35 | * @file mtk.h 36 | * 37 | * @author Thomas Gubler 38 | * @author Julian Oes 39 | */ 40 | 41 | #pragma once 42 | 43 | #include "gps_helper.h" 44 | 45 | #define MTK_SYNC1_V16 0xd0 46 | #define MTK_SYNC1_V19 0xd1 47 | #define MTK_SYNC2 0xdd 48 | 49 | #define MTK_OUTPUT_5HZ "$PMTK220,200*2C\r\n" 50 | #define MTK_SET_BINARY "$PGCMD,16,0,0,0,0,0*6A\r\n" 51 | #define MTK_SBAS_ON "$PMTK313,1*2E\r\n" 52 | #define MTK_WAAS_ON "$PMTK301,2*2E\r\n" 53 | #define MTK_NAVTHRES_OFF "$PMTK397,0*23\r\n" 54 | 55 | #define MTK_TIMEOUT_5HZ 400 56 | #define MTK_BAUDRATE 38400 57 | 58 | typedef enum { 59 | MTK_DECODE_UNINIT = 0, 60 | MTK_DECODE_GOT_CK_A = 1, 61 | MTK_DECODE_GOT_CK_B = 2 62 | } mtk_decode_state_t; 63 | 64 | /** the structures of the binary packets */ 65 | #pragma pack(push, 1) 66 | 67 | typedef struct { 68 | uint8_t payload; ///< Number of payload bytes 69 | int32_t latitude; ///< Latitude in degrees * 10^7 70 | int32_t longitude; ///< Longitude in degrees * 10^7 71 | uint32_t msl_altitude; ///< MSL altitude in meters * 10^2 72 | uint32_t ground_speed; ///< velocity in m/s 73 | int32_t heading; ///< heading in degrees * 10^2 74 | uint8_t satellites; ///< number of satellites used 75 | uint8_t fix_type; ///< fix type: XXX correct for that 76 | uint32_t date; 77 | uint32_t utc_time; 78 | uint16_t hdop; ///< horizontal dilution of position (without unit) 79 | uint8_t ck_a; 80 | uint8_t ck_b; 81 | } gps_mtk_packet_t; 82 | 83 | #pragma pack(pop) 84 | 85 | #define MTK_RECV_BUFFER_SIZE 40 86 | 87 | class GPSDriverMTK : public GPSHelper 88 | { 89 | public: 90 | GPSDriverMTK(GPSCallbackPtr callback, void *callback_user, sensor_gps_s *gps_position); 91 | virtual ~GPSDriverMTK() = default; 92 | 93 | int receive(unsigned timeout) override; 94 | int configure(unsigned &baudrate, const GPSConfig &config) override; 95 | 96 | private: 97 | /** 98 | * Parse the binary MTK packet 99 | */ 100 | int parseChar(uint8_t b, gps_mtk_packet_t &packet); 101 | 102 | /** 103 | * Handle the package once it has arrived 104 | */ 105 | void handleMessage(gps_mtk_packet_t &packet); 106 | 107 | /** 108 | * Reset the parse state machine for a fresh start 109 | */ 110 | void decodeInit(); 111 | 112 | /** 113 | * While parsing add every byte (except the sync bytes) to the checksum 114 | */ 115 | void addByteToChecksum(uint8_t); 116 | 117 | sensor_gps_s *_gps_position {nullptr}; 118 | mtk_decode_state_t _decode_state{MTK_DECODE_UNINIT}; 119 | uint8_t _mtk_revision{0}; 120 | unsigned _rx_count{}; 121 | uint8_t _rx_ck_a{}; 122 | uint8_t _rx_ck_b{}; 123 | }; 124 | -------------------------------------------------------------------------------- /src/nmea.cpp: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * 3 | * Copyright (c) 2020 - 2024 PX4 Development Team. All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions 7 | * are met: 8 | * 9 | * 1. Redistributions of source code must retain the above copyright 10 | * notice, this list of conditions and the following disclaimer. 11 | * 2. Redistributions in binary form must reproduce the above copyright 12 | * notice, this list of conditions and the following disclaimer in 13 | * the documentation and/or other materials provided with the 14 | * distribution. 15 | * 3. Neither the name PX4 nor the names of its contributors may be 16 | * used to endorse or promote products derived from this software 17 | * without specific prior written permission. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 20 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 21 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 22 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 23 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 24 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 25 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS 26 | * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 27 | * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 28 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 29 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 30 | * POSSIBILITY OF SUCH DAMAGE. 31 | * 32 | ****************************************************************************/ 33 | 34 | /** 35 | * @file nmea.cpp 36 | * 37 | * NMEA protocol implementation. 38 | * 39 | * @author WeiPeng Guo 40 | * @author Stone White 41 | * @author Jose Jimenez-Berni 42 | * 43 | */ 44 | 45 | #include 46 | #include 47 | #include 48 | #include 49 | #include 50 | 51 | #include "nmea.h" 52 | #include "rtcm.h" 53 | 54 | #ifndef M_PI_F 55 | # define M_PI_F 3.14159265358979323846f 56 | #endif 57 | 58 | #define MAX(X,Y) ((X) > (Y) ? (X) : (Y)) 59 | #define NMEA_UNUSED(x) (void)x; 60 | 61 | /**** Warning macros, disable to save memory */ 62 | #define NMEA_WARN(...) {GPS_WARN(__VA_ARGS__);} 63 | #define NMEA_DEBUG(...) {/*GPS_WARN(__VA_ARGS__);*/} 64 | 65 | GPSDriverNMEA::GPSDriverNMEA(GPSCallbackPtr callback, void *callback_user, 66 | sensor_gps_s *gps_position, 67 | satellite_info_s *satellite_info, 68 | float heading_offset) : 69 | GPSHelper(callback, callback_user), 70 | _gps_position(gps_position), 71 | _satellite_info(satellite_info), 72 | _heading_offset(heading_offset) 73 | { 74 | decodeInit(); 75 | } 76 | 77 | GPSDriverNMEA::~GPSDriverNMEA() 78 | { 79 | delete _rtcm_parsing; 80 | } 81 | 82 | /* 83 | * All NMEA descriptions are taken from 84 | * http://www.trimble.com/OEM_ReceiverHelp/V4.44/en/NMEA-0183messages_MessageOverview.html 85 | */ 86 | 87 | int GPSDriverNMEA::handleMessage(int len) 88 | { 89 | char *endp; 90 | 91 | if (len < 7) { 92 | return 0; 93 | } 94 | 95 | int fieldCount = 0; 96 | 97 | for (int i = 0 ; i < len; i++) { 98 | if (_rx_buffer[i] == ',') { fieldCount++; } 99 | } 100 | 101 | char *bufptr = (char *)(_rx_buffer + 6); 102 | int ret = 0; 103 | 104 | if ((memcmp(_rx_buffer + 3, "ZDA,", 4) == 0) && (fieldCount == 6)) { 105 | #ifndef NO_MKTIME 106 | /* 107 | UTC day, month, and year, and local time zone offset 108 | An example of the ZDA message string is: 109 | 110 | $GPZDA,172809.456,12,07,1996,00,00*45 111 | 112 | ZDA message fields 113 | Field Meaning 114 | 0 Message ID $GPZDA 115 | 1 UTC 116 | 2 Day, ranging between 01 and 31 117 | 3 Month, ranging between 01 and 12 118 | 4 Year 119 | 5 Local time zone offset from GMT, ranging from 00 through 13 hours 120 | 6 Local time zone offset from GMT, ranging from 00 through 59 minutes 121 | 7 The checksum data, always begins with * 122 | Fields 5 and 6 together yield the total offset. For example, if field 5 is -5 and field 6 is +15, local time is 5 hours and 15 minutes earlier than GMT. 123 | */ 124 | double utc_time = 0.0; 125 | int day = 0, month = 0, year = 0, local_time_off_hour = 0, local_time_off_min = 0; 126 | NMEA_UNUSED(local_time_off_hour); 127 | NMEA_UNUSED(local_time_off_min); 128 | 129 | if (bufptr && *(++bufptr) != ',') { utc_time = strtod(bufptr, &endp); bufptr = endp; } 130 | 131 | if (bufptr && *(++bufptr) != ',') { day = strtol(bufptr, &endp, 10); bufptr = endp; } 132 | 133 | if (bufptr && *(++bufptr) != ',') { month = strtol(bufptr, &endp, 10); bufptr = endp; } 134 | 135 | if (bufptr && *(++bufptr) != ',') { year = strtol(bufptr, &endp, 10); bufptr = endp; } 136 | 137 | if (bufptr && *(++bufptr) != ',') { local_time_off_hour = strtol(bufptr, &endp, 10); bufptr = endp; } 138 | 139 | if (bufptr && *(++bufptr) != ',') { local_time_off_min = strtol(bufptr, &endp, 10); bufptr = endp; } 140 | 141 | int utc_hour = static_cast(utc_time / 10000); 142 | int utc_minute = static_cast((utc_time - utc_hour * 10000) / 100); 143 | double utc_sec = static_cast(utc_time - utc_hour * 10000 - utc_minute * 100); 144 | 145 | 146 | /* 147 | * convert to unix timestamp 148 | */ 149 | struct tm timeinfo = {}; 150 | timeinfo.tm_year = year - 1900; 151 | timeinfo.tm_mon = month - 1; 152 | timeinfo.tm_mday = day; 153 | timeinfo.tm_hour = utc_hour; 154 | timeinfo.tm_min = utc_minute; 155 | timeinfo.tm_sec = int(utc_sec); 156 | timeinfo.tm_isdst = 0; 157 | 158 | 159 | time_t epoch = mktime(&timeinfo); 160 | 161 | if (epoch > GPS_EPOCH_SECS) { 162 | uint64_t usecs = static_cast((utc_sec - static_cast(utc_sec)) * 1000000); 163 | 164 | // FMUv2+ boards have a hardware RTC, but GPS helps us to configure it 165 | // and control its drift. Since we rely on the HRT for our monotonic 166 | // clock, updating it from time to time is safe. 167 | 168 | if (!_clock_set) { 169 | timespec ts{}; 170 | ts.tv_sec = epoch; 171 | ts.tv_nsec = usecs * 1000; 172 | setClock(ts); 173 | _clock_set = true; 174 | } 175 | 176 | _gps_position->time_utc_usec = static_cast(epoch) * 1000000ULL; 177 | _gps_position->time_utc_usec += usecs; 178 | 179 | } else { 180 | _gps_position->time_utc_usec = 0; 181 | } 182 | 183 | #else 184 | _gps_position->time_utc_usec = 0; 185 | #endif 186 | _last_timestamp_time = gps_absolute_time(); 187 | _TIME_received = true; 188 | 189 | } else if ((memcmp(_rx_buffer + 3, "GLL,", 4) == 0) && (fieldCount >= 7)) { 190 | /* 191 | Latitude and Londitude data 192 | Example: 193 | $xxGLL,Lat,N/S,long,E/W,timestamp,A/V data validity,A/V mode indicator 194 | $GNGLL,3150.712345,N,11711.912345,E,062735.00,A,A*76 195 | */ 196 | double utc_time = 0.0, lat = 0.0, lon = 0.0; 197 | char ns = '?', ew = '?', dvalid = '?', modeind = '?'; 198 | 199 | if (bufptr && *(++bufptr) != ',') { lat = strtod(bufptr, &endp); bufptr = endp; } 200 | 201 | if (bufptr && *(++bufptr) != ',') { ns = *(bufptr++); } 202 | 203 | if (bufptr && *(++bufptr) != ',') { lon = strtod(bufptr, &endp); bufptr = endp; } 204 | 205 | if (bufptr && *(++bufptr) != ',') { ew = *(bufptr++); } 206 | 207 | if (bufptr && *(++bufptr) != ',') { utc_time = strtod(bufptr, &endp); bufptr = endp; } 208 | 209 | if (bufptr && *(++bufptr) != ',') { dvalid = *(bufptr++); } 210 | 211 | if (bufptr && *(++bufptr) != ',') { modeind = *(bufptr++); } 212 | 213 | if (ns == 'S') { 214 | lat = -lat; 215 | } 216 | 217 | if (ew == 'W') { 218 | lon = -lon; 219 | } 220 | 221 | 222 | /* only update the values if they are valid */ 223 | if (dvalid == 'A' && modeind == 'A') { 224 | _gps_position->longitude_deg = int(lon * 0.01) + (lon * 0.01 - int(lon * 0.01)) * 100.0 / 60.0; 225 | _gps_position->latitude_deg = int(lat * 0.01) + (lat * 0.01 - int(lat * 0.01)) * 100.0 / 60.0; 226 | 227 | if (!_POS_received && (_last_POS_timeUTC < utc_time)) { 228 | _last_POS_timeUTC = utc_time; 229 | _POS_received = true; 230 | _rate_count_lat_lon++; 231 | } 232 | 233 | } 234 | 235 | } else if ((memcmp(_rx_buffer + 3, "GGA,", 4) == 0) && (fieldCount >= 14)) { 236 | /* 237 | Time, position, and fix related data 238 | An example of the GGA message string is: 239 | $xxGGA,time,lat,NS,long,EW,quality,numSV,HDOP,alt,M,sep,M,diffAge,diffStation*cs 240 | $GPGGA,172814.0,3723.46587704,N,12202.26957864,W,2,6,1.2,18.893,M,-25.669,M,2.0,0031*4F 241 | $GNGGA,092721.00,2926.688113,N,11127.771644,E,2,08,1.11,106.3,M,-20,M,1.0,3721*53 242 | 243 | Note - The data string exceeds the nmea standard length. 244 | GGA message fields 245 | Field Meaning 246 | 0 Message ID $GPGGA 247 | 1 UTC of position fix 248 | 2 Latitude 249 | 3 Direction of latitude: 250 | N: North 251 | S: South 252 | 4 Longitude 253 | 5 Direction of longitude: 254 | E: East 255 | W: West 256 | 6 GPS Quality indicator: 257 | 0: Fix not valid 258 | 1: GPS fix 259 | 2: Differential GPS fix, OmniSTAR VBS 260 | 4: Real-Time Kinematic, fixed integers 261 | 5: Real-Time Kinematic, float integers, OmniSTAR XP/HP or Location RTK 262 | 7 Number of SVs in use, range from 00 through to 24+ 263 | 8 HDOP 264 | 9 Orthometric height (MSL reference) 265 | 10 M: unit of measure for orthometric height is meters 266 | 11 Geoid separation 267 | 12 M: geoid separation measured in meters 268 | 13 Age of differential GPS data record, Type 1 or Type 9. Null field when DGPS is not used. 269 | 14 Reference station ID, range 0000-4095. A null field when any reference station ID is selected and no corrections are received1. 270 | 15 271 | The checksum data, always begins with * 272 | */ 273 | double utc_time = 0.0, lat = 0.0, lon = 0.0; 274 | float alt = 0.f, geoid_h = 0.f; 275 | float hdop = 99.9f, dgps_age = NAN; 276 | int num_of_sv = 0, fix_quality = 0; 277 | char ns = '?', ew = '?'; 278 | 279 | NMEA_UNUSED(dgps_age); 280 | 281 | if (bufptr && *(++bufptr) != ',') { utc_time = strtod(bufptr, &endp); bufptr = endp; } 282 | 283 | if (bufptr && *(++bufptr) != ',') { lat = strtod(bufptr, &endp); bufptr = endp; } 284 | 285 | if (bufptr && *(++bufptr) != ',') { ns = *(bufptr++); } 286 | 287 | if (bufptr && *(++bufptr) != ',') { lon = strtod(bufptr, &endp); bufptr = endp; } 288 | 289 | if (bufptr && *(++bufptr) != ',') { ew = *(bufptr++); } 290 | 291 | if (bufptr && *(++bufptr) != ',') { fix_quality = strtol(bufptr, &endp, 10); bufptr = endp; } 292 | 293 | if (bufptr && *(++bufptr) != ',') { num_of_sv = strtol(bufptr, &endp, 10); bufptr = endp; } 294 | 295 | if (bufptr && *(++bufptr) != ',') { hdop = strtof(bufptr, &endp); bufptr = endp; } 296 | 297 | if (bufptr && *(++bufptr) != ',') { alt = strtof(bufptr, &endp); bufptr = endp; } 298 | 299 | while (*(++bufptr) != ',') {} //skip M 300 | 301 | if (bufptr && *(++bufptr) != ',') { geoid_h = strtof(bufptr, &endp); bufptr = endp; } 302 | 303 | while (*(++bufptr) != ',') {} //skip M 304 | 305 | if (bufptr && *(++bufptr) != ',') { dgps_age = strtof(bufptr, &endp); bufptr = endp; } 306 | 307 | if (ns == 'S') { 308 | lat = -lat; 309 | } 310 | 311 | if (ew == 'W') { 312 | lon = -lon; 313 | } 314 | 315 | /* convert from degrees, minutes and seconds to degrees */ 316 | _gps_position->longitude_deg = int(lon * 0.01) + (lon * 0.01 - int(lon * 0.01)) * 100.0 / 60.0; 317 | _gps_position->latitude_deg = int(lat * 0.01) + (lat * 0.01 - int(lat * 0.01)) * 100.0 / 60.0; 318 | _gps_position->hdop = hdop; 319 | _gps_position->altitude_msl_m = (double)alt; 320 | _gps_position->altitude_ellipsoid_m = (double)(alt + geoid_h); 321 | _sat_num_gga = static_cast(num_of_sv); 322 | 323 | 324 | if (fix_quality <= 0) { 325 | _gps_position->fix_type = 0; 326 | 327 | } else { 328 | /* 329 | * in this NMEA message float integers (value 5) mode has higher value than fixed integers (value 4), whereas it provides lower quality, 330 | * and since value 3 is not being used, I "moved" value 5 to 3 to add it to _gps_position->fix_type 331 | */ 332 | if (fix_quality == 5) { fix_quality = 3; } 333 | 334 | /* 335 | * fix quality 1 means just a normal 3D fix, so I'm subtracting 1 here. This way we'll have 3 for auto, 4 for DGPS, 5 for floats, 6 for fixed. 336 | */ 337 | _gps_position->fix_type = 3 + fix_quality - 1; 338 | } 339 | 340 | if (!_POS_received && (_last_POS_timeUTC < utc_time)) { 341 | _last_POS_timeUTC = utc_time; 342 | _gps_position->timestamp = gps_absolute_time(); 343 | _POS_received = true; 344 | _rate_count_lat_lon++; 345 | } 346 | 347 | _ALT_received = true; 348 | _SVNUM_received = true; 349 | _FIX_received = true; 350 | 351 | _gps_position->c_variance_rad = 0.1f; 352 | 353 | } else if (memcmp(_rx_buffer + 3, "HDT,", 4) == 0 && fieldCount == 2) { 354 | /* 355 | Heading message 356 | Example $GPHDT,121.2,T*35 357 | 358 | f1 Last computed heading value, in degrees (0-359.99) 359 | T "T" for "True" 360 | */ 361 | 362 | float heading_deg = 0.f; 363 | 364 | if (bufptr && *(++bufptr) != ',') { 365 | heading_deg = strtof(bufptr, &endp); bufptr = endp; 366 | handleHeading(heading_deg, NAN); 367 | } 368 | 369 | _HEAD_received = true; 370 | 371 | } else if ((memcmp(_rx_buffer + 3, "GNS,", 4) == 0) && (fieldCount >= 12)) { 372 | 373 | /* 374 | Message GNS 375 | Type Output Message 376 | Time and position, together with GNSS fixing related data (number of satellites in use, and 377 | the resulting HDOP, age of differential data if in use, etc.). 378 | Message Structure: 379 | $xxGNS,time,lat,NS,long,EW,posMode,numSV,HDOP,alt,altRef,diffAge,diffStation,navStatus*cs 380 | Example: 381 | $GPGNS,091547.00,5114.50897,N,00012.28663,W,AA,10,0.83,111.1,45.6,,,V*71 382 | $GNGNS,092721.00,2926.68811,N,11127.77164,E,DNNN,08,1.11,106.3,-20,1.0,3721,V*0D 383 | $GNGNS,182243.00,4908.088781,N,12233.7501,W,AAAAN,24,0.6,191.8178,-33.6291,,,S*50 384 | 385 | FieldNo. Name Unit Format Example Description 386 | 0 xxGNS - string $GPGNS GNS Message ID (xx = current Talker ID) 387 | 1 time - hhmmss.ss 091547.00 UTC time, see note on UTC representation 388 | 2 lat - ddmm.mmmmm 5114.50897 Latitude (degrees & minutes), see format description 389 | 3 NS - character N North/South indicator 390 | 4 long - dddmm.mmmmm 00012.28663 Longitude (degrees & minutes), see format description 391 | 5 EW - character E East/West indicator 392 | 6 posMode - character AA Positioning mode, see position fix flags description. First character for GPS, second character forGLONASS 393 | 7 numSV - numeric 10 Number of satellites used (range: 0-99) 394 | 8 HDOP - numeric 0.83 Horizontal Dilution of Precision 395 | 9 alt m numeric 111.1 Altitude above mean sea level 396 | 10 sep m numeric 45.6 Geoid separation: difference between ellipsoid and mean sea level UBX-18010854 - R05 Advance Information Page 18 of 262 u-blox ZED-F9P Interface Description - Manual GNS continued 397 | 11 diffAge s numeric - Age of differential corrections (blank when DGPS is not used) 398 | 12 diffStation - numeric - ID of station providing differential corrections (blank when DGPS is not used) 399 | 13 navStatus - character V Navigational status indicator (V = Equipment is not providing navigational status information) NMEA v4.10 and above only 400 | 14 cs - hexadecimal *71 Checksum 401 | 15 - character - Carriage return and line feed 402 | */ 403 | double utc_time = 0.0; 404 | double lat = 0.0, lon = 0.0; 405 | char pos_Mode[5] = {'N', 'N', 'N', 'N', 'N'}; 406 | int num_of_sv = 0; 407 | float alt = 0.f, geoid_h = 0.f; 408 | float hdop = 0.f; 409 | char ns = '?', ew = '?'; 410 | int i = 0; 411 | NMEA_UNUSED(pos_Mode); 412 | 413 | if (bufptr && *(++bufptr) != ',') { utc_time = strtod(bufptr, &endp); bufptr = endp; } 414 | 415 | if (bufptr && *(++bufptr) != ',') { lat = strtod(bufptr, &endp); bufptr = endp; } 416 | 417 | if (bufptr && *(++bufptr) != ',') { ns = *(bufptr++); } 418 | 419 | if (bufptr && *(++bufptr) != ',') { lon = strtod(bufptr, &endp); bufptr = endp; } 420 | 421 | if (bufptr && *(++bufptr) != ',') { ew = *(bufptr++);} 422 | 423 | /* as more GPS systems are added this string can grow, so only parse out the first X, but keep going until we hit the end of field */ 424 | do { 425 | if (i < 5) { pos_Mode[i++] = *(bufptr); } 426 | 427 | } while (*(++bufptr) != ','); 428 | 429 | if (bufptr && *(++bufptr) != ',') { num_of_sv = strtol(bufptr, &endp, 10); bufptr = endp; } 430 | 431 | if (bufptr && *(++bufptr) != ',') { hdop = strtof(bufptr, &endp); bufptr = endp; } 432 | 433 | if (bufptr && *(++bufptr) != ',') { alt = strtof(bufptr, &endp); bufptr = endp; } 434 | 435 | if (bufptr && *(++bufptr) != ',') { geoid_h = strtof(bufptr, &endp); bufptr = endp; } 436 | 437 | if (ns == 'S') { 438 | lat = -lat; 439 | } 440 | 441 | if (ew == 'W') { 442 | lon = -lon; 443 | } 444 | 445 | /* convert from degrees, minutes and seconds to degrees */ 446 | _gps_position->latitude_deg = int(lat * 0.01) + (lat * 0.01 - int(lat * 0.01)) * 100.0 / 60.0; 447 | _gps_position->longitude_deg = int(lon * 0.01) + (lon * 0.01 - int(lon * 0.01)) * 100.0 / 60.0; 448 | _gps_position->hdop = hdop; 449 | _gps_position->altitude_msl_m = (double)alt; 450 | _gps_position->altitude_ellipsoid_m = (double)(alt + geoid_h); 451 | _sat_num_gns = static_cast(num_of_sv); 452 | 453 | if (!_POS_received && (_last_POS_timeUTC < utc_time)) { 454 | _last_POS_timeUTC = utc_time; 455 | _gps_position->timestamp = gps_absolute_time(); 456 | _POS_received = true; 457 | _rate_count_lat_lon++; 458 | } 459 | 460 | _ALT_received = true; 461 | _SVNUM_received = true; 462 | 463 | 464 | } else if ((memcmp(_rx_buffer + 3, "RMC,", 4) == 0) && (fieldCount >= 11)) { 465 | 466 | /* 467 | Position, velocity, and time 468 | The RMC string is: 469 | 470 | $xxRMC,time,status,lat,NS,long,EW,spd,cog,date,mv,mvEW,posMode,navStatus*cs 471 | The Talker ID ($--) will vary depending on the satellite system used for the position solution: 472 | $GNRMC,092721.00,A,2926.688113,N,11127.771644,E,0.780,,200520,,,D,V*1D 473 | 474 | GPRMC message fields 475 | Field Meaning 476 | 0 Message ID $GPRMC 477 | 1 UTC of position fix 478 | 2 Status A=active or V=void 479 | 3 Latitude 480 | 4 Longitude 481 | 5 Speed over the ground in knots 482 | 6 Track angle in degrees (True) 483 | 7 Date 484 | 8 Magnetic variation in degrees 485 | 9 The checksum data, always begins with * 486 | */ 487 | double utc_time = 0.0; 488 | char Status = 'V'; 489 | double lat = 0.0, lon = 0.0; 490 | float ground_speed_K = 0.f; 491 | float track_true = 0.f; 492 | int nmea_date = 0; 493 | float Mag_var = 0.f; 494 | char ns = '?', ew = '?'; 495 | NMEA_UNUSED(Mag_var); 496 | 497 | if (bufptr && *(++bufptr) != ',') { utc_time = strtod(bufptr, &endp); bufptr = endp; } 498 | 499 | if (bufptr && *(++bufptr) != ',') { Status = *(bufptr++); } 500 | 501 | if (bufptr && *(++bufptr) != ',') { lat = strtod(bufptr, &endp); bufptr = endp; } 502 | 503 | if (bufptr && *(++bufptr) != ',') { ns = *(bufptr++); } 504 | 505 | if (bufptr && *(++bufptr) != ',') { lon = strtod(bufptr, &endp); bufptr = endp; } 506 | 507 | if (bufptr && *(++bufptr) != ',') { ew = *(bufptr++); } 508 | 509 | if (bufptr && *(++bufptr) != ',') { ground_speed_K = strtof(bufptr, &endp); bufptr = endp; } 510 | 511 | if (bufptr && *(++bufptr) != ',') { track_true = strtof(bufptr, &endp); bufptr = endp; } 512 | 513 | if (bufptr && *(++bufptr) != ',') { nmea_date = static_cast(strtol(bufptr, &endp, 10)); bufptr = endp; } 514 | 515 | if (bufptr && *(++bufptr) != ',') { Mag_var = strtof(bufptr, &endp); bufptr = endp; } 516 | 517 | if (ns == 'S') { 518 | lat = -lat; 519 | } 520 | 521 | if (ew == 'W') { 522 | lon = -lon; 523 | } 524 | 525 | if (Status == 'V') { 526 | _gps_position->fix_type = 0; 527 | } 528 | 529 | float track_rad = track_true * M_PI_F / 180.0f; // rad in range [0, 2pi] 530 | 531 | if (track_rad > M_PI_F) { 532 | track_rad -= 2.f * M_PI_F; // rad in range [-pi, pi] 533 | } 534 | 535 | float velocity_ms = ground_speed_K / 1.9438445f; 536 | float velocity_north = velocity_ms * cosf(track_rad); 537 | float velocity_east = velocity_ms * sinf(track_rad); 538 | 539 | _gps_position->cog_rad = track_rad; 540 | _gps_position->c_variance_rad = 0.1f; 541 | 542 | if (!_unicore_parser.agricaValid()) { 543 | // We ignore RMC position for Unicore, because we have GGA configured at the rate we want. 544 | 545 | /* convert from degrees, minutes and seconds to degrees */ 546 | _gps_position->latitude_deg = int(lat * 0.01) + (lat * 0.01 - int(lat * 0.01)) * 100.0 / 60.0; 547 | _gps_position->longitude_deg = int(lon * 0.01) + (lon * 0.01 - int(lon * 0.01)) * 100.0 / 60.0; 548 | 549 | if (!_POS_received && (_last_POS_timeUTC < utc_time)) { 550 | _gps_position->timestamp = gps_absolute_time(); 551 | _last_POS_timeUTC = utc_time; 552 | _POS_received = true; 553 | _rate_count_lat_lon++; 554 | } 555 | 556 | _gps_position->vel_m_s = velocity_ms; 557 | _gps_position->vel_n_m_s = velocity_north; 558 | _gps_position->vel_e_m_s = velocity_east; 559 | _gps_position->vel_ned_valid = true; /**< Flag to indicate if NED speed is valid */ 560 | _gps_position->s_variance_m_s = 0; 561 | 562 | if (!_VEL_received && (_last_VEL_timeUTC < utc_time)) { 563 | _last_VEL_timeUTC = utc_time; 564 | _VEL_received = true; 565 | _rate_count_vel++; 566 | } 567 | } 568 | 569 | #ifndef NO_MKTIME 570 | int utc_hour = static_cast(utc_time / 10000); 571 | int utc_minute = static_cast((utc_time - utc_hour * 10000) / 100); 572 | double utc_sec = static_cast(utc_time - utc_hour * 10000 - utc_minute * 100); 573 | int nmea_day = static_cast(nmea_date / 10000); 574 | int nmea_mth = static_cast((nmea_date - nmea_day * 10000) / 100); 575 | int nmea_year = static_cast(nmea_date - nmea_day * 10000 - nmea_mth * 100); 576 | /* 577 | * convert to unix timestamp 578 | */ 579 | struct tm timeinfo = {}; 580 | timeinfo.tm_year = nmea_year + 100; 581 | timeinfo.tm_mon = nmea_mth - 1; 582 | timeinfo.tm_mday = nmea_day; 583 | timeinfo.tm_hour = utc_hour; 584 | timeinfo.tm_min = utc_minute; 585 | timeinfo.tm_sec = int(utc_sec); 586 | timeinfo.tm_isdst = 0; 587 | 588 | time_t epoch = mktime(&timeinfo); 589 | 590 | if (epoch > GPS_EPOCH_SECS) { 591 | uint64_t usecs = static_cast((utc_sec - static_cast(utc_sec)) * 1000000); 592 | 593 | // FMUv2+ boards have a hardware RTC, but GPS helps us to configure it 594 | // and control its drift. Since we rely on the HRT for our monotonic 595 | // clock, updating it from time to time is safe. 596 | if (!_clock_set) { 597 | timespec ts{}; 598 | ts.tv_sec = epoch; 599 | ts.tv_nsec = usecs * 1000; 600 | 601 | setClock(ts); 602 | _clock_set = true; 603 | } 604 | 605 | _gps_position->time_utc_usec = static_cast(epoch) * 1000000ULL; 606 | _gps_position->time_utc_usec += usecs; 607 | 608 | } else { 609 | _gps_position->time_utc_usec = 0; 610 | } 611 | 612 | #else 613 | NMEA_UNUSED(utc_time); 614 | NMEA_UNUSED(nmea_date); 615 | _gps_position->time_utc_usec = 0; 616 | #endif 617 | 618 | _last_timestamp_time = gps_absolute_time(); 619 | _TIME_received = true; 620 | 621 | } else if ((memcmp(_rx_buffer + 3, "GST,", 4) == 0) && (fieldCount == 8)) { 622 | 623 | /* 624 | Position error statistics 625 | An example of the GST message string is: 626 | 627 | $GPGST,172814.0,0.006,0.023,0.020,273.6,0.023,0.020,0.031*6A 628 | $GNGST,091200.54,45,,,,1.2,0.77,2.2*70 629 | $GNGST,092720.50,43,,,,2.6,2.6,5.9*49 630 | 631 | The Talker ID ($--) will vary depending on the satellite system used for the position solution: 632 | 633 | $GP - GPS only 634 | $GL - GLONASS only 635 | $GN - Combined 636 | GST message fields 637 | Field Meaning 638 | 0 Message ID $GPGST 639 | 1 UTC of position fix 640 | 2 RMS value of the pseudorange residuals; includes carrier phase residuals during periods of RTK (float) and RTK (fixed) processing 641 | 3 Error ellipse semi-major axis 1 sigma error, in meters 642 | 4 Error ellipse semi-minor axis 1 sigma error, in meters 643 | 5 Error ellipse orientation, degrees from true north 644 | 6 Latitude 1 sigma error, in meters 645 | 7 Longitude 1 sigma error, in meters 646 | 8 Height 1 sigma error, in meters 647 | 9 The checksum data, always begins with * 648 | */ 649 | double utc_time = 0.0; 650 | float lat_err = 0.f, lon_err = 0.f, alt_err = 0.f; 651 | float min_err = 0.f, maj_err = 0.f, deg_from_north = 0.f, rms_err = 0.f; 652 | 653 | NMEA_UNUSED(utc_time); 654 | NMEA_UNUSED(min_err); 655 | NMEA_UNUSED(maj_err); 656 | NMEA_UNUSED(deg_from_north); 657 | NMEA_UNUSED(rms_err); 658 | 659 | if (bufptr && *(++bufptr) != ',') { utc_time = strtod(bufptr, &endp); bufptr = endp; } 660 | 661 | if (bufptr && *(++bufptr) != ',') { rms_err = strtof(bufptr, &endp); bufptr = endp; } 662 | 663 | if (bufptr && *(++bufptr) != ',') { maj_err = strtof(bufptr, &endp); bufptr = endp; } 664 | 665 | if (bufptr && *(++bufptr) != ',') { min_err = strtof(bufptr, &endp); bufptr = endp; } 666 | 667 | if (bufptr && *(++bufptr) != ',') { deg_from_north = strtof(bufptr, &endp); bufptr = endp; } 668 | 669 | if (bufptr && *(++bufptr) != ',') { lat_err = strtof(bufptr, &endp); bufptr = endp; } 670 | 671 | if (bufptr && *(++bufptr) != ',') { lon_err = strtof(bufptr, &endp); bufptr = endp; } 672 | 673 | if (bufptr && *(++bufptr) != ',') { alt_err = strtof(bufptr, &endp); bufptr = endp; } 674 | 675 | _gps_position->eph = sqrtf(static_cast(lat_err) * static_cast(lat_err) 676 | + static_cast(lon_err) * static_cast(lon_err)); 677 | _gps_position->epv = static_cast(alt_err); 678 | 679 | _EPH_received = true; 680 | 681 | } else if ((memcmp(_rx_buffer + 3, "GSA,", 4) == 0) && (fieldCount >= 17)) { 682 | 683 | /* 684 | GPS DOP and active satellites 685 | An example of the GSA message string is: 686 | $GPGSA,<1>,<2>,<3>,<3>,,,,,<3>,<3>,<3>,<4>,<5>,<6>*<7> 687 | $GNGSA,A,3,82,67,74,68,73,80,83,,,,,,0.99,0.53,0.84,2*09 688 | $GNGSA,A,3,12,19,06,17,02,09,28,05,,,,,2.38,1.10,2.11,1*05 689 | $GNGSA,A,3,27,04,16,08,09,26,31,11,,,,,1.96,1.05,1.65,1*08 690 | 691 | GSA message fields 692 | Field Meaning 693 | 0 Message ID $GPGSA 694 | 1 Mode 1, M = manual, A = automatic 695 | 2 Mode 2, Fix type, 1 = not available, 2 = 2D, 3 = 3D 696 | 3 PRN number, 01 through 32 for GPS, 33 through 64 for SBAS, 64+ for GLONASS 697 | 4 PDOP: 0.5 through 99.9 698 | 5 HDOP: 0.5 through 99.9 699 | 6 VDOP: 0.5 through 99.9 700 | 7 The checksum data, always begins with * 701 | */ 702 | char M_pos = ' '; 703 | int fix_mode = 0; 704 | int sat_id[12] {0}; 705 | float pdop = 99.9f, hdop = 99.9f, vdop = 99.9f; 706 | 707 | NMEA_UNUSED(M_pos); 708 | NMEA_UNUSED(sat_id); 709 | NMEA_UNUSED(pdop); 710 | 711 | if (bufptr && *(++bufptr) != ',') { M_pos = *(bufptr++); } 712 | 713 | if (bufptr && *(++bufptr) != ',') { fix_mode = strtol(bufptr, &endp, 10); bufptr = endp; } 714 | 715 | for (int y = 0; y < 12; y++) { 716 | if (bufptr && *(++bufptr) != ',') {sat_id[y] = strtol(bufptr, &endp, 10); bufptr = endp; } 717 | } 718 | 719 | if (bufptr && *(++bufptr) != ',') { pdop = strtof(bufptr, &endp); bufptr = endp; } 720 | 721 | if (bufptr && *(++bufptr) != ',') { hdop = strtof(bufptr, &endp); bufptr = endp; } 722 | 723 | if (bufptr && *(++bufptr) != ',') { vdop = strtof(bufptr, &endp); bufptr = endp; } 724 | 725 | if (fix_mode <= 1) { 726 | _gps_position->fix_type = 0; 727 | 728 | } else { 729 | _gps_position->hdop = static_cast(hdop); 730 | _gps_position->vdop = static_cast(vdop); 731 | _DOP_received = true; 732 | 733 | } 734 | 735 | 736 | } else if ((memcmp(_rx_buffer + 3, "GSV,", 4) == 0)) { 737 | /* 738 | The GSV message string identifies the number of SVs in view, the PRN numbers, elevations, azimuths, and SNR values. An example of the GSV message string is: 739 | 740 | $GPGSV,4,1,13,02,02,213,,03,-3,000,,11,00,121,,14,13,172,05*67 741 | 742 | GSV message fields 743 | Field Meaning 744 | 0 Message ID $GPGSV 745 | 1 Total number of messages of this type in this cycle 746 | 2 Message number 747 | 3 Total number of SVs visible 748 | 4 SV PRN number 749 | 5 Elevation, in degrees, 90 maximum 750 | 6 Azimuth, degrees from True North, 000 through 359 751 | 7 SNR, 00 through 99 dB (null when not tracking) 752 | 8-11 Information about second SV, same format as fields 4 through 7 753 | 12-15 Information about third SV, same format as fields 4 through 7 754 | 16-19 Information about fourth SV, same format as fields 4 through 7 755 | 20 The checksum data, always begins with * 756 | */ 757 | 758 | int all_page_num = 0, this_page_num = 0, tot_sv_visible = 0; 759 | struct gsv_sat { 760 | int svid; 761 | int elevation; 762 | int azimuth; 763 | int snr; 764 | } sat[4] {}; 765 | 766 | if (bufptr && *(++bufptr) != ',') { all_page_num = strtol(bufptr, &endp, 10); bufptr = endp; } 767 | 768 | if (bufptr && *(++bufptr) != ',') { this_page_num = strtol(bufptr, &endp, 10); bufptr = endp; } 769 | 770 | if (bufptr && *(++bufptr) != ',') { tot_sv_visible = strtol(bufptr, &endp, 10); bufptr = endp; } 771 | 772 | if ((this_page_num < 1) || (this_page_num > all_page_num)) { 773 | return 0; 774 | } 775 | 776 | if (memcmp(_rx_buffer, "$GP", 3) == 0) { 777 | _sat_num_gpgsv = tot_sv_visible; 778 | 779 | } else if (memcmp(_rx_buffer, "$GL", 3) == 0) { 780 | _sat_num_glgsv = tot_sv_visible; 781 | 782 | } else if (memcmp(_rx_buffer, "$GA", 3) == 0) { 783 | _sat_num_gagsv = tot_sv_visible; 784 | 785 | } else if (memcmp(_rx_buffer, "$GB", 3) == 0) { 786 | _sat_num_gbgsv = tot_sv_visible; 787 | 788 | } else if (memcmp(_rx_buffer, "$BD", 3) == 0) { 789 | _sat_num_bdgsv = tot_sv_visible; 790 | 791 | } 792 | 793 | if (this_page_num == 0 && _satellite_info) { 794 | memset(_satellite_info->svid, 0, sizeof(_satellite_info->svid)); 795 | memset(_satellite_info->used, 0, sizeof(_satellite_info->used)); 796 | memset(_satellite_info->snr, 0, sizeof(_satellite_info->snr)); 797 | memset(_satellite_info->elevation, 0, sizeof(_satellite_info->elevation)); 798 | memset(_satellite_info->azimuth, 0, sizeof(_satellite_info->azimuth)); 799 | } 800 | 801 | int end = 4; 802 | 803 | if (this_page_num == all_page_num) { 804 | end = tot_sv_visible - (this_page_num - 1) * 4; 805 | 806 | _SVNUM_received = true; 807 | _SVINFO_received = true; 808 | 809 | if (_satellite_info) { 810 | _satellite_info->count = satellite_info_s::SAT_INFO_MAX_SATELLITES; 811 | _satellite_info->timestamp = gps_absolute_time(); 812 | ret |= 2; 813 | } 814 | } 815 | 816 | if (_satellite_info) { 817 | for (int y = 0 ; y < end ; y++) { 818 | if (bufptr && *(++bufptr) != ',') { sat[y].svid = strtol(bufptr, &endp, 10); bufptr = endp; } 819 | 820 | if (bufptr && *(++bufptr) != ',') { sat[y].elevation = strtol(bufptr, &endp, 10); bufptr = endp; } 821 | 822 | if (bufptr && *(++bufptr) != ',') { sat[y].azimuth = strtol(bufptr, &endp, 10); bufptr = endp; } 823 | 824 | if (bufptr && *(++bufptr) != ',') { sat[y].snr = strtol(bufptr, &endp, 10); bufptr = endp; } 825 | 826 | _satellite_info->svid[y + (this_page_num - 1) * 4] = sat[y].svid; 827 | _satellite_info->used[y + (this_page_num - 1) * 4] = (sat[y].snr > 0); 828 | _satellite_info->snr[y + (this_page_num - 1) * 4] = sat[y].snr; 829 | _satellite_info->elevation[y + (this_page_num - 1) * 4] = sat[y].elevation; 830 | _satellite_info->azimuth[y + (this_page_num - 1) * 4] = sat[y].azimuth; 831 | } 832 | } 833 | 834 | 835 | } else if ((memcmp(_rx_buffer + 3, "VTG,", 4) == 0) && (fieldCount >= 8)) { 836 | 837 | /*$GNVTG,,T,,M,0.683,N,1.265,K*30 838 | $GNVTG,,T,,M,0.780,N,1.445,K*33 839 | 840 | Field Meaning 841 | 0 Message ID $GPVTG 842 | 1 Track made good (degrees true) 843 | 2 T: track made good is relative to true north 844 | 3 Track made good (degrees magnetic) 845 | 4 M: track made good is relative to magnetic north 846 | 5 Speed, in knots 847 | 6 N: speed is measured in knots 848 | 7 Speed over ground in kilometers/hour (kph) 849 | 8 K: speed over ground is measured in kph 850 | 9 The checksum data, always begins with * 851 | */ 852 | 853 | float track_true = 0.f; 854 | char T; 855 | float track_mag = 0.f; 856 | char M; 857 | float ground_speed = 0.f; 858 | char N; 859 | float ground_speed_K = 0.f; 860 | char K; 861 | NMEA_UNUSED(T); 862 | NMEA_UNUSED(track_mag); 863 | NMEA_UNUSED(M); 864 | NMEA_UNUSED(N); 865 | NMEA_UNUSED(ground_speed_K); 866 | NMEA_UNUSED(K); 867 | 868 | if (bufptr && *(++bufptr) != ',') {track_true = strtof(bufptr, &endp); bufptr = endp; } 869 | 870 | if (bufptr && *(++bufptr) != ',') { T = *(bufptr++); } 871 | 872 | if (bufptr && *(++bufptr) != ',') {track_mag = strtof(bufptr, &endp); bufptr = endp; } 873 | 874 | if (bufptr && *(++bufptr) != ',') { M = *(bufptr++); } 875 | 876 | if (bufptr && *(++bufptr) != ',') { ground_speed = strtof(bufptr, &endp); bufptr = endp; } 877 | 878 | if (bufptr && *(++bufptr) != ',') { N = *(bufptr++); } 879 | 880 | if (bufptr && *(++bufptr) != ',') { ground_speed_K = strtof(bufptr, &endp); bufptr = endp; } 881 | 882 | if (bufptr && *(++bufptr) != ',') { K = *(bufptr++); } 883 | 884 | float track_rad = track_true * M_PI_F / 180.0f; // rad in range [0, 2pi] 885 | 886 | if (track_rad > M_PI_F) { 887 | track_rad -= 2.f * M_PI_F; // rad in range [-pi, pi] 888 | } 889 | 890 | if (!_unicore_parser.agricaValid()) { 891 | float velocity_ms = ground_speed / 1.9438445f; 892 | float velocity_north = velocity_ms * cosf(track_rad); 893 | float velocity_east = velocity_ms * sinf(track_rad); 894 | _gps_position->vel_m_s = velocity_ms; 895 | _gps_position->vel_n_m_s = velocity_north; 896 | _gps_position->vel_e_m_s = velocity_east; 897 | _gps_position->vel_ned_valid = true; /** Flag to indicate if NED speed is valid */ 898 | _gps_position->c_variance_rad = 0.1f; 899 | _gps_position->s_variance_m_s = 0; 900 | _VEL_received = true; 901 | _rate_count_vel++; 902 | 903 | _gps_position->cog_rad = track_rad; 904 | } 905 | 906 | } else { 907 | NMEA_DEBUG("Unable to parse %c%c%c%c message", _rx_buffer[3], _rx_buffer[4], _rx_buffer[5], _rx_buffer[6]); 908 | } 909 | 910 | if (_sat_num_gga > 0) { 911 | _gps_position->satellites_used = _sat_num_gga; 912 | 913 | } else if (_SVNUM_received && _SVINFO_received && _FIX_received) { 914 | 915 | _sat_num_gsv = _sat_num_gpgsv + _sat_num_glgsv + _sat_num_gagsv 916 | + _sat_num_gbgsv + _sat_num_bdgsv; 917 | _gps_position->satellites_used = MAX(_sat_num_gns, _sat_num_gsv); 918 | } 919 | 920 | if (_VEL_received && _POS_received) { 921 | _gps_position->timestamp_time_relative = (int32_t)(_last_timestamp_time - _gps_position->timestamp); 922 | ret |= 1; 923 | _VEL_received = false; 924 | _POS_received = false; 925 | } 926 | 927 | return ret; 928 | } 929 | 930 | int GPSDriverNMEA::receive(unsigned timeout) 931 | { 932 | uint8_t buf[GPS_READ_BUFFER_SIZE]; 933 | 934 | /* timeout additional to poll */ 935 | gps_abstime time_started = gps_absolute_time(); 936 | 937 | int handled = 0; 938 | 939 | while (true) { 940 | int ret = read(buf, sizeof(buf), timeout); 941 | 942 | if (ret < 0) { 943 | /* something went wrong when polling or reading */ 944 | NMEA_WARN("poll_or_read err"); 945 | return -1; 946 | 947 | } else if (ret != 0) { 948 | 949 | /* pass received bytes to the packet decoder */ 950 | for (int i = 0; i < ret; i++) { 951 | int l = parseChar(buf[i]); 952 | 953 | if (l > 0) { 954 | handled |= handleMessage(l); 955 | } 956 | 957 | UnicoreParser::Result result = _unicore_parser.parseChar(buf[i]); 958 | 959 | if (result == UnicoreParser::Result::GotHeading) { 960 | 961 | // Don't mark this as handled, just publish it with position later. 962 | 963 | _unicore_heading_received_last = gps_absolute_time(); 964 | 965 | // Unicore seems to publish heading and standard deviation of 0 966 | // to signal that it has not initialized the heading yet. 967 | if (_unicore_parser.heading().heading_stddev_deg > 0.0f) { 968 | // Unicore publishes the heading between True North and 969 | // the baseline vector from master antenna to slave 970 | // antenna. 971 | // Assuming that the master is in front and the slave 972 | // in the back, this means that we need to flip the 973 | // heading 180 degrees. 974 | 975 | handleHeading( 976 | _unicore_parser.heading().heading_deg + 180.0f, 977 | _unicore_parser.heading().heading_stddev_deg); 978 | } 979 | 980 | NMEA_DEBUG("Got heading: %.1f deg, stddev: %.1f deg, baseline: %.2f m\n", 981 | (double)_unicore_parser.heading().heading_deg, 982 | (double)_unicore_parser.heading().heading_stddev_deg, 983 | (double)_unicore_parser.heading().baseline_m); 984 | 985 | // We don't specifically publish this but it's just added with the next position 986 | // update. 987 | 988 | } else if (result == UnicoreParser::Result::GotAgrica) { 989 | 990 | // Don't mark this as handled, just publish it with position later. 991 | 992 | // Receiving this message tells us that we are talking to a UM982. If 993 | // UNIHEADINGA is not configured by default, we request it now. 994 | 995 | if (gps_absolute_time() - _unicore_heading_received_last > 1000000) { 996 | request_unicore_messages(); 997 | } 998 | 999 | _gps_position->vel_m_s = _unicore_parser.agrica().velocity_m_s; 1000 | _gps_position->vel_n_m_s = _unicore_parser.agrica().velocity_north_m_s; 1001 | _gps_position->vel_e_m_s = _unicore_parser.agrica().velocity_east_m_s; 1002 | _gps_position->vel_d_m_s = -_unicore_parser.agrica().velocity_up_m_s; 1003 | _gps_position->s_variance_m_s = 1004 | (_unicore_parser.agrica().stddev_velocity_north_m_s * _unicore_parser.agrica().stddev_velocity_north_m_s + 1005 | _unicore_parser.agrica().stddev_velocity_east_m_s * _unicore_parser.agrica().stddev_velocity_east_m_s + 1006 | _unicore_parser.agrica().stddev_velocity_up_m_s * _unicore_parser.agrica().stddev_velocity_up_m_s) 1007 | / 3.0f; 1008 | 1009 | _gps_position->cog_rad = atan2f( 1010 | _unicore_parser.agrica().velocity_north_m_s, 1011 | _unicore_parser.agrica().velocity_east_m_s); 1012 | 1013 | _gps_position->vel_ned_valid = true; 1014 | _VEL_received = true; 1015 | _rate_count_vel++; 1016 | 1017 | // We don't specifically publish this but it's just added with the next position 1018 | // update. 1019 | } 1020 | } 1021 | 1022 | if (handled > 0) { 1023 | return handled; 1024 | } 1025 | } 1026 | 1027 | /* abort after timeout if no useful packets received */ 1028 | if (time_started + timeout * 1000 < gps_absolute_time()) { 1029 | return -1; 1030 | } 1031 | } 1032 | } 1033 | 1034 | void GPSDriverNMEA::handleHeading(float heading_deg, float heading_stddev_deg) 1035 | { 1036 | float heading_rad = heading_deg * M_PI_F / 180.0f; // rad in range [0, 2pi] 1037 | heading_rad -= _heading_offset; // rad in range [-pi, 3pi] 1038 | 1039 | if (heading_rad > M_PI_F) { 1040 | heading_rad -= 2.f * M_PI_F; // rad in range [-pi, pi] 1041 | } 1042 | 1043 | // We are not publishing heading_offset because it wasn't done in the past, 1044 | // and the UBX driver doesn't do it either. I'm assuming it would cause the 1045 | // offset to be applied twice. 1046 | 1047 | _gps_position->heading = heading_rad; 1048 | 1049 | const float heading_stddev_rad = heading_stddev_deg * M_PI_F / 180.0f; 1050 | _gps_position->heading_accuracy = heading_stddev_rad; 1051 | } 1052 | 1053 | void GPSDriverNMEA::request_unicore_messages() 1054 | { 1055 | // Configure position messages on serial port. Don't save it though. 1056 | { 1057 | // position 1058 | uint8_t buf[] = "GPGGA COM1 0.2\r\n"; 1059 | write(buf, sizeof(buf) - 1); 1060 | } 1061 | 1062 | { 1063 | // velocity 1064 | uint8_t buf[] = "UNIAGRICA COM1 0.2\r\n"; 1065 | write(buf, sizeof(buf) - 1); 1066 | } 1067 | 1068 | { 1069 | // heading 1070 | uint8_t buf[] = "UNIHEADINGA COM1 0.2\r\n"; 1071 | write(buf, sizeof(buf) - 1); 1072 | } 1073 | 1074 | { 1075 | // eph, epv 1076 | uint8_t buf[] = "GPGST COM1 1.0\r\n"; 1077 | write(buf, sizeof(buf) - 1); 1078 | } 1079 | 1080 | { 1081 | // vdop 1082 | uint8_t buf[] = "GPGSA COM1 1.0\r\n"; 1083 | write(buf, sizeof(buf) - 1); 1084 | } 1085 | 1086 | { 1087 | // time 1088 | uint8_t buf[] = "GPRMC COM1 1.0\r\n"; 1089 | write(buf, sizeof(buf) - 1); 1090 | } 1091 | } 1092 | 1093 | #define HEXDIGIT_CHAR(d) ((char)((d) + (((d) < 0xA) ? '0' : 'A'-0xA))) 1094 | 1095 | int GPSDriverNMEA::parseChar(uint8_t b) 1096 | { 1097 | int iRet = 0; 1098 | 1099 | if (_rtcm_parsing) { 1100 | if (_rtcm_parsing->addByte(b)) { 1101 | NMEA_DEBUG("got RTCM message with length %i", (int)_rtcm_parsing->messageLength()); 1102 | gotRTCMMessage(_rtcm_parsing->message(), _rtcm_parsing->messageLength()); 1103 | decodeInit(); 1104 | _rtcm_parsing->reset(); 1105 | return iRet; 1106 | } 1107 | } 1108 | 1109 | switch (_decode_state) { 1110 | /* First, look for sync1 */ 1111 | case NMEADecodeState::uninit: 1112 | if (b == '$') { 1113 | _decode_state = NMEADecodeState::got_sync1; 1114 | _rx_buffer_bytes = 0; 1115 | _rx_buffer[_rx_buffer_bytes++] = b; 1116 | } 1117 | 1118 | break; 1119 | 1120 | case NMEADecodeState::got_sync1: 1121 | if (b == '$') { 1122 | _decode_state = NMEADecodeState::got_sync1; 1123 | _rx_buffer_bytes = 0; 1124 | 1125 | } else if (b == '*') { 1126 | _decode_state = NMEADecodeState::got_asteriks; 1127 | } 1128 | 1129 | if (_rx_buffer_bytes >= (sizeof(_rx_buffer) - 5)) { 1130 | _decode_state = NMEADecodeState::uninit; 1131 | _rx_buffer_bytes = 0; 1132 | 1133 | } else { 1134 | _rx_buffer[_rx_buffer_bytes++] = b; 1135 | } 1136 | 1137 | break; 1138 | 1139 | case NMEADecodeState::got_asteriks: 1140 | _rx_buffer[_rx_buffer_bytes++] = b; 1141 | _decode_state = NMEADecodeState::got_first_cs_byte; 1142 | break; 1143 | 1144 | case NMEADecodeState::got_first_cs_byte: { 1145 | _rx_buffer[_rx_buffer_bytes++] = b; 1146 | uint8_t checksum = 0; 1147 | uint8_t *buffer = _rx_buffer + 1; 1148 | uint8_t *bufend = _rx_buffer + _rx_buffer_bytes - 3; 1149 | 1150 | for (; buffer < bufend; buffer++) { checksum ^= *buffer; } 1151 | 1152 | if ((HEXDIGIT_CHAR(checksum >> 4) == *(_rx_buffer + _rx_buffer_bytes - 2)) && 1153 | (HEXDIGIT_CHAR(checksum & 0x0F) == *(_rx_buffer + _rx_buffer_bytes - 1))) { 1154 | iRet = _rx_buffer_bytes; 1155 | 1156 | if (_rtcm_parsing) { 1157 | _rtcm_parsing->reset(); 1158 | } 1159 | } 1160 | 1161 | decodeInit(); 1162 | } 1163 | break; 1164 | } 1165 | 1166 | return iRet; 1167 | } 1168 | 1169 | void GPSDriverNMEA::decodeInit() 1170 | { 1171 | _rx_buffer_bytes = 0; 1172 | _decode_state = NMEADecodeState::uninit; 1173 | } 1174 | 1175 | int GPSDriverNMEA::configure(unsigned &baudrate, const GPSConfig &config) 1176 | { 1177 | _output_mode = config.output_mode; 1178 | 1179 | if (_output_mode != OutputMode::GPS) { 1180 | NMEA_WARN("RTCM output have to be configured manually"); 1181 | 1182 | if (!_rtcm_parsing) { 1183 | _rtcm_parsing = new RTCMParsing(); 1184 | } 1185 | 1186 | _rtcm_parsing->reset(); 1187 | } 1188 | 1189 | // If a baudrate is defined, we test this first 1190 | if (baudrate > 0) { 1191 | setBaudrate(baudrate); 1192 | decodeInit(); 1193 | int ret = receive(400); 1194 | gps_usleep(2000); 1195 | 1196 | // If a valid POS message is received we have GPS 1197 | if (_POS_received || ret > 0) { 1198 | return 0; 1199 | } 1200 | } 1201 | 1202 | // If we haven't found the GPS with the defined baudrate, we try other rates 1203 | const unsigned baudrates_to_try[] = {9600, 19200, 38400, 57600, 115200, 230400}; 1204 | unsigned test_baudrate; 1205 | 1206 | for (unsigned int baud_i = 0; !_POS_received 1207 | && baud_i < sizeof(baudrates_to_try) / sizeof(baudrates_to_try[0]); baud_i++) { 1208 | 1209 | test_baudrate = baudrates_to_try[baud_i]; 1210 | setBaudrate(test_baudrate); 1211 | 1212 | NMEA_DEBUG("baudrate set to %i", test_baudrate); 1213 | 1214 | decodeInit(); 1215 | int ret = receive(400); 1216 | gps_usleep(2000); 1217 | 1218 | // If a valid POS message is received we have GPS 1219 | if (_POS_received || ret > 0) { 1220 | return 0; 1221 | } 1222 | } 1223 | 1224 | // If nothing is found we leave the specified or default 1225 | if (baudrate > 0) { 1226 | return setBaudrate(baudrate); 1227 | } 1228 | 1229 | return setBaudrate(NMEA_DEFAULT_BAUDRATE); 1230 | } 1231 | -------------------------------------------------------------------------------- /src/nmea.h: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * 3 | * Copyright (c) 2020 - 2024 PX4 Development Team. All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions 7 | * are met: 8 | * 9 | * 1. Redistributions of source code must retain the above copyright 10 | * notice, this list of conditions and the following disclaimer. 11 | * 2. Redistributions in binary form must reproduce the above copyright 12 | * notice, this list of conditions and the following disclaimer in 13 | * the documentation and/or other materials provided with the 14 | * distribution. 15 | * 3. Neither the name PX4 nor the names of its contributors may be 16 | * used to endorse or promote products derived from this software 17 | * without specific prior written permission. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 20 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 21 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 22 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 23 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 24 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 25 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS 26 | * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 27 | * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 28 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 29 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 30 | * POSSIBILITY OF SUCH DAMAGE. 31 | * 32 | ****************************************************************************/ 33 | 34 | /** 35 | * @file nmea.h 36 | * 37 | * NMEA protocol definitions 38 | * 39 | * @author WeiPeng Guo 40 | * @author Stone White 41 | * @author Jose Jimenez-Berni 42 | * 43 | */ 44 | 45 | #pragma once 46 | 47 | #include "gps_helper.h" 48 | #include "unicore.h" 49 | 50 | class RTCMParsing; 51 | 52 | #define NMEA_RECV_BUFFER_SIZE 1024 53 | #define NMEA_DEFAULT_BAUDRATE 115200 54 | 55 | class GPSDriverNMEA : public GPSHelper 56 | { 57 | public: 58 | /** 59 | * @param heading_offset heading offset in radians [-pi, pi]. It is substracted from the measurement. 60 | */ 61 | GPSDriverNMEA(GPSCallbackPtr callback, void *callback_user, 62 | sensor_gps_s *gps_position, 63 | satellite_info_s *satellite_info, 64 | float heading_offset = 0.f); 65 | 66 | virtual ~GPSDriverNMEA(); 67 | 68 | int receive(unsigned timeout) override; 69 | int configure(unsigned &baudrate, const GPSConfig &config) override; 70 | 71 | private: 72 | void handleHeading(float heading_deg, float heading_stddev_deg); 73 | void request_unicore_messages(); 74 | 75 | UnicoreParser _unicore_parser; 76 | gps_abstime _unicore_heading_received_last; 77 | 78 | enum class NMEADecodeState { 79 | uninit, 80 | got_sync1, 81 | got_asteriks, 82 | got_first_cs_byte 83 | }; 84 | 85 | void decodeInit(void); 86 | int handleMessage(int len); 87 | int parseChar(uint8_t b); 88 | 89 | int32_t read_int(); 90 | double read_float(); 91 | char read_char(); 92 | 93 | sensor_gps_s *_gps_position {nullptr}; 94 | satellite_info_s *_satellite_info {nullptr}; 95 | double _last_POS_timeUTC{0}; 96 | double _last_VEL_timeUTC{0}; 97 | uint64_t _last_timestamp_time{0}; 98 | 99 | uint8_t _sat_num_gga{0}; 100 | uint8_t _sat_num_gns{0}; 101 | uint8_t _sat_num_gsv{0}; 102 | uint8_t _sat_num_gpgsv{0}; 103 | uint8_t _sat_num_glgsv{0}; 104 | uint8_t _sat_num_gagsv{0}; 105 | uint8_t _sat_num_gbgsv{0}; 106 | uint8_t _sat_num_bdgsv{0}; 107 | 108 | bool _clock_set {false}; 109 | 110 | // check if we got all basic essential packages we need 111 | bool _TIME_received{false}; 112 | bool _POS_received{false}; 113 | bool _ALT_received{false}; 114 | bool _SVNUM_received{false}; 115 | bool _SVINFO_received{false}; 116 | bool _FIX_received{false}; 117 | bool _DOP_received{false}; 118 | bool _VEL_received{false}; 119 | bool _EPH_received{false}; 120 | bool _HEAD_received{false}; 121 | 122 | NMEADecodeState _decode_state{NMEADecodeState::uninit}; 123 | uint8_t _rx_buffer[NMEA_RECV_BUFFER_SIZE] {}; 124 | uint16_t _rx_buffer_bytes{0}; 125 | 126 | OutputMode _output_mode{OutputMode::GPS}; 127 | 128 | RTCMParsing *_rtcm_parsing{nullptr}; 129 | 130 | float _heading_offset; 131 | }; 132 | -------------------------------------------------------------------------------- /src/rtcm.cpp: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * 3 | * Copyright (c) 2018 PX4 Development Team. All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions 7 | * are met: 8 | * 9 | * 1. Redistributions of source code must retain the above copyright 10 | * notice, this list of conditions and the following disclaimer. 11 | * 2. Redistributions in binary form must reproduce the above copyright 12 | * notice, this list of conditions and the following disclaimer in 13 | * the documentation and/or other materials provided with the 14 | * distribution. 15 | * 3. Neither the name PX4 nor the names of its contributors may be 16 | * used to endorse or promote products derived from this software 17 | * without specific prior written permission. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 20 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 21 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 22 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 23 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 24 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 25 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS 26 | * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 27 | * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 28 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 29 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 30 | * POSSIBILITY OF SUCH DAMAGE. 31 | * 32 | ****************************************************************************/ 33 | 34 | #include "rtcm.h" 35 | #include 36 | 37 | RTCMParsing::RTCMParsing() 38 | { 39 | reset(); 40 | } 41 | 42 | RTCMParsing::~RTCMParsing() 43 | { 44 | delete[] _buffer; 45 | } 46 | 47 | void RTCMParsing::reset() 48 | { 49 | if (!_buffer) { 50 | _buffer = new uint8_t[RTCM_INITIAL_BUFFER_LENGTH]; 51 | _buffer_len = RTCM_INITIAL_BUFFER_LENGTH; 52 | } 53 | 54 | _pos = 0; 55 | _message_length = _buffer_len; 56 | _preamble_received = false; 57 | } 58 | 59 | bool RTCMParsing::addByte(uint8_t b) 60 | { 61 | if (!_buffer) { 62 | reset(); 63 | return false; 64 | } 65 | 66 | if (!_preamble_received) { 67 | if (b == RTCM3_PREAMBLE) { 68 | _preamble_received = true; 69 | 70 | } else { 71 | return false; 72 | } 73 | } 74 | 75 | _buffer[_pos++] = b; 76 | 77 | if (_pos == 3) { 78 | _message_length = (((uint16_t)_buffer[1] & 3) << 8) | (_buffer[2]); 79 | 80 | if (_message_length + 6 > _buffer_len) { 81 | uint16_t new_buffer_len = _message_length + 6; 82 | uint8_t *new_buffer = new uint8_t[new_buffer_len]; 83 | 84 | if (!new_buffer) { 85 | delete[](_buffer); 86 | _buffer = nullptr; 87 | reset(); 88 | return false; 89 | } 90 | 91 | memcpy(new_buffer, _buffer, 3); 92 | delete[](_buffer); 93 | _buffer = new_buffer; 94 | _buffer_len = new_buffer_len; 95 | } 96 | } 97 | 98 | if (_message_length + 6 == _pos) { 99 | const uint8_t *crc_buffer = &_buffer[_message_length + 3]; 100 | uint32_t actual_crc = (crc_buffer[0] << 16) | (crc_buffer[1] << 8) | crc_buffer[2]; 101 | uint32_t expected_crc = crc24(_buffer, _message_length + 3); 102 | 103 | if (actual_crc == expected_crc) { 104 | return true; 105 | 106 | } else { 107 | reset(); 108 | return false; 109 | } 110 | } 111 | 112 | return false; 113 | } 114 | 115 | uint32_t RTCMParsing::crc24(const uint8_t *buffer, uint16_t len) 116 | { 117 | constexpr uint32_t poly = 0x1864CFB; 118 | uint32_t crc = 0; 119 | 120 | while (len--) { 121 | crc ^= (*buffer++) << 16; 122 | 123 | for (int i = 0; i < 8; i++) { 124 | crc <<= 1; 125 | 126 | if (crc & 0x1000000) { 127 | crc ^= poly; 128 | } 129 | } 130 | } 131 | 132 | return crc; 133 | } 134 | -------------------------------------------------------------------------------- /src/rtcm.h: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * 3 | * Copyright (c) 2018 PX4 Development Team. All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions 7 | * are met: 8 | * 9 | * 1. Redistributions of source code must retain the above copyright 10 | * notice, this list of conditions and the following disclaimer. 11 | * 2. Redistributions in binary form must reproduce the above copyright 12 | * notice, this list of conditions and the following disclaimer in 13 | * the documentation and/or other materials provided with the 14 | * distribution. 15 | * 3. Neither the name PX4 nor the names of its contributors may be 16 | * used to endorse or promote products derived from this software 17 | * without specific prior written permission. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 20 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 21 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 22 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 23 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 24 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 25 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS 26 | * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 27 | * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 28 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 29 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 30 | * POSSIBILITY OF SUCH DAMAGE. 31 | * 32 | ****************************************************************************/ 33 | 34 | #pragma once 35 | 36 | #include 37 | 38 | /* RTCM3 */ 39 | #define RTCM3_PREAMBLE 0xD3 40 | #define RTCM_INITIAL_BUFFER_LENGTH 300 /**< initial maximum message length of an RTCM message */ 41 | 42 | 43 | class RTCMParsing 44 | { 45 | public: 46 | RTCMParsing(); 47 | ~RTCMParsing(); 48 | 49 | /** 50 | * reset the parsing state 51 | */ 52 | void reset(); 53 | 54 | /** 55 | * add a byte to the message 56 | * @param b byte to add 57 | * @return true, if a message is complete (use @message to get it). The user needs to reset the parser in this case. 58 | * false, if more data is needed or the parser failed. In this case no resetting of the parser is needed. 59 | */ 60 | bool addByte(uint8_t b); 61 | 62 | uint8_t *message() const { return _buffer; } 63 | uint16_t messageLength() const { return _pos; } 64 | uint16_t messageId() const { return (_buffer[3] << 4) | (_buffer[4] >> 4); } 65 | 66 | private: 67 | uint32_t crc24(const uint8_t *buffer, const uint16_t len); 68 | 69 | uint8_t *_buffer{nullptr}; 70 | uint16_t _buffer_len{}; 71 | uint16_t _pos{}; ///< next position in buffer 72 | uint16_t _message_length{}; ///< message length without header & CRC (both 3 bytes) 73 | bool _preamble_received{false}; 74 | }; 75 | -------------------------------------------------------------------------------- /src/sbf.cpp: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * 3 | * Copyright (c) 2018-2024 PX4 Development Team. All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions 7 | * are met: 8 | * 9 | * 1. Redistributions of source code must retain the above copyright 10 | * notice, this list of conditions and the following disclaimer. 11 | * 2. Redistributions in binary form must reproduce the above copyright 12 | * notice, this list of conditions and the following disclaimer in 13 | * the documentation and/or other materials provided with the 14 | * distribution. 15 | * 3. Neither the name PX4 nor the names of its contributors may be 16 | * used to endorse or promote products derived from this software 17 | * without specific prior written permission. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 20 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 21 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 22 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 23 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 24 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 25 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS 26 | * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 27 | * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 28 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 29 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 30 | * POSSIBILITY OF SUCH DAMAGE. 31 | * 32 | ****************************************************************************/ 33 | 34 | /** 35 | * @file sbf.cpp 36 | * 37 | * Septentrio protocol as defined in PPSDK SBF Reference Guide 4.1.8 38 | * 39 | * @author Matej Frančeškin 40 | * @author Seppe Geuens 41 | * 42 | */ 43 | 44 | #include "sbf.h" 45 | #include "rtcm.h" 46 | 47 | #include 48 | #include 49 | #include 50 | 51 | #define SBF_CONFIG_TIMEOUT 1000 // ms, timeout for waiting ACK 52 | #define SBF_PACKET_TIMEOUT 2 // ms, if now data during this delay assume that full update received 53 | #define DISABLE_MSG_INTERVAL 1000000 // us, try to disable message with this interval 54 | #define DNU 100000.0 // Do-Not-Use value for PVTGeodetic 55 | #define MSG_SIZE 100 // size of the message to be sent to the receiver. 56 | 57 | /**** Trace macros, disable for production builds */ 58 | #define SBF_TRACE_PARSER(...) {/*GPS_INFO(__VA_ARGS__);*/} /* decoding progress in parse_char() */ 59 | #define SBF_TRACE_RXMSG(...) {/*GPS_INFO(__VA_ARGS__);*/} /* Rx msgs in payload_rx_done() */ 60 | #define SBF_INFO(...) {GPS_INFO(__VA_ARGS__);} 61 | 62 | /**** Warning macros, disable to save memory */ 63 | #define SBF_WARN(...) {GPS_WARN(__VA_ARGS__);} 64 | #define SBF_DEBUG(...) {/*GPS_WARN(__VA_ARGS__);*/} 65 | 66 | GPSDriverSBF::GPSDriverSBF(GPSCallbackPtr callback, void *callback_user, struct sensor_gps_s *gps_position, 67 | satellite_info_s *satellite_info, float heading_offset, float pitch_offset) 68 | : GPSBaseStationSupport(callback, callback_user), _gps_position(gps_position), _satellite_info(satellite_info), 69 | _heading_offset(heading_offset), _pitch_offset(pitch_offset) 70 | { 71 | decodeInit(); 72 | } 73 | 74 | GPSDriverSBF::~GPSDriverSBF() 75 | { 76 | delete _rtcm_parsing; 77 | } 78 | 79 | int GPSDriverSBF::configure(unsigned &baudrate, const GPSConfig &config) 80 | { 81 | char buf[GPS_READ_BUFFER_SIZE]; 82 | char msg[MSG_SIZE]; 83 | 84 | _configured = false; 85 | 86 | setBaudrate(SBF_TX_CFG_PRT_BAUDRATE); 87 | baudrate = SBF_TX_CFG_PRT_BAUDRATE; 88 | _output_mode = config.output_mode; 89 | 90 | // Make sure we can send commands to the receiver 91 | sendMessage(SBF_CONFIG_FORCE_INPUT); 92 | 93 | // Disable previous output for now so we can detect the COM port 94 | for (int i = 1; i <= 2; i++) { 95 | snprintf(msg, sizeof(msg), SBF_CONFIG_DISABLE_OUTPUT, "COM", i); 96 | sendMessageAndWaitForAck(msg, SBF_CONFIG_TIMEOUT); 97 | } 98 | 99 | for (int i = 1; i <= 4; i++) { 100 | snprintf(msg, sizeof(msg), SBF_CONFIG_DISABLE_OUTPUT, "USB", i); 101 | sendMessageAndWaitForAck(msg, SBF_CONFIG_TIMEOUT); 102 | } 103 | 104 | char com_port[5] {}; 105 | size_t offset = 1; 106 | bool response_detected = false; 107 | gps_abstime time_started = gps_absolute_time(); 108 | sendMessage("\n\r"); 109 | 110 | // Read buffer to get the COM port 111 | do { 112 | --offset; // overwrite the null-char 113 | int ret = read(reinterpret_cast(buf) + offset, sizeof(buf) - offset - 1, SBF_CONFIG_TIMEOUT); 114 | 115 | if (ret < 0) { 116 | // something went wrong when reading 117 | SBF_WARN("sbf read err"); 118 | return ret; 119 | } 120 | 121 | offset += ret; 122 | buf[offset++] = '\0'; 123 | 124 | char *p = strstr(buf, ">"); 125 | 126 | if (p) { //check if the length of the com port == 4 and contains a > sign 127 | for (int i = 0; i < 4; i++) { 128 | com_port[i] = buf[i]; 129 | } 130 | 131 | response_detected = true; 132 | } 133 | 134 | if (offset >= sizeof(buf)) { 135 | offset = 1; 136 | } 137 | 138 | } while (time_started + 1000 * SBF_CONFIG_TIMEOUT > gps_absolute_time() && !response_detected); 139 | 140 | if (response_detected) { 141 | SBF_INFO("Septentrio GNSS receiver COM port: %s", com_port); 142 | response_detected = false; // for future use 143 | 144 | } else { 145 | SBF_WARN("No COM port detected") 146 | return -1; 147 | } 148 | 149 | // Delete all sbf outputs on current COM port to remove clutter data 150 | snprintf(msg, sizeof(msg), SBF_CONFIG_RESET, com_port); 151 | 152 | if (!sendMessageAndWaitForAck(msg, SBF_CONFIG_TIMEOUT)) { 153 | return -1; // connection and/or baudrate detection failed 154 | } 155 | 156 | // Set baudrate, unless we're connected over USB 157 | if (strncmp(com_port, "USB1", 4) != 0 && strncmp(com_port, "USB2", 4) != 0) { 158 | snprintf(msg, sizeof(msg), SBF_CONFIG_BAUDRATE, com_port, baudrate); 159 | 160 | if (!sendMessageAndWaitForAck(msg, SBF_CONFIG_TIMEOUT)) { 161 | SBF_DEBUG("Connection and/or baudrate detection failed (SBF_CONFIG_BAUDRATE)"); 162 | return -1; // connection and/or baudrate detection failed 163 | } 164 | } 165 | 166 | // At this point we have correct baudrate on both ends 167 | SBF_DEBUG("Correct baud rate on both ends"); 168 | 169 | // Define/inquire the type of data that the receiver should accept/send on a given connection descriptor 170 | snprintf(msg, sizeof(msg), SBF_DATA_IO, com_port); 171 | 172 | if (!sendMessageAndWaitForAck(msg, SBF_CONFIG_TIMEOUT)) { 173 | return -1; 174 | } 175 | 176 | // Set the type of dynamics the GNSS antenna is subjected to. 177 | if (_output_mode != OutputMode::RTCM) { 178 | 179 | // Specify the offsets that the receiver applies to the computed attitude angles. 180 | snprintf(msg, sizeof(msg), SBF_CONFIG_ATTITUDE_OFFSET, (double)(_heading_offset * 180 / M_PI_F), (double)_pitch_offset); 181 | 182 | if (!sendMessageAndWaitForAck(msg, SBF_CONFIG_TIMEOUT)) { 183 | return -1; 184 | } 185 | 186 | if (_dynamic_model < 6) { 187 | snprintf(msg, sizeof(msg), SBF_CONFIG_RECEIVER_DYNAMICS, "low"); 188 | 189 | } else if (_dynamic_model < 7) { 190 | snprintf(msg, sizeof(msg), SBF_CONFIG_RECEIVER_DYNAMICS, "moderate"); 191 | 192 | } else if (_dynamic_model < 8) { 193 | snprintf(msg, sizeof(msg), SBF_CONFIG_RECEIVER_DYNAMICS, "high"); 194 | 195 | } else { 196 | snprintf(msg, sizeof(msg), SBF_CONFIG_RECEIVER_DYNAMICS, "max"); 197 | } 198 | 199 | sendMessageAndWaitForAck(msg, SBF_CONFIG_TIMEOUT); 200 | 201 | snprintf(msg, sizeof(msg), SBF_CONFIG, com_port); 202 | 203 | sendMessageAndWaitForAck(msg, SBF_CONFIG_TIMEOUT); 204 | } 205 | 206 | int i = 0; 207 | 208 | do { 209 | ++i; 210 | 211 | if (!sendMessageAndWaitForAck(msg, SBF_CONFIG_TIMEOUT)) { 212 | if (i >= 5) { 213 | return -1; // connection and/or baudrate detection failed 214 | } 215 | 216 | } else { 217 | response_detected = true; 218 | } 219 | } while (i < 5 && !response_detected); 220 | 221 | if (_output_mode == OutputMode::GPSAndRTCM || _output_mode == OutputMode::RTCM) { 222 | if (!_rtcm_parsing) { 223 | _rtcm_parsing = new RTCMParsing(); 224 | } 225 | 226 | _rtcm_parsing->reset(); 227 | } 228 | 229 | if (_output_mode == OutputMode::RTCM) { 230 | if (_base_settings.type == BaseSettingsType::fixed_position) { 231 | snprintf(msg, sizeof(msg), SBF_CONFIG_RTCM_STATIC_COORDINATES, 232 | _base_settings.settings.fixed_position.latitude, 233 | _base_settings.settings.fixed_position.longitude, 234 | static_cast(_base_settings.settings.fixed_position.altitude)); 235 | sendMessageAndWaitForAck(msg, SBF_CONFIG_TIMEOUT); 236 | snprintf(msg, sizeof(msg), SBF_CONFIG_RTCM_STATIC_OFFSET, 0.0, 0.0, 0.0); 237 | sendMessageAndWaitForAck(msg, SBF_CONFIG_TIMEOUT); 238 | sendMessageAndWaitForAck(SBF_CONFIG_RTCM_STATIC1, SBF_CONFIG_TIMEOUT); 239 | sendMessageAndWaitForAck(SBF_CONFIG_RTCM_STATIC2, SBF_CONFIG_TIMEOUT); 240 | 241 | } else { 242 | sendMessageAndWaitForAck(SBF_CONFIG_RTCM, SBF_CONFIG_TIMEOUT); 243 | } 244 | } 245 | 246 | _configured = true; 247 | return 0; 248 | } 249 | 250 | bool GPSDriverSBF::sendMessage(const char *msg) 251 | { 252 | // Send message 253 | SBF_DEBUG("Send MSG: %s", msg); 254 | int length = static_cast(strlen(msg)); 255 | 256 | return (write(msg, length) == length); 257 | } 258 | 259 | bool GPSDriverSBF::sendMessageAndWaitForAck(const char *msg, const int timeout) 260 | { 261 | SBF_DEBUG("Send MSG: %s", msg); 262 | 263 | // Send message 264 | int length = static_cast(strlen(msg)); 265 | 266 | if (write(msg, length) != length) { 267 | return false; 268 | } 269 | 270 | // Wait for acknowledge 271 | // For all valid set -, get - and exe -commands, the first line of the reply is an exact copy 272 | // of the command as entered by the user, preceded with "$R:" 273 | char buf[GPS_READ_BUFFER_SIZE]; 274 | size_t offset = 1; 275 | gps_abstime time_started = gps_absolute_time(); 276 | 277 | bool found_response = false; 278 | 279 | do { 280 | --offset; //overwrite the null-char 281 | int ret = read(reinterpret_cast(buf) + offset, sizeof(buf) - offset - 1, timeout); 282 | 283 | if (ret < 0) { 284 | // something went wrong when reading 285 | SBF_WARN("sbf read err"); 286 | return false; 287 | } 288 | 289 | offset += ret; 290 | buf[offset++] = '\0'; 291 | 292 | if (!found_response && strstr(buf, "$R: ") != nullptr) { 293 | //SBF_DEBUG("READ %d: %s", (int) offset, buf); 294 | found_response = true; 295 | } 296 | 297 | if (offset >= sizeof(buf)) { 298 | offset = 1; 299 | } 300 | 301 | } while (time_started + 1000 * timeout > gps_absolute_time()); 302 | 303 | SBF_DEBUG("response: %u", found_response) 304 | return found_response; 305 | } 306 | 307 | // return value: 308 | // 0b1111_1111 = an error occurred 309 | // 0b0000_0000 = no message handled (not set up yet) 310 | // 0b0000_0001 = message handled 311 | // 0b0000_0010 = sat info message handled 312 | int GPSDriverSBF::receive(unsigned timeout) 313 | { 314 | int handled = 0; 315 | gps_abstime time_started; 316 | uint8_t buf[GPS_READ_BUFFER_SIZE]; 317 | 318 | // Do not receive messages until we're configured 319 | if (!_configured) { 320 | gps_usleep(timeout * 1000); 321 | return 0; 322 | } 323 | 324 | // Timeout after not receiving a complete message for a certain time 325 | time_started = gps_absolute_time(); 326 | 327 | while (true) { 328 | // Wait for only SBF_PACKET_TIMEOUT if something already received. 329 | int ret = read(buf, sizeof(buf), handled ? SBF_PACKET_TIMEOUT : timeout); 330 | 331 | if (ret < 0) { 332 | // something went wrong when reading 333 | SBF_WARN("ubx read err"); 334 | return -1; 335 | 336 | } else { 337 | SBF_DEBUG("Read %d bytes (receive)", ret); 338 | 339 | // pass received bytes to the packet decoder 340 | for (int i = 0; i < ret; i++) { 341 | handled |= parseChar(buf[i]); 342 | SBF_DEBUG("parsed %d: 0x%x", i, buf[i]); 343 | } 344 | } 345 | 346 | if (handled > 0) { 347 | return handled; 348 | } 349 | 350 | // abort after timeout if no useful packets received 351 | if (time_started + timeout * 1000 < gps_absolute_time()) { 352 | SBF_DEBUG("timed out, returning"); 353 | return -1; 354 | } 355 | } 356 | } 357 | 358 | // return value: 359 | // 0b0000_0000 = still decoding 360 | // 0b0000_0001 = message handled 361 | // 0b0000_0010 = sat info message handled 362 | int GPSDriverSBF::parseChar(const uint8_t b) 363 | { 364 | int ret = 0; 365 | 366 | if (_rtcm_parsing) { 367 | if (_rtcm_parsing->addByte(b)) { 368 | SBF_DEBUG("got RTCM message with length %i", (int) _rtcm_parsing->messageLength()); 369 | gotRTCMMessage(_rtcm_parsing->message(), _rtcm_parsing->messageLength()); 370 | decodeInit(); 371 | _rtcm_parsing->reset(); 372 | return ret; 373 | } 374 | } 375 | 376 | switch (_decode_state) { 377 | 378 | // Expecting Sync1 379 | case SBF_DECODE_SYNC1: 380 | if (b == SBF_SYNC1) { // Sync1 found --> expecting Sync2 381 | SBF_TRACE_PARSER("A"); 382 | payloadRxAdd(b); // add a payload byte 383 | _decode_state = SBF_DECODE_SYNC2; 384 | } 385 | 386 | break; 387 | 388 | // Expecting Sync2 389 | case SBF_DECODE_SYNC2: 390 | if (b == SBF_SYNC2) { // Sync2 found --> expecting CRC 391 | SBF_TRACE_PARSER("B"); 392 | payloadRxAdd(b); // add a payload byte 393 | _decode_state = SBF_DECODE_PAYLOAD; 394 | 395 | } else { // Sync1 not followed by Sync2: reset parser 396 | decodeInit(); 397 | } 398 | 399 | break; 400 | 401 | // Expecting payload 402 | case SBF_DECODE_PAYLOAD: SBF_TRACE_PARSER("."); 403 | 404 | ret = payloadRxAdd(b); // add a payload byte 405 | 406 | if (ret < 0) { 407 | // payload not handled, discard message 408 | ret = 0; 409 | decodeInit(); 410 | 411 | } else if (ret > 0) { 412 | ret = payloadRxDone(); // finish payload processing 413 | 414 | if (_rtcm_parsing) { 415 | _rtcm_parsing->reset(); 416 | } 417 | 418 | decodeInit(); 419 | 420 | } else { 421 | // expecting more payload, stay in state SBF_DECODE_PAYLOAD 422 | ret = 0; 423 | } 424 | 425 | break; 426 | 427 | default: 428 | break; 429 | } 430 | 431 | return ret; 432 | } 433 | 434 | /** 435 | * Add payload rx byte 436 | */ 437 | // -1 = error, 0 = ok, 1 = payload completed 438 | int GPSDriverSBF::payloadRxAdd(const uint8_t b) 439 | { 440 | int ret = 0; 441 | uint8_t *p_buf = reinterpret_cast(&_buf); 442 | 443 | p_buf[_rx_payload_index++] = b; 444 | 445 | if ((_rx_payload_index > 7 && _rx_payload_index >= _buf.length) || _rx_payload_index >= sizeof(_buf)) { 446 | ret = 1; // payload received completely 447 | } 448 | 449 | return ret; 450 | } 451 | 452 | /** 453 | * Calculate buffer CRC16 454 | */ 455 | uint16_t crc16(const uint8_t *data_p, uint32_t length) 456 | { 457 | uint8_t x; 458 | uint16_t crc = 0; 459 | 460 | while (length--) { 461 | x = crc >> 8 ^ *data_p++; 462 | x ^= x >> 4; 463 | crc = static_cast((crc << 8) ^ (x << 12) ^ (x << 5) ^ x); 464 | } 465 | 466 | return crc; 467 | } 468 | 469 | /** 470 | * Finish payload rx 471 | */ 472 | // 0 = no message handled, 1 = message handled, 2 = sat info message handled 473 | int GPSDriverSBF::payloadRxDone() 474 | { 475 | int ret = 0; 476 | #ifndef NO_MKTIME 477 | struct tm timeinfo; 478 | time_t epoch; 479 | #endif 480 | 481 | if (_buf.length <= 4 || 482 | _buf.length > _rx_payload_index || 483 | _buf.crc16 != crc16(reinterpret_cast(&_buf) + 4, _buf.length - 4)) { 484 | return 0; 485 | } 486 | 487 | // handle message 488 | switch (_buf.msg_id) { 489 | case SBF_ID_PVTGeodetic: SBF_TRACE_RXMSG("Rx PVTGeodetic"); 490 | _msg_status |= 1; 491 | 492 | if (_buf.payload_pvt_geodetic.mode_type < 1) { 493 | _gps_position->fix_type = 1; 494 | 495 | } else if (_buf.payload_pvt_geodetic.mode_type == 6) { 496 | _gps_position->fix_type = 4; 497 | 498 | } else if (_buf.payload_pvt_geodetic.mode_type == 5 || _buf.payload_pvt_geodetic.mode_type == 8) { 499 | _gps_position->fix_type = 5; 500 | 501 | } else if (_buf.payload_pvt_geodetic.mode_type == 4 || _buf.payload_pvt_geodetic.mode_type == 7) { 502 | _gps_position->fix_type = 6; 503 | 504 | } else { 505 | _gps_position->fix_type = 3; 506 | } 507 | 508 | // Check fix and error code 509 | _gps_position->vel_ned_valid = _gps_position->fix_type > 1 && _buf.payload_pvt_geodetic.error == 0; 510 | 511 | // Check boundaries and invalidate GPS velocities 512 | // We're not just checking for the do-not-use value (-2*10^10) but for any value beyond the specified max values 513 | if (fabsf(_buf.payload_pvt_geodetic.vn) > 600.0f || fabsf(_buf.payload_pvt_geodetic.ve) > 600.0f || 514 | fabsf(_buf.payload_pvt_geodetic.vu) > 600.0f) { 515 | _gps_position->vel_ned_valid = false; 516 | } 517 | 518 | // Check boundaries and invalidate position 519 | // We're not just checking for the do-not-use value (-2*10^10) but for any value beyond the specified max values 520 | if (fabs(_buf.payload_pvt_geodetic.latitude) > (double)(M_PI_F / 2.0f) || 521 | fabs(_buf.payload_pvt_geodetic.longitude) > (double) M_PI_F || 522 | fabs(_buf.payload_pvt_geodetic.height) > DNU || 523 | fabsf(_buf.payload_pvt_geodetic.undulation) > (float) DNU) { 524 | _gps_position->fix_type = 0; 525 | } 526 | 527 | if (_buf.payload_pvt_geodetic.nr_sv < 255) { // 255 = do not use value 528 | _gps_position->satellites_used = _buf.payload_pvt_geodetic.nr_sv; 529 | 530 | if (_satellite_info) { 531 | // Only fill in the satellite count for now (we could use the ChannelStatus message for the 532 | // other data, but it's really large: >800B) 533 | _satellite_info->timestamp = gps_absolute_time(); 534 | _satellite_info->count = _gps_position->satellites_used; 535 | ret = 2; 536 | } 537 | 538 | } else { 539 | _gps_position->satellites_used = 0; 540 | } 541 | 542 | _gps_position->latitude_deg = _buf.payload_pvt_geodetic.latitude * M_RAD_TO_DEG; 543 | _gps_position->longitude_deg = _buf.payload_pvt_geodetic.longitude * M_RAD_TO_DEG; 544 | _gps_position->altitude_ellipsoid_m = _buf.payload_pvt_geodetic.height; 545 | _gps_position->altitude_msl_m = _buf.payload_pvt_geodetic.height - static_cast 546 | (_buf.payload_pvt_geodetic.undulation); 547 | 548 | /* H and V accuracy are reported in 2DRMS, but based off the uBlox reporting we expect RMS. 549 | * Devide by 100 from cm to m and in addition divide by 2 to get RMS. */ 550 | _gps_position->eph = static_cast(_buf.payload_pvt_geodetic.h_accuracy) / 200.0f; 551 | _gps_position->epv = static_cast(_buf.payload_pvt_geodetic.v_accuracy) / 200.0f; 552 | 553 | _gps_position->vel_n_m_s = static_cast(_buf.payload_pvt_geodetic.vn); 554 | _gps_position->vel_e_m_s = static_cast(_buf.payload_pvt_geodetic.ve); 555 | _gps_position->vel_d_m_s = -1.0f * static_cast(_buf.payload_pvt_geodetic.vu); 556 | _gps_position->vel_m_s = sqrtf(_gps_position->vel_n_m_s * _gps_position->vel_n_m_s + 557 | _gps_position->vel_e_m_s * _gps_position->vel_e_m_s); 558 | 559 | _gps_position->cog_rad = static_cast(_buf.payload_pvt_geodetic.cog) * M_DEG_TO_RAD_F; 560 | _gps_position->c_variance_rad = 1.0f * M_DEG_TO_RAD_F; 561 | 562 | // _buf.payload_pvt_geodetic.cog is set to -2*10^10 for velocities below 0.1m/s 563 | if (_buf.payload_pvt_geodetic.cog > 360.0f) { 564 | _buf.payload_pvt_geodetic.cog = NAN; 565 | } 566 | 567 | _gps_position->time_utc_usec = 0; 568 | #ifndef NO_MKTIME 569 | /* convert to unix timestamp */ 570 | memset(&timeinfo, 0, sizeof(timeinfo)); 571 | 572 | timeinfo.tm_year = 1980 - 1900; 573 | timeinfo.tm_mon = 0; 574 | timeinfo.tm_mday = 6 + _buf.WNc * 7; 575 | timeinfo.tm_hour = 0; 576 | timeinfo.tm_min = 0; 577 | timeinfo.tm_sec = _buf.TOW / 1000; 578 | 579 | epoch = mktime(&timeinfo); 580 | 581 | if (epoch > GPS_EPOCH_SECS) { 582 | // FMUv2+ boards have a hardware RTC, but GPS helps us to configure it 583 | // and control its drift. Since we rely on the HRT for our monotonic 584 | // clock, updating it from time to time is safe. 585 | 586 | timespec ts; 587 | memset(&ts, 0, sizeof(ts)); 588 | ts.tv_sec = epoch; 589 | ts.tv_nsec = (_buf.TOW % 1000) * 1000 * 1000; 590 | setClock(ts); 591 | 592 | _gps_position->time_utc_usec = static_cast(epoch) * 1000000ULL; 593 | _gps_position->time_utc_usec += (_buf.TOW % 1000) * 1000; 594 | } 595 | 596 | #endif 597 | _gps_position->timestamp = gps_absolute_time(); 598 | _last_timestamp_time = _gps_position->timestamp; 599 | _rate_count_vel++; 600 | _rate_count_lat_lon++; 601 | ret |= (_msg_status == 7) ? 1 : 0; 602 | //SBF_DEBUG("PVTGeodetic handled"); 603 | break; 604 | 605 | case SBF_ID_VelCovGeodetic: SBF_TRACE_RXMSG("Rx VelCovGeodetic"); 606 | _msg_status |= 2; 607 | _gps_position->s_variance_m_s = _buf.payload_vel_col_geodetic.cov_ve_ve; 608 | 609 | if (_gps_position->s_variance_m_s < _buf.payload_vel_col_geodetic.cov_vn_vn) { 610 | _gps_position->s_variance_m_s = _buf.payload_vel_col_geodetic.cov_vn_vn; 611 | } 612 | 613 | if (_gps_position->s_variance_m_s < _buf.payload_vel_col_geodetic.cov_vu_vu) { 614 | _gps_position->s_variance_m_s = _buf.payload_vel_col_geodetic.cov_vu_vu; 615 | } 616 | 617 | //SBF_DEBUG("VelCovGeodetic handled"); 618 | break; 619 | 620 | case SBF_ID_DOP: SBF_TRACE_RXMSG("Rx DOP"); 621 | _msg_status |= 4; 622 | _gps_position->hdop = _buf.payload_dop.hDOP * 0.01f; 623 | _gps_position->vdop = _buf.payload_dop.vDOP * 0.01f; 624 | //SBF_DEBUG("DOP handled"); 625 | break; 626 | 627 | case SBF_ID_AttEuler: SBF_TRACE_RXMSG("Rx AttEuler"); 628 | 629 | if (!_buf.payload_att_euler.error_not_requested) { 630 | 631 | int error_aux1 = _buf.payload_att_euler.error_aux1; 632 | int error_aux2 = _buf.payload_att_euler.error_aux2; 633 | 634 | // SBF_DEBUG("Mode: %u", _buf.payload_att_euler.mode) 635 | if (error_aux1 == 0 && error_aux2 == 0) { 636 | float heading = _buf.payload_att_euler.heading; 637 | heading *= M_PI_F / 180.0f; // deg to rad, now in range [0, 2pi] 638 | 639 | 640 | if (heading > M_PI_F) { 641 | heading -= 2.f * M_PI_F; // final range is [-pi, pi] 642 | } 643 | 644 | _gps_position->heading = heading; 645 | // SBF_DEBUG("Heading: %.3f rad", (double) _gps_position->heading) 646 | //SBF_DEBUG("AttEuler handled"); 647 | 648 | } else if (error_aux1 != 0) { 649 | //SBF_DEBUG("Error code for Main-Aux1 baseline: Not enough measurements"); 650 | } else if (error_aux2 != 0) { 651 | //SBF_DEBUG("Error code for Main-Aux2 baseline: Not enough measurements"); 652 | } 653 | } else { 654 | //SBF_DEBUG("AttEuler: attitude not requested by user"); 655 | } 656 | 657 | 658 | break; 659 | 660 | case SBF_ID_AttCovEuler: SBF_TRACE_RXMSG("Rx AttCovEuler"); 661 | 662 | if (!_buf.payload_att_cov_euler.error_not_requested) { 663 | int error_aux1 = _buf.payload_att_cov_euler.error_aux1; 664 | int error_aux2 = _buf.payload_att_cov_euler.error_aux2; 665 | 666 | if (error_aux1 == 0 && error_aux2 == 0) { 667 | float heading_acc = _buf.payload_att_cov_euler.cov_headhead; 668 | heading_acc *= M_PI_F / 180.0f; // deg to rad, now in range [0, 2pi] 669 | _gps_position->heading_accuracy = heading_acc; 670 | // SBF_DEBUG("Heading-Accuracy: %.3f rad", (double) _gps_position->heading_accuracy) 671 | //SBF_DEBUG("AttCovEuler handled"); 672 | 673 | } else if (error_aux1 != 0) { 674 | //SBF_DEBUG("Error code for Main-Aux1 baseline: %u: Not enough measurements", error_aux1); 675 | } else if (error_aux2 != 0) { 676 | //SBF_DEBUG("Error code for Main-Aux2 baseline: %u: Not enough measurements", error_aux2); 677 | } 678 | } else { 679 | //SBF_DEBUG("AttCovEuler: attitude not requested by user"); 680 | } 681 | 682 | break; 683 | 684 | default: 685 | break; 686 | } 687 | 688 | if (ret > 0) { 689 | _gps_position->timestamp_time_relative = static_cast(_last_timestamp_time - _gps_position->timestamp); 690 | } 691 | 692 | if (ret == 1) { 693 | _msg_status &= ~1; 694 | } 695 | 696 | return ret; 697 | } 698 | 699 | void GPSDriverSBF::decodeInit() 700 | { 701 | _decode_state = SBF_DECODE_SYNC1; 702 | _rx_payload_index = 0; 703 | } 704 | 705 | int GPSDriverSBF::reset(GPSRestartType restart_type) 706 | { 707 | bool res = false; 708 | 709 | switch (restart_type) { 710 | case GPSRestartType::Hot: 711 | res = sendMessageAndWaitForAck(SBF_CONFIG_RESET_HOT, SBF_CONFIG_TIMEOUT); 712 | break; 713 | 714 | case GPSRestartType::Warm: 715 | res = sendMessageAndWaitForAck(SBF_CONFIG_RESET_WARM, SBF_CONFIG_TIMEOUT); 716 | break; 717 | 718 | case GPSRestartType::Cold: 719 | res = sendMessageAndWaitForAck(SBF_CONFIG_RESET_COLD, SBF_CONFIG_TIMEOUT); 720 | break; 721 | 722 | default: 723 | break; 724 | } 725 | 726 | return (res) ? 0 : -2; 727 | } 728 | -------------------------------------------------------------------------------- /src/sbf.h: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * 3 | * Copyright (c) 2018 PX4 Development Team. All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions 7 | * are met: 8 | * 9 | * 1. Redistributions of source code must retain the above copyright 10 | * notice, this list of conditions and the following disclaimer. 11 | * 2. Redistributions in binary form must reproduce the above copyright 12 | * notice, this list of conditions and the following disclaimer in 13 | * the documentation and/or other materials provided with the 14 | * distribution. 15 | * 3. Neither the name PX4 nor the names of its contributors may be 16 | * used to endorse or promote products derived from this software 17 | * without specific prior written permission. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 20 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 21 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 22 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 23 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 24 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 25 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS 26 | * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 27 | * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 28 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 29 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 30 | * POSSIBILITY OF SUCH DAMAGE. 31 | * 32 | ****************************************************************************/ 33 | 34 | /** 35 | * @file sbf.h 36 | * 37 | * Septentrio protocol as defined in PPSDK SBF Reference Guide 4.1.8 38 | * 39 | * @author Matej Franceskin 40 | * @author Seppe Geuens 41 | * 42 | */ 43 | 44 | #pragma once 45 | 46 | #include "base_station.h" 47 | #include "rtcm.h" 48 | 49 | 50 | #define SBF_CONFIG_FORCE_INPUT "SSSSSSSSSS\n" 51 | 52 | #define SBF_CONFIG_BAUDRATE "setCOMSettings, %s, baud%d\n" 53 | 54 | #define SBF_CONFIG_RESET "setSBFOutput, Stream1, %s, none, off\n" 55 | 56 | #define SBF_CONFIG_RECEIVER_DYNAMICS "setReceiverDynamics, %s, UAV\n" 57 | 58 | #define SBF_CONFIG_ATTITUDE_OFFSET "setAttitudeOffset, %.3f, %.3f\n" 59 | 60 | #define SBF_TX_CFG_PRT_BAUDRATE 115200 61 | 62 | #define SBF_DATA_IO "setDataInOut, %s, Auto, SBF\n" 63 | 64 | #define SBF_CONFIG "setSBFOutput, Stream1, %s, PVTGeodetic+VelCovGeodetic+DOP+AttEuler+AttCovEuler, msec100\n" 65 | 66 | #define SBF_CONFIG_DISABLE_OUTPUT "setDataInOut,%s%d,,-RTCMv3\n" 67 | 68 | #define SBF_CONFIG_RTCM "" \ 69 | "setDataInOut, USB1, Auto, RTCMv3\n" \ 70 | "setPVTMode, Static, All, auto\n" 71 | 72 | #define SBF_CONFIG_RTCM_STATIC1 "" \ 73 | "setReceiverDynamics, Low, Static\n" 74 | 75 | #define SBF_CONFIG_RTCM_STATIC2 "" \ 76 | "setPVTMode, Static, , Geodetic1\n" 77 | 78 | #define SBF_CONFIG_RTCM_STATIC_COORDINATES "" \ 79 | "setStaticPosGeodetic, Geodetic1, %f, %f, %f\n" 80 | 81 | #define SBF_CONFIG_RTCM_STATIC_OFFSET "" \ 82 | "setAntennaOffset, Main, %f, %f, %f\n" 83 | 84 | #define SBF_CONFIG_RESET_HOT "" \ 85 | SBF_CONFIG_FORCE_INPUT"ExeResetReceiver, soft, none\n" 86 | 87 | #define SBF_CONFIG_RESET_WARM "" \ 88 | SBF_CONFIG_FORCE_INPUT"ExeResetReceiver, soft, PVTData\n" 89 | 90 | #define SBF_CONFIG_RESET_COLD "" \ 91 | SBF_CONFIG_FORCE_INPUT"ExeResetReceiver, hard, SatData\n" 92 | 93 | #define SBF_SYNC1 0x24 94 | #define SBF_SYNC2 0x40 95 | 96 | /* Block IDs */ 97 | #define SBF_ID_DOP 4001 98 | #define SBF_ID_PVTGeodetic 4007 99 | #define SBF_ID_ChannelStatus 4013 100 | #define SBF_ID_VelCovGeodetic 5908 101 | #define SBF_ID_AttEuler 5938 102 | #define SBF_ID_AttCovEuler 5939 103 | 104 | /*** SBF protocol binary message and payload definitions ***/ 105 | #pragma pack(push, 1) 106 | 107 | typedef struct { 108 | uint8_t mode_type: 4; /**< Bit field indicating the PVT mode type, as follows: 109 | 0: No PVT available (the Error field indicates the cause of the absence of the PVT solution) 110 | 1: Stand-Alone PVT 111 | 2: Differential PVT 112 | 3: Fixed location 113 | 4: RTK with fixed ambiguities 114 | 5: RTK with float ambiguities 115 | 6: SBAS aided PVT 116 | 7: moving-base RTK with fixed ambiguities 117 | 8: moving-base RTK with float ambiguities 118 | 10:Precise Point Positioning (PPP) */ 119 | uint8_t mode_reserved: 2; /**< Reserved */ 120 | uint8_t mode_base_fixed: 1; /**< Set if the user has entered the command setPVTMode,base,auto and the receiver 121 | is still in the process of determining its fixed position. */ 122 | uint8_t mode_2d: 1; /**< 2D/3D flag: set in 2D mode(height assumed constant and not computed). */ 123 | uint8_t error; /**< PVT error code. The following values are defined: 124 | 0: No Error 125 | 1: Not enough measurements 126 | 2: Not enough ephemerides available 127 | 3: DOP too large (larger than 15) 128 | 4: Sum of squared residuals too large 129 | 5: No convergence 130 | 6: Not enough measurements after outlier rejection 131 | 7: Position output prohibited due to export laws 132 | 8: Not enough differential corrections available 133 | 9: Base station coordinates unavailable 134 | 10:Ambiguities not fixed and user requested to only output RTK-fixed positions 135 | Note: if this field has a non-zero value, all following fields are set to their Do-Not-Use value. */ 136 | double latitude; /**< Marker latitude, from -PI/2 to +PI/2, positive North of Equator */ 137 | double longitude; /**< Marker longitude, from -PI to +PI, positive East of Greenwich */ 138 | double height; /**< Marker ellipsoidal height (with respect to the ellipsoid specified by Datum) */ 139 | float undulation; /**< Geoid undulation computed from the global geoid model defined in 140 | the document 'Technical Characteristics of the NAVSTAR GPS, NATO, June 1991' */ 141 | float vn; /**< Velocity in the North direction */ 142 | float ve; /**< Velocity in the East direction */ 143 | float vu; /**< Velocity in the Up direction */ 144 | float cog; /**< Course over ground: this is defined as the angle of the vehicle with respect 145 | to the local level North, ranging from 0 to 360, and increasing towards east. 146 | Set to the do-not-use value when the speed is lower than 0.1m/s. */ 147 | double rx_clk_bias; /**< Receiver clock bias relative to system time reported in the Time System field. 148 | To transfer the receiver time to the system time, use: tGPS/GST=trx-RxClkBias */ 149 | float RxClkDrift; /**< Receiver clock drift relative to system time (relative frequency error) */ 150 | uint8_t time_system; /**< Time system of which the offset is provided in this sub-block: 151 | 0:GPStime 152 | 1:Galileotime 153 | 3:GLONASStime */ 154 | uint8_t datum; /**< This field defines in which datum the coordinates are expressed: 155 | 0: WGS84/ITRS 156 | 19: Datum equal to that used by the DGNSS/RTK basestation 157 | 30: ETRS89(ETRF2000 realization) 158 | 31: NAD83(2011), North American Datum(2011) 159 | 32: NAD83(PA11), North American Datum, Pacificplate (2011) 160 | 33: NAD83(MA11), North American Datum, Marianas plate(2011) 161 | 34: GDA94(2010), Geocentric Datum of Australia (2010) 162 | 250:First user-defined datum 163 | 251:Second user-defined datum */ 164 | uint8_t nr_sv; /**< Total number of satellites used in the PVT computation. */ 165 | uint8_t wa_corr_info; /**< Bit field providing information about which wide area corrections have been applied: 166 | Bit 0: set if orbit and satellite clock correction information is used 167 | Bit 1: set if range correction information is used 168 | Bit 2: set if ionospheric information is used 169 | Bit 3: set if orbit accuracy information is used(UERE/SISA) 170 | Bit 4: set if DO229 Precision Approach mode is active 171 | Bits 5-7: Reserved */ 172 | uint16_t reference_id; /**< In case of DGPS or RTK operation, this field is to be interpreted as the base station identifier. 173 | In SBAS operation, this field is to be interpreted as the PRN of the geostationary satellite 174 | used (from 120 to 158). If multiple base stations or multiple geostationary satellites are used 175 | the value is set to 65534.*/ 176 | uint16_t mean_corr_age; /**< In case of DGPS or RTK, this field is the mean age of the differential corrections. 177 | In case of SBAS operation, this field is the mean age of the 'fast corrections' 178 | provided by the SBAS satellites */ 179 | uint32_t signal_info; /**< Bit field indicating the type of GNSS signals having been used in the PVT computations. 180 | If a bit i is set, the signal type having index i has been used. */ 181 | uint8_t alert_flag; /**< Bit field indicating integrity related information */ 182 | 183 | // Revision 1 184 | uint8_t nr_bases; 185 | uint16_t ppp_info; 186 | // Revision 2 187 | uint16_t latency; 188 | uint16_t h_accuracy; 189 | uint16_t v_accuracy; 190 | } sbf_payload_pvt_geodetic_t; 191 | 192 | typedef struct { 193 | uint8_t mode_type: 4; /**< Bit field indicating the PVT mode type, as follows: 194 | 0: No PVT available (the Error field indicates the cause of the absence of the PVT solution) 195 | 1: Stand-Alone PVT 196 | 2: Differential PVT 197 | 3: Fixed location 198 | 4: RTK with fixed ambiguities 199 | 5: RTK with float ambiguities 200 | 6: SBAS aided PVT 201 | 7: moving-base RTK with fixed ambiguities 202 | 8: moving-base RTK with float ambiguities 203 | 10:Precise Point Positioning (PPP) */ 204 | uint8_t mode_reserved: 2; /**< Reserved */ 205 | uint8_t mode_base_fixed: 1;/**< Set if the user has entered the command setPVTMode,base,auto and the receiver 206 | is still in the process of determining its fixed position. */ 207 | uint8_t mode_2d: 1; /**< 2D/3D flag: set in 2D mode(height assumed constant and not computed). */ 208 | uint8_t error; /**< PVT error code. The following values are defined: 209 | 0: No Error 210 | 1: Not enough measurements 211 | 2: Not enough ephemerides available 212 | 3: DOP too large (larger than 15) 213 | 4: Sum of squared residuals too large 214 | 5: No convergence 215 | 6: Not enough measurements after outlier rejection 216 | 7: Position output prohibited due to export laws 217 | 8: Not enough differential corrections available 218 | 9: Base station coordinates unavailable 219 | 10:Ambiguities not fixed and user requested to only output RTK-fixed positions 220 | Note: if this field has a non-zero value, all following fields are set to their Do-Not-Use value. */ 221 | float cov_vn_vn; /**< Variance of the north-velocity estimate */ 222 | float cov_ve_ve; /**< Variance of the east-velocity estimate */ 223 | float cov_vu_vu; /**< Variance of the up - velocity estimate */ 224 | float cov_dt_dt; /**< Variance of the clock drift estimate */ 225 | float cov_vn_ve; /**< Covariance between the north - and east - velocity estimates */ 226 | float cov_vn_vu; /**< Covariance between the north - and up - velocity estimates */ 227 | float cov_vn_dt; /**< Covariance between the north - velocity and clock drift estimates */ 228 | float cov_ve_vu; /**< Covariance between the east - and up - velocity estimates */ 229 | float cov_ve_dt; /**< Covariance between the east - velocity and clock drift estimates */ 230 | float cov_vu_dt; /**< Covariance between the up - velocity and clock drift estimates */ 231 | } sbf_payload_vel_cov_geodetic_t; 232 | 233 | typedef struct { 234 | uint8_t nr_sv; /**< Total number of satellites used in the PVT computation. */ 235 | uint8_t reserved; 236 | uint16_t pDOP; 237 | uint16_t tDOP; 238 | uint16_t hDOP; 239 | uint16_t vDOP; 240 | float hpl; /**< Horizontal Protection Level (see the DO229 standard). */ 241 | float vpl; /**< Vertical Protection Level (see the DO229 standard). */ 242 | } sbf_payload_dop_t; 243 | 244 | typedef struct { 245 | uint8_t antenna; 246 | uint8_t reserved; 247 | uint16_t tracking_status; 248 | uint16_t pvt_status; 249 | uint16_t pvt_info; 250 | } sbf_payload_channel_state_info_t; 251 | 252 | typedef struct { 253 | uint8_t nr_sv; /**< The average over all antennas of the number of satellites currently included in the attitude calculations. */ 254 | uint8_t error_aux1: 2; /**< Bits 0-1: Error code for Main-Aux1 baseline: 255 | 0: No error 256 | 1: Not enough measurements 257 | 2: Reserved 258 | 3: Reserved */ 259 | uint8_t error_aux2: 2; /**< Bits 2-3: Error code for Main-Aux2 baseline, same definition as bit 0-1. */ 260 | uint8_t error_reserved: 3; /**< Bits 4-6: Reserved */ 261 | uint8_t error_not_requested: 262 | 1; /**< Bit 7: Set when GNSS-based attitude not requested by user. In that case, the other bits are all zero. */ 263 | 264 | uint16_t mode; /**< Attitude mode code: 265 | 0: No attitude 266 | 1: Heading, pitch (roll = 0), aux antenna positions obtained with float 267 | ambiguities 268 | 2: Heading, pitch (roll = 0), aux antenna positions obtained with fixed 269 | ambiguities 270 | 3: Heading, pitch, roll, aux antenna positions obtained with float ambiguities 271 | 4: Heading, pitch, roll, aux antenna positions obtained with fixed ambiguities */ 272 | uint16_t reserved; /**< Reserved for future use, to be ignored by decoding software */ 273 | 274 | float heading; /**< Heading */ 275 | float pitch; /**< Pitch */ 276 | float roll; /**< Roll */ 277 | float pitch_dot; /**< Rate of change of the pitch angle */ 278 | float roll_dot; /**< Rate of change of the roll angle */ 279 | float heading_dot; /**< Rate of change of the heading angle */ 280 | } sbf_payload_att_euler; 281 | 282 | typedef struct { 283 | uint8_t reserved; /**< Reserved for future use, to be ignored by decoding software */ 284 | 285 | uint8_t error_aux1: 2; /**< Bits 0-1: Error code for Main-Aux1 baseline: 286 | 0: No error 287 | 1: Not enough measurements 288 | 2: Reserved 289 | 3: Reserved */ 290 | uint8_t error_aux2: 2; /**< Bits 2-3: Error code for Main-Aux2 baseline, same definition as bit 0-1. */ 291 | uint8_t error_reserved: 3; /**< Bits 4-6: Reserved */ 292 | uint8_t error_not_requested: 293 | 1; /**< Bit 7: Set when GNSS-based attitude not requested by user. In that case, the other bits are all zero. */ 294 | 295 | float cov_headhead; /**< Variance of the heading estimate */ 296 | float cov_pitchpitch; /**< Variance of the pitch estimate */ 297 | float cov_rollroll; /**< Variance of the roll estimate */ 298 | float cov_headpitch; /**< Covariance between Euler angle estimates. 299 | Future functionality. The values are currently set to their Do-Not-Use values. */ 300 | float cov_headroll; /**< Covariance between Euler angle estimates. 301 | Future functionality. The values are currently set to their Do-Not-Use values. */ 302 | float cov_pitchroll; /**< Covariance between Euler angle estimates. 303 | Future functionality. The values are currently set to their Do-Not-Use values. */ 304 | } sbf_payload_att_cov_euler; 305 | 306 | /* General message and payload buffer union */ 307 | 308 | typedef struct { 309 | uint16_t sync; /** The Sync field is a 2-byte array always set to 0x24, 0x40. The first byte of every SBF block has 310 | hexadecimal value 24 (decimal 36, ASCII '$'). The second byte of every SBF block has hexadecimal 311 | value 40 (decimal 64, ASCII '@'). */ 312 | uint16_t crc16; /** The CRC field is the 16-bit CRC of all the bytes in an SBF block from and including the ID field 313 | to the last byte of the block. The generator polynomial for this CRC is the so-called CRC-CCITT 314 | polynomial: x 16 + x 12 + x 5 + x 0 . The CRC is computed in the forward direction using a seed of 0, no 315 | reverse and no final XOR. */ 316 | uint16_t msg_id: 317 | 13; /** The ID field is a 2-byte block ID, which uniquely identifies the block type and its contents */ 318 | uint8_t msg_revision: 319 | 3; /** block revision number, starting from 0 at the initial block definition, and incrementing 320 | each time backwards - compatible changes are performed to the block */ 321 | uint16_t length; /** The Length field is a 2-byte unsigned integer containing the size of the SBF block. 322 | It is the total number of bytes in the SBF block including the header. 323 | It is always a multiple of 4. */ 324 | uint32_t TOW; /**< Time-Of-Week: Time-tag, expressed in whole milliseconds from 325 | the beginning of the current Galileo/GPSweek. */ 326 | uint16_t WNc; /**< The GPS week number associated with the TOW. WNc is a continuous 327 | weekcount (hence the "c"). It is not affected by GPS week roll overs, 328 | which occur every 1024 weeks. By definition of the Galileo system time, 329 | WNc is also the Galileo week number + 1024. */ 330 | union { 331 | sbf_payload_pvt_geodetic_t payload_pvt_geodetic; 332 | sbf_payload_vel_cov_geodetic_t payload_vel_col_geodetic; 333 | sbf_payload_dop_t payload_dop; 334 | sbf_payload_att_euler payload_att_euler; 335 | sbf_payload_att_cov_euler payload_att_cov_euler; 336 | }; 337 | 338 | uint8_t padding[16]; 339 | } sbf_buf_t; 340 | 341 | #pragma pack(pop) 342 | /*** END OF SBF protocol binary message and payload definitions ***/ 343 | 344 | /* Decoder state */ 345 | typedef enum { 346 | SBF_DECODE_SYNC1 = 0, 347 | SBF_DECODE_SYNC2, 348 | SBF_DECODE_PAYLOAD 349 | } sbf_decode_state_t; 350 | 351 | class GPSDriverSBF : public GPSBaseStationSupport 352 | { 353 | public: 354 | /** 355 | * @param heading_offset heading offset in radians [-pi, pi]. It is subtracted from the measurement. 356 | * @param pitch_offset pitch_offset in deg [-90, 90]. This will be send as a cmd to the receiver. 357 | */ 358 | GPSDriverSBF(GPSCallbackPtr callback, 359 | void *callback_user, 360 | struct sensor_gps_s *gps_position, 361 | satellite_info_s *satellite_info = nullptr, 362 | float heading_offset = 0.f, 363 | float pitch_offset = 0.f); 364 | 365 | virtual ~GPSDriverSBF(); 366 | 367 | int receive(unsigned timeout) override; 368 | 369 | int configure(unsigned &baudrate, const GPSConfig &config) override; 370 | 371 | int reset(GPSRestartType restart_type) override; 372 | 373 | private: 374 | 375 | /** 376 | * @brief Parse the binary SBF packet 377 | */ 378 | int parseChar(const uint8_t b); 379 | 380 | /** 381 | * @brief Add payload rx byte 382 | */ 383 | int payloadRxAdd(const uint8_t b); 384 | 385 | /** 386 | * @brief Parses incoming SBF blocks 387 | */ 388 | int payloadRxDone(void); 389 | 390 | /** 391 | * @brief Reset the parse state machine for a fresh start 392 | */ 393 | void decodeInit(void); 394 | 395 | /** 396 | * @brief Send a message 397 | * @return true on success, false on write error (errno set) 398 | */ 399 | bool sendMessage(const char *msg); 400 | 401 | /** 402 | * @brief Send a message and waits for acknowledge 403 | * @return true on success, false on write error (errno set) or ack wait timeout 404 | */ 405 | bool sendMessageAndWaitForAck(const char *msg, const int timeout); 406 | 407 | /** 408 | * @brief Configures the SBF Output blocks 409 | * @return true on success, false on write error (errno set) or ack wait timeout 410 | */ 411 | bool configSBFOutput(const char *com_port); 412 | 413 | sensor_gps_s *_gps_position{nullptr}; 414 | satellite_info_s *_satellite_info{nullptr}; 415 | uint8_t _dynamic_model{7}; 416 | uint64_t _last_timestamp_time{0}; 417 | bool _configured{false}; 418 | uint8_t _msg_status{0}; 419 | sbf_decode_state_t _decode_state{SBF_DECODE_SYNC1}; 420 | uint16_t _rx_payload_index{0}; 421 | sbf_buf_t _buf; 422 | OutputMode _output_mode{OutputMode::GPS}; 423 | RTCMParsing *_rtcm_parsing{nullptr}; 424 | 425 | const float _heading_offset; 426 | const float _pitch_offset; 427 | }; 428 | 429 | uint16_t crc16(const uint8_t *buf, uint32_t len); 430 | 431 | -------------------------------------------------------------------------------- /src/unicore.cpp: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * 3 | * Copyright (c) 2023-2024 PX4 Development Team. All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions 7 | * are met: 8 | * 9 | * 1. Redistributions of source code must retain the above copyright 10 | * notice, this list of conditions and the following disclaimer. 11 | * 2. Redistributions in binary form must reproduce the above copyright 12 | * notice, this list of conditions and the following disclaimer in 13 | * the documentation and/or other materials provided with the 14 | * distribution. 15 | * 3. Neither the name PX4 nor the names of its contributors may be 16 | * used to endorse or promote products derived from this software 17 | * without specific prior written permission. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 20 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 21 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 22 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 23 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 24 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 25 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS 26 | * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 27 | * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 28 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 29 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 30 | * POSSIBILITY OF SUCH DAMAGE. 31 | * 32 | ****************************************************************************/ 33 | 34 | #include "unicore.h" 35 | #include "crc.h" 36 | #include 37 | #include 38 | #include 39 | 40 | UnicoreParser::Result UnicoreParser::parseChar(char c) 41 | { 42 | switch (_state) { 43 | case State::Uninit: 44 | if (c == '#') { 45 | _state = State::GotHashtag; 46 | } 47 | 48 | break; 49 | 50 | case State::GotHashtag: 51 | if (c == '*') { 52 | _state = State::GotStar; 53 | 54 | // Make sure buffer is zero terminated. 55 | _buffer[_buffer_pos] = '\0'; 56 | 57 | } else { 58 | if (_buffer_pos >= sizeof(_buffer) - 1) { 59 | reset(); 60 | return Result::None; 61 | } 62 | 63 | _buffer[_buffer_pos++] = c; 64 | } 65 | 66 | break; 67 | 68 | case State::GotStar: 69 | _buffer_crc[_buffer_crc_pos++] = c; 70 | 71 | if (_buffer_crc_pos >= 8) { 72 | 73 | // Make sure the CRC buffer is zero terminated. 74 | _buffer_crc[_buffer_crc_pos] = '\0'; 75 | 76 | if (!crcCorrect()) { 77 | reset(); 78 | return Result::WrongCrc; 79 | } 80 | 81 | if (isHeading()) { 82 | if (extractHeading()) { 83 | reset(); 84 | return Result::GotHeading; 85 | 86 | } else { 87 | reset(); 88 | return Result::WrongStructure; 89 | } 90 | 91 | } else if (isAgrica()) { 92 | if (extractAgrica()) { 93 | reset(); 94 | return Result::GotAgrica; 95 | 96 | } else { 97 | reset(); 98 | return Result::WrongStructure; 99 | } 100 | 101 | } else { 102 | reset(); 103 | return Result::UnknownSentence; 104 | } 105 | } 106 | 107 | break; 108 | } 109 | 110 | return Result::None; 111 | } 112 | 113 | void UnicoreParser::reset() 114 | { 115 | _state = State::Uninit; 116 | _buffer_pos = 0; 117 | _buffer_crc_pos = 0; 118 | } 119 | 120 | bool UnicoreParser::crcCorrect() const 121 | { 122 | const uint32_t crc_calculated = calculateCRC32(_buffer_pos, (uint8_t *)_buffer, 0); 123 | const uint32_t crc_parsed = (uint32_t)strtoul(_buffer_crc, nullptr, 16); 124 | return (crc_calculated == crc_parsed); 125 | } 126 | 127 | bool UnicoreParser::isHeading() const 128 | { 129 | const char header[] = "UNIHEADINGA"; 130 | 131 | return strncmp(header, _buffer, strlen(header)) == 0; 132 | } 133 | 134 | bool UnicoreParser::isAgrica() const 135 | { 136 | const char header[] = "AGRICA"; 137 | 138 | return strncmp(header, _buffer, strlen(header)) == 0; 139 | } 140 | 141 | bool UnicoreParser::extractHeading() 142 | { 143 | // The basline starts after ;,, and then follows the heading. 144 | 145 | // Skip to ; 146 | char *ptr = strchr(_buffer, ';'); 147 | 148 | if (ptr == nullptr) { 149 | return false; 150 | } 151 | 152 | ptr = strtok(ptr, ","); 153 | 154 | unsigned i = 0; 155 | 156 | while (ptr != nullptr) { 157 | ptr = strtok(nullptr, ","); 158 | 159 | if (ptr == nullptr) { 160 | return false; 161 | } 162 | 163 | if (i == 1) { 164 | _heading.baseline_m = strtof(ptr, nullptr); 165 | 166 | } else if (i == 2) { 167 | _heading.heading_deg = strtof(ptr, nullptr); 168 | 169 | } else if (i == 5) { 170 | _heading.heading_stddev_deg = strtof(ptr, nullptr); 171 | return true; 172 | } 173 | 174 | ++i; 175 | } 176 | 177 | return false; 178 | } 179 | 180 | bool UnicoreParser::extractAgrica() 181 | { 182 | // Skip to ; 183 | char *ptr = strchr(_buffer, ';'); 184 | 185 | if (ptr == nullptr) { 186 | return false; 187 | } 188 | 189 | ptr = strtok(ptr, ","); 190 | 191 | unsigned i = 0; 192 | 193 | while (ptr != nullptr) { 194 | ptr = strtok(nullptr, ","); 195 | 196 | if (ptr == nullptr) { 197 | return false; 198 | } 199 | 200 | if (i == 21) { 201 | _agrica.velocity_m_s = strtof(ptr, nullptr); 202 | } 203 | 204 | else if (i == 22) { 205 | _agrica.velocity_north_m_s = strtof(ptr, nullptr); 206 | } 207 | 208 | else if (i == 23) { 209 | _agrica.velocity_east_m_s = strtof(ptr, nullptr); 210 | } 211 | 212 | else if (i == 24) { 213 | _agrica.velocity_up_m_s = strtof(ptr, nullptr); 214 | } 215 | 216 | else if (i == 25) { 217 | _agrica.stddev_velocity_north_m_s = strtof(ptr, nullptr); 218 | } 219 | 220 | else if (i == 26) { 221 | _agrica.stddev_velocity_east_m_s = strtof(ptr, nullptr); 222 | } 223 | 224 | else if (i == 27) { 225 | _agrica.stddev_velocity_up_m_s = strtof(ptr, nullptr); 226 | _agrica_valid = true; 227 | return true; 228 | } 229 | 230 | ++i; 231 | } 232 | 233 | return false; 234 | } -------------------------------------------------------------------------------- /src/unicore.h: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * 3 | * Copyright (c) 2023 PX4 Development Team. All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions 7 | * are met: 8 | * 9 | * 1. Redistributions of source code must retain the above copyright 10 | * notice, this list of conditions and the following disclaimer. 11 | * 2. Redistributions in binary form must reproduce the above copyright 12 | * notice, this list of conditions and the following disclaimer in 13 | * the documentation and/or other materials provided with the 14 | * distribution. 15 | * 3. Neither the name PX4 nor the names of its contributors may be 16 | * used to endorse or promote products derived from this software 17 | * without specific prior written permission. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 20 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 21 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 22 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 23 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 24 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 25 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS 26 | * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 27 | * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 28 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 29 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 30 | * POSSIBILITY OF SUCH DAMAGE. 31 | * 32 | ****************************************************************************/ 33 | 34 | #pragma once 35 | 36 | #include 37 | 38 | 39 | class UnicoreParser 40 | { 41 | public: 42 | enum class Result { 43 | None, 44 | WrongCrc, 45 | WrongStructure, 46 | GotHeading, 47 | GotAgrica, 48 | UnknownSentence, 49 | }; 50 | 51 | Result parseChar(char c); 52 | 53 | struct Heading { 54 | float heading_deg; 55 | float heading_stddev_deg; 56 | float baseline_m; 57 | }; 58 | 59 | struct Agrica { 60 | float velocity_m_s; 61 | float velocity_north_m_s; 62 | float velocity_east_m_s; 63 | float velocity_up_m_s; 64 | float stddev_velocity_north_m_s; 65 | float stddev_velocity_east_m_s; 66 | float stddev_velocity_up_m_s; 67 | }; 68 | 69 | Heading heading() const 70 | { 71 | return _heading; 72 | } 73 | 74 | Agrica agrica() const 75 | { 76 | return _agrica; 77 | } 78 | 79 | bool agricaValid() const { return _agrica_valid; } 80 | 81 | private: 82 | void reset(); 83 | bool crcCorrect() const; 84 | bool isHeading() const; 85 | bool isAgrica() const; 86 | bool extractHeading(); 87 | bool extractAgrica(); 88 | 89 | // We have seen buffers with 540 bytes for AGRICA. 90 | char _buffer[600]; 91 | unsigned _buffer_pos {0}; 92 | char _buffer_crc[9]; 93 | unsigned _buffer_crc_pos {0}; 94 | 95 | enum class State { 96 | Uninit, 97 | GotHashtag, 98 | GotStar, 99 | } _state {State::Uninit}; 100 | 101 | Heading _heading{}; 102 | Agrica _agrica{}; 103 | 104 | bool _agrica_valid{false}; 105 | }; --------------------------------------------------------------------------------