main.cpp

00001 /*
00002  * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
00003  */
00004 
00005 #include <cstdio>
00006 #include <cstdlib>
00007 #include <algorithm>
00008 #include <board.hpp>
00009 #include <chip.h>
00010 #include <uavcan_lpc11c24/uavcan_lpc11c24.hpp>
00011 #include <uavcan/protocol/global_time_sync_slave.hpp>
00012 #include <uavcan/protocol/dynamic_node_id_client.hpp>
00013 #include <uavcan/protocol/logger.hpp>
00014 
00015 /*
00016  * GCC 4.9 cannot generate a working binary with higher optimization levels, although
00017  *         rest of the firmware can be compiled with -Os.
00018  * GCC 4.8 and earlier don't work at all on this firmware.
00019  */
00020 #if __GNUC__
00021 # pragma GCC optimize 1
00022 #endif
00023 
00024 /**
00025  * This function re-defines the standard ::rand(), which is used by the class uavcan::DynamicNodeIDClient.
00026  * Redefinition is normally not needed, but GCC 4.9 tends to generate broken binaries if it is not redefined.
00027  */
00028 int rand()
00029 {
00030     static int x = 1;
00031     x = x * 48271 % 2147483647;
00032     return x;
00033 }
00034 
00035 namespace
00036 {
00037 
00038 static constexpr unsigned NodeMemoryPoolSize = 2800;
00039 
00040 /**
00041  * This is a compact, reentrant and thread-safe replacement to standard llto().
00042  * It returns the string by value, no extra storage is needed.
00043  */
00044 typename uavcan::MakeString<22>::Type intToString(long long n)
00045 {
00046     char buf[24] = {};
00047     const short sign = (n < 0) ? -1 : 1;
00048     if (sign < 0)
00049     {
00050         n = -n;
00051     }
00052     unsigned pos = 0;
00053     do
00054     {
00055         buf[pos++] = char(n % 10 + '0');
00056     }
00057     while ((n /= 10) > 0);
00058     if (sign < 0)
00059     {
00060         buf[pos++] = '-';
00061     }
00062     buf[pos] = '\0';
00063     for (unsigned i = 0, j = pos - 1U; i < j; i++, j--)
00064     {
00065         std::swap(buf[i], buf[j]);
00066     }
00067     return static_cast<const char*>(buf);
00068 }
00069 
00070 uavcan::Node<NodeMemoryPoolSize>& getNode()
00071 {
00072     static uavcan::Node<NodeMemoryPoolSize> node(uavcan_lpc11c24::CanDriver::instance(),
00073                                                  uavcan_lpc11c24::SystemClock::instance());
00074     return node;
00075 }
00076 
00077 uavcan::GlobalTimeSyncSlave& getTimeSyncSlave()
00078 {
00079     static uavcan::GlobalTimeSyncSlave tss(getNode());
00080     return tss;
00081 }
00082 
00083 uavcan::Logger& getLogger()
00084 {
00085     static uavcan::Logger logger(getNode());
00086     return logger;
00087 }
00088 
00089 uavcan::NodeID performDynamicNodeIDAllocation()
00090 {
00091     uavcan::DynamicNodeIDClient client(getNode());
00092 
00093     const int client_start_res = client.start(getNode().getHardwareVersion().unique_id);
00094     if (client_start_res < 0)
00095     {
00096         board::die();
00097     }
00098 
00099     while (!client.isAllocationComplete())
00100     {
00101         board::resetWatchdog();
00102         (void)getNode().spin(uavcan::MonotonicDuration::fromMSec(100));
00103     }
00104 
00105     return client.getAllocatedNodeID();
00106 }
00107 
00108 void init()
00109 {
00110     board::resetWatchdog();
00111     board::syslog("Boot\r\n");
00112 
00113     board::setErrorLed(false);
00114     board::setStatusLed(true);
00115 
00116     /*
00117      * Configuring the clock - this must be done before the CAN controller is initialized
00118      */
00119     uavcan_lpc11c24::clock::init();
00120 
00121     /*
00122      * Configuring the CAN controller
00123      */
00124     std::uint32_t bit_rate = 0;
00125     while (bit_rate == 0)
00126     {
00127         board::syslog("CAN auto bitrate...\r\n");
00128         bit_rate = uavcan_lpc11c24::CanDriver::detectBitRate(&board::resetWatchdog);
00129     }
00130     board::syslog("Bitrate: ");
00131     board::syslog(intToString(bit_rate).c_str());
00132     board::syslog("\r\n");
00133 
00134     if (uavcan_lpc11c24::CanDriver::instance().init(bit_rate) < 0)
00135     {
00136         board::die();
00137     }
00138 
00139     board::syslog("CAN init ok\r\n");
00140 
00141     board::resetWatchdog();
00142 
00143     /*
00144      * Configuring the node
00145      */
00146     getNode().setName("org.uavcan.lpc11c24_test");
00147 
00148     uavcan::protocol::SoftwareVersion swver;
00149     swver.major = FW_VERSION_MAJOR;
00150     swver.minor = FW_VERSION_MINOR;
00151     swver.vcs_commit = GIT_HASH;
00152     swver.optional_field_flags = swver.OPTIONAL_FIELD_FLAG_VCS_COMMIT;
00153     getNode().setSoftwareVersion(swver);
00154 
00155     uavcan::protocol::HardwareVersion hwver;
00156     std::uint8_t uid[board::UniqueIDSize] = {};
00157     board::readUniqueID(uid);
00158     std::copy(std::begin(uid), std::end(uid), std::begin(hwver.unique_id));
00159     getNode().setHardwareVersion(hwver);
00160 
00161     board::resetWatchdog();
00162 
00163     /*
00164      * Starting the node and performing dynamic node ID allocation
00165      */
00166     if (getNode().start() < 0)
00167     {
00168         board::die();
00169     }
00170 
00171     board::syslog("Node ID allocation...\r\n");
00172 
00173     getNode().setNodeID(performDynamicNodeIDAllocation());
00174 
00175     board::syslog("Node ID ");
00176     board::syslog(intToString(getNode().getNodeID().get()).c_str());
00177     board::syslog("\r\n");
00178 
00179     board::resetWatchdog();
00180 
00181     /*
00182      * Example filter configuration.
00183      * Can be removed safely.
00184      */
00185     {
00186         constexpr unsigned NumFilters = 3;
00187         uavcan::CanFilterConfig filters[NumFilters];
00188 
00189         // Acepting all service transfers addressed to us
00190         filters[0].id   = (unsigned(getNode().getNodeID().get()) << 8) | (1U << 7) | uavcan::CanFrame::FlagEFF;
00191         filters[0].mask = 0x7F80 | uavcan::CanFrame::FlagEFF;
00192 
00193         // Accepting time sync messages
00194         filters[1].id   = (4U << 8) | uavcan::CanFrame::FlagEFF;
00195         filters[1].mask = 0xFFFF80 | uavcan::CanFrame::FlagEFF;
00196 
00197         // Accepting zero CAN ID (just for the sake of testing)
00198         filters[2].id   = 0 | uavcan::CanFrame::FlagEFF;
00199         filters[2].mask = uavcan::CanFrame::MaskExtID | uavcan::CanFrame::FlagEFF;
00200 
00201         if (uavcan_lpc11c24::CanDriver::instance().configureFilters(filters, NumFilters) < 0)
00202         {
00203             board::syslog("Filter init failed\r\n");
00204             board::die();
00205         }
00206     }
00207 
00208     /*
00209      * Initializing other libuavcan-related objects
00210      */
00211     if (getTimeSyncSlave().start() < 0)
00212     {
00213         board::die();
00214     }
00215 
00216     if (getLogger().init() < 0)
00217     {
00218         board::die();
00219     }
00220 
00221     getLogger().setLevel(uavcan::protocol::debug::LogLevel::DEBUG);
00222 
00223     board::resetWatchdog();
00224 }
00225 
00226 }
00227 
00228 int main()
00229 {
00230     init();
00231 
00232     getNode().setModeOperational();
00233 
00234     uavcan::MonotonicTime prev_log_at;
00235 
00236     while (true)
00237     {
00238         const int res = getNode().spin(uavcan::MonotonicDuration::fromMSec(25));
00239         board::setErrorLed(res < 0);
00240         board::setStatusLed(uavcan_lpc11c24::CanDriver::instance().hadActivity());
00241 
00242         const auto ts = uavcan_lpc11c24::clock::getMonotonic();
00243         if ((ts - prev_log_at).toMSec() >= 1000)
00244         {
00245             prev_log_at = ts;
00246 
00247             /*
00248              * CAN bus off state monitoring
00249              */
00250             if (uavcan_lpc11c24::CanDriver::instance().isInBusOffState())
00251             {
00252                 board::syslog("CAN BUS OFF\r\n");
00253             }
00254 
00255             /*
00256              * CAN error counter, for debugging purposes
00257              */
00258             board::syslog("CAN errors: ");
00259             board::syslog(intToString(static_cast<long long>(uavcan_lpc11c24::CanDriver::instance().getErrorCount())).c_str());
00260             board::syslog(" ");
00261             board::syslog(intToString(uavcan_lpc11c24::CanDriver::instance().getRxQueueOverflowCount()).c_str());
00262             board::syslog("\r\n");
00263 
00264             /*
00265              * We don't want to use formatting functions provided by libuavcan because they rely on std::snprintf(),
00266              * so we need to construct the message manually:
00267              */
00268             uavcan::protocol::debug::LogMessage logmsg;
00269             logmsg.level.value = uavcan::protocol::debug::LogLevel::INFO;
00270             logmsg.source = "app";
00271             logmsg.text = intToString(uavcan_lpc11c24::clock::getPrevUtcAdjustment().toUSec()).c_str();
00272             (void)getLogger().log(logmsg);
00273         }
00274 
00275         board::resetWatchdog();
00276     }
00277 }