• Specification
    • 1. Introduction
    • 2. Basic concepts
    • 3. Data structure description language
    • 4.1 CAN bus transport layer
    • 4.2 UDP bus transport layer
    • 4.3 MAVLink bus transport layer
    • 4.4 CANFD bus transport layer
    • 5. Application level conventions
    • 6. Application level functions
    • 7. List of standard data types
    • 8. Hardware design recommendations
    • dsdl
      • ardupilot
      • uavcan
  • Implementations
    • Libuavcan
      • Platforms
      • Tutorials
        • 1. Library build configuration
        • 2. Node initialization and startup
        • 3. Publishers and subscribers
        • 4. Services
        • 5. Timers
        • 6. Time synchronization
        • 7. Remote node reconfiguration
        • 8. Custom data types
        • 9. Node discovery
        • 10. Dynamic node ID allocation
        • 11. Firmware update
        • 12. Multithreading
        • 13. CAN acceptance filters
      • FAQ
    • Pydronecan
      • Examples
        • Automated ESC enumeration
        • Dump All Messages
        • ESC throttle control
      • Tutorials
        • 1. Setup
        • 2. Basic usage
        • 3. Advanced usage
        • 4. Parsing DSDL definitions
    • AP Periph
    • Libcanard
    • dronecan dsdlc
  • GUI Tool
    • Overview
    • Examples
    • User guide
  • Examples
    • Simple sensor node
  • Discussions
Examples /  Simple sensor node

Simple sensor node

This example is not based on any existing DroneCAN implementation - it is completely standalone featuring zero third-party dependencies.

Source code

/*
 * This program is an example implementation of an extremely lightweight DroneCAN node with zero third-party dependencies.
 * This example is based on Linux SocketCAN, but can be easily adapted to any other platform.
 *
 * GCC invocation command:
 *     gcc simple_sensor_node.c -lrt -std=gnu99 -o simple_sensor_node
 * With warnings:
 *     gcc simple_sensor_node.c -lrt -std=gnu99 -o simple_sensor_node -Wall -Werror -Wextra -pedantic -Wsign-conversion
 *
 * Author: Pavel Kirienko <pavel.kirienko@zubax.com>
 * License: CC0, no copyright reserved
 * Language: C99
 */

#include <stdio.h>
#include <unistd.h>
#include <stdint.h>
#include <stdlib.h>
#include <string.h>
#include <math.h>

/*
 * This part is specific for Linux. It can be replaced for use on other platforms.
 * Note that under Linux this source must be linked with librt (for GCC add -lrt)
 */
#include <sys/time.h>
#include <net/if.h>
#include <sys/types.h>
#include <sys/socket.h>
#include <sys/ioctl.h>
#include <linux/can.h>
#include <linux/can/raw.h>
#include <time.h>

static int can_socket = -1;

int can_init(const char* can_iface_name)
{
    // Open the SocketCAN socket
    const int sock = socket(PF_CAN, SOCK_RAW, CAN_RAW);
    if (sock < 0)
    {
        return -1;
    }

    // Resolve the iface index
    struct ifreq ifr;
    (void)memset(&ifr, 0, sizeof(ifr));
    (void)strncpy(ifr.ifr_name, can_iface_name, IFNAMSIZ);
    const int ioctl_result = ioctl(sock, SIOCGIFINDEX, &ifr);
    if (ioctl_result < 0)
    {
        return -1;
    }

    // Assign the iface
    struct sockaddr_can addr;
    (void)memset(&addr, 0, sizeof(addr));
    addr.can_family  = AF_CAN;
    addr.can_ifindex = ifr.ifr_ifindex;

    // Bind the socket to the iface
    const int bind_result = bind(sock, (struct sockaddr*)&addr, sizeof(addr));
    if (bind_result < 0)
    {
        return -1;
    }

    can_socket = sock;
    return 0;
}

int can_send(uint32_t extended_can_id, const uint8_t* frame_data, uint8_t frame_data_len)
{
    if (frame_data_len > 8 || frame_data == NULL)
    {
        return -1;
    }

    struct can_frame frame;
    frame.can_id  = extended_can_id | CAN_EFF_FLAG;
    frame.can_dlc = frame_data_len;
    memcpy(frame.data, frame_data, frame_data_len);

    return write(can_socket, &frame, sizeof(struct can_frame));
}

uint64_t get_monotonic_usec(void)
{
    struct timespec ts;
    if (clock_gettime(CLOCK_MONOTONIC, &ts) != 0)
    {
        return 0;
    }
    return (uint64_t)ts.tv_sec * 1000000ULL + (uint64_t)ts.tv_nsec / 1000UL;
}
/*
 * End of the platform-specific part
 */

/*
 * DroneCAN transport layer
 */
/// Arbitrary priority values
static const uint8_t PRIORITY_HIGHEST = 0;
static const uint8_t PRIORITY_HIGH    = 8;
static const uint8_t PRIORITY_MEDIUM  = 16;
static const uint8_t PRIORITY_LOW     = 24;
static const uint8_t PRIORITY_LOWEST  = 31;

static uint8_t uavcan_node_id;

int uavcan_broadcast(uint8_t priority,
                     uint16_t data_type_id,
                     uint8_t transfer_id,
                     const uint8_t* payload,
                     uint16_t payload_len)
{
    if (payload == NULL || payload_len > 7)
    {
        return -1;  // In this super-simple implementation we don't support multi-frame transfers.
    }

    if (priority > 31)
    {
        return -1;
    }

    const uint32_t can_id = ((uint32_t)priority << 24) | ((uint32_t)data_type_id << 8) | (uint32_t)uavcan_node_id;

    uint8_t payload_with_tail_byte[8];

    memcpy(payload_with_tail_byte, payload, payload_len);

    payload_with_tail_byte[payload_len] = 0xC0 | (transfer_id & 31);

    return can_send(can_id, payload_with_tail_byte, payload_len + 1);
}

/*
 * Float16 support
 */
uint16_t make_float16(float value)
{
    union fp32
    {
        uint32_t u;
        float f;
    };

    const union fp32 f32infty = { 255U << 23 };
    const union fp32 f16infty = { 31U << 23 };
    const union fp32 magic = { 15U << 23 };
    const uint32_t sign_mask = 0x80000000U;
    const uint32_t round_mask = ~0xFFFU;

    union fp32 in;
    uint16_t out = 0;

    in.f = value;

    uint32_t sign = in.u & sign_mask;
    in.u ^= sign;

    if (in.u >= f32infty.u)
    {
        out = (in.u > f32infty.u) ? 0x7FFFU : 0x7C00U;
    }
    else
    {
        in.u &= round_mask;
        in.f *= magic.f;
        in.u -= round_mask;
        if (in.u > f16infty.u)
        {
            in.u = f16infty.u;
        }
        out = (uint16_t)(in.u >> 13);
    }

    out |= (uint16_t)(sign >> 16);

    return out;
}

/*
 * Application logic
 */
/// Defined for the standard data type uavcan.protocol.NodeStatus
enum node_health
{
    HEALTH_OK       = 0,
    HEALTH_WARNING  = 1,
    HEALTH_ERROR    = 2,
    HEALTH_CRITICAL = 3
};

/// Defined for the standard data type uavcan.protocol.NodeStatus
enum node_mode
{
    MODE_OPERATIONAL     = 0,
    MODE_INITIALIZATION  = 1,
    MODE_MAINTENANCE     = 2,
    MODE_SOFTWARE_UPDATE = 3,
    MODE_OFFLINE         = 7
};

/// Standard data type: uavcan.protocol.NodeStatus
int publish_node_status(enum node_health health, enum node_mode mode, uint16_t vendor_specific_status_code)
{
    static uint64_t startup_timestamp_usec;
    if (startup_timestamp_usec == 0)
    {
        startup_timestamp_usec = get_monotonic_usec();
    }

    uint8_t payload[7];

    // Uptime in seconds
    const uint32_t uptime_sec = (get_monotonic_usec() - startup_timestamp_usec) / 1000000ULL;
    payload[0] = (uptime_sec >> 0)  & 0xFF;
    payload[1] = (uptime_sec >> 8)  & 0xFF;
    payload[2] = (uptime_sec >> 16) & 0xFF;
    payload[3] = (uptime_sec >> 24) & 0xFF;

    // Health and mode
    payload[4] = ((uint8_t)health << 6) | ((uint8_t)mode << 3);

    // Vendor-specific status code
    payload[5] = (vendor_specific_status_code >> 0) & 0xFF;
    payload[6] = (vendor_specific_status_code >> 8) & 0xFF;

    static const uint16_t data_type_id = 341;
    static uint8_t transfer_id;

    return uavcan_broadcast(PRIORITY_LOW, data_type_id, transfer_id++, payload, sizeof(payload));
}

/// Standard data type: uavcan.equipment.air_data.TrueAirspeed
int publish_true_airspeed(float mean, float variance)
{
    const uint16_t f16_mean     = make_float16(mean);
    const uint16_t f16_variance = make_float16(variance);

    uint8_t payload[4];
    payload[0] = (f16_mean >> 0) & 0xFF;
    payload[1] = (f16_mean >> 8) & 0xFF;
    payload[2] = (f16_variance >> 0) & 0xFF;
    payload[3] = (f16_variance >> 8) & 0xFF;

    static const uint16_t data_type_id = 1020;
    static uint8_t transfer_id;
    transfer_id += 1;
    return uavcan_broadcast(PRIORITY_MEDIUM, data_type_id, transfer_id, payload, sizeof(payload));
}

int compute_true_airspeed(float* out_airspeed, float* out_variance)
{
    *out_airspeed = 1.2345F; // This is a stub.
    *out_variance = 0.0F;    // By convention, zero represents unknown error variance.
    return 0;
}

int main(int argc, char** argv)
{
    /*
     * Node initialization
     */
    if (argc < 3)
    {
        puts("Args: <self-node-id> <can-iface-name>");
        return 1;
    }

    uavcan_node_id = (uint8_t)atoi(argv[1]);
    if (uavcan_node_id < 1 || uavcan_node_id > 127)
    {
        printf("%i is not a valid node ID\n", (int)uavcan_node_id);
        return 1;
    }

    if (can_init(argv[2]) < 0)
    {
        printf("Failed to open iface %s\n", argv[2]);
        return 1;
    }

    /*
     * Main loop
     */
    enum node_health health = HEALTH_OK;

    for (;;)
    {
        float airspeed = 0.0F;
        float airspeed_variance = 0.0F;
        const int airspeed_computation_result = compute_true_airspeed(&airspeed, &airspeed_variance);

        if (airspeed_computation_result == 0)
        {
            const int publication_result = publish_true_airspeed(airspeed, airspeed_variance);
            health = (publication_result < 0) ? HEALTH_ERROR : HEALTH_OK;
        }
        else
        {
            health = HEALTH_ERROR;
        }

        const uint16_t vendor_specific_status_code = rand(); // Can be used to report vendor-specific status info

        (void)publish_node_status(health, MODE_OPERATIONAL, vendor_specific_status_code);

        usleep(500000);
    }
}

Testing against libuavcan

The following code can be used to test the node against the reference implementation of the DroneCAN stack.

/*
 * This program subscribes to airspeed messages using libuavcan, and prints them into stdout in YAML format.
 * It can be used to test alternative implementations of the DroneCAN stack against the reference implementation.
 *
 * GCC invocation command:
 *     g++ libuavcan_airspeed_subscriber.cpp -std=c++11 -lrt -luavcan
 *
 * Author: Pavel Kirienko <pavel.kirienko@zubax.com>
 * License: CC0, no copyright reserved
 * Language: C++11
 */

#include <iostream>
#include <algorithm>
#include <uavcan_linux/uavcan_linux.hpp>
#include <uavcan/equipment/air_data/TrueAirspeed.hpp>

uavcan_linux::NodePtr initNode(const std::vector<std::string>& ifaces, uavcan::NodeID nid, const std::string& name)
{
    auto node = uavcan_linux::makeNode(ifaces);
    node->setNodeID(nid);
    node->setName(name.c_str());

    if (node->start() < 0)
    {
        throw std::runtime_error("Failed to start DroneCAN node");
    }

    node->setModeOperational();
    return node;
}

template <typename DataType>
static void printMessage(const uavcan::ReceivedDataStructure<DataType>& msg)
{
    std::cout << "[" << DataType::getDataTypeFullName() << "]\n" << msg << "\n---" << std::endl;
}

template <typename DataType>
static std::shared_ptr<uavcan::Subscriber<DataType>> makePrintingSubscriber(const uavcan_linux::NodePtr& node)
{
    return node->makeSubscriber<DataType>(&printMessage<DataType>);
}

static void runForever(const uavcan_linux::NodePtr& node)
{
    auto sub_true_airspeed = makePrintingSubscriber<uavcan::equipment::air_data::TrueAirspeed>(node);

    while (true)
    {
        const int res = node->spin(uavcan::MonotonicDuration::getInfinite());
        if (res < 0)
        {
            node->logError("spin", "Error %*", res);
        }
    }
}

int main(int argc, const char** argv)
{
    if (argc < 3)
    {
        std::cout << "Usage:\n\t" << argv[0] << " <node-id> <can-iface-name-1> [can-iface-name-N...]" << std::endl;
        return 1;
    }
    const int self_node_id = std::stoi(argv[1]);
    std::vector<std::string> iface_names(argv + 2, argv + argc);
    uavcan_linux::NodePtr node = initNode(iface_names, self_node_id, "org.uavcan.example.airspeed_subscriber");
    runForever(node);
    return 0;
}


Examples

  • Simple sensor node

License

This work is licensed under a Creative Commons Attribution 4.0 International License.
Much of the content of this site is based upon prior work by Pavel Kirienko and the UAVCAN Development Team.
  • https://dronecan.org/discord
  • https://github.com/DroneCAN
  • Report a problem with this website

Generated Sun, 02 Jun 2024 21:47:21 +0000 © DroneCAN development team