Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-30 04:06:33 +08:00
Code Issues Actions 9 Packages Projects Releases Wiki Activity

rtl: switch to events

Browse Source
This commit is contained in:
Beat Küng
2021-04-21 11:04:19 +02:00
committed by Daniel Agar
parent 59f359f7dc
commit ab701ae502
1 changed files with 33 additions and 12 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/navigator/rtl.cpp
+33 -12
View File
@@ -43,6 +43,7 @@
#include "rtl.h" #include "rtl.h"
#include "navigator.h" #include "navigator.h"
#include <dataman/dataman.h> #include <dataman/dataman.h>
#include <px4_platform_common/events.h>
#include <lib/ecl/geo/geo.h> #include <lib/ecl/geo/geo.h>
@@ -210,7 +211,9 @@ void RTL::find_RTL_destination()
break; break;
default: 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<uint8_t>(events::ID("rtl_unsupported_mav_frame"), events::Log::Error, "RTL: unsupported MAV_FRAME ({1})",
closest_safe_point.frame);
break; break;
} }
} }
@@ -243,15 +246,18 @@ void RTL::on_activation()
// output the correct message, depending on where the RTL destination is // output the correct message, depending on where the RTL destination is
switch (_destination.type) { switch (_destination.type) {
case RTL_DESTINATION_HOME: 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; break;
case RTL_DESTINATION_MISSION_LANDING: 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; break;
case RTL_DESTINATION_SAFE_POINT: 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; break;
} }
@@ -318,12 +324,15 @@ void RTL::set_rtl_item()
if (_destination.type == RTL_DESTINATION_MISSION_LANDING) { if (_destination.type == RTL_DESTINATION_MISSION_LANDING) {
if (_rtl_state > RTL_STATE_RETURN) { if (_rtl_state > RTL_STATE_RETURN) {
if (_navigator->start_mission_landing()) { 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; return;
} else { } else {
// Otherwise use regular RTL. // 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.autocontinue = true;
_mission_item.origin = ORIGIN_ONBOARD; _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)); (int)ceilf(_rtl_alt), (int)ceilf(_rtl_alt - _destination.alt));
events::send<int32_t, int32_t>(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; break;
} }
@@ -388,8 +400,11 @@ void RTL::set_rtl_item()
_mission_item.autocontinue = true; _mission_item.autocontinue = true;
_mission_item.origin = ORIGIN_ONBOARD; _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)); (int)ceilf(_mission_item.altitude), (int)ceilf(_mission_item.altitude - _destination.alt));
events::send<int32_t, int32_t>(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; break;
} }
@@ -424,8 +439,11 @@ void RTL::set_rtl_item()
// Disable previous setpoint to prevent drift. // Disable previous setpoint to prevent drift.
pos_sp_triplet->previous.valid = false; 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)); (int)ceilf(_mission_item.altitude), (int)ceilf(_mission_item.altitude - _destination.alt));
events::send<int32_t, int32_t>(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; break;
} }
@@ -448,12 +466,14 @@ void RTL::set_rtl_item()
if (autoland) { if (autoland) {
_mission_item.nav_cmd = NAV_CMD_LOITER_TIME_LIMIT; _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)); (double)get_time_inside(_mission_item));
events::send<float>(events::ID("rtl_loiter"), events::Log::Info, "RTL: loiter {1:.1}s", get_time_inside(_mission_item));
} else { } else {
_mission_item.nav_cmd = NAV_CMD_LOITER_UNLIMITED; _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; break;
@@ -517,7 +537,8 @@ void RTL::set_rtl_item()
_navigator->get_precland()->on_activation(); _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; break;
} }