| 78 | } |
| 79 | |
| 80 | void MEndpoint::parseNewData(const uint8_t* data, const int data_len) { |
| 81 | //<<TAG<<" received data:"<<data_len<<" |
| 82 | //"<<MavlinkHelpers::raw_content(data,data_len)<<"\n"; |
| 83 | m_rx_n_bytes += data_len; |
| 84 | std::vector<MavlinkMessage> messages; |
| 85 | mavlink_message_t msg; |
| 86 | for (int i = 0; i < data_len; i++) { |
| 87 | uint8_t res = mavlink_parse_char(m_mavlink_channel, (uint8_t)data[i], &msg, |
| 88 | &receiveMavlinkStatus); |
| 89 | if (res) { |
| 90 | messages.push_back(MavlinkMessage{msg}); |
| 91 | // From |
| 92 | // https://github.com/mavlink/c_uart_interface_example/blob/master/serial_port.cpp |
| 93 | if ((m_last_status.packet_rx_drop_count != |
| 94 | receiveMavlinkStatus.packet_rx_drop_count) && |
| 95 | m_debug_mavlink_msg_packet_loss) { |
| 96 | openhd::log::get_default()->warn( |
| 97 | "DROPPED {} PACKETS", receiveMavlinkStatus.packet_rx_drop_count); |
| 98 | } |
| 99 | m_last_status = receiveMavlinkStatus; |
| 100 | } |
| 101 | } |
| 102 | onNewMavlinkMessages(messages); |
| 103 | } |
| 104 | |
| 105 | void MEndpoint::onNewMavlinkMessages(std::vector<MavlinkMessage> messages) { |
| 106 | if (messages.empty()) return; |