Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Dependents: UAVCAN UAVCAN_Subscriber
Diff: libuavcan_drivers/stm32/test_stm32f107/src/main.cpp
- Revision:
- 0:dfe6edabb8ec
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/libuavcan_drivers/stm32/test_stm32f107/src/main.cpp Sat Apr 14 10:25:32 2018 +0000 @@ -0,0 +1,215 @@ +/* + * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com> + */ + +#include <unistd.h> +#include <zubax_chibios/sys/sys.h> +#include <uavcan_stm32/uavcan_stm32.hpp> +#include <uavcan/protocol/global_time_sync_slave.hpp> +#include <uavcan/protocol/dynamic_node_id_client.hpp> +#include "board/board.hpp" + +namespace app +{ +namespace +{ + +uavcan_stm32::CanInitHelper<128> can; + +constexpr unsigned NodePoolSize = 16384; + +uavcan::Node<NodePoolSize>& getNode() +{ + static uavcan::Node<NodePoolSize> node(can.driver, uavcan_stm32::SystemClock::instance()); + return node; +} + +void init() +{ + board::init(); + + /* + * CAN auto bit rate detection. + * Automatic bit rate detection requires that the bus has at least one other CAN node publishing some + * frames periodically. + * Auto bit rate detection can be bypassed byif the desired bit rate is passed directly to can.init(), e.g.: + * can.init(1000000); + */ + int res = 0; + do + { + ::sleep(1); + ::lowsyslog("CAN auto bit rate detection...\n"); + + std::uint32_t bitrate = 0; + res = can.init([]() { ::usleep(can.getRecommendedListeningDelay().toUSec()); }, bitrate); + if (res >= 0) + { + ::lowsyslog("CAN inited at %u bps\n", unsigned(bitrate)); + } + } + while (res < 0); +} + +class : public chibios_rt::BaseStaticThread<8192> +{ + void configureNodeInfo() + { + getNode().setName("org.uavcan.stm32_test_stm32f107"); + + /* + * Software version + * TODO: Fill other fields too + */ + uavcan::protocol::SoftwareVersion swver; + + swver.vcs_commit = GIT_HASH; + swver.optional_field_flags = swver.OPTIONAL_FIELD_FLAG_VCS_COMMIT; + + getNode().setSoftwareVersion(swver); + + lowsyslog("Git commit hash: 0x%08x\n", GIT_HASH); + + /* + * Hardware version + * TODO: Fill other fields too + */ + 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); + + lowsyslog("UDID:"); + for (auto b : hwver.unique_id) + { + lowsyslog(" %02x", unsigned(b)); + } + lowsyslog("\n"); + } + + void performDynamicNodeIDAllocation() + { + uavcan::DynamicNodeIDClient client(getNode()); + + const int client_start_res = client.start(getNode().getHardwareVersion().unique_id); + if (client_start_res < 0) + { + board::die(client_start_res); + } + + lowsyslog("Waiting for dynamic node ID allocation...\n"); + while (!client.isAllocationComplete()) + { + const int spin_res = getNode().spin(uavcan::MonotonicDuration::fromMSec(100)); + if (spin_res < 0) + { + lowsyslog("Spin failure: %i\n", spin_res); + } + } + + lowsyslog("Dynamic node ID %d allocated by %d\n", + int(client.getAllocatedNodeID().get()), + int(client.getAllocatorNodeID().get())); + + getNode().setNodeID(client.getAllocatedNodeID()); + } + +public: + msg_t main() + { + /* + * Setting up the node parameters + */ + configureNodeInfo(); + + /* + * Initializing the UAVCAN node + */ + const int node_init_res = getNode().start(); + if (node_init_res < 0) + { + board::die(node_init_res); + } + + /* + * Waiting for a dynamic node ID allocation + */ + performDynamicNodeIDAllocation(); + + /* + * Time synchronizer + */ + static uavcan::GlobalTimeSyncSlave time_sync_slave(getNode()); + { + const int res = time_sync_slave.start(); + if (res < 0) + { + board::die(res); + } + } + + /* + * Main loop + */ + lowsyslog("UAVCAN node started\n"); + getNode().setModeOperational(); + while (true) + { + const int spin_res = getNode().spin(uavcan::MonotonicDuration::fromMSec(5000)); + if (spin_res < 0) + { + lowsyslog("Spin failure: %i\n", spin_res); + } + + lowsyslog("Time sync master: %u\n", unsigned(time_sync_slave.getMasterNodeID().get())); + + lowsyslog("Memory usage: free=%u used=%u worst=%u\n", + getNode().getAllocator().getNumFreeBlocks(), + getNode().getAllocator().getNumUsedBlocks(), + getNode().getAllocator().getPeakNumUsedBlocks()); + + lowsyslog("CAN errors: %lu %lu\n", + static_cast<unsigned long>(can.driver.getIface(0)->getErrorCount()), + static_cast<unsigned long>(can.driver.getIface(1)->getErrorCount())); + +#if !UAVCAN_TINY + node.getLogger().setLevel(uavcan::protocol::debug::LogLevel::INFO); + node.logInfo("app", "UTC %* sec, %* corr, %* jumps", + uavcan_stm32::clock::getUtc().toMSec() / 1000, + uavcan_stm32::clock::getUtcSpeedCorrectionPPM(), + uavcan_stm32::clock::getUtcAjdustmentJumpCount()); +#endif + } + return msg_t(); + } +} uavcan_node_thread; + +} +} + +int main() +{ + app::init(); + + lowsyslog("Starting the UAVCAN thread\n"); + app::uavcan_node_thread.start(LOWPRIO); + + while (true) + { + for (int i = 0; i < 200; i++) + { + board::setLed(app::can.driver.hadActivity()); + ::usleep(25000); + } + + const uavcan::UtcTime utc = uavcan_stm32::clock::getUtc(); + lowsyslog("UTC %lu sec Rate corr: %fPPM Jumps: %lu Locked: %i\n", + static_cast<unsigned long>(utc.toMSec() / 1000), + uavcan_stm32::clock::getUtcRateCorrectionPPM(), + uavcan_stm32::clock::getUtcJumpCount(), + int(uavcan_stm32::clock::isUtcLocked())); + } +}