dynamixel_reader.h

Go to the documentation of this file.

/*
 * SPDX-FileCopyrightText: 2017 Inria
 * SPDX-License-Identifier: LGPL-2.1-only
 */
 
#pragma once

 
#include <stdlib.h>
#include <stdbool.h>
 
#include "dynamixel_protocol.h"
 
#ifdef __cplusplus
extern "C" {
#endif
 
#define DXL_PING_SIZE        (10)
#define DXL_STATUS_SIZE(len) (11+len)
#define DXL_READ_SIZE        (14)
#define DXL_WRITE_SIZE(len)  (12+len)

typedef struct {
    const uint8_t *buffer; 
    size_t size;           
} dynamixel_reader_t;

static inline void dynamixel_reader_init(dynamixel_reader_t *reader, const uint8_t *buffer, size_t size)
{
    reader->buffer = buffer;
    reader->size = size;
}

bool dynamixel_reader_is_valid(const dynamixel_reader_t *reader);

static inline uint8_t dynamixel_reader_get_id(const dynamixel_reader_t *reader)
{
    return reader->buffer[4];
}

static inline uint8_t dynamixel_reader_get_instr(const dynamixel_reader_t *reader)
{
    return reader->buffer[7];
}

static inline uint16_t dynamixel_reader_get_length(const dynamixel_reader_t *reader)
{
    return
            (((uint16_t)reader->buffer[5]) & 0xFF) |
            ((((uint16_t)reader->buffer[6]) & 0xFF) << 8);
}

static inline uint16_t dynamixel_reader_get_crc(const dynamixel_reader_t *reader)
{
    return
            (((uint16_t)reader->buffer[reader->size - 2]) & 0xFF) |
            ((((uint16_t)reader->buffer[reader->size - 1]) & 0xFF) << 8);
}

static inline const uint8_t *dynamixel_reader_status_get_payload(const dynamixel_reader_t *reader)
{
    return &reader->buffer[9];
}

static inline size_t dynamixel_reader_status_get_payload_size(const dynamixel_reader_t *reader)
{
    return dynamixel_reader_get_length(reader) - 4;
}
 
#ifdef __cplusplus
}
#endif