| 370 | } |
| 371 | |
| 372 | void OHDMainComponent::process_command_self( |
| 373 | const mavlink_command_long_t& command, int source_sys_id, |
| 374 | int source_comp_id, std::vector<MavlinkMessage>& message_buffer) { |
| 375 | assert(command.target_system == m_sys_id); |
| 376 | assert(command.target_component == m_comp_id); |
| 377 | m_console->debug("Got MAVLINK_MSG_ID_COMMAND_LONG: {} {}", command.command, |
| 378 | static_cast<uint32_t>(command.param1)); |
| 379 | if (command.command == MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN) { |
| 380 | // https://mavlink.io/en/messages/common.html#MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN |
| 381 | m_console->debug("Got shutdown command"); |
| 382 | // we are a companion computer, so we use param2 to get the actual action |
| 383 | const auto action_for_companion = command.param2; |
| 384 | if (action_for_companion > 0) { |
| 385 | message_buffer.push_back( |
| 386 | ack_command(source_sys_id, source_comp_id, command.command)); |
| 387 | const bool shutdownOnly = action_for_companion == 2; |
| 388 | openhd::reboot::handle_power_command_async(std::chrono::seconds(1), |
| 389 | shutdownOnly); |
| 390 | } |
| 391 | // dirty, we don't have a custom message for that yet |
| 392 | if (command.param3 == 1) { |
| 393 | message_buffer.push_back( |
| 394 | ack_command(source_sys_id, source_comp_id, command.command)); |
| 395 | m_console->debug("Unimplemented"); |
| 396 | } |
| 397 | |
| 398 | } else if (command.command == MAV_CMD_REQUEST_MESSAGE) { |
| 399 | const auto requested_message_id = static_cast<uint32_t>(command.param1); |
| 400 | m_console->debug("Someone requested a specific message: {}", |
| 401 | requested_message_id); |
| 402 | if (requested_message_id == MAVLINK_MSG_ID_OPENHD_VERSION_MESSAGE) { |
| 403 | m_console->info("Sent OpenHD version"); |
| 404 | message_buffer.push_back(generate_ohd_version()); |
| 405 | } else if (requested_message_id == |
| 406 | MAVLINK_MSG_ID_OPENHD_WIFBROADCAST_SUPPORTED_CHANNELS) { |
| 407 | m_console->debug("Supported channels requested"); |
| 408 | if (openhd::LinkActionHandler::instance().wb_get_supported_channels != |
| 409 | nullptr) { |
| 410 | auto channels = |
| 411 | openhd::LinkActionHandler::instance().wb_get_supported_channels(); |
| 412 | message_buffer.push_back( |
| 413 | openhd::LinkStatisticsHelper:: |
| 414 | generate_msg_openhd_wifibroadcast_supported_channels( |
| 415 | m_sys_id, m_comp_id, channels)); |
| 416 | m_console->info("Sent supported channels"); |
| 417 | } else { |
| 418 | m_console->warn("Cannot get channels from wb (no handler"); |
| 419 | } |
| 420 | } else { |
| 421 | m_console->info("Message {} request not supported", requested_message_id); |
| 422 | } |
| 423 | } else if (command.command == OPENHD_CMD_INITIATE_CHANNEL_SEARCH) { |
| 424 | if (RUNS_ON_AIR) { |
| 425 | m_console->debug("Scan channels is only a feature for ground unit"); |
| 426 | return; |
| 427 | } else { |
| 428 | const auto channels_to_scan = static_cast<uint32_t>(command.param1); |
| 429 | m_console->debug("OPENHD_CMD_INITIATE_CHANNEL_SEARCH {}", |