diff --git a/libraries/AP_GPS/AP_GPS.cpp b/libraries/AP_GPS/AP_GPS.cpp index 950a0ab75d..e5ea8e6763 100644 --- a/libraries/AP_GPS/AP_GPS.cpp +++ b/libraries/AP_GPS/AP_GPS.cpp @@ -560,7 +560,7 @@ void AP_GPS::send_blob_start(uint8_t instance, const char *_blob, uint16_t size) */ void AP_GPS::send_blob_start(uint8_t instance) { - if (_type[instance] == GPS_TYPE_UBLOX && (_driver_options & AP_GPS_Backend::DriverOptions::UBX_Use115200)) { + if (_type[instance] == GPS_TYPE_UBLOX && option_set(DriverOptions::UBX_Use115200)) { static const char blob[] = UBLOX_SET_BINARY_115200; send_blob_start(instance, blob, sizeof(blob)); return; @@ -569,7 +569,7 @@ void AP_GPS::send_blob_start(uint8_t instance) #if GPS_MOVING_BASELINE if ((_type[instance] == GPS_TYPE_UBLOX_RTK_BASE || _type[instance] == GPS_TYPE_UBLOX_RTK_ROVER) && - ((_driver_options.get() & AP_GPS_Backend::DriverOptions::UBX_MBUseUart2) == 0)) { + !option_set(DriverOptions::UBX_MBUseUart2)) { // we use 460800 when doing moving baseline as we need // more bandwidth. We don't do this if using UART2, as // in that case the RTCMv3 data doesn't go over the @@ -738,13 +738,13 @@ void AP_GPS::detect_instance(uint8_t instance) if ((_type[instance] == GPS_TYPE_AUTO || _type[instance] == GPS_TYPE_UBLOX) && ((!_auto_config && _baudrates[dstate->current_baud] >= 38400) || - (_baudrates[dstate->current_baud] >= 115200 && (_driver_options & AP_GPS_Backend::DriverOptions::UBX_Use115200)) || + (_baudrates[dstate->current_baud] >= 115200 && option_set(DriverOptions::UBX_Use115200)) || _baudrates[dstate->current_baud] == 230400) && AP_GPS_UBLOX::_detect(dstate->ublox_detect_state, data)) { new_gps = new AP_GPS_UBLOX(*this, state[instance], _port[instance], GPS_ROLE_NORMAL); } - const uint32_t ublox_mb_required_baud = (_driver_options.get() & AP_GPS_Backend::DriverOptions::UBX_MBUseUart2)?230400:460800; + const uint32_t ublox_mb_required_baud = option_set(DriverOptions::UBX_MBUseUart2)?230400:460800; if ((_type[instance] == GPS_TYPE_UBLOX_RTK_BASE || _type[instance] == GPS_TYPE_UBLOX_RTK_ROVER) && _baudrates[dstate->current_baud] == ublox_mb_required_baud && diff --git a/libraries/AP_GPS/AP_GPS.h b/libraries/AP_GPS/AP_GPS.h index cf73c3985d..85d4dffab2 100644 --- a/libraries/AP_GPS/AP_GPS.h +++ b/libraries/AP_GPS/AP_GPS.h @@ -588,6 +588,18 @@ protected: uint32_t _log_gps_bit = -1; + enum DriverOptions : int16_t { + UBX_MBUseUart2 = (1U << 0U), + SBF_UseBaseForYaw = (1U << 1U), + UBX_Use115200 = (1U << 2U), + UAVCAN_MBUseDedicatedBus = (1 << 3U), + }; + + // check if an option is set + bool option_set(const DriverOptions option) const { + return (uint8_t(_driver_options.get()) & uint8_t(option)) != 0; + } + private: static AP_GPS *_singleton; HAL_Semaphore rsem; diff --git a/libraries/AP_GPS/AP_GPS_SBF.cpp b/libraries/AP_GPS/AP_GPS_SBF.cpp index bf4af75b9b..60d6a80c92 100644 --- a/libraries/AP_GPS/AP_GPS_SBF.cpp +++ b/libraries/AP_GPS/AP_GPS_SBF.cpp @@ -68,7 +68,7 @@ AP_GPS_SBF::AP_GPS_SBF(AP_GPS &_gps, AP_GPS::GPS_State &_state, // if we ever parse RTK observations it will always be of type NED, so set it once state.rtk_baseline_coords_type = RTK_BASELINE_COORDINATE_SYSTEM_NED; - if (driver_options() & DriverOptions::SBF_UseBaseForYaw) { + if (option_set(AP_GPS::DriverOptions::SBF_UseBaseForYaw)) { state.gps_yaw_configured = true; } } @@ -533,7 +533,7 @@ AP_GPS_SBF::process_message(void) #if GPS_MOVING_BASELINE // copy the baseline data as a yaw source - if (driver_options() & DriverOptions::SBF_UseBaseForYaw) { + if (option_set(AP_GPS::DriverOptions::SBF_UseBaseForYaw)) { calculate_moving_base_yaw(temp.info.Azimuth * 0.01f + 180.0f, Vector3f(temp.info.DeltaNorth, temp.info.DeltaEast, temp.info.DeltaUp).length(), -temp.info.DeltaUp); diff --git a/libraries/AP_GPS/AP_GPS_UAVCAN.cpp b/libraries/AP_GPS/AP_GPS_UAVCAN.cpp index ddb6205bf6..3df4804aa3 100644 --- a/libraries/AP_GPS/AP_GPS_UAVCAN.cpp +++ b/libraries/AP_GPS/AP_GPS_UAVCAN.cpp @@ -955,14 +955,14 @@ bool AP_GPS_UAVCAN::handle_param_get_set_response_int(AP_UAVCAN* ap_uavcan, uint } if (strcmp(name, "GPS_MB_ONLY_PORT") == 0 && cfg_step == STEP_SET_MB_CAN_TX) { - if ((gps._driver_options & UAVCAN_MBUseDedicatedBus) && !value) { + if (option_set(AP_GPS::UAVCAN_MBUseDedicatedBus) && !value) { // set up so that another CAN port is used for the Moving Baseline Data // setting this value will allow another CAN port to be used as dedicated // line for the Moving Baseline Data value = 1; requires_save_and_reboot = true; return true; - } else if (!(gps._driver_options & UAVCAN_MBUseDedicatedBus) && value) { + } else if (!option_set(AP_GPS::UAVCAN_MBUseDedicatedBus) && value) { // set up so that all CAN ports are used for the Moving Baseline Data value = 0; requires_save_and_reboot = true; diff --git a/libraries/AP_GPS/AP_GPS_UBLOX.h b/libraries/AP_GPS/AP_GPS_UBLOX.h index 4afef3d92f..b44f67395d 100644 --- a/libraries/AP_GPS/AP_GPS_UBLOX.h +++ b/libraries/AP_GPS/AP_GPS_UBLOX.h @@ -757,7 +757,7 @@ private: #if GPS_MOVING_BASELINE // see if we should use uart2 for moving baseline config bool mb_use_uart2(void) const { - return (driver_options() & DriverOptions::UBX_MBUseUart2)?true:false; + return option_set(AP_GPS::DriverOptions::UBX_MBUseUart2)?true:false; } #endif diff --git a/libraries/AP_GPS/GPS_Backend.h b/libraries/AP_GPS/GPS_Backend.h index 937542f249..6aba501137 100644 --- a/libraries/AP_GPS/GPS_Backend.h +++ b/libraries/AP_GPS/GPS_Backend.h @@ -98,12 +98,10 @@ public: return (_pseudo_itow_delta_ms == 0)?(_last_itow_ms):((_pseudo_itow/1000ULL) + _pseudo_itow_delta_ms); } - enum DriverOptions : int16_t { - UBX_MBUseUart2 = (1U << 0U), - SBF_UseBaseForYaw = (1U << 1U), - UBX_Use115200 = (1U << 2U), - UAVCAN_MBUseDedicatedBus = (1 << 3U), - }; + // check if an option is set + bool option_set(const AP_GPS::DriverOptions option) const { + return gps.option_set(option); + } protected: AP_HAL::UARTDriver *port; ///< UART we are attached to @@ -141,13 +139,6 @@ protected: void check_new_itow(uint32_t itow, uint32_t msg_length); - /* - access to driver option bits - */ - DriverOptions driver_options(void) const { - return DriverOptions(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); bool calculate_moving_base_yaw(AP_GPS::GPS_State &interim_state, const float reported_heading_deg, const float reported_distance, const float reported_D);