mavlink_decoder.h

Go to the documentation of this file.

// Copyright (c) 2018, Smart Projects Holdings Ltd
// All rights reserved.
// See LICENSE file for license details.

#include <ugcs/vsm/callback.h>
#include <ugcs/vsm/mavlink.h>

#include <unordered_map>

#ifndef _UGCS_VSM_MAVLINK_DECODER_H_
#define _UGCS_VSM_MAVLINK_DECODER_H_

namespace ugcs {
namespace vsm {

class Mavlink_decoder {
private:
    enum class State {
        STX,
        VER1,
        VER2
    };

public:
    typedef Callback_proxy<void, Io_buffer::Ptr, mavlink::MESSAGE_ID_TYPE,
        uint8_t, uint8_t, uint32_t> Handler;

    typedef Callback_proxy<void, Io_buffer::Ptr> Raw_data_handler;

    DEFINE_CALLBACK_BUILDER(
            Make_decoder_handler,
            (Io_buffer::Ptr, mavlink::MESSAGE_ID_TYPE, uint8_t, uint8_t, uint32_t),
            (nullptr, mavlink::MESSAGE_ID::DEBUG_VALUE, mavlink::SYSTEM_ID_NONE, 0, 0))

    
    DEFINE_CALLBACK_BUILDER(Make_raw_data_handler, (Io_buffer::Ptr), (nullptr))

    struct Stats {
        uint64_t handled = 0;
        uint64_t no_handler = 0;
        uint64_t bad_checksum = 0;
        uint64_t bad_length = 0;
        uint64_t unknown_id = 0;
        uint64_t bytes_received = 0;
        uint64_t stx_syncs = 0;
    };


    Mavlink_decoder():
        packet_buf(ugcs::vsm::Io_buffer::Create())
    {
    }

    Mavlink_decoder(const Mavlink_decoder&) = delete;

    void
    Disable()
    {
        handler = Handler();
        data_handler = Raw_data_handler();
    }

    void
    Register_handler(Handler handler)
    {
        this->handler = handler;
    }

    void
    Register_raw_data_handler(Raw_data_handler handler)
    {
        data_handler = handler;
    }

    void
    Decode(Io_buffer::Ptr buffer)
    {
        // LOG_DBG("read: %s", buffer->Get_hex().c_str());
        if (data_handler) {
            data_handler(buffer);
        }
        packet_buf = packet_buf->Concatenate(buffer);
        size_t packet_len;
        const uint8_t* data;
        next_read_len = 0;

        while (true) {
            size_t buffer_len = packet_buf->Get_length();
            {   // lock scope
                std::lock_guard<std::mutex> stats_lock(stats_mutex);
                stats[mavlink::SYSTEM_ID_ANY].bytes_received += buffer_len;
                if (state == State::STX) {
                    size_t len_skipped = 0;
                    if (buffer_len < mavlink::MAVLINK_1_MIN_FRAME_LEN) {
                        // need at least minimum frame length of data.
                        next_read_len = mavlink::MAVLINK_1_MIN_FRAME_LEN - buffer_len;
                        break;
                    }
                    // look for signature in received data.
                    data = static_cast<const uint8_t*>(packet_buf->Get_data());
                    for (; len_skipped < buffer_len; len_skipped++, data++) {
                        if (*data == mavlink::START_SIGN) {
                            // found preamble. Start receiving payload.
                            state = State::VER1;
                            stats[mavlink::SYSTEM_ID_ANY].stx_syncs++;
                            // slice off the preamble.
                            packet_buf = packet_buf->Slice(1);
                            break;
                        }
                        if (*data == mavlink::START_SIGN2) {
                            // found preamble. Start receiving payload.
                            state = State::VER2;
                            stats[mavlink::SYSTEM_ID_ANY].stx_syncs++;
                            // slice off the preamble.
                            packet_buf = packet_buf->Slice(1);
                            break;
                        }
                    }
                    if (len_skipped) {
                        // slice off the skipped bytes.
                        packet_buf = packet_buf->Slice(len_skipped);
                    }
                }
            }
            if (state == State::VER1 || state == State::VER2) {
                size_t wrapper_len; // non-payload data len excluding signature.
                if (state == State::VER1) {
                    wrapper_len = mavlink::MAVLINK_1_HEADER_LEN - 1 + 2;
                } else {
                    wrapper_len = mavlink::MAVLINK_2_HEADER_LEN - 1 + 2;
                }
                buffer_len = packet_buf->Get_length();
                if (buffer_len == 0) {
                    // need at least the minimum packet len. Initiate next read.
                    next_read_len = wrapper_len;
                    break;
                }
                data = static_cast<const uint8_t*>(packet_buf->Get_data());
                packet_len = wrapper_len + static_cast<size_t>(*data);
                if (packet_len > buffer_len) {
                    // need the whole packet. Initiate next read.
                    next_read_len = packet_len - buffer_len;
                    break;
                }
                if (Decode_packet(packet_buf)) {
                    // decoder suceeded. Slice off the decoded packet.
                    packet_buf = packet_buf->Slice(packet_len);
                }
                // if decoder failed, we restart the search for
                // next preamble in existing data otherwise
                // continue with next byte after the decoded packet.
                state = State::STX;
            }
        }
    }

    size_t
    Get_next_read_size() const
    {
        return next_read_len;
    }

    const Mavlink_decoder::Stats
    Get_stats(int system_id)
    {
        std::lock_guard<std::mutex> lock(stats_mutex);
        return stats[system_id];
    }

    const Mavlink_decoder::Stats
    Get_common_stats()
    {
        std::lock_guard<std::mutex> lock(stats_mutex);
        return stats[mavlink::SYSTEM_ID_ANY];
    }

private:
    bool
    Decode_packet(Io_buffer::Ptr buffer)
    {
        auto data = static_cast<const uint8_t*>(buffer->Get_data());
        uint16_t payload_len = data[0];
        uint8_t system_id;
        uint8_t component_id;
        uint8_t seq;
        uint8_t header_len;
        mavlink::MESSAGE_ID_TYPE msg_id;

        if (state == State::VER2) {
            seq = data[3];
            system_id = data[4];
            component_id = data[5];
            msg_id =
                static_cast<int>(data[6]) +
                (static_cast<int>(data[7]) << 8) +
                (static_cast<int>(data[8]) << 16);
            header_len = mavlink::MAVLINK_2_HEADER_LEN - 1;
        } else {
            seq = data[1];
            system_id = data[2];
            component_id = data[3];
            msg_id = data[4];
            header_len = mavlink::MAVLINK_1_HEADER_LEN - 1;
        }

        mavlink::Checksum sum(data, header_len);

        mavlink::Extra_byte_length_pair crc_byte_len_pair;
        // Try all extensions.
        if (    !sum.Get_extra_byte_length_pair(msg_id, crc_byte_len_pair, mavlink::Extension::Get())
            &&  !sum.Get_extra_byte_length_pair(msg_id, crc_byte_len_pair, mavlink::apm::Extension::Get())
            &&  !sum.Get_extra_byte_length_pair(msg_id, crc_byte_len_pair, mavlink::sph::Extension::Get())) {
            std::lock_guard<std::mutex> stats_lock(stats_mutex);
            stats[mavlink::SYSTEM_ID_ANY].unknown_id++;
            // LOG_DEBUG("Unknown Mavlink message id: %d (%X)", msg_id, msg_id);
            return false;
        }

        sum.Accumulate(data + header_len, payload_len);

        uint16_t sum_calc = sum.Accumulate(crc_byte_len_pair.first);
        /* Convert checksum in Mavlink byte order to a host byte order
         * compatible type. */
        auto sum_recv = reinterpret_cast<const mavlink::Uint16*>(data + header_len + payload_len);

        bool cksum_ok = sum_calc == *sum_recv;
        bool length_ok = crc_byte_len_pair.second == payload_len;

        std::unique_lock<std::mutex> stats_lock(stats_mutex);
        if (cksum_ok && (length_ok || state == State::VER2)) {
            /*
             * Fully valid packet received.
             */
            if (handler) {
                stats[system_id].handled++;
                stats[mavlink::SYSTEM_ID_ANY].handled++;
                stats_lock.unlock();
                handler(Io_buffer::Create(*buffer, header_len, payload_len), msg_id, system_id, component_id, seq);
            } else {
                stats[system_id].no_handler++;
                stats[mavlink::SYSTEM_ID_ANY].no_handler++;
                LOG_DEBUG("Mavlink message %d handler not registered.", msg_id);
            }
            return true;
        } else {
            if (cksum_ok) {
                stats[system_id].bad_length++;
                stats[mavlink::SYSTEM_ID_ANY].bad_length++;
                LOG_DEBUG("Mavlink payload length mismatch, recv=%d wanted=%d.",
                    payload_len, crc_byte_len_pair.second);
            } else {
                stats[mavlink::SYSTEM_ID_ANY].bad_checksum++;
            }
            return false;
        }
    }


    State state = State::STX;

    Handler handler;

    Raw_data_handler data_handler;

    std::unordered_map<int, Stats> stats;
    std::mutex stats_mutex;

    ugcs::vsm::Io_buffer::Ptr packet_buf;

    size_t next_read_len = mavlink::MAVLINK_1_MIN_FRAME_LEN;
};

} /* namespace vsm */
} /* namespace ugcs */

#endif /* _UGCS_VSM_MAVLINK_DECODER_H_ */