From bd04f15f3234cc08eaecf5bf70a91203a9203ddf Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Mon, 30 May 2022 10:24:09 +1200 Subject: [PATCH 1/2] telemetry: sys_status for position health flags With this commit we start to use the sys_status flags to determine whether local and global position are ok. If the flags don't appear to be set, we fall back to counting GPS satellites like before. --- .../plugins/telemetry/telemetry_impl.cpp | 54 ++++++++++++++++--- src/mavsdk/plugins/telemetry/telemetry_impl.h | 10 ++++ 2 files changed, 56 insertions(+), 8 deletions(-) diff --git a/src/mavsdk/plugins/telemetry/telemetry_impl.cpp b/src/mavsdk/plugins/telemetry/telemetry_impl.cpp index 2abe1c9163..cfe5f71213 100644 --- a/src/mavsdk/plugins/telemetry/telemetry_impl.cpp +++ b/src/mavsdk/plugins/telemetry/telemetry_impl.cpp @@ -177,6 +177,8 @@ void TelemetryImpl::deinit() std::lock_guard lock(_ap_calibration_mutex); _ap_calibration = {}; } + + _sys_status_used_for_position = SysStatusUsed::Unknown; } void TelemetryImpl::enable() @@ -978,12 +980,12 @@ void TelemetryImpl::process_gps_raw_int(const mavlink_message_t& message) raw_gps_info.yaw_deg = static_cast(gps_raw_int.yaw) * 1e-2f; set_raw_gps(raw_gps_info); - // TODO: This is just an interim hack, we will have to look at - // estimator flags in order to decide if the position - // estimate is good enough. - const bool gps_ok = ((gps_raw_int.fix_type >= 3) && (gps_raw_int.satellites_visible >= 8)); + if (_sys_status_used_for_position == SysStatusUsed::No) { + // This is just a fallback if sys_status does not contain the appropriate flags yet. + const bool gps_ok = ((gps_raw_int.fix_type >= 3) && (gps_raw_int.satellites_visible >= 8)); - set_health_global_position(gps_ok); + set_health_global_position(gps_ok); + } { std::lock_guard lock(_subscription_mutex); @@ -1132,6 +1134,32 @@ void TelemetryImpl::process_sys_status(const mavlink_message_t& message) // If the flag is not supported yet, we fall back to the param. } + const bool global_position_ok = + sys_status_present_enabled_health(sys_status, MAV_SYS_STATUS_SENSOR_GPS); + + // FIXME: There is nothing really set in PX4 for local position from what I can tell, + // so the best we can do for now is to set it based on GPS as a fallback. + + const bool local_position_ok = + global_position_ok || + sys_status_present_enabled_health(sys_status, MAV_SYS_STATUS_SENSOR_OPTICAL_FLOW) || + sys_status_present_enabled_health(sys_status, MAV_SYS_STATUS_SENSOR_VISION_POSITION); + + set_health_local_position(local_position_ok); + set_health_global_position(global_position_ok); + + // If any of these sensors were marked present, we don't have to fall back to check for + // satellite count. + _sys_status_used_for_position = + ((sys_status.onboard_control_sensors_present & MAV_SYS_STATUS_SENSOR_GPS) != 0 || + (sys_status.onboard_control_sensors_present & MAV_SYS_STATUS_SENSOR_OPTICAL_FLOW) != 0 || + (sys_status.onboard_control_sensors_present & MAV_SYS_STATUS_SENSOR_VISION_POSITION) != + 0) ? + SysStatusUsed::Yes : + SysStatusUsed::No; + + set_rc_status({rc_ok}, std::nullopt); + std::lock_guard lock(_subscription_mutex); if (_rc_status_subscription) { auto callback = _rc_status_subscription; @@ -1148,6 +1176,14 @@ void TelemetryImpl::process_sys_status(const mavlink_message_t& message) } } +bool TelemetryImpl::sys_status_present_enabled_health( + const mavlink_sys_status_t& sys_status, MAV_SYS_STATUS_SENSOR flag) +{ + return (sys_status.onboard_control_sensors_present & flag) != 0 && + (sys_status.onboard_control_sensors_enabled & flag) != 0 && + (sys_status.onboard_control_sensors_health & flag) != 0; +} + void TelemetryImpl::process_battery_status(const mavlink_message_t& message) { mavlink_battery_status_t bat_status; @@ -1703,9 +1739,11 @@ void TelemetryImpl::receive_param_hitl(MAVLinkParameters::Result result, int val void TelemetryImpl::receive_gps_raw_timeout() { - const bool position_ok = false; - set_health_local_position(position_ok); - set_health_global_position(position_ok); + if (_sys_status_used_for_position == SysStatusUsed::No) { + const bool position_ok = false; + set_health_local_position(position_ok); + set_health_global_position(position_ok); + } } void TelemetryImpl::receive_unix_epoch_timeout() diff --git a/src/mavsdk/plugins/telemetry/telemetry_impl.h b/src/mavsdk/plugins/telemetry/telemetry_impl.h index 6ee4ccfe20..431bb3c1ec 100644 --- a/src/mavsdk/plugins/telemetry/telemetry_impl.h +++ b/src/mavsdk/plugins/telemetry/telemetry_impl.h @@ -236,6 +236,9 @@ class TelemetryImpl : public PluginImplBase { void request_home_position_async(); void check_calibration(); + static bool sys_status_present_enabled_health( + const mavlink_sys_status_t& sys_status, MAV_SYS_STATUS_SENSOR flag); + static Telemetry::Result telemetry_result_from_command_result(MavlinkCommandSender::Result command_result); @@ -416,5 +419,12 @@ class TelemetryImpl : public PluginImplBase { OffsetStatus gyro_offset; } _ap_calibration{}; + + enum class SysStatusUsed { + Unknown, + Yes, + No, + }; + std::atomic _sys_status_used_for_position{SysStatusUsed::Unknown}; }; } // namespace mavsdk From 7cb968b554d1a7b2e04331ee9588cffd19e31d68 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 1 Jun 2022 12:55:51 +1200 Subject: [PATCH 2/2] telemetry: workaround for PX4 For some reason PX4 doesn't set enabled for GPS, sadly. --- src/mavsdk/plugins/telemetry/telemetry_impl.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/mavsdk/plugins/telemetry/telemetry_impl.cpp b/src/mavsdk/plugins/telemetry/telemetry_impl.cpp index cfe5f71213..c933a76910 100644 --- a/src/mavsdk/plugins/telemetry/telemetry_impl.cpp +++ b/src/mavsdk/plugins/telemetry/telemetry_impl.cpp @@ -1179,8 +1179,9 @@ void TelemetryImpl::process_sys_status(const mavlink_message_t& message) bool TelemetryImpl::sys_status_present_enabled_health( const mavlink_sys_status_t& sys_status, MAV_SYS_STATUS_SENSOR flag) { + // FIXME: it doesn't look like PX4 sets enabled for GPS return (sys_status.onboard_control_sensors_present & flag) != 0 && - (sys_status.onboard_control_sensors_enabled & flag) != 0 && + // (sys_status.onboard_control_sensors_enabled & flag) != 0 && (sys_status.onboard_control_sensors_health & flag) != 0; }