From ab701ae5024850fdad2ef41b6d1505f98deab621 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Beat=20K=C3=BCng?= Date: Wed, 21 Apr 2021 11:04:19 +0200 Subject: [PATCH] rtl: switch to events --- src/modules/navigator/rtl.cpp | 45 +++++++++++++++++++++++++---------- 1 file changed, 33 insertions(+), 12 deletions(-) diff --git a/src/modules/navigator/rtl.cpp b/src/modules/navigator/rtl.cpp index f10dd8f951..63b9e54039 100644 --- a/src/modules/navigator/rtl.cpp +++ b/src/modules/navigator/rtl.cpp @@ -43,6 +43,7 @@ #include "rtl.h" #include "navigator.h" #include +#include #include @@ -210,7 +211,9 @@ void RTL::find_RTL_destination() break; default: - mavlink_log_critical(_navigator->get_mavlink_log_pub(), "RTL: unsupported MAV_FRAME"); + mavlink_log_critical(_navigator->get_mavlink_log_pub(), "RTL: unsupported MAV_FRAME\t"); + events::send(events::ID("rtl_unsupported_mav_frame"), events::Log::Error, "RTL: unsupported MAV_FRAME ({1})", + closest_safe_point.frame); break; } } @@ -243,15 +246,18 @@ void RTL::on_activation() // output the correct message, depending on where the RTL destination is switch (_destination.type) { case RTL_DESTINATION_HOME: - mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: landing at home position."); + mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: landing at home position.\t"); + events::send(events::ID("rtl_land_at_home"), events::Log::Info, "RTL: landing at home position"); break; case RTL_DESTINATION_MISSION_LANDING: - mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: landing at mission landing."); + mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: landing at mission landing.\t"); + events::send(events::ID("rtl_land_at_mission"), events::Log::Info, "RTL: landing at mission landing"); break; case RTL_DESTINATION_SAFE_POINT: - mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: landing at safe landing point."); + mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: landing at safe landing point.\t"); + events::send(events::ID("rtl_land_at_safe_point"), events::Log::Info, "RTL: landing at safe landing point"); break; } @@ -318,12 +324,15 @@ void RTL::set_rtl_item() if (_destination.type == RTL_DESTINATION_MISSION_LANDING) { if (_rtl_state > RTL_STATE_RETURN) { if (_navigator->start_mission_landing()) { - mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: using mission landing"); + mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: using mission landing\t"); + events::send(events::ID("rtl_using_mission_landing"), events::Log::Info, "RTL: using mission landing"); return; } else { // Otherwise use regular RTL. - mavlink_log_critical(_navigator->get_mavlink_log_pub(), "RTL: unable to use mission landing"); + mavlink_log_critical(_navigator->get_mavlink_log_pub(), "RTL: unable to use mission landing\t"); + events::send(events::ID("rtl_not_using_mission_landing"), events::Log::Error, + "RTL: unable to use mission landing, doing regular RTL"); } } } @@ -360,8 +369,11 @@ void RTL::set_rtl_item() _mission_item.autocontinue = true; _mission_item.origin = ORIGIN_ONBOARD; - mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: climb to %d m (%d m above destination)", + mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: climb to %d m (%d m above destination)\t", (int)ceilf(_rtl_alt), (int)ceilf(_rtl_alt - _destination.alt)); + events::send(events::ID("rtl_climb_to"), events::Log::Info, + "RTL: climb to {1m_v} ({2m_v} above destination)", + (int32_t)ceilf(_rtl_alt), (int32_t)ceilf(_rtl_alt - _destination.alt)); break; } @@ -388,8 +400,11 @@ void RTL::set_rtl_item() _mission_item.autocontinue = true; _mission_item.origin = ORIGIN_ONBOARD; - mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: return at %d m (%d m above destination)", + mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: return at %d m (%d m above destination)\t", (int)ceilf(_mission_item.altitude), (int)ceilf(_mission_item.altitude - _destination.alt)); + events::send(events::ID("rtl_return_at"), events::Log::Info, + "RTL: return at {1m_v} ({2m_v} above destination)", + (int32_t)ceilf(_mission_item.altitude), (int32_t)ceilf(_mission_item.altitude - _destination.alt)); break; } @@ -424,8 +439,11 @@ void RTL::set_rtl_item() // Disable previous setpoint to prevent drift. pos_sp_triplet->previous.valid = false; - mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: descend to %d m (%d m above destination)", + mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: descend to %d m (%d m above destination)\t", (int)ceilf(_mission_item.altitude), (int)ceilf(_mission_item.altitude - _destination.alt)); + events::send(events::ID("rtl_descend_to"), events::Log::Info, + "RTL: descend to {1m_v} ({2m_v} above destination)", + (int32_t)ceilf(_mission_item.altitude), (int32_t)ceilf(_mission_item.altitude - _destination.alt)); break; } @@ -448,12 +466,14 @@ void RTL::set_rtl_item() if (autoland) { _mission_item.nav_cmd = NAV_CMD_LOITER_TIME_LIMIT; - mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: loiter %.1fs", + mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: loiter %.1fs\t", (double)get_time_inside(_mission_item)); + events::send(events::ID("rtl_loiter"), events::Log::Info, "RTL: loiter {1:.1}s", get_time_inside(_mission_item)); } else { _mission_item.nav_cmd = NAV_CMD_LOITER_UNLIMITED; - mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: completed, loitering"); + mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: completed, loitering\t"); + events::send(events::ID("rtl_completed_loiter"), events::Log::Info, "RTL: completed, loitering"); } break; @@ -517,7 +537,8 @@ void RTL::set_rtl_item() _navigator->get_precland()->on_activation(); } - mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: land at destination"); + mavlink_log_info(_navigator->get_mavlink_log_pub(), "RTL: land at destination\t"); + events::send(events::ID("rtl_land_at_destination"), events::Log::Info, "RTL: land at destination"); break; }