main.cpp

00001 /*
00002  * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
00003  */
00004 
00005 #include <unistd.h>
00006 #include <zubax_chibios/sys/sys.h>
00007 #include <uavcan_stm32/uavcan_stm32.hpp>
00008 #include <uavcan/protocol/global_time_sync_slave.hpp>
00009 #include <uavcan/protocol/dynamic_node_id_client.hpp>
00010 #include "board/board.hpp"
00011 
00012 namespace app
00013 {
00014 namespace
00015 {
00016 
00017 uavcan_stm32::CanInitHelper<128> can;
00018 
00019 constexpr unsigned NodePoolSize = 16384;
00020 
00021 uavcan::Node<NodePoolSize>& getNode()
00022 {
00023     static uavcan::Node<NodePoolSize> node(can.driver, uavcan_stm32::SystemClock::instance());
00024     return node;
00025 }
00026 
00027 void init()
00028 {
00029     board::init();
00030 
00031     /*
00032      * CAN auto bit rate detection.
00033      * Automatic bit rate detection requires that the bus has at least one other CAN node publishing some
00034      * frames periodically.
00035      * Auto bit rate detection can be bypassed byif the desired bit rate is passed directly to can.init(), e.g.:
00036      *   can.init(1000000);
00037      */
00038     int res = 0;
00039     do
00040     {
00041         ::sleep(1);
00042         ::lowsyslog("CAN auto bit rate detection...\n");
00043 
00044         std::uint32_t bitrate = 0;
00045         res = can.init([]() { ::usleep(can.getRecommendedListeningDelay().toUSec()); }, bitrate);
00046         if (res >= 0)
00047         {
00048             ::lowsyslog("CAN inited at %u bps\n", unsigned(bitrate));
00049         }
00050     }
00051     while (res < 0);
00052 }
00053 
00054 class : public chibios_rt::BaseStaticThread<8192>
00055 {
00056     void configureNodeInfo()
00057     {
00058         getNode().setName("org.uavcan.stm32_test_stm32f107");
00059 
00060         /*
00061          * Software version
00062          * TODO: Fill other fields too
00063          */
00064         uavcan::protocol::SoftwareVersion swver;
00065 
00066         swver.vcs_commit = GIT_HASH;
00067         swver.optional_field_flags = swver.OPTIONAL_FIELD_FLAG_VCS_COMMIT;
00068 
00069         getNode().setSoftwareVersion(swver);
00070 
00071         lowsyslog("Git commit hash: 0x%08x\n", GIT_HASH);
00072 
00073         /*
00074          * Hardware version
00075          * TODO: Fill other fields too
00076          */
00077         uavcan::protocol::HardwareVersion hwver;
00078 
00079         std::uint8_t uid[board::UniqueIDSize] = {};
00080         board::readUniqueID(uid);
00081         std::copy(std::begin(uid), std::end(uid), std::begin(hwver.unique_id));
00082 
00083         getNode().setHardwareVersion(hwver);
00084 
00085         lowsyslog("UDID:");
00086         for (auto b : hwver.unique_id)
00087         {
00088             lowsyslog(" %02x", unsigned(b));
00089         }
00090         lowsyslog("\n");
00091     }
00092 
00093     void performDynamicNodeIDAllocation()
00094     {
00095         uavcan::DynamicNodeIDClient client(getNode());
00096 
00097         const int client_start_res = client.start(getNode().getHardwareVersion().unique_id);
00098         if (client_start_res < 0)
00099         {
00100             board::die(client_start_res);
00101         }
00102 
00103         lowsyslog("Waiting for dynamic node ID allocation...\n");
00104         while (!client.isAllocationComplete())
00105         {
00106             const int spin_res = getNode().spin(uavcan::MonotonicDuration::fromMSec(100));
00107             if (spin_res < 0)
00108             {
00109                 lowsyslog("Spin failure: %i\n", spin_res);
00110             }
00111         }
00112 
00113         lowsyslog("Dynamic node ID %d allocated by %d\n",
00114                   int(client.getAllocatedNodeID().get()),
00115                   int(client.getAllocatorNodeID().get()));
00116 
00117         getNode().setNodeID(client.getAllocatedNodeID());
00118     }
00119 
00120 public:
00121     msg_t main()
00122     {
00123         /*
00124          * Setting up the node parameters
00125          */
00126         configureNodeInfo();
00127 
00128         /*
00129          * Initializing the UAVCAN node
00130          */
00131         const int node_init_res = getNode().start();
00132         if (node_init_res < 0)
00133         {
00134             board::die(node_init_res);
00135         }
00136 
00137         /*
00138          * Waiting for a dynamic node ID allocation
00139          */
00140         performDynamicNodeIDAllocation();
00141 
00142         /*
00143          * Time synchronizer
00144          */
00145         static uavcan::GlobalTimeSyncSlave time_sync_slave(getNode());
00146         {
00147             const int res = time_sync_slave.start();
00148             if (res < 0)
00149             {
00150                 board::die(res);
00151             }
00152         }
00153 
00154         /*
00155          * Main loop
00156          */
00157         lowsyslog("UAVCAN node started\n");
00158         getNode().setModeOperational();
00159         while (true)
00160         {
00161             const int spin_res = getNode().spin(uavcan::MonotonicDuration::fromMSec(5000));
00162             if (spin_res < 0)
00163             {
00164                 lowsyslog("Spin failure: %i\n", spin_res);
00165             }
00166 
00167             lowsyslog("Time sync master: %u\n", unsigned(time_sync_slave.getMasterNodeID().get()));
00168 
00169             lowsyslog("Memory usage: free=%u used=%u worst=%u\n",
00170                       getNode().getAllocator().getNumFreeBlocks(),
00171                       getNode().getAllocator().getNumUsedBlocks(),
00172                       getNode().getAllocator().getPeakNumUsedBlocks());
00173 
00174             lowsyslog("CAN errors: %lu %lu\n",
00175                       static_cast<unsigned long>(can.driver.getIface(0)->getErrorCount()),
00176                       static_cast<unsigned long>(can.driver.getIface(1)->getErrorCount()));
00177 
00178 #if !UAVCAN_TINY
00179             node.getLogger().setLevel(uavcan::protocol::debug::LogLevel::INFO);
00180             node.logInfo("app", "UTC %* sec, %* corr, %* jumps",
00181                          uavcan_stm32::clock::getUtc().toMSec() / 1000,
00182                          uavcan_stm32::clock::getUtcSpeedCorrectionPPM(),
00183                          uavcan_stm32::clock::getUtcAjdustmentJumpCount());
00184 #endif
00185         }
00186         return msg_t();
00187     }
00188 } uavcan_node_thread;
00189 
00190 }
00191 }
00192 
00193 int main()
00194 {
00195     app::init();
00196 
00197     lowsyslog("Starting the UAVCAN thread\n");
00198     app::uavcan_node_thread.start(LOWPRIO);
00199 
00200     while (true)
00201     {
00202         for (int i = 0; i < 200; i++)
00203         {
00204             board::setLed(app::can.driver.hadActivity());
00205             ::usleep(25000);
00206         }
00207 
00208         const uavcan::UtcTime utc = uavcan_stm32::clock::getUtc();
00209         lowsyslog("UTC %lu sec   Rate corr: %fPPM   Jumps: %lu   Locked: %i\n",
00210                   static_cast<unsigned long>(utc.toMSec() / 1000),
00211                   uavcan_stm32::clock::getUtcRateCorrectionPPM(),
00212                   uavcan_stm32::clock::getUtcJumpCount(),
00213                   int(uavcan_stm32::clock::isUtcLocked()));
00214     }
00215 }