From bc8ee8af36eae545cb05de48d8cc2be12380e47a Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sun, 20 Dec 2015 13:56:10 +1100 Subject: [PATCH] AP_GPS: added QURT GPS --- libraries/AP_GPS/AP_GPS.cpp | 10 ++- libraries/AP_GPS/AP_GPS.h | 2 + libraries/AP_GPS/AP_GPS_QURT.cpp | 114 +++++++++++++++++++++++++++++++ libraries/AP_GPS/AP_GPS_QURT.h | 35 ++++++++++ 4 files changed, 160 insertions(+), 1 deletion(-) create mode 100644 libraries/AP_GPS/AP_GPS_QURT.cpp create mode 100644 libraries/AP_GPS/AP_GPS_QURT.h diff --git a/libraries/AP_GPS/AP_GPS.cpp b/libraries/AP_GPS/AP_GPS.cpp index e54eb796a5..0ee0eeba3b 100644 --- a/libraries/AP_GPS/AP_GPS.cpp +++ b/libraries/AP_GPS/AP_GPS.cpp @@ -196,6 +196,14 @@ AP_GPS::detect_instance(uint8_t instance) } #endif +#if CONFIG_HAL_BOARD == HAL_BOARD_QURT + if (_type[instance] == GPS_TYPE_QURT) { + hal.console->print(" QURTGPS "); + new_gps = new AP_GPS_QURT(*this, state[instance], _port[instance]); + goto found_gps; + } +#endif + if (_port[instance] == NULL) { // UART not available return; @@ -288,7 +296,7 @@ AP_GPS::detect_instance(uint8_t instance) } } -#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 +#if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_QURT found_gps: #endif if (new_gps != NULL) { diff --git a/libraries/AP_GPS/AP_GPS.h b/libraries/AP_GPS/AP_GPS.h index bfd4b63099..5fe6bed8a8 100644 --- a/libraries/AP_GPS/AP_GPS.h +++ b/libraries/AP_GPS/AP_GPS.h @@ -69,6 +69,7 @@ public: GPS_TYPE_PX4 = 9, GPS_TYPE_SBF = 10, GPS_TYPE_GSOF = 11, + GPS_TYPE_QURT = 12, }; /// GPS status codes @@ -395,6 +396,7 @@ private: #include "AP_GPS_SIRF.h" #include "AP_GPS_SBP.h" #include "AP_GPS_PX4.h" +#include "AP_GPS_QURT.h" #include "AP_GPS_SBF.h" #include "AP_GPS_GSOF.h" diff --git a/libraries/AP_GPS/AP_GPS_QURT.cpp b/libraries/AP_GPS/AP_GPS_QURT.cpp new file mode 100644 index 0000000000..56c2b0a775 --- /dev/null +++ b/libraries/AP_GPS/AP_GPS_QURT.cpp @@ -0,0 +1,114 @@ +// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- +/* + 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 if (data.nav_type & NAV_TYPE_1SV_KF_SOLUTION) { + state.status = AP_GPS::NO_FIX; + } else { + state.status = AP_GPS::NO_GPS; + } + + 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_cd = data.course_over_ground; + + // 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_cd * 0.01f); + 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 new file mode 100644 index 0000000000..b3bdf1e540 --- /dev/null +++ b/libraries/AP_GPS/AP_GPS_QURT.h @@ -0,0 +1,35 @@ +// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- +/* + 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" +#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; + +private: + bool initialised = false; + uint32_t last_tow; +}; +#endif // CONFIG_HAL_BOARD +