mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-03 06:28:27 -04:00
AP_GPS: add support for dual GPS heading using Periph GPSes
This commit is contained in:
bugobliterator
Andrew Tridgell
parent
e29ddebe3c
commit
9fc57e40b4
@ -81,7 +81,7 @@ const AP_Param::GroupInfo AP_GPS::var_info[] = {
|
||||
// @Param: _TYPE
|
||||
// @DisplayName: 1st GPS type
|
||||
// @Description: GPS type of 1st 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,13:ERB,14:MAV,15:NOVA,16:HemisphereNMEA,17:uBlox-MovingBaseline-Base,18:uBlox-MovingBaseline-Rover,19:MSP,20:AllyStar,21:ExternalAHRS
|
||||
// @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,16:HemisphereNMEA,17:uBlox-MovingBaseline-Base,18:uBlox-MovingBaseline-Rover,19:MSP,20:AllyStar,21:ExternalAHRS,22:UAVCAN-MovingBaseline-Base,23:UAVCAN-MovingBaseline-Rover
|
||||
// @RebootRequired: True
|
||||
// @User: Advanced
|
||||
AP_GROUPINFO("_TYPE", 0, AP_GPS, _type[0], HAL_GPS_TYPE_DEFAULT),
|
||||
@ -90,7 +90,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,13:ERB,14:MAV,15:NOVA,16:HemisphereNMEA,17:uBlox-MovingBaseline-Base,18:uBlox-MovingBaseline-Rover,19:MSP,20:AllyStar,21:ExternalAHRS
|
||||
// @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,16:HemisphereNMEA,17:uBlox-MovingBaseline-Base,18:uBlox-MovingBaseline-Rover,19:MSP,20:AllyStar,21:ExternalAHRS,22:UAVCAN-MovingBaseline-Base,23:UAVCAN-MovingBaseline-Rover
|
||||
// @RebootRequired: True
|
||||
// @User: Advanced
|
||||
AP_GROUPINFO("_TYPE2", 1, AP_GPS, _type[1], 0),
|
||||
@ -401,6 +401,8 @@ bool AP_GPS::needs_uart(GPS_Type type) const
|
||||
case GPS_TYPE_NONE:
|
||||
case GPS_TYPE_HIL:
|
||||
case GPS_TYPE_UAVCAN:
|
||||
case GPS_TYPE_UAVCAN_RTK_BASE:
|
||||
case GPS_TYPE_UAVCAN_RTK_ROVER:
|
||||
case GPS_TYPE_MAV:
|
||||
case GPS_TYPE_MSP:
|
||||
case GPS_TYPE_EXTERNAL_AHRS:
|
||||
@ -573,6 +575,8 @@ void AP_GPS::detect_instance(uint8_t instance)
|
||||
|
||||
// user has to explicitly set the UAVCAN type, do not use AUTO
|
||||
case GPS_TYPE_UAVCAN:
|
||||
case GPS_TYPE_UAVCAN_RTK_BASE:
|
||||
case GPS_TYPE_UAVCAN_RTK_ROVER:
|
||||
#if HAL_ENABLE_LIBUAVCAN_DRIVERS
|
||||
dstate->auto_detected_baud = false; // specified, not detected
|
||||
new_gps = AP_GPS_UAVCAN::probe(*this, state[instance]);
|
||||
@ -824,7 +828,10 @@ void AP_GPS::update_instance(uint8_t instance)
|
||||
timing[instance].last_message_time_ms = tnow;
|
||||
timing[instance].delta_time_ms = GPS_TIMEOUT_MS;
|
||||
// do not try to detect again if type is MAV or UAVCAN
|
||||
if (_type[instance] == GPS_TYPE_MAV || _type[instance] == GPS_TYPE_UAVCAN) {
|
||||
if (_type[instance] == GPS_TYPE_MAV ||
|
||||
_type[instance] == GPS_TYPE_UAVCAN ||
|
||||
_type[instance] == GPS_TYPE_UAVCAN_RTK_BASE ||
|
||||
_type[instance] == GPS_TYPE_UAVCAN_RTK_ROVER) {
|
||||
state[instance].status = NO_FIX;
|
||||
} else {
|
||||
// free the driver before we run the next detection, so we
|
||||
@ -911,6 +918,55 @@ void AP_GPS::update_instance(uint8_t instance)
|
||||
#endif
|
||||
}
|
||||
|
||||
|
||||
#if GPS_MOVING_BASELINE
|
||||
void AP_GPS::get_RelPosHeading(uint32_t ×tamp, float &relPosHeading, float &relPosLength, float &relPosD, float &accHeading)
|
||||
{
|
||||
for (uint8_t i=0; i< GPS_MAX_RECEIVERS; i++) {
|
||||
if (drivers[i] && _type[i] == GPS_TYPE_UBLOX_RTK_ROVER) {
|
||||
relPosHeading = state[i].relPosHeading;
|
||||
relPosLength = state[i].relPosLength;
|
||||
relPosD = state[i].relPosD;
|
||||
accHeading = state[i].accHeading;
|
||||
timestamp = state[i].relposheading_ts;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
bool AP_GPS::get_RTCMV3(const uint8_t *&bytes, uint16_t &len)
|
||||
{
|
||||
for (uint8_t i=0; i< GPS_MAX_RECEIVERS; i++) {
|
||||
if (drivers[i] && _type[i] == GPS_TYPE_UBLOX_RTK_BASE) {
|
||||
return drivers[i]->get_RTCMV3(bytes, len);
|
||||
}
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
void AP_GPS::clear_RTCMV3()
|
||||
{
|
||||
for (uint8_t i=0; i< GPS_MAX_RECEIVERS; i++) {
|
||||
if (drivers[i] && _type[i] == GPS_TYPE_UBLOX_RTK_BASE) {
|
||||
drivers[i]->clear_RTCMV3();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/*
|
||||
inject Moving Baseline Data messages.
|
||||
*/
|
||||
void AP_GPS::inject_MBL_data(uint8_t* data, uint16_t length)
|
||||
{
|
||||
for (uint8_t i=0; i< GPS_MAX_RECEIVERS; i++) {
|
||||
if (_type[i] == GPS_TYPE_UBLOX_RTK_ROVER) {
|
||||
// pass the data to the rover
|
||||
inject_data(i, data, length);
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
#endif //#if GPS_MOVING_BASELINE
|
||||
|
||||
/*
|
||||
update all GPS instances
|
||||
*/
|
||||
@ -1003,8 +1059,8 @@ void AP_GPS::update_primary(void)
|
||||
// rover as it typically is in fix type 6 (RTK) whereas base is
|
||||
// usually fix type 3
|
||||
for (uint8_t i=0; i<GPS_MAX_RECEIVERS; i++) {
|
||||
if (_type[i] == GPS_TYPE_UBLOX_RTK_BASE &&
|
||||
_type[i^1] == GPS_TYPE_UBLOX_RTK_ROVER &&
|
||||
if (((_type[i] == GPS_TYPE_UBLOX_RTK_BASE) || (_type[i] == GPS_TYPE_UAVCAN_RTK_BASE)) &&
|
||||
((_type[i^1] == GPS_TYPE_UBLOX_RTK_ROVER) || (_type[i^1] == GPS_TYPE_UAVCAN_RTK_ROVER)) &&
|
||||
((state[i].status >= GPS_OK_FIX_3D) || (state[i].status >= state[i^1].status))) {
|
||||
if (primary_instance != i) {
|
||||
_last_instance_swap_ms = now;
|
||||
@ -1158,7 +1214,7 @@ void AP_GPS::inject_data(const uint8_t *data, uint16_t len)
|
||||
//Support broadcasting to all GPSes.
|
||||
if (_inject_to == GPS_RTK_INJECT_TO_ALL) {
|
||||
for (uint8_t i=0; i<GPS_MAX_RECEIVERS; i++) {
|
||||
if (_type[i] == GPS_TYPE_UBLOX_RTK_ROVER) {
|
||||
if ((_type[i] == GPS_TYPE_UBLOX_RTK_ROVER) || (_type[i] == GPS_TYPE_UAVCAN_RTK_ROVER)) {
|
||||
// we don't externally inject to moving baseline rover
|
||||
continue;
|
||||
}
|
||||
@ -1859,7 +1915,7 @@ bool AP_GPS::is_healthy(uint8_t instance) const
|
||||
happens with the RTCMv3 data
|
||||
*/
|
||||
const uint8_t delay_threshold = 2;
|
||||
const float delay_avg_max = _type[instance] == GPS_TYPE_UBLOX_RTK_ROVER?245:215;
|
||||
const float delay_avg_max = _type[instance] == (GPS_TYPE_UBLOX_RTK_ROVER || GPS_TYPE_UAVCAN_RTK_ROVER)?245:215;
|
||||
const GPS_timing &t = timing[instance];
|
||||
bool delay_ok = (t.delayed_count < delay_threshold) &&
|
||||
t.average_delta_ms < delay_avg_max &&
|
||||
@ -1888,13 +1944,16 @@ bool AP_GPS::prepare_for_arming(void) {
|
||||
bool AP_GPS::backends_healthy(char failure_msg[], uint16_t failure_msg_len) {
|
||||
for (uint8_t i = 0; i < GPS_MAX_RECEIVERS; i++) {
|
||||
#if HAL_ENABLE_LIBUAVCAN_DRIVERS
|
||||
if (_type[i] == GPS_TYPE_UAVCAN) {
|
||||
if (_type[i] == GPS_TYPE_UAVCAN ||
|
||||
_type[i] == GPS_TYPE_UAVCAN_RTK_BASE ||
|
||||
_type[i] == GPS_TYPE_UAVCAN_RTK_ROVER) {
|
||||
if (!AP_GPS_UAVCAN::backends_healthy(failure_msg, failure_msg_len)) {
|
||||
return false;
|
||||
}
|
||||
}
|
||||
#endif
|
||||
if (_type[i] == GPS_TYPE_UBLOX_RTK_ROVER) {
|
||||
if (_type[i] == GPS_TYPE_UBLOX_RTK_ROVER ||
|
||||
_type[i] == GPS_TYPE_UAVCAN_RTK_ROVER) {
|
||||
if (AP_HAL::millis() - state[i].gps_yaw_time_ms > 15000) {
|
||||
hal.util->snprintf(failure_msg, failure_msg_len, "GPS[%u] yaw not available", unsigned(i+1));
|
||||
return false;
|
||||
@ -1995,8 +2054,8 @@ bool AP_GPS::gps_yaw_deg(uint8_t instance, float &yaw_deg, float &accuracy_deg,
|
||||
{
|
||||
#if GPS_MAX_RECEIVERS > 1
|
||||
if (instance < GPS_MAX_RECEIVERS &&
|
||||
_type[instance] == GPS_TYPE_UBLOX_RTK_BASE &&
|
||||
_type[instance^1] == GPS_TYPE_UBLOX_RTK_ROVER) {
|
||||
((_type[instance] == GPS_TYPE_UBLOX_RTK_BASE) || (_type[instance] == GPS_TYPE_UAVCAN_RTK_BASE)) &&
|
||||
((_type[instance^1] == GPS_TYPE_UBLOX_RTK_ROVER) || (_type[instance^1] == GPS_TYPE_UAVCAN_RTK_ROVER))) {
|
||||
// return the yaw from the rover
|
||||
instance ^= 1;
|
||||
}
|
||||
|
||||
@ -119,6 +119,8 @@ public:
|
||||
GPS_TYPE_MSP = 19,
|
||||
GPS_TYPE_ALLYSTAR = 20, // AllyStar NMEA
|
||||
GPS_TYPE_EXTERNAL_AHRS = 21,
|
||||
GPS_TYPE_UAVCAN_RTK_BASE = 22,
|
||||
GPS_TYPE_UAVCAN_RTK_ROVER = 23,
|
||||
};
|
||||
|
||||
/// GPS status codes
|
||||
@ -199,6 +201,13 @@ public:
|
||||
int32_t rtk_baseline_z_mm; ///< Current baseline in ECEF z or NED down component in mm
|
||||
uint32_t rtk_accuracy; ///< Current estimate of 3D baseline accuracy (receiver dependent, typical 0 to 9999)
|
||||
int32_t rtk_iar_num_hypotheses; ///< Current number of integer ambiguity hypotheses
|
||||
|
||||
// UBX Relative Position and Heading message information
|
||||
float relPosHeading; ///< Reported Heading in degrees
|
||||
float relPosLength; ///< Reported Position horizontal distance in meters
|
||||
float relPosD; ///< Reported Vertical distance in meters
|
||||
float accHeading; ///< Reported Heading Accuracy in degrees
|
||||
uint32_t relposheading_ts; ///< True if new data has been received since last time it was false
|
||||
};
|
||||
|
||||
/// Startup initialisation.
|
||||
@ -520,6 +529,14 @@ public:
|
||||
DoNotChange = 2,
|
||||
};
|
||||
|
||||
#if GPS_MOVING_BASELINE
|
||||
// methods used by UAVCAN GPS driver and AP_Periph for moving baseline
|
||||
void inject_MBL_data(uint8_t* data, uint16_t length);
|
||||
void get_RelPosHeading(uint32_t ×tamp, float &relPosHeading, float &relPosLength, float &relPosD, float &accHeading);
|
||||
bool get_RTCMV3(const uint8_t *&bytes, uint16_t &len);
|
||||
void clear_RTCMV3();
|
||||
#endif // GPS_MOVING_BASELINE
|
||||
|
||||
protected:
|
||||
|
||||
// configuration parameters
|
||||
|
||||
@ -32,6 +32,10 @@
|
||||
#include <uavcan/equipment/gnss/Auxiliary.hpp>
|
||||
#include <ardupilot/gnss/Heading.hpp>
|
||||
#include <ardupilot/gnss/Status.hpp>
|
||||
#if GPS_MOVING_BASELINE
|
||||
#include <ardupilot/gnss/MovingBaselineData.hpp>
|
||||
#include <ardupilot/gnss/RelPosHeading.hpp>
|
||||
#endif
|
||||
|
||||
extern const AP_HAL::HAL& hal;
|
||||
|
||||
@ -42,13 +46,18 @@ UC_REGISTRY_BINDER(Fix2Cb, uavcan::equipment::gnss::Fix2);
|
||||
UC_REGISTRY_BINDER(AuxCb, uavcan::equipment::gnss::Auxiliary);
|
||||
UC_REGISTRY_BINDER(HeadingCb, ardupilot::gnss::Heading);
|
||||
UC_REGISTRY_BINDER(StatusCb, ardupilot::gnss::Status);
|
||||
#if GPS_MOVING_BASELINE
|
||||
UC_REGISTRY_BINDER(MovingBaselineDataCb, ardupilot::gnss::MovingBaselineData);
|
||||
UC_REGISTRY_BINDER(RelPosHeadingCb, ardupilot::gnss::RelPosHeading);
|
||||
#endif
|
||||
|
||||
AP_GPS_UAVCAN::DetectedModules AP_GPS_UAVCAN::_detected_modules[] = {0};
|
||||
HAL_Semaphore AP_GPS_UAVCAN::_sem_registry;
|
||||
|
||||
// Member Methods
|
||||
AP_GPS_UAVCAN::AP_GPS_UAVCAN(AP_GPS &_gps, AP_GPS::GPS_State &_state) :
|
||||
AP_GPS_Backend(_gps, _state, nullptr)
|
||||
AP_GPS_UAVCAN::AP_GPS_UAVCAN(AP_GPS &_gps, AP_GPS::GPS_State &_state, AP_GPS::GPS_Role _role) :
|
||||
AP_GPS_Backend(_gps, _state, nullptr),
|
||||
role(_role)
|
||||
{}
|
||||
|
||||
AP_GPS_UAVCAN::~AP_GPS_UAVCAN()
|
||||
@ -56,6 +65,10 @@ AP_GPS_UAVCAN::~AP_GPS_UAVCAN()
|
||||
WITH_SEMAPHORE(_sem_registry);
|
||||
|
||||
_detected_modules[_detected_module].driver = nullptr;
|
||||
|
||||
#if GPS_MOVING_BASELINE
|
||||
delete rtcm3_parser;
|
||||
#endif
|
||||
}
|
||||
|
||||
void AP_GPS_UAVCAN::subscribe_msgs(AP_UAVCAN* ap_uavcan)
|
||||
@ -105,6 +118,24 @@ void AP_GPS_UAVCAN::subscribe_msgs(AP_UAVCAN* ap_uavcan)
|
||||
AP_HAL::panic("UAVCAN GNSS subscriber start problem\n\r");
|
||||
return;
|
||||
}
|
||||
|
||||
#if GPS_MOVING_BASELINE
|
||||
uavcan::Subscriber<ardupilot::gnss::MovingBaselineData, MovingBaselineDataCb> *gnss_moving_baseline;
|
||||
gnss_moving_baseline = new uavcan::Subscriber<ardupilot::gnss::MovingBaselineData, MovingBaselineDataCb>(*node);
|
||||
const int gnss_moving_baseline_start_res = gnss_moving_baseline->start(MovingBaselineDataCb(ap_uavcan, &handle_moving_baseline_msg_trampoline));
|
||||
if (gnss_moving_baseline_start_res < 0) {
|
||||
AP_HAL::panic("UAVCAN GNSS subscriber start problem\n\r");
|
||||
return;
|
||||
}
|
||||
|
||||
uavcan::Subscriber<ardupilot::gnss::RelPosHeading, RelPosHeadingCb> *gnss_relposheading;
|
||||
gnss_relposheading = new uavcan::Subscriber<ardupilot::gnss::RelPosHeading, RelPosHeadingCb>(*node);
|
||||
const int gnss_relposheading_start_res = gnss_relposheading->start(RelPosHeadingCb(ap_uavcan, &handle_relposheading_msg_trampoline));
|
||||
if (gnss_relposheading_start_res < 0) {
|
||||
AP_HAL::panic("UAVCAN GNSS subscriber start problem\n\r");
|
||||
return;
|
||||
}
|
||||
#endif
|
||||
}
|
||||
|
||||
AP_GPS_Backend* AP_GPS_UAVCAN::probe(AP_GPS &_gps, AP_GPS::GPS_State &_state)
|
||||
@ -153,7 +184,22 @@ AP_GPS_Backend* AP_GPS_UAVCAN::probe(AP_GPS &_gps, AP_GPS::GPS_State &_state)
|
||||
if (found_match == -1) {
|
||||
return NULL;
|
||||
}
|
||||
backend = new AP_GPS_UAVCAN(_gps, _state);
|
||||
// initialise the backend based on the UAVCAN Moving baseline selection
|
||||
switch (_gps.get_type(found_match)) {
|
||||
case AP_GPS::GPS_TYPE_UAVCAN:
|
||||
backend = new AP_GPS_UAVCAN(_gps, _state, AP_GPS::GPS_ROLE_NORMAL);
|
||||
break;
|
||||
#if GPS_MOVING_BASELINE
|
||||
case AP_GPS::GPS_TYPE_UAVCAN_RTK_BASE:
|
||||
backend = new AP_GPS_UAVCAN(_gps, _state, AP_GPS::GPS_ROLE_MB_BASE);
|
||||
break;
|
||||
case AP_GPS::GPS_TYPE_UAVCAN_RTK_ROVER:
|
||||
backend = new AP_GPS_UAVCAN(_gps, _state, AP_GPS::GPS_ROLE_MB_ROVER);
|
||||
break;
|
||||
#endif
|
||||
default:
|
||||
return NULL;
|
||||
}
|
||||
if (backend == nullptr) {
|
||||
AP::can().log_text(AP_CANManager::LOG_ERROR,
|
||||
LOG_TAG,
|
||||
@ -183,7 +229,16 @@ AP_GPS_Backend* AP_GPS_UAVCAN::probe(AP_GPS &_gps, AP_GPS::GPS_State &_state)
|
||||
AP::gps()._node_id[i].set_and_notify(tmp);
|
||||
}
|
||||
}
|
||||
#if GPS_MOVING_BASELINE
|
||||
if (_detected_modules[found_match].driver->role == AP_GPS::GPS_ROLE_MB_BASE) {
|
||||
_detected_modules[found_match].driver->rtcm3_parser = new RTCM3_Parser;
|
||||
if (_detected_modules[found_match].driver->rtcm3_parser == nullptr) {
|
||||
GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "UAVCAN%u-%u: failed RTCMv3 parser allocation", _detected_modules[found_match].ap_uavcan->get_driver_index()+1, _detected_modules[found_match].node_id);
|
||||
}
|
||||
}
|
||||
#endif // GPS_MOVING_BASELINE
|
||||
}
|
||||
|
||||
return backend;
|
||||
}
|
||||
|
||||
@ -550,6 +605,67 @@ void AP_GPS_UAVCAN::handle_status_msg(const StatusCb &cb)
|
||||
}
|
||||
}
|
||||
|
||||
#if GPS_MOVING_BASELINE
|
||||
/*
|
||||
handle moving baseline data.
|
||||
*/
|
||||
void AP_GPS_UAVCAN::handle_moving_baseline_msg(const MovingBaselineDataCb &cb)
|
||||
{
|
||||
WITH_SEMAPHORE(sem);
|
||||
if ((rtcm3_parser == nullptr) || (role != AP_GPS::GPS_ROLE_MB_BASE)) {
|
||||
return;
|
||||
}
|
||||
for (const auto &c : cb.msg->data) {
|
||||
rtcm3_parser->read(c);
|
||||
}
|
||||
}
|
||||
|
||||
/*
|
||||
handle relposheading message
|
||||
*/
|
||||
void AP_GPS_UAVCAN::handle_relposheading_msg(const RelPosHeadingCb &cb)
|
||||
{
|
||||
if (role != AP_GPS::GPS_ROLE_MB_ROVER) {
|
||||
return;
|
||||
}
|
||||
|
||||
WITH_SEMAPHORE(sem);
|
||||
|
||||
interim_state.gps_yaw_configured = true;
|
||||
// push raw heading data to calculate moving baseline heading states
|
||||
if (calculate_moving_base_yaw(interim_state,
|
||||
cb.msg->reported_heading_deg,
|
||||
cb.msg->relative_distance_m,
|
||||
cb.msg->relative_down_pos_m)) {
|
||||
if (cb.msg->reported_heading_acc_available) {
|
||||
interim_state.gps_yaw_accuracy = cb.msg->reported_heading_acc_deg;
|
||||
}
|
||||
interim_state.have_gps_yaw_accuracy = cb.msg->reported_heading_acc_available;
|
||||
}
|
||||
}
|
||||
|
||||
// support for retrieving RTCMv3 data from a moving baseline base
|
||||
bool AP_GPS_UAVCAN::get_RTCMV3(const uint8_t *&bytes, uint16_t &len)
|
||||
{
|
||||
WITH_SEMAPHORE(sem);
|
||||
if (rtcm3_parser != nullptr) {
|
||||
len = rtcm3_parser->get_len(bytes);
|
||||
return len > 0;
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
// clear previous RTCM3 packet
|
||||
void AP_GPS_UAVCAN::clear_RTCMV3(void)
|
||||
{
|
||||
WITH_SEMAPHORE(sem);
|
||||
if (rtcm3_parser != nullptr) {
|
||||
rtcm3_parser->clear_packet();
|
||||
}
|
||||
}
|
||||
|
||||
#endif // GPS_MOVING_BASELINE
|
||||
|
||||
void AP_GPS_UAVCAN::handle_fix_msg_trampoline(AP_UAVCAN* ap_uavcan, uint8_t node_id, const FixCb &cb)
|
||||
{
|
||||
WITH_SEMAPHORE(_sem_registry);
|
||||
@ -600,6 +716,28 @@ void AP_GPS_UAVCAN::handle_status_msg_trampoline(AP_UAVCAN* ap_uavcan, uint8_t n
|
||||
}
|
||||
}
|
||||
|
||||
#if GPS_MOVING_BASELINE
|
||||
// Moving Baseline msg trampoline
|
||||
void AP_GPS_UAVCAN::handle_moving_baseline_msg_trampoline(AP_UAVCAN* ap_uavcan, uint8_t node_id, const MovingBaselineDataCb &cb)
|
||||
{
|
||||
WITH_SEMAPHORE(_sem_registry);
|
||||
AP_GPS_UAVCAN* driver = get_uavcan_backend(ap_uavcan, node_id);
|
||||
if (driver != nullptr) {
|
||||
driver->handle_moving_baseline_msg(cb);
|
||||
}
|
||||
}
|
||||
|
||||
// RelPosHeading msg trampoline
|
||||
void AP_GPS_UAVCAN::handle_relposheading_msg_trampoline(AP_UAVCAN* ap_uavcan, uint8_t node_id, const RelPosHeadingCb &cb)
|
||||
{
|
||||
WITH_SEMAPHORE(_sem_registry);
|
||||
AP_GPS_UAVCAN* driver = get_uavcan_backend(ap_uavcan, node_id);
|
||||
if (driver != nullptr) {
|
||||
driver->handle_relposheading_msg(cb);
|
||||
}
|
||||
}
|
||||
#endif
|
||||
|
||||
// Consume new data and mark it received
|
||||
bool AP_GPS_UAVCAN::read(void)
|
||||
{
|
||||
|
||||
@ -22,7 +22,7 @@
|
||||
#include <AP_HAL/AP_HAL.h>
|
||||
#include "AP_GPS.h"
|
||||
#include "GPS_Backend.h"
|
||||
|
||||
#include "RTCM3_Parser.h"
|
||||
#include <AP_UAVCAN/AP_UAVCAN.h>
|
||||
|
||||
class FixCb;
|
||||
@ -30,10 +30,14 @@ class Fix2Cb;
|
||||
class AuxCb;
|
||||
class HeadingCb;
|
||||
class StatusCb;
|
||||
#if GPS_MOVING_BASELINE
|
||||
class MovingBaselineDataCb;
|
||||
class RelPosHeadingCb;
|
||||
#endif
|
||||
|
||||
class AP_GPS_UAVCAN : public AP_GPS_Backend {
|
||||
public:
|
||||
AP_GPS_UAVCAN(AP_GPS &_gps, AP_GPS::GPS_State &_state);
|
||||
AP_GPS_UAVCAN(AP_GPS &_gps, AP_GPS::GPS_State &_state, AP_GPS::GPS_Role role);
|
||||
~AP_GPS_UAVCAN();
|
||||
|
||||
bool read() override;
|
||||
@ -54,12 +58,19 @@ public:
|
||||
static void handle_aux_msg_trampoline(AP_UAVCAN* ap_uavcan, uint8_t node_id, const AuxCb &cb);
|
||||
static void handle_heading_msg_trampoline(AP_UAVCAN* ap_uavcan, uint8_t node_id, const HeadingCb &cb);
|
||||
static void handle_status_msg_trampoline(AP_UAVCAN* ap_uavcan, uint8_t node_id, const StatusCb &cb);
|
||||
|
||||
#if GPS_MOVING_BASELINE
|
||||
static void handle_moving_baseline_msg_trampoline(AP_UAVCAN* ap_uavcan, uint8_t node_id, const MovingBaselineDataCb &cb);
|
||||
static void handle_relposheading_msg_trampoline(AP_UAVCAN* ap_uavcan, uint8_t node_id, const RelPosHeadingCb &cb);
|
||||
#endif
|
||||
static bool backends_healthy(char failure_msg[], uint16_t failure_msg_len);
|
||||
void inject_data(const uint8_t *data, uint16_t len) override;
|
||||
|
||||
bool get_error_codes(uint32_t &error_codes) const override { error_codes = error_code; return seen_status; };
|
||||
|
||||
#if GPS_MOVING_BASELINE
|
||||
bool get_RTCMV3(const uint8_t *&data, uint16_t &len) override;
|
||||
void clear_RTCMV3() override;
|
||||
#endif
|
||||
private:
|
||||
void handle_fix_msg(const FixCb &cb);
|
||||
void handle_fix2_msg(const Fix2Cb &cb);
|
||||
@ -67,6 +78,11 @@ private:
|
||||
void handle_heading_msg(const HeadingCb &cb);
|
||||
void handle_status_msg(const StatusCb &cb);
|
||||
|
||||
#if GPS_MOVING_BASELINE
|
||||
void handle_moving_baseline_msg(const MovingBaselineDataCb &cb);
|
||||
void handle_relposheading_msg(const RelPosHeadingCb &cb);
|
||||
#endif
|
||||
|
||||
static bool take_registry();
|
||||
static void give_registry();
|
||||
static AP_GPS_UAVCAN* get_uavcan_backend(AP_UAVCAN* ap_uavcan, uint8_t node_id);
|
||||
@ -96,4 +112,12 @@ private:
|
||||
} _detected_modules[GPS_MAX_RECEIVERS];
|
||||
|
||||
static HAL_Semaphore _sem_registry;
|
||||
|
||||
#if GPS_MOVING_BASELINE
|
||||
// RTCM3 parser for when in moving baseline base mode
|
||||
RTCM3_Parser *rtcm3_parser;
|
||||
#endif
|
||||
// the role set from GPS_TYPE
|
||||
AP_GPS::GPS_Role role;
|
||||
|
||||
};
|
||||
|
||||
@ -50,13 +50,27 @@
|
||||
extern const AP_HAL::HAL& hal;
|
||||
|
||||
#if UBLOX_DEBUGGING
|
||||
#if defined(HAL_BUILD_AP_PERIPH)
|
||||
extern "C" {
|
||||
void can_printf(const char *fmt, ...);
|
||||
}
|
||||
# define Debug(fmt, args ...) do {can_printf("%s:%d: " fmt "\n", __FUNCTION__, __LINE__, ## args);} while(0)
|
||||
#else
|
||||
# define Debug(fmt, args ...) do {hal.console->printf("%s:%d: " fmt "\n", __FUNCTION__, __LINE__, ## args); hal.scheduler->delay(1); } while(0)
|
||||
#endif
|
||||
#else
|
||||
# define Debug(fmt, args ...)
|
||||
#endif
|
||||
|
||||
#if UBLOX_MB_DEBUGGING
|
||||
#if defined(HAL_BUILD_AP_PERIPH)
|
||||
extern "C" {
|
||||
void can_printf(const char *fmt, ...);
|
||||
}
|
||||
# define MB_Debug(fmt, args ...) do {can_printf("%s:%d: " fmt "\n", __FUNCTION__, __LINE__, ## args);} while(0)
|
||||
#else
|
||||
# define MB_Debug(fmt, args ...) do {hal.console->printf("%s:%d: " fmt "\n", __FUNCTION__, __LINE__, ## args); hal.scheduler->delay(1); } while(0)
|
||||
#endif
|
||||
#else
|
||||
# define MB_Debug(fmt, args ...)
|
||||
#endif
|
||||
@ -1353,15 +1367,21 @@ AP_GPS_UBLOX::_parse_gps(void)
|
||||
MB_Debug("RELPOSNED ITOW %u %u\n", unsigned(_buffer.relposned.iTOW), unsigned(_last_relposned_itow));
|
||||
}
|
||||
_last_relposned_itow = _buffer.relposned.iTOW;
|
||||
|
||||
MB_Debug("RELPOSNED flags: %lx valid: %lx invalid: %lx\n", _buffer.relposned.flags, valid_mask, invalid_mask);
|
||||
if (((_buffer.relposned.flags & valid_mask) == valid_mask) &&
|
||||
((_buffer.relposned.flags & invalid_mask) == 0) &&
|
||||
calculate_moving_base_yaw(_buffer.relposned.relPosHeading * 1e-5,
|
||||
((_buffer.relposned.flags & invalid_mask) == 0)) {
|
||||
if (calculate_moving_base_yaw(_buffer.relposned.relPosHeading * 1e-5,
|
||||
_buffer.relposned.relPosLength * 0.01,
|
||||
_buffer.relposned.relPosD*0.01)) {
|
||||
state.gps_yaw_accuracy = _buffer.relposned.accHeading * 1e-5;
|
||||
state.have_gps_yaw_accuracy = true;
|
||||
state.gps_yaw_accuracy = _buffer.relposned.accHeading * 1e-5;
|
||||
_last_relposned_ms = AP_HAL::millis();
|
||||
}
|
||||
state.relPosHeading = _buffer.relposned.relPosHeading * 1e-5;
|
||||
state.relPosLength = _buffer.relposned.relPosLength * 0.01;
|
||||
state.relPosD = _buffer.relposned.relPosD * 0.01;
|
||||
state.accHeading = _buffer.relposned.accHeading * 1e-5;
|
||||
state.relposheading_ts = AP_HAL::millis();
|
||||
} else {
|
||||
state.have_gps_yaw_accuracy = false;
|
||||
}
|
||||
|
||||
@ -302,20 +302,23 @@ void AP_GPS_Backend::check_new_itow(uint32_t itow, uint32_t msg_length)
|
||||
}
|
||||
|
||||
#if GPS_MOVING_BASELINE
|
||||
bool AP_GPS_Backend::calculate_moving_base_yaw(const float reported_heading_deg, const float reported_distance, const float reported_D)
|
||||
{
|
||||
bool AP_GPS_Backend::calculate_moving_base_yaw(float reported_heading_deg, const float reported_distance, const float reported_D) {
|
||||
return calculate_moving_base_yaw(state, reported_heading_deg, reported_distance, reported_D);
|
||||
}
|
||||
|
||||
bool AP_GPS_Backend::calculate_moving_base_yaw(AP_GPS::GPS_State &interim_state, const float reported_heading_deg, const float reported_distance, const float reported_D) {
|
||||
constexpr float minimum_antenna_seperation = 0.05; // meters
|
||||
constexpr float permitted_error_length_pct = 0.2; // percentage
|
||||
|
||||
bool selectedOffset = false;
|
||||
Vector3f offset;
|
||||
switch (MovingBase::Type(gps.mb_params[state.instance].type.get())) {
|
||||
switch (MovingBase::Type(gps.mb_params[interim_state.instance].type.get())) {
|
||||
case MovingBase::Type::RelativeToAlternateInstance:
|
||||
offset = gps._antenna_offset[state.instance^1].get() - gps._antenna_offset[state.instance].get();
|
||||
offset = gps._antenna_offset[interim_state.instance^1].get() - gps._antenna_offset[interim_state.instance].get();
|
||||
selectedOffset = true;
|
||||
break;
|
||||
case MovingBase::Type::RelativeToCustomBase:
|
||||
offset = gps.mb_params[state.instance].base_offset.get();
|
||||
offset = gps.mb_params[interim_state.instance].base_offset.get();
|
||||
selectedOffset = true;
|
||||
break;
|
||||
}
|
||||
@ -385,13 +388,16 @@ bool AP_GPS_Backend::calculate_moving_base_yaw(const float reported_heading_deg,
|
||||
state.gps_yaw = wrap_360(reported_heading_deg - degrees(rotation_offset_rad));
|
||||
state.have_gps_yaw = true;
|
||||
state.gps_yaw_time_ms = AP_HAL::millis();
|
||||
interim_state.gps_yaw = state.gps_yaw;
|
||||
interim_state.have_gps_yaw = state.have_gps_yaw;
|
||||
interim_state.gps_yaw_time_ms = AP_HAL::millis();
|
||||
}
|
||||
}
|
||||
|
||||
return true;
|
||||
|
||||
bad_yaw:
|
||||
state.have_gps_yaw = false;
|
||||
interim_state.have_gps_yaw = false;
|
||||
return false;
|
||||
}
|
||||
#endif // GPS_MOVING_BASELINE
|
||||
|
||||
@ -131,6 +131,7 @@ protected:
|
||||
|
||||
#if GPS_MOVING_BASELINE
|
||||
bool calculate_moving_base_yaw(const float reported_heading_deg, const float reported_distance, const float reported_D);
|
||||
bool calculate_moving_base_yaw(AP_GPS::GPS_State &interim_state, const float reported_heading_deg, const float reported_distance, const float reported_D);
|
||||
#endif //GPS_MOVING_BASELINE
|
||||
|
||||
// get GPS type, for subtype config
|
||||
|
||||
@ -16,9 +16,10 @@
|
||||
RTCMv3 parser, used to support moving baseline RTK mode between two
|
||||
GPS modules
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
#include <stdint.h>
|
||||
|
||||
#define RTCM3_MAX_PACKET_LEN 300
|
||||
class RTCM3_Parser {
|
||||
public:
|
||||
// process one byte, return true if packet found
|
||||
@ -40,7 +41,7 @@ private:
|
||||
const uint8_t RTCMv3_PREAMBLE = 0xD3;
|
||||
|
||||
// raw packet, we shouldn't need over 300 bytes for the MB configs we use
|
||||
uint8_t pkt[300];
|
||||
uint8_t pkt[RTCM3_MAX_PACKET_LEN];
|
||||
|
||||
// number of bytes in pkt[]
|
||||
uint16_t pkt_bytes;
|
||||
|
||||
Loading…
Reference in New Issue
Block a user