From 4161e2eb2887ba3cabb747c12bd0e2e05a2bee38 Mon Sep 17 00:00:00 2001 From: Michael du Breuil Date: Tue, 29 Sep 2020 12:47:37 -0700 Subject: [PATCH] AP_GPS: Factor out moving base offset helper --- libraries/AP_GPS/AP_GPS.cpp | 16 ++++++- libraries/AP_GPS/AP_GPS.h | 12 ++++- libraries/AP_GPS/AP_GPS_UBLOX.cpp | 74 +++++++++-------------------- libraries/AP_GPS/AP_GPS_UBLOX.h | 6 +-- libraries/AP_GPS/GPS_Backend.cpp | 78 +++++++++++++++++++++++++++++++ libraries/AP_GPS/GPS_Backend.h | 4 ++ libraries/AP_GPS/MovingBase.cpp | 43 +++++++++++++++++ libraries/AP_GPS/MovingBase.h | 24 ++++++++++ 8 files changed, 199 insertions(+), 58 deletions(-) create mode 100644 libraries/AP_GPS/MovingBase.cpp create mode 100644 libraries/AP_GPS/MovingBase.h diff --git a/libraries/AP_GPS/AP_GPS.cpp b/libraries/AP_GPS/AP_GPS.cpp index 6382e955fb..e86a95a1b8 100644 --- a/libraries/AP_GPS/AP_GPS.cpp +++ b/libraries/AP_GPS/AP_GPS.cpp @@ -293,7 +293,7 @@ const AP_Param::GroupInfo AP_GPS::var_info[] = { AP_GROUPINFO("BLEND_TC", 21, AP_GPS, _blend_tc, 10.0f), #endif -#if GPS_UBLOX_MOVING_BASELINE +#if GPS_MOVING_BASELINE // @Param: DRV_OPTIONS // @DisplayName: driver options // @Description: Additional backend specific options @@ -322,6 +322,20 @@ const AP_Param::GroupInfo AP_GPS::var_info[] = { AP_GROUPINFO("COM_PORT2", 24, AP_GPS, _com_port[1], 1), #endif +#if GPS_MOVING_BASELINE + + // @Group: MB1_ + // @Path: MovingBase.cpp + AP_SUBGROUPINFO(mb_params[0], "MB1_", 25, AP_GPS, MovingBase), + +#if GPS_MAX_RECEIVERS > 1 + // @Group: MB2_ + // @Path: MovingBase.cpp + AP_SUBGROUPINFO(mb_params[1], "MB2_", 26, AP_GPS, MovingBase), +#endif // GPS_MAX_RECEIVERS > 1 + +#endif // GPS_MOVING_BASELINE + AP_GROUPEND }; diff --git a/libraries/AP_GPS/AP_GPS.h b/libraries/AP_GPS/AP_GPS.h index 7c84db331b..0bb37527e2 100644 --- a/libraries/AP_GPS/AP_GPS.h +++ b/libraries/AP_GPS/AP_GPS.h @@ -43,14 +43,18 @@ #define UNIX_OFFSET_MSEC (17000ULL * 86400ULL + 52ULL * 10ULL * AP_MSEC_PER_WEEK - GPS_LEAPSECONDS_MILLIS) -#ifndef GPS_UBLOX_MOVING_BASELINE -#define GPS_UBLOX_MOVING_BASELINE !HAL_MINIMIZE_FEATURES && GPS_MAX_RECEIVERS>1 +#ifndef GPS_MOVING_BASELINE +#define GPS_MOVING_BASELINE !HAL_MINIMIZE_FEATURES && GPS_MAX_RECEIVERS>1 #endif #ifndef HAL_MSP_GPS_ENABLED #define HAL_MSP_GPS_ENABLED HAL_MSP_SENSORS_ENABLED #endif +#if GPS_MOVING_BASELINE +#include "MovingBase.h" +#endif // GPS_MOVING_BASELINE + class AP_GPS_Backend; /// @class AP_GPS @@ -519,6 +523,10 @@ protected: AP_Float _blend_tc; AP_Int16 _driver_options; +#if GPS_MOVING_BASELINE + MovingBase mb_params[GPS_MAX_RECEIVERS]; +#endif // GPS_MOVING_BASELINE + uint32_t _log_gps_bit = -1; private: diff --git a/libraries/AP_GPS/AP_GPS_UBLOX.cpp b/libraries/AP_GPS/AP_GPS_UBLOX.cpp index eb4087cb75..187025039e 100644 --- a/libraries/AP_GPS/AP_GPS_UBLOX.cpp +++ b/libraries/AP_GPS/AP_GPS_UBLOX.cpp @@ -79,7 +79,7 @@ AP_GPS_UBLOX::AP_GPS_UBLOX(AP_GPS &_gps, AP_GPS::GPS_State &_state, AP_HAL::UART _unconfigured_messages |= CONFIG_TP5; #endif -#if GPS_UBLOX_MOVING_BASELINE +#if GPS_MOVING_BASELINE if (role == AP_GPS::GPS_ROLE_MB_BASE && !mb_use_uart2()) { rtcm3_parser = new RTCM3_Parser; if (rtcm3_parser == nullptr) { @@ -96,14 +96,14 @@ AP_GPS_UBLOX::AP_GPS_UBLOX(AP_GPS &_gps, AP_GPS::GPS_State &_state, AP_HAL::UART AP_GPS_UBLOX::~AP_GPS_UBLOX() { -#if GPS_UBLOX_MOVING_BASELINE +#if GPS_MOVING_BASELINE if (rtcm3_parser) { delete rtcm3_parser; } #endif } -#if GPS_UBLOX_MOVING_BASELINE +#if GPS_MOVING_BASELINE /* config for F9 GPS in moving baseline base role See ZED-F9P integration manual section 3.1.5.6.1 @@ -205,7 +205,7 @@ const AP_GPS_UBLOX::config_list AP_GPS_UBLOX::config_MB_Rover_uart2[] { { ConfigKey::MSGOUT_RTCM_3X_TYPE1127_UART1, 0}, { ConfigKey::MSGOUT_RTCM_3X_TYPE1230_UART1, 0}, }; -#endif // GPS_UBLOX_MOVING_BASELINE +#endif // GPS_MOVING_BASELINE void @@ -357,7 +357,7 @@ AP_GPS_UBLOX::_request_next_config(void) } break; case STEP_RTK_MOVBASE: -#if GPS_UBLOX_MOVING_BASELINE +#if GPS_MOVING_BASELINE if (supports_F9_config()) { static_assert(sizeof(active_config.done_mask)*8 >= ARRAY_SIZE(config_MB_Base_uart1), "done_mask too small, base1"); static_assert(sizeof(active_config.done_mask)*8 >= ARRAY_SIZE(config_MB_Base_uart2), "done_mask too small, base2"); @@ -572,7 +572,7 @@ AP_GPS_UBLOX::read(void) // read the next byte data = port->read(); -#if GPS_UBLOX_MOVING_BASELINE +#if GPS_MOVING_BASELINE if (rtcm3_parser) { if (rtcm3_parser->read(data)) { // we've found a RTCMv3 packet. We stop parsing at @@ -682,7 +682,7 @@ AP_GPS_UBLOX::read(void) break; // bad checksum } -#if GPS_UBLOX_MOVING_BASELINE +#if GPS_MOVING_BASELINE if (rtcm3_parser) { // this is a uBlox packet, discard any partial RTCMv3 state rtcm3_parser->reset(); @@ -856,7 +856,7 @@ uint8_t AP_GPS_UBLOX::config_key_size(ConfigKey key) const */ int8_t AP_GPS_UBLOX::find_active_config_index(ConfigKey key) const { -#if GPS_UBLOX_MOVING_BASELINE +#if GPS_MOVING_BASELINE if (active_config.list == nullptr) { return -1; } @@ -1122,7 +1122,7 @@ AP_GPS_UBLOX::_parse_gps(void) default: break; } -#if GPS_UBLOX_MOVING_BASELINE +#if GPS_MOVING_BASELINE // see if it is in active config list int8_t cfg_idx = find_active_config_index(id); if (cfg_idx >= 0) { @@ -1141,7 +1141,7 @@ AP_GPS_UBLOX::_parse_gps(void) } } } -#endif // GPS_UBLOX_MOVING_BASELINE +#endif // GPS_MOVING_BASELINE // step over the value uint8_t step_size = config_key_size(id); @@ -1326,10 +1326,10 @@ AP_GPS_UBLOX::_parse_gps(void) state.hdop = 130; #endif break; + +#if GPS_MOVING_BASELINE case MSG_RELPOSNED: { - const Vector3f &offset0 = gps._antenna_offset[state.instance^1].get(); - const Vector3f &offset1 = gps._antenna_offset[state.instance].get(); // note that we require the yaw to come from a fixed solution, not a float solution // yaw from a float solution would only be acceptable with a very large separation between // GPS modules @@ -1342,21 +1342,6 @@ AP_GPS_UBLOX::_parse_gps(void) static_cast(RELPOSNED::refObsMiss) | static_cast(RELPOSNED::carrSolnFloat); - const Vector3f antenna_offset = offset0 - offset1; - const float offset_dist = antenna_offset.length(); - const float rel_dist = _buffer.relposned.relPosLength * 0.01; - const float dist_error = offset_dist - rel_dist; - const float strict_length_error_allowed = 0.2; // allow for up to 20% error - const float min_separation = 0.05; - bool tilt_ok = true; - const float min_dist = MIN(offset_dist, rel_dist); -#ifndef HAL_BUILD_AP_PERIPH - // when ahrs is available use it to constrain vertical component - const Vector3f antenna_tilt = AP::ahrs().get_rotation_body_to_ned() * antenna_offset; - const float alt_error = _buffer.relposned.relPosD*0.01 + antenna_tilt.z; - tilt_ok = fabsf(alt_error) < strict_length_error_allowed * min_dist; -#endif - _check_new_itow(_buffer.relposned.iTOW); if (_buffer.relposned.iTOW != _last_relposned_itow+200) { // useful for looking at packet loss on links @@ -1365,35 +1350,20 @@ AP_GPS_UBLOX::_parse_gps(void) _last_relposned_itow = _buffer.relposned.iTOW; _last_relposned_ms = AP_HAL::millis(); - /* - RELPOSNED messages gives the NED distance from base to - rover. It comes from the rover - */ - MB_Debug("RELPOSNED[%u]: od:%.2f rd:%.2f ae:%.2f flags:0x%04x t=%u", - state.instance+1, - offset_dist, rel_dist, alt_error, - unsigned(_buffer.relposned.flags), - unsigned(_buffer.relposned.iTOW)); - if (((_buffer.relposned.flags & valid_mask) == valid_mask) && ((_buffer.relposned.flags & invalid_mask) == 0) && - rel_dist > min_separation && - offset_dist > min_separation && - fabsf(dist_error) < strict_length_error_allowed * min_dist && - tilt_ok) { - float rotation_offset_rad; - const Vector3f diff = offset1 - offset0; - rotation_offset_rad = Vector2f(diff.x, diff.y).angle(); - state.gps_yaw = wrap_360(_buffer.relposned.relPosHeading * 1e-5 - degrees(rotation_offset_rad)); - state.have_gps_yaw = true; + 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; } else { - state.have_gps_yaw = false; state.have_gps_yaw_accuracy = false; } } break; +#endif // GPS_MOVING_BASELINE + case MSG_PVT: Debug("MSG_PVT"); @@ -1707,7 +1677,7 @@ AP_GPS_UBLOX::_configure_valget(ConfigKey key) bool AP_GPS_UBLOX::_configure_config_set(const config_list *list, uint8_t count, uint32_t unconfig_bit) { -#if GPS_UBLOX_MOVING_BASELINE +#if GPS_MOVING_BASELINE active_config.list = list; active_config.count = count; active_config.done_mask = 0; @@ -1887,7 +1857,7 @@ bool AP_GPS_UBLOX::get_lag(float &lag_sec) const // F9 lag not verified yet from flight log, but likely to be at least // as good as M8 lag_sec = 0.12f; -#if GPS_UBLOX_MOVING_BASELINE +#if GPS_MOVING_BASELINE if (role == AP_GPS::GPS_ROLE_MB_BASE || role == AP_GPS::GPS_ROLE_MB_ROVER) { // the moving baseline rover will lag about 40ms from the @@ -1924,7 +1894,7 @@ void AP_GPS_UBLOX::_check_new_itow(uint32_t itow) // support for retrieving RTCMv3 data from a moving baseline base bool AP_GPS_UBLOX::get_RTCMV3(const uint8_t *&bytes, uint16_t &len) { -#if GPS_UBLOX_MOVING_BASELINE +#if GPS_MOVING_BASELINE if (rtcm3_parser) { len = rtcm3_parser->get_len(bytes); return len > 0; @@ -1936,7 +1906,7 @@ bool AP_GPS_UBLOX::get_RTCMV3(const uint8_t *&bytes, uint16_t &len) // clear previous RTCM3 packet void AP_GPS_UBLOX::clear_RTCMV3(void) { -#if GPS_UBLOX_MOVING_BASELINE +#if GPS_MOVING_BASELINE if (rtcm3_parser) { rtcm3_parser->clear_packet(); } @@ -1952,7 +1922,7 @@ bool AP_GPS_UBLOX::is_healthy(void) const return true; } #endif -#if GPS_UBLOX_MOVING_BASELINE +#if GPS_MOVING_BASELINE if ((role == AP_GPS::GPS_ROLE_MB_BASE || role == AP_GPS::GPS_ROLE_MB_ROVER) && !supports_F9_config()) { diff --git a/libraries/AP_GPS/AP_GPS_UBLOX.h b/libraries/AP_GPS/AP_GPS_UBLOX.h index b8d6ee29e2..510bb69879 100644 --- a/libraries/AP_GPS/AP_GPS_UBLOX.h +++ b/libraries/AP_GPS/AP_GPS_UBLOX.h @@ -742,7 +742,7 @@ private: return (uint8_t)(ubx_msg + (state.instance * UBX_MSG_TYPES)); } -#if GPS_UBLOX_MOVING_BASELINE +#if GPS_MOVING_BASELINE // see if we should use uart2 for moving baseline config bool mb_use_uart2(void) const { return (driver_options() & unsigned(DRV_OPTIONS::MB_USE_UART2))?true:false; @@ -770,7 +770,7 @@ private: // return true if GPS is capable of F9 config bool supports_F9_config(void) const; -#if GPS_UBLOX_MOVING_BASELINE +#if GPS_MOVING_BASELINE // config for moving baseline base static const config_list config_MB_Base_uart1[]; static const config_list config_MB_Base_uart2[]; @@ -789,5 +789,5 @@ private: // RTCM3 parser for when in moving baseline base mode RTCM3_Parser *rtcm3_parser; -#endif // GPS_UBLOX_MOVING_BASELINE +#endif // GPS_MOVING_BASELINE }; diff --git a/libraries/AP_GPS/GPS_Backend.cpp b/libraries/AP_GPS/GPS_Backend.cpp index 9ce166704e..cf1d237d12 100644 --- a/libraries/AP_GPS/GPS_Backend.cpp +++ b/libraries/AP_GPS/GPS_Backend.cpp @@ -18,6 +18,7 @@ #include #include #include +#include #define GPS_BACKEND_DEBUGGING 0 @@ -301,3 +302,80 @@ 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) +{ + 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())) { + case MovingBase::Type::RelativeToAlternateInstance: + offset = gps._antenna_offset[state.instance^1].get() - gps._antenna_offset[state.instance].get(); + selectedOffset = true; + break; + case MovingBase::Type::RelativeToCustomBase: + offset = gps.mb_params[state.instance].base_offset.get(); + selectedOffset = true; + break; + } + + if (!selectedOffset) { + // invalid type, let's throw up a flag + INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control); + goto bad_yaw; + } + + { + const float offset_dist = offset.length(); + const float min_dist = MIN(offset_dist, reported_distance); + + if (offset_dist < minimum_antenna_seperation) { + // offsets have to be sufficently large to get a meaningful angle off of them + Debug("Insufficent antenna offset (%f, %f, %f)", (double)offset.x, (double)offset.y, (double)offset.z); + goto bad_yaw; + } + + if (reported_distance < minimum_antenna_seperation) { + // if the reported distance is less then the minimum seperation it's not sufficently robust + Debug("Reported baseline distance (%f) was less then the minimum antenna seperation (%f)", + (double)reported_distance, (double)minimum_antenna_seperation); + goto bad_yaw; + } + + + if ((offset_dist - reported_distance) > (min_dist * permitted_error_length_pct)) { + // the magnitude of the vector is much further then we were expecting + Debug("Exceeded the permitted error margin %f > %f", + (double)(offset_dist - reported_distance), (double)(min_dist * permitted_error_length_pct)); + goto bad_yaw; + } + +#ifndef HAL_BUILD_AP_PERIPH + { + const Vector3f antenna_tilt = AP::ahrs().get_rotation_body_to_ned() * offset; + const float alt_error = reported_D + antenna_tilt.z; + if (fabsf(alt_error) > permitted_error_length_pct * min_dist) { + // the vertical component is out of range, reject it + goto bad_yaw; + } + } +#endif // HAL_BUILD_AP_PERIPH + + { + // at this point the offsets are looking okay, go ahead and actually calculate a useful heading + const float rotation_offset_rad = Vector2f(-offset.x, -offset.y).angle(); + state.gps_yaw = wrap_360(reported_heading_deg - degrees(rotation_offset_rad)); + state.have_gps_yaw = true; + } + } + + return true; + +bad_yaw: + 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 a9303b122a..f1a82ec09b 100644 --- a/libraries/AP_GPS/GPS_Backend.h +++ b/libraries/AP_GPS/GPS_Backend.h @@ -118,6 +118,10 @@ protected: return uint16_t(gps._driver_options.get()); } +#if GPS_MOVING_BASELINE + bool calculate_moving_base_yaw(const float reported_heading_deg, const float reported_distance, const float reported_D); +#endif //GPS_MOVING_BASELINE + private: // itow from previous message uint32_t _last_itow; diff --git a/libraries/AP_GPS/MovingBase.cpp b/libraries/AP_GPS/MovingBase.cpp new file mode 100644 index 0000000000..9dc4d1c56c --- /dev/null +++ b/libraries/AP_GPS/MovingBase.cpp @@ -0,0 +1,43 @@ +#include "MovingBase.h" + +const AP_Param::GroupInfo MovingBase::var_info[] = { + // @Param: TYPE + // @DisplayName: Moving base type + // @Description: Controls the type of moving base used if using moving base. + // @Values: 0:Relative to alternate GPS instance,1:RelativeToCustomBase + // @User: Advanced + // @RebootRequired: True + AP_GROUPINFO_FLAGS("TYPE", 1, MovingBase, type, int8_t(Type::RelativeToAlternateInstance), AP_PARAM_FLAG_ENABLE), + + // @Param: OFS_X + // @DisplayName: Base antenna X position offset + // @Description: X position of the base GPS antenna in body frame. Positive X is forward of the origin. Use antenna phase centroid location if provided by the manufacturer. + // @Units: m + // @Range: -5 5 + // @Increment: 0.01 + // @User: Advanced + + // @Param: OFS_Y + // @DisplayName: Base antenna Y position offset + // @Description: Y position of the base GPS antenna in body frame. Positive Y is to the right of the origin. Use antenna phase centroid location if provided by the manufacturer. + // @Units: m + // @Range: -5 5 + // @Increment: 0.01 + // @User: Advanced + + // @Param: OFS_Z + // @DisplayName: Base antenna Z position offset + // @Description: Z position of the base GPS antenna in body frame. Positive Z is down from the origin. Use antenna phase centroid location if provided by the manufacturer. + // @Units: m + // @Range: -5 5 + // @Increment: 0.01 + // @User: Advanced + AP_GROUPINFO("OFS", 2, MovingBase, base_offset, 0.0f), + + AP_GROUPEND + +}; + +MovingBase::MovingBase(void) { + AP_Param::setup_object_defaults(this, var_info); +} diff --git a/libraries/AP_GPS/MovingBase.h b/libraries/AP_GPS/MovingBase.h new file mode 100644 index 0000000000..91f22ad518 --- /dev/null +++ b/libraries/AP_GPS/MovingBase.h @@ -0,0 +1,24 @@ +#pragma once + +#include +#include + +class MovingBase { +public: + static const struct AP_Param::GroupInfo var_info[]; + + MovingBase(void); + + /* Do not allow copies */ + MovingBase(const MovingBase &other) = delete; + MovingBase &operator=(const MovingBase&) = delete; + + enum class Type : int8_t { + RelativeToAlternateInstance = 0, + RelativeToCustomBase = 1, + }; + + AP_Int8 type; // an option from MovingBaseType + AP_Vector3f base_offset; // base position offset from the selected GPS reciever + +};