MavlinkDriver.h

Go to the documentation of this file.

/* Copyright (c) 2018-2019 Skyward Experimental Rocketry
 * Author: Alvise de'Faveri Tron
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the "Software"), to deal
 * in the Software without restriction, including without limitation the rights
 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included in
 * all copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
 * THE SOFTWARE.
 */
 
#pragma once
 
#include <vector>
 
#ifndef MAVLINK_H
#error \
    "[MavlinkDriver] Mavlink header not found! Please include your mavlink.h \
implementation before including MavlinkDriver.h"
#endif
 
#include <diagnostic/PrintLogger.h>
#include <diagnostic/SkywardStack.h>
#include <diagnostic/StackLogger.h>
#include <mavlink_lib/mavlink_types.h>
#include <radio/Transceiver.h>
#include <utils/collections/SyncPacketQueue.h>
 
#include <functional>
 
#include "MavlinkStatus.h"
 
namespace Boardcore
{
 
template <unsigned int PktLength, unsigned int OutQueueSize,
          unsigned int MavMsgLength = MAVLINK_MAX_PAYLOAD_LEN>
class MavlinkDriver
{
    using MavHandler = std::function<void(MavlinkDriver* channel,
                                          const mavlink_message_t& msg)>;
 
public:
    MavlinkDriver(Transceiver* device, MavHandler onReceive = nullptr,
                  uint16_t sleepAfterSend = 0, size_t outBufferMaxAge = 1000);
 
    bool start();
 
    bool isStarted();
 
    void stop();
 
    bool enqueueMsg(const mavlink_message_t& msg);
 
    bool enqueueRaw(uint8_t* msg, size_t size);
 
    MavlinkStatus getStatus();
 
    void setSleepAfterSend(uint16_t newSleepTime);
 
private:
    void runReceiver();
 
    void runSender();
 
    static void rcvLauncher(void* arg)
    {
        reinterpret_cast<MavlinkDriver*>(arg)->runReceiver();
    }
 
    static void sndLauncher(void* arg)
    {
        reinterpret_cast<MavlinkDriver*>(arg)->runSender();
    }
 
    void updateQueueStats(bool appended);
 
    void updateSenderStats(size_t msgCount, bool sent);
 
    Transceiver* device;   
    MavHandler onReceive;  
 
    // Tweakable params
    uint16_t sleepAfterSend;
    size_t outBufferMaxAge;
    uint16_t pollingTime = 100;  // ms
 
    // Buffers (equal to MTU for ethernet)
    static constexpr size_t MAV_IN_BUFFER_SIZE = 1500;
 
    SyncPacketQueue<PktLength, OutQueueSize> outQueue;
    std::unique_ptr<uint8_t[]> rcvBuffer =
        std::make_unique<uint8_t[]>(MAV_IN_BUFFER_SIZE);
 
    // Status
    MavlinkStatus status;
    miosix::FastMutex mtxStatus;
 
    // Threads
    bool stopFlag   = false;
    bool sndStarted = false;
    bool rcvStarted = false;
 
    miosix::Thread* sndThread = nullptr;
    miosix::Thread* rcvThread = nullptr;
 
    PrintLogger logger = Logging::getLogger("mavlinkdriver");
};
 
template <unsigned int PktLength, unsigned int OutQueueSize,
          unsigned int MavMsgLength>
MavlinkDriver<PktLength, OutQueueSize, MavMsgLength>::MavlinkDriver(
    Transceiver* device, MavHandler onReceive, uint16_t sleepAfterSend,
    size_t outBufferMaxAge)
    : device(device), onReceive(onReceive), sleepAfterSend(sleepAfterSend),
      outBufferMaxAge(outBufferMaxAge)
{
    memset(&status, 0, sizeof(MavlinkStatus));
}
 
template <unsigned int PktLength, unsigned int OutQueueSize,
          unsigned int MavMsgLength>
bool MavlinkDriver<PktLength, OutQueueSize, MavMsgLength>::start()
{
    stopFlag = false;
 
    // Start sender (joinable thread)
    if (!sndStarted)
    {
        sndThread = miosix::Thread::create(
            sndLauncher, skywardStack(4 * 1024), miosix::MAIN_PRIORITY,
            reinterpret_cast<void*>(this), miosix::Thread::JOINABLE);
 
        if (sndThread != nullptr)
            sndStarted = true;
        else
            LOG_ERR(logger, "Could not start sender!");
    }
 
    // Start receiver
    if (!rcvStarted)
    {
        rcvThread = miosix::Thread::create(rcvLauncher, skywardStack(4 * 1024),
                                           miosix::MAIN_PRIORITY,
                                           reinterpret_cast<void*>(this));
 
        if (rcvThread != nullptr)
            rcvStarted = true;
        else
            LOG_ERR(logger, "Could not start receiver!");
    }
 
    if (sndStarted && rcvStarted)
        LOG_DEBUG(logger, "Sender and receiver started");
 
    return sndStarted && rcvStarted;
}
 
template <unsigned int PktLength, unsigned int OutQueueSize,
          unsigned int MavMsgLength>
bool MavlinkDriver<PktLength, OutQueueSize, MavMsgLength>::isStarted()
{
    return sndStarted && rcvStarted;
}
 
template <unsigned int PktLength, unsigned int OutQueueSize,
          unsigned int MavMsgLength>
void MavlinkDriver<PktLength, OutQueueSize, MavMsgLength>::stop()
{
    stopFlag = true;
 
    // Wait for sender to stop
    sndThread->join();
}
 
template <unsigned int PktLength, unsigned int OutQueueSize,
          unsigned int MavMsgLength>
bool MavlinkDriver<PktLength, OutQueueSize, MavMsgLength>::enqueueMsg(
    const mavlink_message_t& msg)
{
    // Convert mavlink message to a byte array
    uint8_t msgTempBuf[MAVLINK_NUM_NON_PAYLOAD_BYTES + MavMsgLength];
    int msgLen = mavlink_msg_to_send_buffer(msgTempBuf, &msg);
 
    // Append the message to the queue
    bool appended = outQueue.put(msgTempBuf, msgLen);
 
    // Update stats
    updateQueueStats(appended);
 
    // Return ok even if a packet was discarded
    return appended;
}
 
template <unsigned int PktLength, unsigned int OutQueueSize,
          unsigned int MavMsgLength>
bool MavlinkDriver<PktLength, OutQueueSize, MavMsgLength>::enqueueRaw(
    uint8_t* msg, size_t size)
{
    // Append message to the queue
    bool appended = outQueue.put(msg, size);
 
    // Update stats
    updateQueueStats(appended);
 
    // Return ok even if a packet was discarded
    return appended;
}
 
template <unsigned int PktLength, unsigned int OutQueueSize,
          unsigned int MavMsgLength>
void MavlinkDriver<PktLength, OutQueueSize, MavMsgLength>::updateQueueStats(
    bool appended)
{
    miosix::Lock<miosix::FastMutex> l(mtxStatus);
 
    if (!appended)
    {
        LOG_ERR(logger, "Buffer full, the oldest message has been discarded");
        status.nDroppedPackets++;
    }
 
    status.nSendQueue++;
 
    if (status.nSendQueue > status.maxSendQueue)
        status.maxSendQueue = status.nSendQueue;
}
 
template <unsigned int PktLength, unsigned int OutQueueSize,
          unsigned int MavMsgLength>
void MavlinkDriver<PktLength, OutQueueSize, MavMsgLength>::runReceiver()
{
    mavlink_message_t msg;
    ssize_t rcvSize;
    uint8_t parseResult = 0;
 
    while (!stopFlag)
    {
        // Check for a new message on the device
        rcvSize = device->receive(rcvBuffer.get(), MAV_IN_BUFFER_SIZE);
 
        // If there's a new message ...
        if (rcvSize > 0)
        {
            parseResult = 0;
            miosix::Lock<miosix::FastMutex> l(mtxStatus);
 
            for (ssize_t i = 0; i < rcvSize; i++)
            {
                // ... parse received bytes
                parseResult =
                    mavlink_parse_char(MAVLINK_COMM_0,
                                       rcvBuffer[i],  // byte to parse
                                       &msg,          // where to parse it
                                       &(status.mavStats));  // stats to update
 
                // When a valid message is found ...
                if (parseResult == 1)
                {
                    // Unlock mutex before calling the callback, no one knows
                    // what could happen.
                    miosix::Unlock<miosix::FastMutex> unlock(l);
 
                    LOG_DEBUG(logger,
                              "Received message with ID {}, sequence: {} from "
                              "component {} of system {}",
                              msg.msgid, msg.seq, msg.compid, msg.sysid);
 
                    // ... handle the command
                    if (onReceive != nullptr)
                        onReceive(this, msg);
 
                    StackLogger::getInstance().updateStack(THID_MAV_RECEIVER);
                }
            }
        }
    }
}
 
template <unsigned int PktLength, unsigned int OutQueueSize,
          unsigned int MavMsgLength>
void MavlinkDriver<PktLength, OutQueueSize, MavMsgLength>::runSender()
{
    LOG_DEBUG(logger, "Sender is running");
    Packet<PktLength> pkt;
 
    while (!stopFlag)
    {
        outQueue.waitUntilNotEmpty();
 
        if (!outQueue.isEmpty())
        {
            // Get the first packet in the queue, without removing it
            pkt = outQueue.get();
 
            // If the packet is ready or too old, send it
            uint64_t age = TimestampTimer::getTimestamp() - pkt.getTimestamp();
            if (pkt.isReady() || age >= outBufferMaxAge * 1e3)
            {
                outQueue.pop();  //  Remove the packet from queue
 
                LOG_DEBUG(logger, "Sending packet. Size: {} (age: {})",
                          pkt.size(), age);
 
                bool sent = device->send(pkt.content.data(), pkt.size());
                updateSenderStats(pkt.getMsgCount(), sent);
 
                miosix::Thread::sleep(sleepAfterSend);
            }
            else
            {
                // Wait before sending something else
                miosix::Thread::sleep(50);
            }
        }
    }
}
 
template <unsigned int PktLength, unsigned int OutQueueSize,
          unsigned int MavMsgLength>
void MavlinkDriver<PktLength, OutQueueSize, MavMsgLength>::updateSenderStats(
    size_t msgCount, bool sent)
{
    {
        miosix::Lock<miosix::FastMutex> l(mtxStatus);
        status.nSendQueue -= msgCount;
 
        if (!sent)
        {
            status.nSendErrors++;
            LOG_ERR(logger, "Could not send message");
        }
    }
 
    StackLogger::getInstance().updateStack(THID_MAV_SENDER);
}
 
template <unsigned int PktLength, unsigned int OutQueueSize,
          unsigned int MavMsgLength>
MavlinkStatus MavlinkDriver<PktLength, OutQueueSize, MavMsgLength>::getStatus()
{
    miosix::Lock<miosix::FastMutex> l(mtxStatus);
    status.timestamp = TimestampTimer::getTimestamp();
    return status;
}
 
template <unsigned int PktLength, unsigned int OutQueueSize,
          unsigned int MavMsgLength>
void MavlinkDriver<PktLength, OutQueueSize, MavMsgLength>::setSleepAfterSend(
    uint16_t newSleepTime)
{
    sleepAfterSend = newSleepTime;
}
 
}  // namespace Boardcore