AP_GPS: added QURT GPS

This commit is contained in:
Andrew Tridgell 2015-12-20 13:56:10 +11:00
parent b967140572
commit bc8ee8af36
4 changed files with 160 additions and 1 deletions

View File

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

View File

@ -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"

View File

@ -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

View File

@ -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