From 3d4f6cc2c4da827de77e273a36836995e42e0a78 Mon Sep 17 00:00:00 2001 From: Hwurzburg Date: Sun, 9 Jan 2022 17:15:32 -0600 Subject: [PATCH] AP_GPS: add build options for GPS backends --- libraries/AP_GPS/AP_GPS.cpp | 41 ++++++++++++++++++++++++-------- libraries/AP_GPS/AP_GPS_ERB.cpp | 3 +++ libraries/AP_GPS/AP_GPS_ERB.h | 6 +++++ libraries/AP_GPS/AP_GPS_GSOF.cpp | 4 +++- libraries/AP_GPS/AP_GPS_GSOF.h | 6 +++++ libraries/AP_GPS/AP_GPS_MAV.cpp | 3 +++ libraries/AP_GPS/AP_GPS_MAV.h | 6 +++++ libraries/AP_GPS/AP_GPS_NMEA.cpp | 2 ++ libraries/AP_GPS/AP_GPS_NMEA.h | 6 +++++ libraries/AP_GPS/AP_GPS_NOVA.cpp | 3 +++ libraries/AP_GPS/AP_GPS_NOVA.h | 6 +++++ libraries/AP_GPS/AP_GPS_SBF.cpp | 2 ++ libraries/AP_GPS/AP_GPS_SBF.h | 7 ++++++ libraries/AP_GPS/AP_GPS_SBP.cpp | 4 ++++ libraries/AP_GPS/AP_GPS_SBP.h | 6 +++++ libraries/AP_GPS/AP_GPS_SBP2.cpp | 4 ++++ libraries/AP_GPS/AP_GPS_SBP2.h | 7 +++++- libraries/AP_GPS/AP_GPS_SIRF.cpp | 2 ++ libraries/AP_GPS/AP_GPS_SIRF.h | 7 ++++++ 19 files changed, 113 insertions(+), 12 deletions(-) diff --git a/libraries/AP_GPS/AP_GPS.cpp b/libraries/AP_GPS/AP_GPS.cpp index 8cfb06cae5..4b0369d135 100644 --- a/libraries/AP_GPS/AP_GPS.cpp +++ b/libraries/AP_GPS/AP_GPS.cpp @@ -140,12 +140,14 @@ const AP_Param::GroupInfo AP_GPS::var_info[] = { // @User: Advanced AP_GROUPINFO("_INJECT_TO", 7, AP_GPS, _inject_to, GPS_RTK_INJECT_TO_ALL), +#if AP_GPS_SBP2_ENABLED || AP_GPS_SBP_ENABLED // @Param: _SBP_LOGMASK // @DisplayName: Swift Binary Protocol Logging Mask // @Description: Masked with the SBP msg_type field to determine whether SBR1/SBR2 data is logged // @Values: 0:None (0x0000),-1:All (0xFFFF),-256:External only (0xFF00) // @User: Advanced AP_GROUPINFO("_SBP_LOGMASK", 8, AP_GPS, _sbp_logmask, -256), +#endif //AP_GPS_SBP2_ENABLED || AP_GPS_SBP_ENABLED // @Param: _RAW_DATA // @DisplayName: Raw data logging @@ -303,6 +305,7 @@ const AP_Param::GroupInfo AP_GPS::var_info[] = { AP_GROUPINFO("_DRV_OPTIONS", 22, AP_GPS, _driver_options, 0), #endif +#if AP_GPS_SBF_ENABLED // @Param: _COM_PORT // @DisplayName: GPS physical COM port // @Description: The physical COM port on the connected device, currently only applies to SBF GPS @@ -322,6 +325,7 @@ const AP_Param::GroupInfo AP_GPS::var_info[] = { // @RebootRequired: True AP_GROUPINFO("_COM_PORT2", 24, AP_GPS, _com_port[1], HAL_GPS_COM_PORT_DEFAULT), #endif +#endif //AP_GPS_SBF_ENABLED #if GPS_MOVING_BASELINE @@ -568,9 +572,11 @@ void AP_GPS::detect_instance(uint8_t instance) // do not try to detect the MAV type, assume it's there case GPS_TYPE_MAV: #ifndef HAL_BUILD_AP_PERIPH +#if AP_GPS_MAV_ENABLED dstate->auto_detected_baud = false; // specified, not detected new_gps = new AP_GPS_MAV(*this, state[instance], nullptr); goto found_gps; +#endif //AP_GPS_MAV_ENABLED #endif break; @@ -612,19 +618,22 @@ void AP_GPS::detect_instance(uint8_t instance) // don't build the less common GPS drivers on F1 AP_Periph #if !defined(HAL_BUILD_AP_PERIPH) || !defined(STM32F1) switch (_type[instance]) { +#if AP_GPS_SBF_ENABLED // by default the sbf/trimble gps outputs no data on its port, until configured. case GPS_TYPE_SBF: new_gps = new AP_GPS_SBF(*this, state[instance], _port[instance]); break; - +#endif //AP_GPS_SBF_ENABLED +#if AP_GPS_GSOF_ENABLED case GPS_TYPE_GSOF: new_gps = new AP_GPS_GSOF(*this, state[instance], _port[instance]); break; - +#endif //AP_GPS_GSOF_ENABLED +#if AP_GPS_NOVA_ENABLED case GPS_TYPE_NOVA: new_gps = new AP_GPS_NOVA(*this, state[instance], _port[instance]); break; - +#endif //AP_GPS_NOVA_ENABLED default: break; } @@ -644,8 +653,9 @@ void AP_GPS::detect_instance(uint8_t instance) dstate->last_baud_change_ms = now; if (_auto_config >= GPS_AUTO_CONFIG_ENABLE_SERIAL_ONLY && new_gps == nullptr) { - if (_type[instance] == GPS_TYPE_HEMI) { - send_blob_start(instance, AP_GPS_NMEA_HEMISPHERE_INIT_STRING, strlen(AP_GPS_NMEA_HEMISPHERE_INIT_STRING)); + if (_type[instance] == GPS_TYPE_UBLOX && (_driver_options & AP_GPS_Backend::DriverOptions::UBX_Use115200)) { + static const char blob[] = UBLOX_SET_BINARY_115200; + send_blob_start(instance, blob, sizeof(blob)); } else 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)) { @@ -655,9 +665,10 @@ void AP_GPS::detect_instance(uint8_t instance) // link to the flight controller static const char blob[] = UBLOX_SET_BINARY_460800; send_blob_start(instance, blob, sizeof(blob)); - } else if (_type[instance] == GPS_TYPE_UBLOX && (_driver_options & AP_GPS_Backend::DriverOptions::UBX_Use115200)) { - static const char blob[] = UBLOX_SET_BINARY_115200; - send_blob_start(instance, blob, sizeof(blob)); +#if AP_GPS_NMEA_ENABLED + } else if (_type[instance] == GPS_TYPE_HEMI) { + send_blob_start(instance, AP_GPS_NMEA_HEMISPHERE_INIT_STRING, strlen(AP_GPS_NMEA_HEMISPHERE_INIT_STRING)); +#endif // AP_GPS_NMEA_ENABLED } else { send_blob_start(instance, _initialisation_blob, sizeof(_initialisation_blob)); } @@ -702,29 +713,39 @@ void AP_GPS::detect_instance(uint8_t instance) new_gps = new AP_GPS_UBLOX(*this, state[instance], _port[instance], role); } #ifndef HAL_BUILD_AP_PERIPH +#if AP_GPS_SBP2_ENABLED else if ((_type[instance] == GPS_TYPE_AUTO || _type[instance] == GPS_TYPE_SBP) && AP_GPS_SBP2::_detect(dstate->sbp2_detect_state, data)) { new_gps = new AP_GPS_SBP2(*this, state[instance], _port[instance]); } +#endif //AP_GPS_SBP2_ENABLED +#if AP_GPS_SBP_ENABLED else if ((_type[instance] == GPS_TYPE_AUTO || _type[instance] == GPS_TYPE_SBP) && AP_GPS_SBP::_detect(dstate->sbp_detect_state, data)) { new_gps = new AP_GPS_SBP(*this, state[instance], _port[instance]); } -#if !HAL_MINIMIZE_FEATURES +#endif //AP_GPS_SBP_ENABLED +#if !HAL_MINIMIZE_FEATURES && AP_GPS_SIRF_ENABLED else if ((_type[instance] == GPS_TYPE_AUTO || _type[instance] == GPS_TYPE_SIRF) && AP_GPS_SIRF::_detect(dstate->sirf_detect_state, data)) { new_gps = new AP_GPS_SIRF(*this, state[instance], _port[instance]); } #endif +#if AP_GPS_ERB_ENABLED else if ((_type[instance] == GPS_TYPE_AUTO || _type[instance] == GPS_TYPE_ERB) && AP_GPS_ERB::_detect(dstate->erb_detect_state, data)) { new_gps = new AP_GPS_ERB(*this, state[instance], _port[instance]); - } else if ((_type[instance] == GPS_TYPE_NMEA || + } +#endif // AP_GPS_ERB_ENABLED +#if AP_GPS_NMEA_ENABLED + else if ((_type[instance] == GPS_TYPE_NMEA || _type[instance] == GPS_TYPE_HEMI || _type[instance] == GPS_TYPE_ALLYSTAR) && AP_GPS_NMEA::_detect(dstate->nmea_detect_state, data)) { new_gps = new AP_GPS_NMEA(*this, state[instance], _port[instance]); } +#endif //AP_GPS_NMEA_ENABLED + #endif // HAL_BUILD_AP_PERIPH if (new_gps) { goto found_gps; diff --git a/libraries/AP_GPS/AP_GPS_ERB.cpp b/libraries/AP_GPS/AP_GPS_ERB.cpp index a1be658010..7468224abd 100644 --- a/libraries/AP_GPS/AP_GPS_ERB.cpp +++ b/libraries/AP_GPS/AP_GPS_ERB.cpp @@ -20,6 +20,8 @@ #include "AP_GPS.h" #include "AP_GPS_ERB.h" +#if AP_GPS_ERB_ENABLED + #define ERB_DEBUGGING 0 #define STAT_FIX_VALID 0x01 @@ -285,3 +287,4 @@ reset: } return false; } +#endif diff --git a/libraries/AP_GPS/AP_GPS_ERB.h b/libraries/AP_GPS/AP_GPS_ERB.h index 449b2e4b1a..c4f3337f1a 100644 --- a/libraries/AP_GPS/AP_GPS_ERB.h +++ b/libraries/AP_GPS/AP_GPS_ERB.h @@ -24,6 +24,11 @@ #include "AP_GPS.h" #include "GPS_Backend.h" +#ifndef AP_GPS_ERB_ENABLED + #define AP_GPS_ERB_ENABLED 1 +#endif + +#if AP_GPS_ERB_ENABLED class AP_GPS_ERB : public AP_GPS_Backend { public: @@ -153,3 +158,4 @@ private: // used to update fix between status and position packets AP_GPS::GPS_Status next_fix = AP_GPS::NO_FIX; }; +#endif diff --git a/libraries/AP_GPS/AP_GPS_GSOF.cpp b/libraries/AP_GPS/AP_GPS_GSOF.cpp index 89476a1a4f..bb420b1f59 100644 --- a/libraries/AP_GPS/AP_GPS_GSOF.cpp +++ b/libraries/AP_GPS/AP_GPS_GSOF.cpp @@ -24,6 +24,8 @@ #include "AP_GPS_GSOF.h" #include +#if AP_GPS_GSOF_ENABLED + extern const AP_HAL::HAL& hal; #define gsof_DEBUGGING 0 @@ -343,4 +345,4 @@ AP_GPS_GSOF::process_message(void) return false; } - +#endif diff --git a/libraries/AP_GPS/AP_GPS_GSOF.h b/libraries/AP_GPS/AP_GPS_GSOF.h index be62ab69ac..c920c78486 100644 --- a/libraries/AP_GPS/AP_GPS_GSOF.h +++ b/libraries/AP_GPS/AP_GPS_GSOF.h @@ -22,6 +22,11 @@ #include "AP_GPS.h" #include "GPS_Backend.h" +#ifndef AP_GPS_GSOF_ENABLED + #define AP_GPS_GSOF_ENABLED 1 +#endif + +#if AP_GPS_GSOF_ENABLED class AP_GPS_GSOF : public AP_GPS_Backend { public: @@ -82,3 +87,4 @@ private: uint8_t gsofmsgreq_index = 0; uint8_t gsofmsgreq[5] = {1,2,8,9,12}; }; +#endif diff --git a/libraries/AP_GPS/AP_GPS_MAV.cpp b/libraries/AP_GPS/AP_GPS_MAV.cpp index 0f4cdf715d..04630f4d8c 100644 --- a/libraries/AP_GPS/AP_GPS_MAV.cpp +++ b/libraries/AP_GPS/AP_GPS_MAV.cpp @@ -19,6 +19,8 @@ #include "AP_GPS_MAV.h" #include +#if AP_GPS_MAV_ENABLED + // Reading does nothing in this class; we simply return whether or not // the latest reading has been consumed. By calling this function we assume // the caller is consuming the new data; @@ -172,3 +174,4 @@ void AP_GPS_MAV::handle_msg(const mavlink_message_t &msg) break; } } +#endif diff --git a/libraries/AP_GPS/AP_GPS_MAV.h b/libraries/AP_GPS/AP_GPS_MAV.h index 7566e5f22a..82e1b58fcb 100644 --- a/libraries/AP_GPS/AP_GPS_MAV.h +++ b/libraries/AP_GPS/AP_GPS_MAV.h @@ -25,6 +25,11 @@ #include "AP_GPS.h" #include "GPS_Backend.h" +#ifndef AP_GPS_MAV_ENABLED + #define AP_GPS_MAV_ENABLED 1 +#endif + +#if AP_GPS_MAV_ENABLED class AP_GPS_MAV : public AP_GPS_Backend { public: @@ -43,3 +48,4 @@ private: uint32_t first_week; JitterCorrection jitter{2000}; }; +#endif diff --git a/libraries/AP_GPS/AP_GPS_NMEA.cpp b/libraries/AP_GPS/AP_GPS_NMEA.cpp index 0af6e1f574..826613747c 100644 --- a/libraries/AP_GPS/AP_GPS_NMEA.cpp +++ b/libraries/AP_GPS/AP_GPS_NMEA.cpp @@ -38,6 +38,7 @@ #include "AP_GPS_NMEA.h" +#if AP_GPS_NMEA_ENABLED extern const AP_HAL::HAL& hal; // Convenience macros ////////////////////////////////////////////////////////// @@ -603,3 +604,4 @@ AP_GPS_NMEA::_detect(struct NMEA_detect_state &state, uint8_t data) } return false; } +#endif diff --git a/libraries/AP_GPS/AP_GPS_NMEA.h b/libraries/AP_GPS/AP_GPS_NMEA.h index fd0c2e7ed3..f9abe85e14 100644 --- a/libraries/AP_GPS/AP_GPS_NMEA.h +++ b/libraries/AP_GPS/AP_GPS_NMEA.h @@ -46,6 +46,11 @@ #include "AP_GPS.h" #include "GPS_Backend.h" +#ifndef AP_GPS_NMEA_ENABLED + #define AP_GPS_NMEA_ENABLED 1 +#endif + +#if AP_GPS_NMEA_ENABLED /// NMEA parser /// class AP_GPS_NMEA : public AP_GPS_Backend @@ -199,3 +204,4 @@ private: "$JASC,GPVTG,5\r\n" /* VTG at 5Hz */ \ "$JASC,GPHDT,5\r\n" /* HDT at 5Hz */ \ "$JMODE,SBASR,YES\r\n" /* Enable SBAS */ +#endif diff --git a/libraries/AP_GPS/AP_GPS_NOVA.cpp b/libraries/AP_GPS/AP_GPS_NOVA.cpp index 8d453ae6cc..60b17df383 100644 --- a/libraries/AP_GPS/AP_GPS_NOVA.cpp +++ b/libraries/AP_GPS/AP_GPS_NOVA.cpp @@ -21,6 +21,8 @@ #include "AP_GPS_NOVA.h" #include +#if AP_GPS_NOVA_ENABLED + extern const AP_HAL::HAL& hal; #define NOVA_DEBUGGING 0 @@ -304,3 +306,4 @@ uint32_t AP_GPS_NOVA::CalculateBlockCRC32(uint32_t length, uint8_t *buffer, uint } return( crc ); } +#endif diff --git a/libraries/AP_GPS/AP_GPS_NOVA.h b/libraries/AP_GPS/AP_GPS_NOVA.h index 6ba6428d5f..defcb52416 100644 --- a/libraries/AP_GPS/AP_GPS_NOVA.h +++ b/libraries/AP_GPS/AP_GPS_NOVA.h @@ -22,6 +22,11 @@ #include "AP_GPS.h" #include "GPS_Backend.h" +#ifndef AP_GPS_NOVA_ENABLED + #define AP_GPS_NOVA_ENABLED 1 +#endif + +#if AP_GPS_NOVA_ENABLED class AP_GPS_NOVA : public AP_GPS_Backend { public: @@ -175,3 +180,4 @@ private: uint16_t read; } nova_msg; }; +#endif diff --git a/libraries/AP_GPS/AP_GPS_SBF.cpp b/libraries/AP_GPS/AP_GPS_SBF.cpp index 7b56827e23..bf4af75b9b 100644 --- a/libraries/AP_GPS/AP_GPS_SBF.cpp +++ b/libraries/AP_GPS/AP_GPS_SBF.cpp @@ -25,6 +25,7 @@ #include #include +#if AP_GPS_SBF_ENABLED extern const AP_HAL::HAL& hal; #define SBF_DEBUGGING 0 @@ -614,3 +615,4 @@ bool AP_GPS_SBF::prepare_for_arming(void) { return is_logging; } +#endif diff --git a/libraries/AP_GPS/AP_GPS_SBF.h b/libraries/AP_GPS/AP_GPS_SBF.h index e80255daad..6812b7a126 100644 --- a/libraries/AP_GPS/AP_GPS_SBF.h +++ b/libraries/AP_GPS/AP_GPS_SBF.h @@ -22,6 +22,12 @@ #include "AP_GPS.h" #include "GPS_Backend.h" +#ifndef AP_GPS_SBF_ENABLED + #define AP_GPS_SBF_ENABLED 1 +#endif + +#if AP_GPS_SBF_ENABLED + #define SBF_DISK_ACTIVITY (1 << 7) #define SBF_DISK_FULL (1 << 8) #define SBF_DISK_MOUNTED (1 << 9) @@ -262,3 +268,4 @@ private: uint8_t portLength; bool readyForCommand; }; +#endif diff --git a/libraries/AP_GPS/AP_GPS_SBP.cpp b/libraries/AP_GPS/AP_GPS_SBP.cpp index 590f2ef3bb..ae08e4f3f4 100644 --- a/libraries/AP_GPS/AP_GPS_SBP.cpp +++ b/libraries/AP_GPS/AP_GPS_SBP.cpp @@ -20,10 +20,13 @@ // Swift Binary Protocol format: http://docs.swift-nav.com/ // + #include "AP_GPS.h" #include "AP_GPS_SBP.h" #include +#if AP_GPS_SBP_ENABLED + extern const AP_HAL::HAL& hal; #define SBP_DEBUGGING 1 @@ -456,3 +459,4 @@ AP_GPS_SBP::logging_log_raw_sbp(uint16_t msg_type, }; #endif // SBP_HW_LOGGING +#endif // AP_GPS_SBP_ENABLED diff --git a/libraries/AP_GPS/AP_GPS_SBP.h b/libraries/AP_GPS/AP_GPS_SBP.h index 1c37b18d4c..10d6dcae72 100644 --- a/libraries/AP_GPS/AP_GPS_SBP.h +++ b/libraries/AP_GPS/AP_GPS_SBP.h @@ -24,6 +24,11 @@ #include "AP_GPS.h" #include "GPS_Backend.h" +#ifndef AP_GPS_SBP_ENABLED + #define AP_GPS_SBP_ENABLED 1 +#endif + +#if AP_GPS_SBP_ENABLED class AP_GPS_SBP : public AP_GPS_Backend { public: @@ -185,3 +190,4 @@ private: }; +#endif diff --git a/libraries/AP_GPS/AP_GPS_SBP2.cpp b/libraries/AP_GPS/AP_GPS_SBP2.cpp index f3da3dee92..14ed7695a7 100644 --- a/libraries/AP_GPS/AP_GPS_SBP2.cpp +++ b/libraries/AP_GPS/AP_GPS_SBP2.cpp @@ -20,12 +20,15 @@ // Swift Binary Protocol format: http://docs.swift-nav.com/ // + #include "AP_GPS.h" #include "AP_GPS_SBP2.h" #include #include #include +#if AP_GPS_SBP2_ENABLED + extern const AP_HAL::HAL& hal; #define SBP_DEBUGGING 0 @@ -522,3 +525,4 @@ AP_GPS_SBP2::logging_ext_event() { }; AP::logger().WriteBlock(&pkt, sizeof(pkt)); }; +#endif //AP_GPS_SBP2_ENABLED diff --git a/libraries/AP_GPS/AP_GPS_SBP2.h b/libraries/AP_GPS/AP_GPS_SBP2.h index 676b76bce3..d48fe555f0 100644 --- a/libraries/AP_GPS/AP_GPS_SBP2.h +++ b/libraries/AP_GPS/AP_GPS_SBP2.h @@ -24,6 +24,11 @@ #include "AP_GPS.h" #include "GPS_Backend.h" +#ifndef AP_GPS_SBP2_ENABLED + #define AP_GPS_SBP2_ENABLED 1 +#endif + +#if AP_GPS_SBP2_ENABLED class AP_GPS_SBP2 : public AP_GPS_Backend { public: @@ -201,4 +206,4 @@ private: int32_t distMod(int32_t tow1_ms, int32_t tow2_ms, int32_t mod); }; - +#endif diff --git a/libraries/AP_GPS/AP_GPS_SIRF.cpp b/libraries/AP_GPS/AP_GPS_SIRF.cpp index ef1e4b4559..18f42155c7 100644 --- a/libraries/AP_GPS/AP_GPS_SIRF.cpp +++ b/libraries/AP_GPS/AP_GPS_SIRF.cpp @@ -21,6 +21,7 @@ #include "AP_GPS_SIRF.h" #include +#if AP_GPS_SIRF_ENABLED // Initialisation messages // // Turn off all messages except for 0x29. @@ -245,3 +246,4 @@ bool AP_GPS_SIRF::_detect(struct SIRF_detect_state &state, uint8_t data) } return false; } +#endif diff --git a/libraries/AP_GPS/AP_GPS_SIRF.h b/libraries/AP_GPS/AP_GPS_SIRF.h index 297f2025d0..4aa8696909 100644 --- a/libraries/AP_GPS/AP_GPS_SIRF.h +++ b/libraries/AP_GPS/AP_GPS_SIRF.h @@ -25,6 +25,12 @@ #include "AP_GPS.h" #include "GPS_Backend.h" +#ifndef AP_GPS_SIRF_ENABLED + #define AP_GPS_SIRF_ENABLED 1 +#endif + +#if AP_GPS_SIRF_ENABLED + #define SIRF_SET_BINARY "$PSRF100,0,38400,8,1,0*3C\r\n" class AP_GPS_SIRF : public AP_GPS_Backend { @@ -107,3 +113,4 @@ private: static const uint8_t _initialisation_blob[]; }; +#endif