20604212380

Committed 30 Dec 2025 07:24PM UTC coverage: 48.017% (-0.06%) from 48.078%

Build # 20604212380

Build Type

Pull #2746

github

Committed by

web-flow

Commit Message

Merge 5269e5f1e into 5d2947b34

Pull Request Pull Request #2746: Configuration and component cleanup

Run Details

47 of 158 new or added lines in 19 files covered. (29.75%)

21 existing lines in 9 files now uncovered.

17769 of 37006 relevant lines covered (48.02%)

483.39 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.
    // Pass the ADDRESS of the cookie (same as was passed during registration).
    _system_impl->unregister_all_mavlink_message_handlers_blocking(&_current_mode_cookie);
    _system_impl->unregister_all_mavlink_message_handlers_blocking(&_heartbeat_mode_cookie);

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

mavlink / MAVSDK / 20604212380

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