zrzk
/
px4_autopilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

rtl: switch to events

master
Beat Küng 4 years ago committed by Daniel Agar
parent
59f359f7dc
commit
ab701ae502
1 changed files with 33 additions and 12 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 45
      src/modules/navigator/rtl.cpp

45
src/modules/navigator/rtl.cpp
Unescape 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;
} }

Write Preview
Loading…
Cancel
Save