From 9fc57e40b4ceb89b1b63efec95f16183651c591f Mon Sep 17 00:00:00 2001 From: bugobliterator Date: Wed, 11 Aug 2021 15:43:55 +0530 Subject: [PATCH] AP_GPS: add support for dual GPS heading using Periph GPSes --- libraries/AP_GPS/AP_GPS.cpp | 81 +++++++++++++--- libraries/AP_GPS/AP_GPS.h | 17 ++++ libraries/AP_GPS/AP_GPS_UAVCAN.cpp | 144 ++++++++++++++++++++++++++++- libraries/AP_GPS/AP_GPS_UAVCAN.h | 30 +++++- libraries/AP_GPS/AP_GPS_UBLOX.cpp | 32 +++++-- libraries/AP_GPS/GPS_Backend.cpp | 18 ++-- libraries/AP_GPS/GPS_Backend.h | 1 + libraries/AP_GPS/RTCM3_Parser.h | 5 +- 8 files changed, 297 insertions(+), 31 deletions(-) diff --git a/libraries/AP_GPS/AP_GPS.cpp b/libraries/AP_GPS/AP_GPS.cpp index ca67a78bd7..2f6c11d24e 100644 --- a/libraries/AP_GPS/AP_GPS.cpp +++ b/libraries/AP_GPS/AP_GPS.cpp @@ -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_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 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; } diff --git a/libraries/AP_GPS/AP_GPS.h b/libraries/AP_GPS/AP_GPS.h index 495c475123..110c6efae0 100644 --- a/libraries/AP_GPS/AP_GPS.h +++ b/libraries/AP_GPS/AP_GPS.h @@ -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 diff --git a/libraries/AP_GPS/AP_GPS_UAVCAN.cpp b/libraries/AP_GPS/AP_GPS_UAVCAN.cpp index 0e5203793e..596418cb42 100644 --- a/libraries/AP_GPS/AP_GPS_UAVCAN.cpp +++ b/libraries/AP_GPS/AP_GPS_UAVCAN.cpp @@ -32,6 +32,10 @@ #include #include #include +#if GPS_MOVING_BASELINE +#include +#include +#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 *gnss_moving_baseline; + gnss_moving_baseline = new uavcan::Subscriber(*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 *gnss_relposheading; + gnss_relposheading = new uavcan::Subscriber(*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) { diff --git a/libraries/AP_GPS/AP_GPS_UAVCAN.h b/libraries/AP_GPS/AP_GPS_UAVCAN.h index 81afa889ce..a3a28aeaeb 100644 --- a/libraries/AP_GPS/AP_GPS_UAVCAN.h +++ b/libraries/AP_GPS/AP_GPS_UAVCAN.h @@ -22,7 +22,7 @@ #include #include "AP_GPS.h" #include "GPS_Backend.h" - +#include "RTCM3_Parser.h" #include 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; + }; diff --git a/libraries/AP_GPS/AP_GPS_UBLOX.cpp b/libraries/AP_GPS/AP_GPS_UBLOX.cpp index 3c5987f35f..f5ad34acf1 100644 --- a/libraries/AP_GPS/AP_GPS_UBLOX.cpp +++ b/libraries/AP_GPS/AP_GPS_UBLOX.cpp @@ -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; - _last_relposned_ms = AP_HAL::millis(); + 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; } diff --git a/libraries/AP_GPS/GPS_Backend.cpp b/libraries/AP_GPS/GPS_Backend.cpp index 88a431c459..f26cf7db29 100644 --- a/libraries/AP_GPS/GPS_Backend.cpp +++ b/libraries/AP_GPS/GPS_Backend.cpp @@ -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 diff --git a/libraries/AP_GPS/GPS_Backend.h b/libraries/AP_GPS/GPS_Backend.h index 59d2e526d4..a78bf38088 100644 --- a/libraries/AP_GPS/GPS_Backend.h +++ b/libraries/AP_GPS/GPS_Backend.h @@ -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 diff --git a/libraries/AP_GPS/RTCM3_Parser.h b/libraries/AP_GPS/RTCM3_Parser.h index 462e4777fa..4c514abb45 100644 --- a/libraries/AP_GPS/RTCM3_Parser.h +++ b/libraries/AP_GPS/RTCM3_Parser.h @@ -16,9 +16,10 @@ RTCMv3 parser, used to support moving baseline RTK mode between two GPS modules */ - +#pragma once #include +#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;