diff --git a/libraries/AP_GPS/AP_GPS.cpp b/libraries/AP_GPS/AP_GPS.cpp index c5c4dc1805..4f98ac33f5 100644 --- a/libraries/AP_GPS/AP_GPS.cpp +++ b/libraries/AP_GPS/AP_GPS.cpp @@ -28,7 +28,6 @@ #include "AP_GPS_MTK.h" #include "AP_GPS_MTK19.h" #include "AP_GPS_NMEA.h" -#include "AP_GPS_QURT.h" #include "AP_GPS_SBF.h" #include "AP_GPS_SBP.h" #include "AP_GPS_SBP2.h" @@ -68,7 +67,7 @@ const AP_Param::GroupInfo AP_GPS::var_info[] = { // @Param: TYPE // @DisplayName: GPS type // @Description: GPS type - // @Values: 0:None,1:AUTO,2:uBlox,3:MTK,4:MTK19,5:NMEA,6:SiRF,7:HIL,8:SwiftNav,9:UAVCAN,10:SBF,11:GSOF,12:QURT,13:ERB,14:MAV,15:NOVA + // @Values: 0:None,1:AUTO,2:uBlox,3:MTK,4:MTK19,5:NMEA,6:SiRF,7:HIL,8:SwiftNav,9:UAVCAN,10:SBF,11:GSOF,13:ERB,14:MAV,15:NOVA // @RebootRequired: True // @User: Advanced AP_GROUPINFO("TYPE", 0, AP_GPS, _type[0], HAL_GPS_TYPE_DEFAULT), @@ -76,7 +75,7 @@ const AP_Param::GroupInfo AP_GPS::var_info[] = { // @Param: TYPE2 // @DisplayName: 2nd GPS type // @Description: GPS type of 2nd GPS - // @Values: 0:None,1:AUTO,2:uBlox,3:MTK,4:MTK19,5:NMEA,6:SiRF,7:HIL,8:SwiftNav,9:UAVCAN,10:SBF,11:GSOF,12:QURT,13:ERB,14:MAV,15:NOVA + // @Values: 0:None,1:AUTO,2:uBlox,3:MTK,4:MTK19,5:NMEA,6:SiRF,7:HIL,8:SwiftNav,9:UAVCAN,10:SBF,11:GSOF,13:ERB,14:MAV,15:NOVA // @RebootRequired: True // @User: Advanced AP_GROUPINFO("TYPE2", 1, AP_GPS, _type[1], 0), @@ -417,14 +416,6 @@ void AP_GPS::detect_instance(uint8_t instance) state[instance].vdop = GPS_UNKNOWN_DOP; switch (_type[instance]) { -#if CONFIG_HAL_BOARD == HAL_BOARD_QURT - case GPS_TYPE_QURT: - dstate->auto_detected_baud = false; // specified, not detected - new_gps = new AP_GPS_QURT(*this, state[instance], _port[instance]); - goto found_gps; - break; -#endif - // user has to explicitly set the MAV type, do not use AUTO // do not try to detect the MAV type, assume it's there case GPS_TYPE_MAV: diff --git a/libraries/AP_GPS/AP_GPS.h b/libraries/AP_GPS/AP_GPS.h index e90d84e97b..2e3e0ea0f7 100644 --- a/libraries/AP_GPS/AP_GPS.h +++ b/libraries/AP_GPS/AP_GPS.h @@ -55,7 +55,6 @@ class AP_GPS friend class AP_GPS_NMEA; friend class AP_GPS_NOVA; friend class AP_GPS_PX4; - friend class AP_GPS_QURT; friend class AP_GPS_SBF; friend class AP_GPS_SBP; friend class AP_GPS_SBP2; @@ -88,7 +87,6 @@ public: GPS_TYPE_UAVCAN = 9, GPS_TYPE_SBF = 10, GPS_TYPE_GSOF = 11, - GPS_TYPE_QURT = 12, GPS_TYPE_ERB = 13, GPS_TYPE_MAV = 14, GPS_TYPE_NOVA = 15 diff --git a/libraries/AP_GPS/AP_GPS_QURT.cpp b/libraries/AP_GPS/AP_GPS_QURT.cpp deleted file mode 100644 index 4f08018568..0000000000 --- a/libraries/AP_GPS/AP_GPS_QURT.cpp +++ /dev/null @@ -1,111 +0,0 @@ -/* - This program is free software: you can redistribute it and/or modify - it under the terms of the GNU General Public License as published by - the Free Software Foundation, either version 3 of the License, or - (at your option) any later version. - - This program is distributed in the hope that it will be useful, - but WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - GNU General Public License for more details. - - You should have received a copy of the GNU General Public License - along with this program. If not, see . - */ - -#include -#if CONFIG_HAL_BOARD == HAL_BOARD_QURT -#include "AP_GPS_QURT.h" -extern "C" { -#include -#include -} - -extern const AP_HAL::HAL& hal; - -AP_GPS_QURT::AP_GPS_QURT(AP_GPS &_gps, AP_GPS::GPS_State &_state, AP_HAL::UARTDriver *_port) : - AP_GPS_Backend(_gps, _state, _port) -{ - HAP_PRINTF("Trying csr_gps_init"); - int ret = csr_gps_init("/dev/tty-3"); - if (ret == -1) { - HAP_PRINTF("Trying csr_gps_deinit"); - csr_gps_deinit(); - ret = csr_gps_init("/dev/tty-3"); - } - if (ret == 0) { - HAP_PRINTF("Initialised csr_gps"); - initialised = true; - } else { - HAP_PRINTF("Failed to initialise csr_gps ret=%d", ret); - initialised = false; - } -} - -AP_GPS_QURT::~AP_GPS_QURT(void) -{ - if (initialised) { - csr_gps_deinit(); - } -} - - -// update internal state if new GPS information is available -bool -AP_GPS_QURT::read(void) -{ - if (!initialised) { - return false; - } - struct osp_geo_data data {}; - if (csr_gps_get_geo_data(&data) != 0) { - return false; - } - state.last_gps_time_ms = AP_HAL::millis(); - if (data.tow == last_tow) { - // same data again - return false; - } - - if (data.nav_type & NAV_TYPE_4SV_OR_MORE_KF_SOLUTION || - data.nav_type & NAV_TYPE_3SV_KF_SOLUTION) { - state.status = AP_GPS::GPS_OK_FIX_3D; - } else if (data.nav_type & NAV_TYPE_2SV_KF_SOLUTION) { - state.status = AP_GPS::GPS_OK_FIX_2D; - } else { - state.status = AP_GPS::NO_FIX; - } - - state.num_sats = data.sv_in_fix; - state.hdop = data.HDOP; - state.vdop = 0; - - state.location.lat = bswap_32(data.lat); - state.location.lng = bswap_32(data.lon); - state.location.alt = data.alt_from_MSL; - - state.ground_speed = data.speed_over_ground; - state.ground_course = data.course_over_ground*0.01f; - - // convert epoch timestamp back to gps epoch - evil hack until we get the genuine - // raw week information (or APM switches to Posix epoch ;-) ) - state.time_week = data.ext_week_num; - state.time_week_ms = data.tow; - - if (state.time_week == 0) { - // reject bad time - state.status = AP_GPS::NO_FIX; - } - - state.have_vertical_velocity = true; - float gps_heading = radians(state.ground_course); - state.velocity.x = state.ground_speed * cosf(gps_heading); - state.velocity.y = state.ground_speed * sinf(gps_heading); - state.velocity.z = -data.climb_rate; - state.speed_accuracy = data.est_hor_vel_error * 0.01f; - state.horizontal_accuracy = data.est_hor_pos_error * 0.01f; - state.vertical_accuracy = data.est_vert_pos_error * 0.01f; - state.have_speed_accuracy = true; - return true; -} -#endif diff --git a/libraries/AP_GPS/AP_GPS_QURT.h b/libraries/AP_GPS/AP_GPS_QURT.h deleted file mode 100644 index 7ab61c99d6..0000000000 --- a/libraries/AP_GPS/AP_GPS_QURT.h +++ /dev/null @@ -1,39 +0,0 @@ -/* - This program is free software: you can redistribute it and/or modify - it under the terms of the GNU General Public License as published by - the Free Software Foundation, either version 3 of the License, or - (at your option) any later version. - - This program is distributed in the hope that it will be useful, - but WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - GNU General Public License for more details. - - You should have received a copy of the GNU General Public License - along with this program. If not, see . - */ - -#pragma once - -#include - -#include "AP_GPS.h" -#include "GPS_Backend.h" - -#if CONFIG_HAL_BOARD == HAL_BOARD_QURT - -class AP_GPS_QURT : public AP_GPS_Backend { -public: - AP_GPS_QURT(AP_GPS &_gps, AP_GPS::GPS_State &_state, AP_HAL::UARTDriver *_port); - ~AP_GPS_QURT(); - - bool read() override; - - const char *name() const override { return "QURT"; } - -private: - bool initialised = false; - uint32_t last_tow; -}; -#endif // CONFIG_HAL_BOARD -