void FPGACamToolApp::handle_input(const mavlink_message_t &msg) { static int data_transmission_size; log("FPGACamToolApp got mavlink_message", static_cast<int>(msg.msgid), Logger::LOGLEVEL_DEBUG); switch(msg.msgid) { case MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE: { mavlink_data_transmission_handshake_t mav_data_handshake; mavlink_msg_data_transmission_handshake_decode(&msg, &mav_data_handshake); log("FPGACamToolApp got data transmission handshake message", static_cast<int>(mav_data_handshake.packets), Logger::LOGLEVEL_DEBUG); Array2D::setupImage(mav_data_handshake.width, mav_data_handshake.height, mav_data_handshake.packets); data_transmission_size = mav_data_handshake.payload; break; } case MAVLINK_MSG_ID_ENCAPSULATED_DATA: { mavlink_encapsulated_data_t data; mavlink_msg_encapsulated_data_decode(&msg, &data); log("FPGACamToolApp got encapsuled data message", static_cast<int>(data.seqnr), Logger::LOGLEVEL_DEBUG); Array2D::addImageData(data.data, data_transmission_size, data.seqnr); break; } default:; } }
bool Flow_px4::update(void) { Mavlink_stream::msg_received_t* rec = NULL; // Receive incoming bytes while (mavlink_stream_.receive(rec)) { // Get pointer to new message mavlink_message_t* msg = &rec->msg; // declare messages mavlink_optical_flow_t optical_flow_msg; mavlink_data_transmission_handshake_t handshake_msg; mavlink_encapsulated_data_t data_msg; switch (msg->msgid) { case MAVLINK_MSG_ID_OPTICAL_FLOW: // Decode message mavlink_msg_optical_flow_decode(msg, &optical_flow_msg); break; case MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE: // Decode message mavlink_msg_data_transmission_handshake_decode(msg, &handshake_msg); // Get type of handshake switch (handshake_msg.jpg_quality) { case 0: handshake_state = FLOW_HANDSHAKE_DATA; break; case 255: handshake_state = FLOW_HANDSHAKE_METADATA; break; default: handshake_state = FLOW_HANDSHAKE_DATA; break; } // Get number of of vectors of_count = handshake_msg.width; if (of_count > 125) { of_count = 125; } // Get total payload size packet_count_ = handshake_msg.packets; byte_count_ = handshake_msg.size; if (byte_count_ > 500) { byte_count_ = 500; } break; case MAVLINK_MSG_ID_ENCAPSULATED_DATA: // Decode message mavlink_msg_encapsulated_data_decode(msg, &data_msg); // Handle according to hanshake state switch (handshake_state) { case FLOW_HANDSHAKE_METADATA: if (data_msg.seqnr < packet_count_ - 1) { // not last packet for (uint32_t i = 0; i < MAVLINK_MSG_ENCAPSULATED_DATA_FIELD_DATA_LEN; ++i) { of_loc_tmp_.data[i + data_msg.seqnr * MAVLINK_MSG_ENCAPSULATED_DATA_FIELD_DATA_LEN] = data_msg.data[i]; } } else if (data_msg.seqnr < packet_count_) { // last packet for (uint32_t i = 0; i < byte_count_; ++i) { of_loc_tmp_.data[i + data_msg.seqnr * MAVLINK_MSG_ENCAPSULATED_DATA_FIELD_DATA_LEN] = data_msg.data[i]; } // swap bytes for (uint32_t i = 0; i < of_count; ++i) { of_loc.x[i] = endian_rev16s(of_loc_tmp_.x[i]); of_loc.y[i] = endian_rev16s(of_loc_tmp_.y[i]); } } break; default: if (data_msg.seqnr < (packet_count_ - 1)) { // not last packet for (uint32_t i = 0; i < MAVLINK_MSG_ENCAPSULATED_DATA_FIELD_DATA_LEN; ++i) { of_tmp_.data[i + data_msg.seqnr * MAVLINK_MSG_ENCAPSULATED_DATA_FIELD_DATA_LEN] = data_msg.data[i]; } } else if (data_msg.seqnr == (packet_count_ - 1)) { // last packet for (uint32_t i = 0; i < byte_count_ % MAVLINK_MSG_ENCAPSULATED_DATA_FIELD_DATA_LEN; ++i) { of_tmp_.data[i + data_msg.seqnr * MAVLINK_MSG_ENCAPSULATED_DATA_FIELD_DATA_LEN] = data_msg.data[i]; } // swap bytes and filter for high frequency noise for (int i = 0; i < of_count; ++i) { of.x[i] = filter_constant * 0.001f * (float)(endian_rev16s(of_tmp_.x[i])) + (1.0f - filter_constant) * of.x[i]; of.y[i] = filter_constant * 0.001f * (float)(endian_rev16s(of_tmp_.y[i])) + (1.0f - filter_constant) * of.y[i]; } // Update time last_update_us = time_keeper_get_us(); } break; } break; default: break; } } return true; }