You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
111 lines
3.3 KiB
111 lines
3.3 KiB
/* |
|
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 { |
|
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
|
|
|