main.cpp

Go to the documentation of this file.

/*
 * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
 */

#include <cstdio>
#include <cstdlib>
#include <algorithm>
#include <board.hpp>
#include <chip.h>
#include <uavcan_lpc11c24/uavcan_lpc11c24.hpp>
#include <uavcan/protocol/global_time_sync_slave.hpp>
#include <uavcan/protocol/dynamic_node_id_client.hpp>
#include <uavcan/protocol/logger.hpp>

/*
 * GCC 4.9 cannot generate a working binary with higher optimization levels, although
 *         rest of the firmware can be compiled with -Os.
 * GCC 4.8 and earlier don't work at all on this firmware.
 */
#if __GNUC__
# pragma GCC optimize 1
#endif

int rand()
{
    static int x = 1;
    x = x * 48271 % 2147483647;
    return x;
}

namespace
{

static constexpr unsigned NodeMemoryPoolSize = 2800;

typename uavcan::MakeString<22>::Type intToString(long long n)
{
    char buf[24] = {};
    const short sign = (n < 0) ? -1 : 1;
    if (sign < 0)
    {
        n = -n;
    }
    unsigned pos = 0;
    do
    {
        buf[pos++] = char(n % 10 + '0');
    }
    while ((n /= 10) > 0);
    if (sign < 0)
    {
        buf[pos++] = '-';
    }
    buf[pos] = '\0';
    for (unsigned i = 0, j = pos - 1U; i < j; i++, j--)
    {
        std::swap(buf[i], buf[j]);
    }
    return static_cast<const char*>(buf);
}

uavcan::Node<NodeMemoryPoolSize>& getNode()
{
    static uavcan::Node<NodeMemoryPoolSize> node(uavcan_lpc11c24::CanDriver::instance(),
                                                 uavcan_lpc11c24::SystemClock::instance());
    return node;
}

uavcan::GlobalTimeSyncSlave& getTimeSyncSlave()
{
    static uavcan::GlobalTimeSyncSlave tss(getNode());
    return tss;
}

uavcan::Logger& getLogger()
{
    static uavcan::Logger logger(getNode());
    return logger;
}

uavcan::NodeID performDynamicNodeIDAllocation()
{
    uavcan::DynamicNodeIDClient client(getNode());

    const int client_start_res = client.start(getNode().getHardwareVersion().unique_id);
    if (client_start_res < 0)
    {
        board::die();
    }

    while (!client.isAllocationComplete())
    {
        board::resetWatchdog();
        (void)getNode().spin(uavcan::MonotonicDuration::fromMSec(100));
    }

    return client.getAllocatedNodeID();
}

void init()
{
    board::resetWatchdog();
    board::syslog("Boot\r\n");

    board::setErrorLed(false);
    board::setStatusLed(true);

    /*
     * Configuring the clock - this must be done before the CAN controller is initialized
     */
    uavcan_lpc11c24::clock::init();

    /*
     * Configuring the CAN controller
     */
    std::uint32_t bit_rate = 0;
    while (bit_rate == 0)
    {
        board::syslog("CAN auto bitrate...\r\n");
        bit_rate = uavcan_lpc11c24::CanDriver::detectBitRate(&board::resetWatchdog);
    }
    board::syslog("Bitrate: ");
    board::syslog(intToString(bit_rate).c_str());
    board::syslog("\r\n");

    if (uavcan_lpc11c24::CanDriver::instance().init(bit_rate) < 0)
    {
        board::die();
    }

    board::syslog("CAN init ok\r\n");

    board::resetWatchdog();

    /*
     * Configuring the node
     */
    getNode().setName("org.uavcan.lpc11c24_test");

    uavcan::protocol::SoftwareVersion swver;
    swver.major = FW_VERSION_MAJOR;
    swver.minor = FW_VERSION_MINOR;
    swver.vcs_commit = GIT_HASH;
    swver.optional_field_flags = swver.OPTIONAL_FIELD_FLAG_VCS_COMMIT;
    getNode().setSoftwareVersion(swver);

    uavcan::protocol::HardwareVersion hwver;
    std::uint8_t uid[board::UniqueIDSize] = {};
    board::readUniqueID(uid);
    std::copy(std::begin(uid), std::end(uid), std::begin(hwver.unique_id));
    getNode().setHardwareVersion(hwver);

    board::resetWatchdog();

    /*
     * Starting the node and performing dynamic node ID allocation
     */
    if (getNode().start() < 0)
    {
        board::die();
    }

    board::syslog("Node ID allocation...\r\n");

    getNode().setNodeID(performDynamicNodeIDAllocation());

    board::syslog("Node ID ");
    board::syslog(intToString(getNode().getNodeID().get()).c_str());
    board::syslog("\r\n");

    board::resetWatchdog();

    /*
     * Example filter configuration.
     * Can be removed safely.
     */
    {
        constexpr unsigned NumFilters = 3;
        uavcan::CanFilterConfig filters[NumFilters];

        // Acepting all service transfers addressed to us
        filters[0].id   = (unsigned(getNode().getNodeID().get()) << 8) | (1U << 7) | uavcan::CanFrame::FlagEFF;
        filters[0].mask = 0x7F80 | uavcan::CanFrame::FlagEFF;

        // Accepting time sync messages
        filters[1].id   = (4U << 8) | uavcan::CanFrame::FlagEFF;
        filters[1].mask = 0xFFFF80 | uavcan::CanFrame::FlagEFF;

        // Accepting zero CAN ID (just for the sake of testing)
        filters[2].id   = 0 | uavcan::CanFrame::FlagEFF;
        filters[2].mask = uavcan::CanFrame::MaskExtID | uavcan::CanFrame::FlagEFF;

        if (uavcan_lpc11c24::CanDriver::instance().configureFilters(filters, NumFilters) < 0)
        {
            board::syslog("Filter init failed\r\n");
            board::die();
        }
    }

    /*
     * Initializing other libuavcan-related objects
     */
    if (getTimeSyncSlave().start() < 0)
    {
        board::die();
    }

    if (getLogger().init() < 0)
    {
        board::die();
    }

    getLogger().setLevel(uavcan::protocol::debug::LogLevel::DEBUG);

    board::resetWatchdog();
}

}

int main()
{
    init();

    getNode().setModeOperational();

    uavcan::MonotonicTime prev_log_at;

    while (true)
    {
        const int res = getNode().spin(uavcan::MonotonicDuration::fromMSec(25));
        board::setErrorLed(res < 0);
        board::setStatusLed(uavcan_lpc11c24::CanDriver::instance().hadActivity());

        const auto ts = uavcan_lpc11c24::clock::getMonotonic();
        if ((ts - prev_log_at).toMSec() >= 1000)
        {
            prev_log_at = ts;

            /*
             * CAN bus off state monitoring
             */
            if (uavcan_lpc11c24::CanDriver::instance().isInBusOffState())
            {
                board::syslog("CAN BUS OFF\r\n");
            }

            /*
             * CAN error counter, for debugging purposes
             */
            board::syslog("CAN errors: ");
            board::syslog(intToString(static_cast<long long>(uavcan_lpc11c24::CanDriver::instance().getErrorCount())).c_str());
            board::syslog(" ");
            board::syslog(intToString(uavcan_lpc11c24::CanDriver::instance().getRxQueueOverflowCount()).c_str());
            board::syslog("\r\n");

            /*
             * We don't want to use formatting functions provided by libuavcan because they rely on std::snprintf(),
             * so we need to construct the message manually:
             */
            uavcan::protocol::debug::LogMessage logmsg;
            logmsg.level.value = uavcan::protocol::debug::LogLevel::INFO;
            logmsg.source = "app";
            logmsg.text = intToString(uavcan_lpc11c24::clock::getPrevUtcAdjustment().toUSec()).c_str();
            (void)getLogger().log(logmsg);
        }

        board::resetWatchdog();
    }
}