11588874603

Committed 30 Oct 2024 07:37AM UTC coverage: 38.621% (+0.7%) from 37.918%

Build # 11588874603

Build Type

Pull #2394

github

Committed by

web-flow

Commit Message

Merge 991248b3a into cebb708a4

Pull Request Pull Request #2394: Consolidate CI

Run Details

12034 of 31159 relevant lines covered (38.62%)

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

mavlink / MAVSDK / 11588874603

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