AP_GPS: added QURT GPS

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) {

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"
 

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 <http://www.gnu.org/licenses/>.
+ */
+
+#include <AP_HAL/AP_HAL.h>
+#if CONFIG_HAL_BOARD == HAL_BOARD_QURT
+#include "AP_GPS_QURT.h"
+extern "C" {
+#include <csr_gps_api.h>
+#include <csr_gps_common.h>
+}
+
+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

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 <http://www.gnu.org/licenses/>.
+ */
+
+#pragma once
+
+#include <AP_HAL/AP_HAL.h>
+#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
+