19833112496

Committed 01 Dec 2025 06:23PM UTC coverage: 48.262% (+0.05%) from 48.21%

Build # 19833112496

Build Type

push

github

Committed by

web-flow

Commit Message

Merge pull request #2721 from mavlink/pr-destruction-segfault

Fix segfaults on events destruction

Run Details

19 of 35 new or added lines in 19 files covered. (54.29%)

6 existing lines in 5 files now uncovered.

17657 of 36586 relevant lines covered (48.26%)

467.21 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);

    // Use blocking version to ensure any in-flight callbacks complete before destruction.
    if (_current_mode_cookie) {
        _system_impl->unregister_all_mavlink_message_handlers_blocking(_current_mode_cookie);
        _current_mode_cookie = nullptr;
    }
    if (_heartbeat_mode_cookie) {
        _system_impl->unregister_all_mavlink_message_handlers_blocking(_heartbeat_mode_cookie);
        _heartbeat_mode_cookie = nullptr;
    }

    // Clear all EventHandlers - their destructors will unregister message handlers
    {
        const std::lock_guard lg{_mutex};
        _events.clear();
    }
}

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

mavlink / MAVSDK / 19833112496

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