dynamixel_reader.h
3.3 KB
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
/*
* Copyright (C) 2017 Inria
*
* This file is subject to the terms and conditions of the GNU Lesser General
* Public License v2.1. See the file LICENSE in the top level directory for more
* details.
*/
/**
* @ingroup drivers_dynamixel
*
* @{
*
* @file
* @brief Interface definition for Dynamixel packet reader
*
* @author Loïc Dauphin <loic.dauphin@inria.fr>
*/
#ifndef DYNAMIXEL_READER_H
#define DYNAMIXEL_READER_H
#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)
/**
* @brief Dynamixel packet reader struct
*/
typedef struct {
const uint8_t *buffer; /**< data buffer */
size_t size; /**< data buffer's size */
} dynamixel_reader_t;
/**
* @brief Initialize the Dynamixel packet reader
*
* @param[out] reader the packet reader
* @param[in] buffer the buffer used to store data
* @param[in] size the size of the buffer
*/
static inline void dynamixel_reader_init(dynamixel_reader_t *reader, const uint8_t *buffer, size_t size)
{
reader->buffer = buffer;
reader->size = size;
}
/**
* @brief Check if the packet is valid
*
* @param[in] reader the packet reader
*
* @return true if the packet is valid
* @return false otherwise
*/
bool dynamixel_reader_is_valid(const dynamixel_reader_t *reader);
/**
* @brief Get the packet's device id
*
* @param[in] reader the packet reader
*
* @return the packet's device id
*/
static inline uint8_t dynamixel_reader_get_id(const dynamixel_reader_t *reader)
{
return reader->buffer[4];
}
/**
* @brief Get the packet's instruction code
*
* @param[in] reader the packet reader
*
* @return the packet's instruction code
*/
static inline uint8_t dynamixel_reader_get_instr(const dynamixel_reader_t *reader)
{
return reader->buffer[7];
}
/**
* @brief Get the packet's length field
*
* @param[in] reader the packet reader
*
* @return the packet's length field
*/
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);
}
/**
* @brief Get the packet's crc
*
* @param[in] reader the packet reader
*
* @return the packet's length field
*/
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);
}
/**
* @brief Get the packet's payload (response)
*
* @param[in] reader the packet reader
*
* @return the addess of the begining of the payload
*/
static inline const uint8_t *dynamixel_reader_status_get_payload(const dynamixel_reader_t *reader)
{
return &reader->buffer[9];
}
/**
* @brief Get the packet's payload size (response)
*
* @param[in] reader the packet reader
*
* @return the size of the payload
*/
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
#endif /* DYNAMIXEL_READER_H */
/** @} */