diff --git a/AntennaTracker/GCS_Mavlink.cpp b/AntennaTracker/GCS_Mavlink.cpp index 31c1148db9ea8..93e6638304440 100644 --- a/AntennaTracker/GCS_Mavlink.cpp +++ b/AntennaTracker/GCS_Mavlink.cpp @@ -276,10 +276,16 @@ static const ap_message STREAM_EXTENDED_STATUS_msgs[] = { #endif MSG_MEMINFO, MSG_NAV_CONTROLLER_OUTPUT, +#if AP_GPS_GPS_RAW_INT_SENDING_ENABLED MSG_GPS_RAW, +#endif +#if AP_GPS_GPS_RTK_SENDING_ENABLED MSG_GPS_RTK, -#if GPS_MAX_RECEIVERS > 1 +#endif +#if AP_GPS_GPS2_RAW_SENDING_ENABLED MSG_GPS2_RAW, +#endif +#if AP_GPS_GPS2_RTK_SENDING_ENABLED MSG_GPS2_RTK, #endif }; diff --git a/ArduCopter/GCS_Mavlink.cpp b/ArduCopter/GCS_Mavlink.cpp index 6c2d169db8af6..c07b4807a2e09 100644 --- a/ArduCopter/GCS_Mavlink.cpp +++ b/ArduCopter/GCS_Mavlink.cpp @@ -505,10 +505,16 @@ static const ap_message STREAM_EXTENDED_STATUS_msgs[] = { #endif MSG_MEMINFO, MSG_CURRENT_WAYPOINT, // MISSION_CURRENT +#if AP_GPS_GPS_RAW_INT_SENDING_ENABLED MSG_GPS_RAW, +#endif +#if AP_GPS_GPS_RTK_SENDING_ENABLED MSG_GPS_RTK, -#if GPS_MAX_RECEIVERS > 1 +#endif +#if AP_GPS_GPS2_RAW_SENDING_ENABLED MSG_GPS2_RAW, +#endif +#if AP_GPS_GPS2_RTK_SENDING_ENABLED MSG_GPS2_RTK, #endif MSG_NAV_CONTROLLER_OUTPUT, diff --git a/ArduPlane/GCS_Mavlink.cpp b/ArduPlane/GCS_Mavlink.cpp index 9256fdfe01cb3..b733f7b7bff13 100644 --- a/ArduPlane/GCS_Mavlink.cpp +++ b/ArduPlane/GCS_Mavlink.cpp @@ -630,10 +630,16 @@ static const ap_message STREAM_EXTENDED_STATUS_msgs[] = { #endif MSG_MEMINFO, MSG_CURRENT_WAYPOINT, +#if AP_GPS_GPS_RAW_INT_SENDING_ENABLED MSG_GPS_RAW, +#endif +#if AP_GPS_GPS_RTK_SENDING_ENABLED MSG_GPS_RTK, -#if GPS_MAX_RECEIVERS > 1 +#endif +#if AP_GPS_GPS2_RAW_SENDING_ENABLED MSG_GPS2_RAW, +#endif +#if AP_GPS_GPS2_RTK_SENDING_ENABLED MSG_GPS2_RTK, #endif MSG_NAV_CONTROLLER_OUTPUT, diff --git a/ArduSub/GCS_Mavlink.cpp b/ArduSub/GCS_Mavlink.cpp index 0d0bad47ebd89..c2bbf8b5c42bb 100644 --- a/ArduSub/GCS_Mavlink.cpp +++ b/ArduSub/GCS_Mavlink.cpp @@ -369,10 +369,16 @@ static const ap_message STREAM_EXTENDED_STATUS_msgs[] = { #endif MSG_MEMINFO, MSG_CURRENT_WAYPOINT, +#if AP_GPS_GPS_RAW_INT_SENDING_ENABLED MSG_GPS_RAW, +#endif +#if AP_GPS_GPS_RTK_SENDING_ENABLED MSG_GPS_RTK, -#if GPS_MAX_RECEIVERS > 1 +#endif +#if AP_GPS_GPS2_RAW_SENDING_ENABLED MSG_GPS2_RAW, +#endif +#if AP_GPS_GPS2_RTK_SENDING_ENABLED MSG_GPS2_RTK, #endif MSG_NAV_CONTROLLER_OUTPUT, diff --git a/Blimp/GCS_Mavlink.cpp b/Blimp/GCS_Mavlink.cpp index b132d80160a5c..4d91191a02863 100644 --- a/Blimp/GCS_Mavlink.cpp +++ b/Blimp/GCS_Mavlink.cpp @@ -327,10 +327,16 @@ static const ap_message STREAM_EXTENDED_STATUS_msgs[] = { #endif MSG_MEMINFO, MSG_CURRENT_WAYPOINT, // MISSION_CURRENT +#if AP_GPS_GPS_RAW_INT_SENDING_ENABLED MSG_GPS_RAW, +#endif +#if AP_GPS_GPS_RTK_SENDING_ENABLED MSG_GPS_RTK, -#if GPS_MAX_RECEIVERS > 1 +#endif +#if AP_GPS_GPS2_RAW_SENDING_ENABLED MSG_GPS2_RAW, +#endif +#if AP_GPS_GPS2_RTK_SENDING_ENABLED MSG_GPS2_RTK, #endif MSG_NAV_CONTROLLER_OUTPUT, diff --git a/Rover/GCS_Mavlink.cpp b/Rover/GCS_Mavlink.cpp index 6a39311eade23..3704110ec9e2f 100644 --- a/Rover/GCS_Mavlink.cpp +++ b/Rover/GCS_Mavlink.cpp @@ -541,10 +541,16 @@ static const ap_message STREAM_EXTENDED_STATUS_msgs[] = { #endif MSG_MEMINFO, MSG_CURRENT_WAYPOINT, +#if AP_GPS_GPS_RAW_INT_SENDING_ENABLED MSG_GPS_RAW, +#endif +#if AP_GPS_GPS_RTK_SENDING_ENABLED MSG_GPS_RTK, -#if GPS_MAX_RECEIVERS > 1 +#endif +#if AP_GPS_GPS2_RAW_SENDING_ENABLED MSG_GPS2_RAW, +#endif +#if AP_GPS_GPS2_RTK_SENDING_ENABLED MSG_GPS2_RTK, #endif MSG_NAV_CONTROLLER_OUTPUT, diff --git a/Tools/AP_Periph/GCS_MAVLink.cpp b/Tools/AP_Periph/GCS_MAVLink.cpp index a3df4631fef87..f35ee3a580c8c 100644 --- a/Tools/AP_Periph/GCS_MAVLink.cpp +++ b/Tools/AP_Periph/GCS_MAVLink.cpp @@ -32,8 +32,10 @@ static const ap_message STREAM_EXTENDED_STATUS_msgs[] = { MSG_MCU_STATUS, #endif MSG_MEMINFO, -#if AP_GPS_ENABLED +#if AP_GPS_GPS_RAW_INT_SENDING_ENABLED MSG_GPS_RAW, +#endif +#if AP_GPS_GPS_RTK_SENDING_ENABLED MSG_GPS_RTK, #endif }; diff --git a/libraries/AP_GPS/AP_GPS.cpp b/libraries/AP_GPS/AP_GPS.cpp index 070276dd45c9f..79a69b88b2962 100644 --- a/libraries/AP_GPS/AP_GPS.cpp +++ b/libraries/AP_GPS/AP_GPS.cpp @@ -1367,6 +1367,7 @@ uint16_t AP_GPS::gps_yaw_cdeg(uint8_t instance) const return yaw_cd; } +#if AP_GPS_GPS_RAW_INT_SENDING_ENABLED void AP_GPS::send_mavlink_gps_raw(mavlink_channel_t chan) { const Location &loc = location(0); @@ -1400,8 +1401,9 @@ void AP_GPS::send_mavlink_gps_raw(mavlink_channel_t chan) 0, // TODO one-sigma heading accuracy standard deviation gps_yaw_cdeg(0)); } +#endif // AP_GPS_GPS_RAW_INT_SENDING_ENABLED -#if GPS_MAX_RECEIVERS > 1 +#if AP_GPS_GPS2_RAW_SENDING_ENABLED void AP_GPS::send_mavlink_gps2_raw(mavlink_channel_t chan) { // always send the message if 2nd GPS is configured @@ -1442,9 +1444,9 @@ void AP_GPS::send_mavlink_gps2_raw(mavlink_channel_t chan) sacc * 1000, // one-sigma standard deviation in mm/s 0); // TODO one-sigma heading accuracy standard deviation } -#endif // GPS_MAX_RECEIVERS +#endif // AP_GPS_GPS2_RAW_SENDING_ENABLED -#if HAL_GCS_ENABLED +#if AP_GPS_GPS_RTK_SENDING_ENABLED || AP_GPS_GPS2_RTK_SENDING_ENABLED void AP_GPS::send_mavlink_gps_rtk(mavlink_channel_t chan, uint8_t inst) { if (inst >= GPS_MAX_RECEIVERS) { diff --git a/libraries/AP_GPS/AP_GPS_config.h b/libraries/AP_GPS/AP_GPS_config.h index 08ebdac605938..550add0e1fa62 100644 --- a/libraries/AP_GPS/AP_GPS_config.h +++ b/libraries/AP_GPS/AP_GPS_config.h @@ -104,3 +104,20 @@ #ifndef HAL_GPS_COM_PORT_DEFAULT #define HAL_GPS_COM_PORT_DEFAULT 1 #endif + + +#ifndef AP_GPS_GPS_RAW_INT_SENDING_ENABLED +#define AP_GPS_GPS_RAW_INT_SENDING_ENABLED HAL_GCS_ENABLED && AP_GPS_ENABLED +#endif + +#ifndef AP_GPS_GPS2_RAW_SENDING_ENABLED +#define AP_GPS_GPS2_RAW_SENDING_ENABLED HAL_GCS_ENABLED && AP_GPS_ENABLED && GPS_MAX_RECEIVERS > 1 +#endif + +#ifndef AP_GPS_GPS_RTK_SENDING_ENABLED +#define AP_GPS_GPS_RTK_SENDING_ENABLED HAL_GCS_ENABLED && AP_GPS_ENABLED && (AP_GPS_SBF_ENABLED || AP_GPS_ERB_ENABLED) +#endif + +#ifndef AP_GPS_GPS2_RTK_SENDING_ENABLED +#define AP_GPS_GPS2_RTK_SENDING_ENABLED HAL_GCS_ENABLED && AP_GPS_ENABLED && GPS_MAX_RECEIVERS > 1 && (AP_GPS_SBF_ENABLED || AP_GPS_ERB_ENABLED) +#endif diff --git a/libraries/AP_GPS/GPS_Backend.cpp b/libraries/AP_GPS/GPS_Backend.cpp index eb55bd76e433d..380730a9af679 100644 --- a/libraries/AP_GPS/GPS_Backend.cpp +++ b/libraries/AP_GPS/GPS_Backend.cpp @@ -172,7 +172,7 @@ bool AP_GPS_Backend::should_log() const #endif -#if HAL_GCS_ENABLED +#if AP_GPS_GPS_RTK_SENDING_ENABLED || AP_GPS_GPS2_RTK_SENDING_ENABLED void AP_GPS_Backend::send_mavlink_gps_rtk(mavlink_channel_t chan) { const uint8_t instance = state.instance; @@ -212,7 +212,8 @@ void AP_GPS_Backend::send_mavlink_gps_rtk(mavlink_channel_t chan) break; } } -#endif +#endif // AP_GPS_GPS_RTK_SENDING_ENABLED || AP_GPS_GPS2_RTK_SENDING_ENABLED + /* diff --git a/libraries/AP_GPS/GPS_Backend.h b/libraries/AP_GPS/GPS_Backend.h index 70f4cb36fc215..2a70a1c57153e 100644 --- a/libraries/AP_GPS/GPS_Backend.h +++ b/libraries/AP_GPS/GPS_Backend.h @@ -71,7 +71,9 @@ class AP_GPS_Backend #if HAL_GCS_ENABLED //MAVLink methods virtual bool supports_mavlink_gps_rtk_message() const { return false; } +#if AP_GPS_GPS_RTK_SENDING_ENABLED || AP_GPS_GPS2_RTK_SENDING_ENABLED virtual void send_mavlink_gps_rtk(mavlink_channel_t chan); +#endif virtual void handle_msg(const mavlink_message_t &msg) { return ; } #endif diff --git a/libraries/GCS_MAVLink/GCS_Common.cpp b/libraries/GCS_MAVLink/GCS_Common.cpp index 9a1528b9dca4e..69598bf6959cc 100644 --- a/libraries/GCS_MAVLink/GCS_Common.cpp +++ b/libraries/GCS_MAVLink/GCS_Common.cpp @@ -1030,13 +1030,17 @@ ap_message GCS_MAVLINK::mavlink_id_to_ap_message_id(const uint32_t mavlink_id) c { MAVLINK_MSG_ID_SCALED_PRESSURE, MSG_SCALED_PRESSURE}, { MAVLINK_MSG_ID_SCALED_PRESSURE2, MSG_SCALED_PRESSURE2}, { MAVLINK_MSG_ID_SCALED_PRESSURE3, MSG_SCALED_PRESSURE3}, -#if AP_GPS_ENABLED +#if AP_GPS_GPS_RAW_INT_SENDING_ENABLED { MAVLINK_MSG_ID_GPS_RAW_INT, MSG_GPS_RAW}, +#endif +#if AP_GPS_GPS_RTK_SENDING_ENABLED { MAVLINK_MSG_ID_GPS_RTK, MSG_GPS_RTK}, -#if GPS_MAX_RECEIVERS > 1 +#endif +#if AP_GPS_GPS2_RAW_SENDING_ENABLED { MAVLINK_MSG_ID_GPS2_RAW, MSG_GPS2_RAW}, - { MAVLINK_MSG_ID_GPS2_RTK, MSG_GPS2_RTK}, #endif +#if AP_GPS_GPS2_RTK_SENDING_ENABLED + { MAVLINK_MSG_ID_GPS2_RTK, MSG_GPS2_RTK}, #endif { MAVLINK_MSG_ID_SYSTEM_TIME, MSG_SYSTEM_TIME}, { MAVLINK_MSG_ID_RC_CHANNELS_SCALED, MSG_SERVO_OUT}, @@ -6237,28 +6241,32 @@ bool GCS_MAVLINK::try_send_message(const enum ap_message id) CHECK_PAYLOAD_SIZE(SYSTEM_TIME); send_system_time(); break; -#if AP_GPS_ENABLED + +#if AP_GPS_GPS_RAW_INT_SENDING_ENABLED case MSG_GPS_RAW: CHECK_PAYLOAD_SIZE(GPS_RAW_INT); AP::gps().send_mavlink_gps_raw(chan); break; +#endif // AP_GPS_GPS_RAW_INT_SENDING_ENABLED +#if AP_GPS_GPS_RTK_SENDING_ENABLED case MSG_GPS_RTK: CHECK_PAYLOAD_SIZE(GPS_RTK); AP::gps().send_mavlink_gps_rtk(chan, 0); break; -#if GPS_MAX_RECEIVERS > 1 +#endif // AP_GPS_GPS_RTK_SENDING_ENABLED +#if AP_GPS_GPS2_RAW_SENDING_ENABLED case MSG_GPS2_RAW: CHECK_PAYLOAD_SIZE(GPS2_RAW); AP::gps().send_mavlink_gps2_raw(chan); break; -#endif -#if GPS_MAX_RECEIVERS > 1 +#endif // AP_GPS_GPS2_RAW_SENDING_ENABLED +#if AP_GPS_GPS2_RTK_SENDING_ENABLED case MSG_GPS2_RTK: CHECK_PAYLOAD_SIZE(GPS2_RTK); AP::gps().send_mavlink_gps_rtk(chan, 1); break; -#endif -#endif // AP_GPS_ENABLED +#endif // AP_GPS_GPS2_RTK_SENDING_ENABLED + #if AP_AHRS_ENABLED case MSG_LOCAL_POSITION: CHECK_PAYLOAD_SIZE(LOCAL_POSITION_NED);