diff --git a/libraries/AP_GPS/AP_GPS.cpp b/libraries/AP_GPS/AP_GPS.cpp index e6b1f86ed4..da8bc1319d 100644 --- a/libraries/AP_GPS/AP_GPS.cpp +++ b/libraries/AP_GPS/AP_GPS.cpp @@ -77,6 +77,7 @@ const AP_Param::GroupInfo AP_GPS::var_info[] = { // @User: Advanced AP_GROUPINFO("TYPE", 0, AP_GPS, _type[0], HAL_GPS_TYPE_DEFAULT), +#if GPS_MAX_RECEIVERS > 1 // @Param: TYPE2 // @DisplayName: 2nd GPS type // @Description: GPS type of 2nd GPS @@ -84,6 +85,7 @@ const AP_Param::GroupInfo AP_GPS::var_info[] = { // @RebootRequired: True // @User: Advanced AP_GROUPINFO("TYPE2", 1, AP_GPS, _type[1], 0), +#endif // @Param: NAVFILTER // @DisplayName: Navigation filter setting @@ -92,12 +94,14 @@ const AP_Param::GroupInfo AP_GPS::var_info[] = { // @User: Advanced AP_GROUPINFO("NAVFILTER", 2, AP_GPS, _navfilter, GPS_ENGINE_AIRBORNE_4G), +#if GPS_MAX_RECEIVERS > 1 // @Param: AUTO_SWITCH // @DisplayName: Automatic Switchover Setting // @Description: Automatic switchover to GPS reporting best lock // @Values: 0:Disabled,1:UseBest,2:Blend,3:UseSecond // @User: Advanced AP_GROUPINFO("AUTO_SWITCH", 3, AP_GPS, _auto_switch, 1), +#endif // @Param: MIN_DGPS // @DisplayName: Minimum Lock Type Accepted for DGPS @@ -159,6 +163,7 @@ const AP_Param::GroupInfo AP_GPS::var_info[] = { // @User: Advanced AP_GROUPINFO("SAVE_CFG", 11, AP_GPS, _save_config, 2), +#if GPS_MAX_RECEIVERS > 1 // @Param: GNSS_MODE2 // @DisplayName: GNSS system configuration // @Description: Bitmask for what GNSS system to use on the second GPS (all unchecked or zero to leave GPS as configured) @@ -166,6 +171,7 @@ const AP_Param::GroupInfo AP_GPS::var_info[] = { // @Bitmask: 0:GPS,1:SBAS,2:Galileo,3:Beidou,4:IMES,5:QZSS,6:GLOSNASS // @User: Advanced AP_GROUPINFO("GNSS_MODE2", 12, AP_GPS, _gnss_mode[1], 0), +#endif // @Param: AUTO_CONFIG // @DisplayName: Automatic GPS configuration @@ -183,6 +189,7 @@ const AP_Param::GroupInfo AP_GPS::var_info[] = { // @User: Advanced AP_GROUPINFO("RATE_MS", 14, AP_GPS, _rate_ms[0], 200), +#if GPS_MAX_RECEIVERS > 1 // @Param: RATE_MS2 // @DisplayName: GPS 2 update rate in milliseconds // @Description: Controls how often the GPS should provide a position update. Lowering below 5Hz is not allowed @@ -191,6 +198,7 @@ const AP_Param::GroupInfo AP_GPS::var_info[] = { // @Range: 50 200 // @User: Advanced AP_GROUPINFO("RATE_MS2", 15, AP_GPS, _rate_ms[1], 200), +#endif // @Param: POS1_X // @DisplayName: Antenna X position offset @@ -228,6 +236,7 @@ const AP_Param::GroupInfo AP_GPS::var_info[] = { // @Range: -10 10 // @User: Advanced +#if GPS_MAX_RECEIVERS > 1 // @Param: POS2_Z // @DisplayName: Antenna Z position offset // @Description: Z position of the second GPS antenna in body frame. Positive Z is down from the origin. Use antenna phase centroid location if provided by the manufacturer. @@ -235,6 +244,7 @@ const AP_Param::GroupInfo AP_GPS::var_info[] = { // @Range: -10 10 // @User: Advanced AP_GROUPINFO("POS2", 17, AP_GPS, _antenna_offset[1], 0.0f), +#endif // @Param: DELAY_MS // @DisplayName: GPS delay in milliseconds @@ -245,6 +255,7 @@ const AP_Param::GroupInfo AP_GPS::var_info[] = { // @RebootRequired: True AP_GROUPINFO("DELAY_MS", 18, AP_GPS, _delay_ms[0], 0), +#if GPS_MAX_RECEIVERS > 1 // @Param: DELAY_MS2 // @DisplayName: GPS 2 delay in milliseconds // @Description: Controls the amount of GPS measurement delay that the autopilot compensates for. Set to zero to use the default delay for the detected GPS type. @@ -253,7 +264,9 @@ const AP_Param::GroupInfo AP_GPS::var_info[] = { // @User: Advanced // @RebootRequired: True AP_GROUPINFO("DELAY_MS2", 19, AP_GPS, _delay_ms[1], 0), +#endif +#if defined(GPS_BLENDED_INSTANCE) // @Param: BLEND_MASK // @DisplayName: Multi GPS Blending Mask // @Description: Determines which of the accuracy measures Horizontal position, Vertical Position and Speed are used to calculate the weighting on each GPS receiver when soft switching has been selected by setting GPS_AUTO_SWITCH to 2 @@ -268,6 +281,7 @@ const AP_Param::GroupInfo AP_GPS::var_info[] = { // @Range: 5.0 30.0 // @User: Advanced AP_GROUPINFO("BLEND_TC", 21, AP_GPS, _blend_tc, 10.0f), +#endif AP_GROUPEND }; @@ -292,8 +306,9 @@ void AP_GPS::init(const AP_SerialManager& serial_manager) primary_instance = 0; // search for serial ports with gps protocol - _port[0] = serial_manager.find_serial(AP_SerialManager::SerialProtocol_GPS, 0); - _port[1] = serial_manager.find_serial(AP_SerialManager::SerialProtocol_GPS, 1); + for (uint8_t i=0; iauto_detected_baud = false; // specified, not detected new_gps = new AP_GPS_MAV(*this, state[instance], nullptr); goto found_gps; +#endif break; // user has to explicitly set the UAVCAN type, do not use AUTO @@ -456,6 +473,7 @@ void AP_GPS::detect_instance(uint8_t instance) // the correct baud rate, and should have the selected baud broadcast dstate->auto_detected_baud = true; +#ifndef HAL_BUILD_AP_PERIPH switch (_type[instance]) { // by default the sbf/trimble gps outputs no data on its port, until configured. case GPS_TYPE_SBF: @@ -473,6 +491,7 @@ void AP_GPS::detect_instance(uint8_t instance) default: break; } +#endif // HAL_BUILD_AP_PERIPH if (now - dstate->last_baud_change_ms > GPS_BAUD_TIME_MS) { // try the next baud rate @@ -516,6 +535,7 @@ void AP_GPS::detect_instance(uint8_t instance) AP_GPS_UBLOX::_detect(dstate->ublox_detect_state, data)) { new_gps = new AP_GPS_UBLOX(*this, state[instance], _port[instance]); } +#ifndef HAL_BUILD_AP_PERIPH #if !HAL_MINIMIZE_FEATURES // we drop the MTK drivers when building a small build as they are so rarely used // and are surprisingly large @@ -549,6 +569,10 @@ void AP_GPS::detect_instance(uint8_t instance) AP_GPS_NMEA::_detect(dstate->nmea_detect_state, data)) { new_gps = new AP_GPS_NMEA(*this, state[instance], _port[instance]); } +#endif // HAL_BUILD_AP_PERIPH + if (new_gps) { + goto found_gps; + } } found_gps: @@ -571,6 +595,7 @@ AP_GPS::GPS_Status AP_GPS::highest_supported_status(uint8_t instance) const bool AP_GPS::should_log() const { +#ifndef HAL_BUILD_AP_PERIPH AP_Logger *logger = AP_Logger::get_singleton(); if (logger == nullptr) { return false; @@ -582,6 +607,9 @@ bool AP_GPS::should_log() const return false; } return true; +#else + return false; +#endif } @@ -664,6 +692,7 @@ void AP_GPS::update_instance(uint8_t instance) data_should_be_logged = true; } +#ifndef HAL_BUILD_AP_PERIPH if (data_should_be_logged && should_log() && !AP::ahrs().have_ekf_logging()) { @@ -676,6 +705,9 @@ void AP_GPS::update_instance(uint8_t instance) AP::rtc().set_utc_usec(now, AP_RTC::SOURCE_GPS); } } +#else + (void)data_should_be_logged; +#endif } /* @@ -694,6 +726,7 @@ void AP_GPS::update(void) } } +#if defined(GPS_BLENDED_INSTANCE) // if blending is requested, attempt to calculate weighting for each GPS if (_auto_switch == 2) { _output_is_blended = calc_blend_weights(); @@ -776,11 +809,13 @@ void AP_GPS::update(void) state[GPS_BLENDED_INSTANCE] = state[primary_instance]; _blended_antenna_offset = _antenna_offset[primary_instance]; } +#endif // GPS_BLENDED_INSTANCE +#ifndef HAL_BUILD_AP_PERIPH // update notify with gps status. We always base this on the primary_instance AP_Notify::flags.gps_status = state[primary_instance].status; AP_Notify::flags.gps_num_sats = state[primary_instance].num_sats; - +#endif } void AP_GPS::handle_gps_inject(const mavlink_message_t &msg) @@ -947,6 +982,7 @@ void AP_GPS::send_mavlink_gps_raw(mavlink_channel_t chan) 0); // TODO one-sigma heading accuracy standard deviation } +#if GPS_MAX_RECEIVERS > 1 void AP_GPS::send_mavlink_gps2_raw(mavlink_channel_t chan) { static uint32_t last_send_time_ms[MAVLINK_COMM_NUM_BUFFERS]; @@ -975,6 +1011,7 @@ void AP_GPS::send_mavlink_gps2_raw(mavlink_channel_t chan) state[1].rtk_num_sats, state[1].rtk_age_ms); } +#endif // GPS_MAX_RECEIVERS void AP_GPS::send_mavlink_gps_rtk(mavlink_channel_t chan, uint8_t inst) { @@ -1124,6 +1161,7 @@ bool AP_GPS::get_lag(uint8_t instance, float &lag_sec) const return false; } +#if defined(GPS_BLENDED_INSTANCE) // return lag of blended GPS if (instance == GPS_BLENDED_INSTANCE) { lag_sec = _blended_lag_sec; @@ -1131,6 +1169,7 @@ bool AP_GPS::get_lag(uint8_t instance, float &lag_sec) const uint8_t inst; // we don't actually care what the number is, but must pass it return first_unconfigured_gps(inst); } +#endif if (_delay_ms[instance] > 0) { // if the user has specified a non zero time delay, always return that value @@ -1158,10 +1197,12 @@ const Vector3f &AP_GPS::get_antenna_offset(uint8_t instance) const return _antenna_offset[0]; } +#if defined(GPS_BLENDED_INSTANCE) if (instance == GPS_BLENDED_INSTANCE) { // return an offset for the blended GPS solution return _blended_antenna_offset; } +#endif return _antenna_offset[instance]; } @@ -1181,6 +1222,7 @@ uint16_t AP_GPS::get_rate_ms(uint8_t instance) const return MIN(_rate_ms[instance], GPS_MAX_RATE_MS); } +#if defined(GPS_BLENDED_INSTANCE) /* calculate the weightings used to blend GPSs location and velocity data */ @@ -1550,6 +1592,7 @@ void AP_GPS::calc_blended_state(void) timing[GPS_BLENDED_INSTANCE].last_fix_time_ms = (uint32_t)temp_time_1; timing[GPS_BLENDED_INSTANCE].last_message_time_ms = (uint32_t)temp_time_2; } +#endif // GPS_BLENDED_INSTANCE bool AP_GPS::is_healthy(uint8_t instance) const { @@ -1561,9 +1604,11 @@ bool AP_GPS::is_healthy(uint8_t instance) const bool last_msg_valid = last_message_delta_time_ms(instance) < gps_max_delta_ms; +#if defined(GPS_BLENDED_INSTANCE) if (instance == GPS_BLENDED_INSTANCE) { return last_msg_valid && blend_health_check(); } +#endif return last_msg_valid && drivers[instance] != nullptr && diff --git a/libraries/AP_GPS/AP_GPS.h b/libraries/AP_GPS/AP_GPS.h index 00e9838c54..68ef226d64 100644 --- a/libraries/AP_GPS/AP_GPS.h +++ b/libraries/AP_GPS/AP_GPS.h @@ -26,9 +26,15 @@ maximum number of GPS instances available on this platform. If more than 1 then redundant sensors may be available */ +#ifndef GPS_MAX_RECEIVERS #define GPS_MAX_RECEIVERS 2 // maximum number of physical GPS sensors allowed - does not include virtual GPS created by blending receiver data +#endif +#ifndef GPS_MAX_INSTANCES #define GPS_MAX_INSTANCES (GPS_MAX_RECEIVERS + 1) // maximum number of GPS instances including the 'virtual' GPS created by blending receiver data +#endif +#if GPS_MAX_INSTANCES > GPS_MAX_RECEIVERS #define GPS_BLENDED_INSTANCE GPS_MAX_RECEIVERS // the virtual blended GPS is always the highest instance (2) +#endif #define GPS_UNKNOWN_DOP UINT16_MAX // set unknown DOP's to maximum value, which is also correct for MAVLink // the number of GPS leap seconds diff --git a/libraries/AP_GPS/AP_GPS_UBLOX.cpp b/libraries/AP_GPS/AP_GPS_UBLOX.cpp index 7fcc6164b0..2cb7daffea 100644 --- a/libraries/AP_GPS/AP_GPS_UBLOX.cpp +++ b/libraries/AP_GPS/AP_GPS_UBLOX.cpp @@ -38,6 +38,12 @@ extern const AP_HAL::HAL& hal; +#ifdef HAL_NO_GCS +#define GCS_SEND_TEXT(severity, format, args...) +#else +#define GCS_SEND_TEXT(severity, format, args...) gcs().send_text(severity, format, ##args) +#endif + #if UBLOX_DEBUGGING # define Debug(fmt, args ...) do {hal.console->printf("%s:%d: " fmt "\n", __FUNCTION__, __LINE__, ## args); hal.scheduler->delay(1); } while(0) #else @@ -503,6 +509,7 @@ AP_GPS_UBLOX::read(void) // Private Methods ///////////////////////////////////////////////////////////// void AP_GPS_UBLOX::log_mon_hw(void) { +#ifndef HAL_NO_LOGGING if (!should_log()) { return; } @@ -523,10 +530,12 @@ void AP_GPS_UBLOX::log_mon_hw(void) pkt.agcCnt = _buffer.mon_hw_68.agcCnt; } AP::logger().WriteBlock(&pkt, sizeof(pkt)); +#endif } void AP_GPS_UBLOX::log_mon_hw2(void) { +#ifndef HAL_NO_LOGGING if (!should_log()) { return; } @@ -541,11 +550,13 @@ void AP_GPS_UBLOX::log_mon_hw2(void) magQ : _buffer.mon_hw2.magQ, }; AP::logger().WriteBlock(&pkt, sizeof(pkt)); +#endif } #if UBLOX_RXM_RAW_LOGGING void AP_GPS_UBLOX::log_rxm_raw(const struct ubx_rxm_raw &raw) { +#ifndef HAL_NO_LOGGING if (!should_log()) { return; } @@ -568,10 +579,12 @@ void AP_GPS_UBLOX::log_rxm_raw(const struct ubx_rxm_raw &raw) }; AP::logger().WriteBlock(&pkt, sizeof(pkt)); } +#endif } void AP_GPS_UBLOX::log_rxm_rawx(const struct ubx_rxm_rawx &raw) { +#ifndef HAL_NO_LOGGING if (!should_log()) { return; } @@ -608,6 +621,7 @@ void AP_GPS_UBLOX::log_rxm_rawx(const struct ubx_rxm_rawx &raw) }; AP::logger().WriteBlock(&pkt, sizeof(pkt)); } +#endif } #endif // UBLOX_RXM_RAW_LOGGING @@ -872,7 +886,7 @@ AP_GPS_UBLOX::_parse_gps(void) _have_version = true; strncpy(_version.hwVersion, _buffer.mon_ver.hwVersion, sizeof(_version.hwVersion)); strncpy(_version.swVersion, _buffer.mon_ver.swVersion, sizeof(_version.swVersion)); - gcs().send_text(MAV_SEVERITY_INFO, + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "u-blox %d HW: %s SW: %s", state.instance + 1, _version.hwVersion, @@ -1044,7 +1058,7 @@ AP_GPS_UBLOX::_parse_gps(void) state.status = AP_GPS::GPS_OK_FIX_3D_RTK_FIXED; break; case 4: - gcs().send_text(MAV_SEVERITY_INFO, + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Unexpected state %d", _buffer.pvt.flags); state.status = AP_GPS::GPS_OK_FIX_3D; break; @@ -1273,7 +1287,7 @@ AP_GPS_UBLOX::_save_cfg() _send_message(CLASS_CFG, MSG_CFG_CFG, &save_cfg, sizeof(save_cfg)); _last_cfg_sent_time = AP_HAL::millis(); _num_cfg_save_tries++; - gcs().send_text(MAV_SEVERITY_INFO, + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "GPS: u-blox %d saving config", state.instance + 1); } @@ -1380,7 +1394,7 @@ void AP_GPS_UBLOX::broadcast_configuration_failure_reason(void) const { for (uint8_t i = 0; i < ARRAY_SIZE(reasons); i++) { if (_unconfigured_messages & (1 << i)) { - gcs().send_text(MAV_SEVERITY_INFO, "GPS %u: u-blox %s configuration 0x%02x", + GCS_SEND_TEXT(MAV_SEVERITY_INFO, "GPS %u: u-blox %s configuration 0x%02x", (unsigned int)(state.instance + 1), reasons[i], (unsigned int)_unconfigured_messages); break; } @@ -1420,6 +1434,7 @@ bool AP_GPS_UBLOX::get_lag(float &lag_sec) const void AP_GPS_UBLOX::Write_AP_Logger_Log_Startup_messages() const { +#ifndef HAL_NO_LOGGING AP_GPS_Backend::Write_AP_Logger_Log_Startup_messages(); if (_have_version) { @@ -1428,6 +1443,7 @@ void AP_GPS_UBLOX::Write_AP_Logger_Log_Startup_messages() const _version.hwVersion, _version.swVersion); } +#endif } // uBlox specific check_new_itow(), handling message length diff --git a/libraries/AP_GPS/GPS_Backend.cpp b/libraries/AP_GPS/GPS_Backend.cpp index 2b2228fc3b..1a64e3730f 100644 --- a/libraries/AP_GPS/GPS_Backend.cpp +++ b/libraries/AP_GPS/GPS_Backend.cpp @@ -159,16 +159,20 @@ void AP_GPS_Backend::_detection_message(char *buffer, const uint8_t buflen) cons void AP_GPS_Backend::broadcast_gps_type() const { +#ifndef HAL_NO_GCS char buffer[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN+1]; _detection_message(buffer, sizeof(buffer)); gcs().send_text(MAV_SEVERITY_INFO, "%s", buffer); +#endif } void AP_GPS_Backend::Write_AP_Logger_Log_Startup_messages() const { +#ifndef HAL_NO_LOGGING char buffer[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN+1]; _detection_message(buffer, sizeof(buffer)); AP::logger().Write_Message(buffer); +#endif } bool AP_GPS_Backend::should_log() const @@ -179,6 +183,7 @@ bool AP_GPS_Backend::should_log() const void AP_GPS_Backend::send_mavlink_gps_rtk(mavlink_channel_t chan) { +#ifndef HAL_NO_GCS const uint8_t instance = state.instance; // send status switch (instance) { @@ -215,6 +220,7 @@ void AP_GPS_Backend::send_mavlink_gps_rtk(mavlink_channel_t chan) state.rtk_iar_num_hypotheses); break; } +#endif }