writer.c
3.33 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
/*
* 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 Dynamixel messages writer
*
* @author Loïc Dauphin <loic.dauphin@inria.fr>
*
* @}
*/
#include "dynamixel_writer.h"
#include "dynamixel_crc.h"
#include <string.h>
#define LOW(v) (v & 0xFF)
#define HIGH(v) ((v >> 8) & 0xFF)
void dynamixel_writer_init(dynamixel_writer_t *writer, uint8_t *buffer, size_t limit)
{
writer->buffer = buffer;
writer->size = 0;
writer->limit = limit;
}
const uint8_t *dynamixel_writer_get_data(const dynamixel_writer_t *writer)
{
return (const uint8_t*)writer->buffer;
}
size_t dynamixel_writer_get_size(const dynamixel_writer_t *writer)
{
return writer->size;
}
void dynamixel_writer_ping_make(dynamixel_writer_t *writer, uint8_t id)
{
const size_t len = 3;
if (len + 7 <= writer->limit) {
writer->size = len + 7;
writer->buffer[0] = DXL_HEADER[0];
writer->buffer[1] = DXL_HEADER[1];
writer->buffer[2] = DXL_HEADER[2];
writer->buffer[3] = 0x00; /* reserved */
writer->buffer[4] = id;
writer->buffer[5] = LOW(len);
writer->buffer[6] = HIGH(len);
writer->buffer[7] = DXL_INST_PING;
uint16_t crc = dynamixel_crc_update(0, writer->buffer, 8);
writer->buffer[8] = LOW(crc);
writer->buffer[9] = HIGH(crc);
}
else {
writer->size = 0;
}
}
void dynamixel_writer_write_make(dynamixel_writer_t *writer, uint8_t id, uint16_t reg, const uint8_t *buffer, size_t size)
{
const size_t len = 5 + size;
if (len + 7 <= writer->limit) {
writer->size = len + 7;
writer->buffer[0] = DXL_HEADER[0];
writer->buffer[1] = DXL_HEADER[1];
writer->buffer[2] = DXL_HEADER[2];
writer->buffer[3] = 0x00; /* reserved */
writer->buffer[4] = id;
writer->buffer[5] = LOW(len);
writer->buffer[6] = HIGH(len);
writer->buffer[7] = DXL_INST_WRITE;
writer->buffer[8] = LOW(reg);
writer->buffer[9] = HIGH(reg);
memcpy(&writer->buffer[10], buffer, size);
uint16_t crc = dynamixel_crc_update(0, writer->buffer, len + 5);
writer->buffer[writer->size - 2] = LOW(crc);
writer->buffer[writer->size - 1] = HIGH(crc);
}
else {
writer->size = 0;
}
}
void dynamixel_writer_read_make(dynamixel_writer_t *writer, uint8_t id, uint16_t reg, size_t size)
{
const size_t len = 7;
if (len + 7 <= writer->limit) {
writer->size = len + 7;
writer->buffer[0] = DXL_HEADER[0];
writer->buffer[1] = DXL_HEADER[1];
writer->buffer[2] = DXL_HEADER[2];
writer->buffer[3] = 0x00; /* reserved */
writer->buffer[4] = id;
writer->buffer[5] = LOW(len);
writer->buffer[6] = HIGH(len);
writer->buffer[7] = DXL_INST_READ;
writer->buffer[8] = LOW(reg);
writer->buffer[9] = HIGH(reg);
writer->buffer[10] = LOW(size);
writer->buffer[11] = HIGH(size);
uint16_t crc = dynamixel_crc_update(0, writer->buffer, 12);
writer->buffer[12] = LOW(crc);
writer->buffer[13] = HIGH(crc);
}
else {
writer->size = 0;
}
}