15010498442

Committed 14 May 2025 01:52AM UTC coverage: 44.244% (-0.01%) from 44.257%

Build # 15010498442

Build Type

Pull #2574

github

Committed by

web-flow

Commit Message

Merge ee9539a25 into 70f24548b

Pull Request Pull Request #2574: events: unsubscribe from events

Run Details

0 of 2 new or added lines in 1 file covered. (0.0%)

4 existing lines in 3 files now uncovered.

14830 of 33519 relevant lines covered (44.24%)

279.64 hits per line

Source File
Press 'n' to go to next uncovered line, 'b' for previous

0.0

/src/mavsdk/plugins/events/events_impl.cpp


#include "events_impl.h"
#include "callback_list.tpp"
#include "unused.h"

namespace mavsdk {

EventsImpl::EventsImpl(System& system) : PluginImplBase(system)
{
    _system_impl->register_plugin(this);
}

EventsImpl::EventsImpl(std::shared_ptr<System> system) : PluginImplBase(std::move(system))
{
    _system_impl->register_plugin(this);
}

EventsImpl::~EventsImpl()
{
    _system_impl->unregister_plugin(this);
}

void EventsImpl::init()
{
    _system_impl->register_mavlink_message_handler_with_compid(
        MAVLINK_MSG_ID_HEARTBEAT,
        MAV_COMP_ID_AUTOPILOT1,
        std::bind(&EventsImpl::handle_heartbeat, this, std::placeholders::_1),
        &_heartbeat_mode_cookie);
#ifdef MAVLINK_MSG_ID_CURRENT_MODE // Defined in development.xml MAVLink dialect
    _system_impl->register_mavlink_message_handler_with_compid(
        MAVLINK_MSG_ID_CURRENT_MODE,
        MAV_COMP_ID_AUTOPILOT1,
        std::bind(&EventsImpl::handle_current_mode, this, std::placeholders::_1),
        &_current_mode_cookie);
#endif

    const std::lock_guard lg{_mutex};
    // Instantiate the autopilot component. It's not strictly required, it just makes sure to buffer
    // events until we get the metadata
    get_or_create_event_handler(MAV_COMP_ID_AUTOPILOT1);

    // Request metadata
    _subscribe_metadata_handle = _system_impl->component_metadata().subscribe_metadata_available(
        [this](MavlinkComponentMetadata::MetadataUpdate update) {
            if (update.type == MavlinkComponentMetadata::MetadataType::Events) {
                set_metadata(update.compid, update.json_metadata);
            }
        });
    _system_impl->component_metadata().request_autopilot_component();
}

void EventsImpl::deinit()
{
    _system_impl->component_metadata().unsubscribe_metadata_available(_subscribe_metadata_handle);

    if (_current_mode_cookie) {
        _system_impl->unregister_all_mavlink_message_handlers(_current_mode_cookie);
        _current_mode_cookie = nullptr;
    }
    if (_heartbeat_mode_cookie) {
        _system_impl->unregister_all_mavlink_message_handlers(_heartbeat_mode_cookie);
        _heartbeat_mode_cookie = nullptr;
    }
}

void EventsImpl::enable() {}

void EventsImpl::disable() {}

EventHandler& EventsImpl::get_or_create_event_handler(uint8_t compid)
{
    auto event_data = _events.find(compid);
    if (event_data == _events.end()) {
        // Add new component
        const std::string profile = "dev"; // Could also be "normal"

        auto event_handler = std::make_shared<EventHandler>(
            profile,
            std::bind(&EventsImpl::handle_event, this, compid, std::placeholders::_1),
            std::bind(&EventsImpl::handle_health_and_arming_check_update, this),
            *_system_impl,
            _system_impl->get_system_id(),
            compid);
        event_data = _events.insert(std::make_pair(compid, event_handler)).first;
    }
    return *event_data->second;
}

void EventsImpl::set_metadata(uint8_t compid, const std::string& metadata_json)
{
    const std::lock_guard lg{_mutex};
    auto& event_handler = get_or_create_event_handler(compid);
    event_handler.set_metadata(metadata_json);

    if (!event_handler.health_and_arming_check_results_valid() &&
        compid == MAV_COMP_ID_AUTOPILOT1) {
        // Request arming checks to be reported
        MavlinkCommandSender::CommandLong command{};
        command.command = MAV_CMD_RUN_PREARM_CHECKS;
        command.target_component_id = _system_impl->get_autopilot_id();

        _system_impl->send_command_async(
            command, [this](MavlinkCommandSender::Result result, float) {
                if (result != MavlinkCommandSender::Result::Success) {
                    LogWarn() << "command MAV_CMD_RUN_PREARM_CHECKS failed";
                }
            });
    }
}

void EventsImpl::handle_event(uint8_t compid, std::unique_ptr<events::parser::ParsedEvent> event)
{
    std::optional<Events::LogLevel> maybe_log_level =
        external_log_level(event->eventData().log_levels);

    // Handle special groups & protocols
    if (event->group() == "health" || event->group() == "arming_check") {
        // These are displayed separately
        return;
    }

    // Show message according to the log level, don't show unknown event groups (might be part of a
    // new protocol)
    if (event->group() == "default" && maybe_log_level) {
        std::string message = event->message();
        std::string description = event->description();

        if (event->type() == "append_health_and_arming_messages" && event->numArguments() > 0) {
            const uint32_t custom_mode = event->argumentValue(0).value.val_uint32_t;
            const std::shared_ptr<EventHandler>& event_handler = _events[compid];
            const auto maybe_mode_group = event_handler->get_mode_group(custom_mode);
            if (maybe_mode_group) {
                const int mode_group = maybe_mode_group.value();
                std::vector<events::HealthAndArmingChecks::Check> checks =
                    event_handler->health_and_arming_check_results().checks(mode_group);
                std::vector<std::string> message_checks;
                for (const auto& check : checks) {
                    if (external_log_level(check.log_levels).value_or(Events::LogLevel::Debug) <=
                        Events::LogLevel::Warning) {
                        message_checks.push_back(check.message);
                    }
                }
                if (message_checks.empty()) {
                    // Add all
                    for (const auto& check : checks) {
                        message_checks.push_back(check.message);
                    }
                }
                if (!message.empty() && !message_checks.empty()) {
                    message += "\n";
                }
                if (message_checks.size() == 1) {
                    message += message_checks[0];
                } else {
                    for (const auto& message_check : message_checks) {
                        message += "- " + message_check + "\n";
                    }
                }
            }
        }

        if (!message.empty()) {
            on_new_displayable_event(
                compid, std::move(event), maybe_log_level.value(), message, description);
        }
    }
}

void EventsImpl::on_new_displayable_event(
    uint8_t compid,
    std::unique_ptr<events::parser::ParsedEvent> event,
    Events::LogLevel log_level,
    const std::string& message,
    const std::string& description)
{
    // Notify subscribers
    const std::lock_guard lg{_mutex};
    _events_callbacks.queue(
        Events::Event{
            compid, message, description, log_level, event->eventNamespace(), event->name()},
        [this](const auto& func) { _system_impl->call_user_callback(func); });
}

void EventsImpl::handle_health_and_arming_check_update()
{
    // Notify subscribers
    const std::lock_guard lg{_mutex};
    const auto report = get_health_and_arming_checks_report();
    if (report.first == Events::Result::Success) {
        _health_and_arming_checks_callbacks.queue(
            report.second, [this](const auto& func) { _system_impl->call_user_callback(func); });
    }
}
std::optional<Events::LogLevel> EventsImpl::external_log_level(uint8_t log_levels)
{
    const int external_log_level = log_levels & 0xF;
    switch (external_log_level) {
        case 0:
            return Events::LogLevel::Emergency;
        case 1:
            return Events::LogLevel::Alert;
        case 2:
            return Events::LogLevel::Critical;
        case 3:
            return Events::LogLevel::Error;
        case 4:
            return Events::LogLevel::Warning;
        case 5:
            return Events::LogLevel::Notice;
        case 6:
            return Events::LogLevel::Info;
        case 7:
            return Events::LogLevel::Debug;
            // 8: Protocol
            // 9: Disabled
        default:
            break;
    }
    return std::nullopt;
}
Events::EventsHandle EventsImpl::subscribe_events(const Events::EventsCallback& callback)
{
    const std::lock_guard lg{_mutex};
    return _events_callbacks.subscribe(callback);
}
void EventsImpl::unsubscribe_events(Events::EventsHandle handle)
{
    const std::lock_guard lg{_mutex};
    _events_callbacks.unsubscribe(handle);
}
Events::HealthAndArmingChecksHandle EventsImpl::subscribe_health_and_arming_checks(
    const Events::HealthAndArmingChecksCallback& callback)
{
    const std::lock_guard lg{_mutex};
    // Run callback immediately if the report is available
    const auto report = get_health_and_arming_checks_report();
    if (report.first == Events::Result::Success) {
        _system_impl->call_user_callback([temp_callback = callback, temp_report = report.second] {
            temp_callback(temp_report);
        });
    }
    return _health_and_arming_checks_callbacks.subscribe(callback);
}
void EventsImpl::unsubscribe_health_and_arming_checks(Events::HealthAndArmingChecksHandle handle)
{
    const std::lock_guard lg{_mutex};
    _health_and_arming_checks_callbacks.unsubscribe(handle);
}
std::pair<Events::Result, Events::HealthAndArmingCheckReport>
EventsImpl::get_health_and_arming_checks_report()
{
    const auto& event_handler = get_or_create_event_handler(MAV_COMP_ID_AUTOPILOT1);
    if (!event_handler.health_and_arming_check_results_valid()) {
        return std::make_pair(Events::Result::NotAvailable, Events::HealthAndArmingCheckReport{});
    }
    const uint32_t custom_mode = _maybe_custom_mode_user_intention.value_or(_custom_mode);
    const auto maybe_current_mode_group = event_handler.get_mode_group(custom_mode);
    if (!maybe_current_mode_group) {
        LogDebug() << "Current mode not available (yet)";
        return std::make_pair(Events::Result::NotAvailable, Events::HealthAndArmingCheckReport{});
    }

    const auto& event_handler_results = event_handler.health_and_arming_check_results();
    Events::HealthAndArmingCheckReport report{};

    auto get_problems =
        [&event_handler_results](const std::vector<events::HealthAndArmingChecks::Check>& checks) {
            std::vector<Events::HealthAndArmingCheckProblem> problems;
            for (const auto& check : checks) {
                const auto maybe_log_level = external_log_level(check.log_levels);
                if (maybe_log_level) {
                    problems.push_back(Events::HealthAndArmingCheckProblem{
                        check.message,
                        check.description,
                        maybe_log_level.value(),
                        get_health_component_from_index(
                            event_handler_results, check.affected_health_component_index)});
                }
            }
            return problems;
        };

    // All problems
    report.all_problems = get_problems(event_handler_results.checks());

    // Current mode
    const int current_mode_group = maybe_current_mode_group.value();
    report.current_mode_intention.problems =
        get_problems(event_handler_results.checks(current_mode_group));
    report.current_mode_intention.can_arm_or_run =
        _system_impl->is_armed() ? event_handler_results.canRun(current_mode_group) :
                                   event_handler_results.canArm(current_mode_group);
    report.current_mode_intention.mode_name = mode_name_from_custom_mode(custom_mode);
    // We could add the results for other modes, like Takeoff or Mission, with allows to check if
    // arming is possible for that mode independent of the current mode.
    // For that, we need to get the custom_mode for these.

    // Health components
    for (const auto& [component_name, component] :
         event_handler_results.healthComponents().health_components) {
        const Events::HealthComponentReport health_component_report{
            component_name,
            component.label,
            component.health.is_present,
            component.health.error || component.arming_check.error,
            component.health.warning || component.arming_check.warning};
        report.health_components.push_back(health_component_report);
    }

    return std::make_pair(Events::Result::Success, report);
}
std::string EventsImpl::get_health_component_from_index(
    const events::HealthAndArmingChecks::Results& results, int health_component_index)
{
    if (health_component_index != 0) { // 0 == none
        for (const auto& [component_name, component] :
             results.healthComponents().health_components) {
            if (component.bitmask == (1ull << health_component_index)) {
                return component_name;
            }
        }
    }
    return "";
}
void EventsImpl::handle_current_mode(const mavlink_message_t& message)
{
    UNUSED(message);
#ifdef MAVLINK_MSG_ID_CURRENT_MODE
    mavlink_current_mode_t current_mode;
    mavlink_msg_current_mode_decode(&message, &current_mode);
    if (current_mode.intended_custom_mode != 0) { // 0 == unknown/not supplied
        const bool changed =
            _maybe_custom_mode_user_intention.value_or(0) != current_mode.intended_custom_mode;
        _maybe_custom_mode_user_intention = current_mode.intended_custom_mode;
        if (changed) {
            handle_health_and_arming_check_update();
        }
    }
#endif
}
void EventsImpl::handle_heartbeat(const mavlink_message_t& message)
{
    mavlink_heartbeat_t heartbeat;
    mavlink_msg_heartbeat_decode(&message, &heartbeat);
    if (_custom_mode != heartbeat.custom_mode) {
        _custom_mode = heartbeat.custom_mode;
        if (!_maybe_custom_mode_user_intention) {
            handle_health_and_arming_check_update();
        }
    }
}
std::string EventsImpl::mode_name_from_custom_mode(uint32_t custom_mode) const
{
    // TODO: use Standard Modes MAVLink interface
    switch (to_flight_mode_from_custom_mode(
        _system_impl->autopilot(), _system_impl->get_vehicle_type(), custom_mode)) {
        case FlightMode::FBWA:
            return "FBWA";
        case FlightMode::Autotune:
            return "Autotune";
        case FlightMode::Guided:
            return "Guided";
        case FlightMode::Ready:
            return "Ready";
        case FlightMode::Takeoff:
            return "Takeoff";
        case FlightMode::Hold:
            return "Hold";
        case FlightMode::Mission:
            return "Mission";
        case FlightMode::ReturnToLaunch:
            return "RTL";
        case FlightMode::Land:
            return "Land";
        case FlightMode::Offboard:
            return "Offboard";
        case FlightMode::FollowMe:
            return "Follow Me";
        case FlightMode::Manual:
            return "Manual";
        case FlightMode::Altctl:
            return "Altitude";
        case FlightMode::Posctl:
            return "Position";
        case FlightMode::Acro:
            return "Acro";
        case FlightMode::Rattitude:
            return "Rattitude";
        case FlightMode::Stabilized:
            return "Stabilized";
        case FlightMode::Unknown:
            break;
    }
    return "Unknown";
}
} // namespace mavsdk

1
2	#include "events_impl.h"
3	#include "callback_list.tpp"
4	#include "unused.h"
5
6	namespace mavsdk {
7
8	EventsImpl::EventsImpl(System& system) : PluginImplBase(system)	×
9	{
10	_system_impl->register_plugin(this);	×
11	}	×
12
13	EventsImpl::EventsImpl(std::shared_ptr<System> system) : PluginImplBase(std::move(system))	×
14	{
15	_system_impl->register_plugin(this);	×
16	}	×
17
18	EventsImpl::~EventsImpl()	×
19	{
20	_system_impl->unregister_plugin(this);	×
21	}	×
22
23	void EventsImpl::init()	×
24	{
25	_system_impl->register_mavlink_message_handler_with_compid(	×
26	MAVLINK_MSG_ID_HEARTBEAT,
27	MAV_COMP_ID_AUTOPILOT1,
28	std::bind(&EventsImpl::handle_heartbeat, this, std::placeholders::_1),	×
29	&_heartbeat_mode_cookie);	×
30	#ifdef MAVLINK_MSG_ID_CURRENT_MODE // Defined in development.xml MAVLink dialect
31	_system_impl->register_mavlink_message_handler_with_compid(
32	MAVLINK_MSG_ID_CURRENT_MODE,
33	MAV_COMP_ID_AUTOPILOT1,
34	std::bind(&EventsImpl::handle_current_mode, this, std::placeholders::_1),
35	&_current_mode_cookie);
36	#endif
37
38	const std::lock_guard lg{_mutex};	×
39	// Instantiate the autopilot component. It's not strictly required, it just makes sure to buffer
40	// events until we get the metadata
41	get_or_create_event_handler(MAV_COMP_ID_AUTOPILOT1);	×
42
43	// Request metadata
NEW 44	_subscribe_metadata_handle = _system_impl->component_metadata().subscribe_metadata_available(	×
45	[this](MavlinkComponentMetadata::MetadataUpdate update) {	×
46	if (update.type == MavlinkComponentMetadata::MetadataType::Events) {	×
47	set_metadata(update.compid, update.json_metadata);	×
48	}
49	});	×
50	_system_impl->component_metadata().request_autopilot_component();	×
51	}	×
52
53	void EventsImpl::deinit()	×
54	{
NEW 55	_system_impl->component_metadata().unsubscribe_metadata_available(_subscribe_metadata_handle);	×
56
57	if (_current_mode_cookie) {	×
58	_system_impl->unregister_all_mavlink_message_handlers(_current_mode_cookie);	×
59	_current_mode_cookie = nullptr;	×
60	}
61	if (_heartbeat_mode_cookie) {	×
62	_system_impl->unregister_all_mavlink_message_handlers(_heartbeat_mode_cookie);	×
63	_heartbeat_mode_cookie = nullptr;	×
64	}
65	}	×
66
67	void EventsImpl::enable() {}	×
68
69	void EventsImpl::disable() {}	×
70
71	EventHandler& EventsImpl::get_or_create_event_handler(uint8_t compid)	×
72	{
73	auto event_data = _events.find(compid);	×
74	if (event_data == _events.end()) {	×
75	// Add new component
76	const std::string profile = "dev"; // Could also be "normal"	×
77
78	auto event_handler = std::make_shared<EventHandler>(	×
79	profile,
80	std::bind(&EventsImpl::handle_event, this, compid, std::placeholders::_1),	×
81	std::bind(&EventsImpl::handle_health_and_arming_check_update, this),	×
82	*_system_impl,	×
83	_system_impl->get_system_id(),	×
84	compid);	×
85	event_data = _events.insert(std::make_pair(compid, event_handler)).first;	×
86	}	×
87	return *event_data->second;	×
88	}
89
90	void EventsImpl::set_metadata(uint8_t compid, const std::string& metadata_json)	×
91	{
92	const std::lock_guard lg{_mutex};	×
93	auto& event_handler = get_or_create_event_handler(compid);	×
94	event_handler.set_metadata(metadata_json);	×
95
96	if (!event_handler.health_and_arming_check_results_valid() &&	×
97	compid == MAV_COMP_ID_AUTOPILOT1) {
98	// Request arming checks to be reported
99	MavlinkCommandSender::CommandLong command{};	×
100	command.command = MAV_CMD_RUN_PREARM_CHECKS;	×
101	command.target_component_id = _system_impl->get_autopilot_id();	×
102
103	_system_impl->send_command_async(	×
104	command, [this](MavlinkCommandSender::Result result, float) {	×
105	if (result != MavlinkCommandSender::Result::Success) {	×
106	LogWarn() << "command MAV_CMD_RUN_PREARM_CHECKS failed";	×
107	}
108	});	×
109	}
110	}	×
111
112	void EventsImpl::handle_event(uint8_t compid, std::unique_ptr<events::parser::ParsedEvent> event)	×
113	{
114	std::optional<Events::LogLevel> maybe_log_level =	×
115	external_log_level(event->eventData().log_levels);	×
116
117	// Handle special groups & protocols
118	if (event->group() == "health" \|\| event->group() == "arming_check") {	×
119	// These are displayed separately
120	return;	×
121	}
122
123	// Show message according to the log level, don't show unknown event groups (might be part of a
124	// new protocol)
125	if (event->group() == "default" && maybe_log_level) {	×
126	std::string message = event->message();	×
127	std::string description = event->description();	×
128
129	if (event->type() == "append_health_and_arming_messages" && event->numArguments() > 0) {	×
130	const uint32_t custom_mode = event->argumentValue(0).value.val_uint32_t;	×
131	const std::shared_ptr<EventHandler>& event_handler = _events[compid];	×
132	const auto maybe_mode_group = event_handler->get_mode_group(custom_mode);	×
133	if (maybe_mode_group) {	×
134	const int mode_group = maybe_mode_group.value();	×
135	std::vector<events::HealthAndArmingChecks::Check> checks =	×
136	event_handler->health_and_arming_check_results().checks(mode_group);	×
137	std::vector<std::string> message_checks;	×
138	for (const auto& check : checks) {	×
139	if (external_log_level(check.log_levels).value_or(Events::LogLevel::Debug) <=	×
140	Events::LogLevel::Warning) {
141	message_checks.push_back(check.message);	×
142	}
143	}
144	if (message_checks.empty()) {	×
145	// Add all
146	for (const auto& check : checks) {	×
147	message_checks.push_back(check.message);	×
148	}
149	}
150	if (!message.empty() && !message_checks.empty()) {	×
151	message += "\n";	×
152	}
153	if (message_checks.size() == 1) {	×
154	message += message_checks[0];	×
155	} else {
156	for (const auto& message_check : message_checks) {	×
157	message += "- " + message_check + "\n";	×
158	}
159	}
160	}	×
161	}
162
163	if (!message.empty()) {	×
164	on_new_displayable_event(	×
165	compid, std::move(event), maybe_log_level.value(), message, description);	×
166	}
167	}	×
168	}
169
170	void EventsImpl::on_new_displayable_event(	×
171	uint8_t compid,
172	std::unique_ptr<events::parser::ParsedEvent> event,
173	Events::LogLevel log_level,
174	const std::string& message,
175	const std::string& description)
176	{
177	// Notify subscribers
178	const std::lock_guard lg{_mutex};	×
179	_events_callbacks.queue(	×
180	Events::Event{	×
181	compid, message, description, log_level, event->eventNamespace(), event->name()},	×
182	[this](const auto& func) { _system_impl->call_user_callback(func); });	×
183	}	×
184
185	void EventsImpl::handle_health_and_arming_check_update()	×
186	{
187	// Notify subscribers
188	const std::lock_guard lg{_mutex};	×
189	const auto report = get_health_and_arming_checks_report();	×
190	if (report.first == Events::Result::Success) {	×
191	_health_and_arming_checks_callbacks.queue(	×
192	report.second, [this](const auto& func) { _system_impl->call_user_callback(func); });	×
193	}
194	}	×
195	std::optional<Events::LogLevel> EventsImpl::external_log_level(uint8_t log_levels)	×
196	{
197	const int external_log_level = log_levels & 0xF;	×
198	switch (external_log_level) {	×
199	case 0:	×
200	return Events::LogLevel::Emergency;	×
201	case 1:	×
202	return Events::LogLevel::Alert;	×
203	case 2:	×
204	return Events::LogLevel::Critical;	×
205	case 3:	×
206	return Events::LogLevel::Error;	×
207	case 4:	×
208	return Events::LogLevel::Warning;	×
209	case 5:	×
210	return Events::LogLevel::Notice;	×
211	case 6:	×
212	return Events::LogLevel::Info;	×
213	case 7:	×
214	return Events::LogLevel::Debug;	×
215	// 8: Protocol
216	// 9: Disabled
217	default:	×
218	break;	×
219	}
220	return std::nullopt;	×
221	}
222	Events::EventsHandle EventsImpl::subscribe_events(const Events::EventsCallback& callback)	×
223	{
224	const std::lock_guard lg{_mutex};	×
225	return _events_callbacks.subscribe(callback);	×
226	}	×
227	void EventsImpl::unsubscribe_events(Events::EventsHandle handle)	×
228	{
229	const std::lock_guard lg{_mutex};	×
230	_events_callbacks.unsubscribe(handle);	×
231	}	×
232	Events::HealthAndArmingChecksHandle EventsImpl::subscribe_health_and_arming_checks(	×
233	const Events::HealthAndArmingChecksCallback& callback)
234	{
235	const std::lock_guard lg{_mutex};	×
236	// Run callback immediately if the report is available
237	const auto report = get_health_and_arming_checks_report();	×
238	if (report.first == Events::Result::Success) {	×
239	_system_impl->call_user_callback([temp_callback = callback, temp_report = report.second] {	×
240	temp_callback(temp_report);
241	});
242	}
243	return _health_and_arming_checks_callbacks.subscribe(callback);	×
244	}	×
245	void EventsImpl::unsubscribe_health_and_arming_checks(Events::HealthAndArmingChecksHandle handle)	×
246	{
247	const std::lock_guard lg{_mutex};	×
248	_health_and_arming_checks_callbacks.unsubscribe(handle);	×
249	}	×
250	std::pair<Events::Result, Events::HealthAndArmingCheckReport>
251	EventsImpl::get_health_and_arming_checks_report()	×
252	{
253	const auto& event_handler = get_or_create_event_handler(MAV_COMP_ID_AUTOPILOT1);	×
254	if (!event_handler.health_and_arming_check_results_valid()) {	×
255	return std::make_pair(Events::Result::NotAvailable, Events::HealthAndArmingCheckReport{});	×
256	}
257	const uint32_t custom_mode = _maybe_custom_mode_user_intention.value_or(_custom_mode);	×
258	const auto maybe_current_mode_group = event_handler.get_mode_group(custom_mode);	×
259	if (!maybe_current_mode_group) {	×
260	LogDebug() << "Current mode not available (yet)";	×
261	return std::make_pair(Events::Result::NotAvailable, Events::HealthAndArmingCheckReport{});	×
262	}
263
264	const auto& event_handler_results = event_handler.health_and_arming_check_results();	×
265	Events::HealthAndArmingCheckReport report{};	×
266
267	auto get_problems =	×
268	[&event_handler_results](const std::vector<events::HealthAndArmingChecks::Check>& checks) {	×
269	std::vector<Events::HealthAndArmingCheckProblem> problems;	×
270	for (const auto& check : checks) {	×
271	const auto maybe_log_level = external_log_level(check.log_levels);	×
272	if (maybe_log_level) {	×
273	problems.push_back(Events::HealthAndArmingCheckProblem{	×
274	check.message,	×
275	check.description,	×
276	maybe_log_level.value(),	×
277	get_health_component_from_index(
278	event_handler_results, check.affected_health_component_index)});	×
279	}
280	}
281	return problems;	×
282	};	×
283
284	// All problems
285	report.all_problems = get_problems(event_handler_results.checks());	×
286
287	// Current mode
288	const int current_mode_group = maybe_current_mode_group.value();	×
289	report.current_mode_intention.problems =	×
290	get_problems(event_handler_results.checks(current_mode_group));	×
291	report.current_mode_intention.can_arm_or_run =	×
292	_system_impl->is_armed() ? event_handler_results.canRun(current_mode_group) :	×
293	event_handler_results.canArm(current_mode_group);	×
294	report.current_mode_intention.mode_name = mode_name_from_custom_mode(custom_mode);	×
295	// We could add the results for other modes, like Takeoff or Mission, with allows to check if
296	// arming is possible for that mode independent of the current mode.
297	// For that, we need to get the custom_mode for these.
298
299	// Health components
300	for (const auto& [component_name, component] :	×
301	event_handler_results.healthComponents().health_components) {	×
302	const Events::HealthComponentReport health_component_report{	×
303	component_name,
304	component.label,	×
305	component.health.is_present,	×
306	component.health.error \|\| component.arming_check.error,	×
307	component.health.warning \|\| component.arming_check.warning};	×
308	report.health_components.push_back(health_component_report);	×
309	}	×
310
311	return std::make_pair(Events::Result::Success, report);	×
312	}	×
313	std::string EventsImpl::get_health_component_from_index(	×
314	const events::HealthAndArmingChecks::Results& results, int health_component_index)
315	{
316	if (health_component_index != 0) { // 0 == none	×
317	for (const auto& [component_name, component] :	×
318	results.healthComponents().health_components) {	×
319	if (component.bitmask == (1ull << health_component_index)) {	×
320	return component_name;	×
321	}
322	}
323	}
324	return "";	×
325	}
326	void EventsImpl::handle_current_mode(const mavlink_message_t& message)	×
327	{
328	UNUSED(message);	×
329	#ifdef MAVLINK_MSG_ID_CURRENT_MODE
330	mavlink_current_mode_t current_mode;
331	mavlink_msg_current_mode_decode(&message, &current_mode);
332	if (current_mode.intended_custom_mode != 0) { // 0 == unknown/not supplied
333	const bool changed =
334	_maybe_custom_mode_user_intention.value_or(0) != current_mode.intended_custom_mode;
335	_maybe_custom_mode_user_intention = current_mode.intended_custom_mode;
336	if (changed) {
337	handle_health_and_arming_check_update();
338	}
339	}
340	#endif
341	}	×
342	void EventsImpl::handle_heartbeat(const mavlink_message_t& message)	×
343	{
344	mavlink_heartbeat_t heartbeat;	×
345	mavlink_msg_heartbeat_decode(&message, &heartbeat);	×
346	if (_custom_mode != heartbeat.custom_mode) {	×
347	_custom_mode = heartbeat.custom_mode;	×
348	if (!_maybe_custom_mode_user_intention) {	×
349	handle_health_and_arming_check_update();	×
350	}
351	}
352	}	×
353	std::string EventsImpl::mode_name_from_custom_mode(uint32_t custom_mode) const	×
354	{
355	// TODO: use Standard Modes MAVLink interface
356	switch (to_flight_mode_from_custom_mode(	×
357	_system_impl->autopilot(), _system_impl->get_vehicle_type(), custom_mode)) {	×
358	case FlightMode::FBWA:	×
359	return "FBWA";	×
360	case FlightMode::Autotune:	×
361	return "Autotune";	×
362	case FlightMode::Guided:	×
363	return "Guided";	×
364	case FlightMode::Ready:	×
365	return "Ready";	×
366	case FlightMode::Takeoff:	×
367	return "Takeoff";	×
368	case FlightMode::Hold:	×
369	return "Hold";	×
370	case FlightMode::Mission:	×
371	return "Mission";	×
372	case FlightMode::ReturnToLaunch:	×
373	return "RTL";	×
374	case FlightMode::Land:	×
375	return "Land";	×
376	case FlightMode::Offboard:	×
377	return "Offboard";	×
378	case FlightMode::FollowMe:	×
379	return "Follow Me";	×
380	case FlightMode::Manual:	×
381	return "Manual";	×
382	case FlightMode::Altctl:	×
383	return "Altitude";	×
384	case FlightMode::Posctl:	×
385	return "Position";	×
386	case FlightMode::Acro:	×
387	return "Acro";	×
388	case FlightMode::Rattitude:	×
389	return "Rattitude";	×
390	case FlightMode::Stabilized:	×
391	return "Stabilized";	×
392	case FlightMode::Unknown:	×
393	break;	×
394	}
395	return "Unknown";	×
396	}
397	} // namespace mavsdk

mavlink / MAVSDK / 15010498442

Source File Press 'n' to go to next uncovered line, 'b' for previous

Source File
Press 'n' to go to next uncovered line, 'b' for previous