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;
}
