Line data Source code
1 : /*
2 : This program is free software: you can redistribute it and/or modify
3 : it under the terms of the GNU General Public License as published by
4 : the Free Software Foundation, either version 3 of the License, or
5 : (at your option) any later version.
6 :
7 : This program is distributed in the hope that it will be useful,
8 : but WITHOUT ANY WARRANTY; without even the implied warranty of
9 : MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
10 : GNU General Public License for more details.
11 :
12 : You should have received a copy of the GNU General Public License
13 : along with this program. If not, see <http://www.gnu.org/licenses/>.
14 : */
15 : /*
16 : implementation of Robotis Dynamixel 2.0 protocol for controlling servos
17 :
18 : Portions of this code are based on the dynamixel_sdk code:
19 : https://github.com/ROBOTIS-GIT/DynamixelSDK
20 : which is under the following license:
21 :
22 : * Copyright 2017 ROBOTIS CO., LTD.
23 : *
24 : * Licensed under the Apache License, Version 2.0 (the "License");
25 : * you may not use this file except in compliance with the License.
26 : * You may obtain a copy of the License at
27 : *
28 : * http://www.apache.org/licenses/LICENSE-2.0
29 : *
30 : * Unless required by applicable law or agreed to in writing, software
31 : * distributed under the License is distributed on an "AS IS" BASIS,
32 : * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
33 : * See the License for the specific language governing permissions and
34 : * limitations under the License.
35 : */
36 :
37 : #include "AP_RobotisServo.h"
38 :
39 : #if AP_ROBOTISSERVO_ENABLED
40 :
41 : #include <AP_HAL/AP_HAL.h>
42 : #include <AP_HAL/utility/sparse-endian.h>
43 : #include <AP_Math/AP_Math.h>
44 : #include <AP_SerialManager/AP_SerialManager.h>
45 : #include <SRV_Channel/SRV_Channel.h>
46 :
47 : extern const AP_HAL::HAL& hal;
48 :
49 : #define BROADCAST_ID 0xFE
50 : #define MAX_ID 0xFC
51 :
52 : // DXL protocol common commands
53 : #define INST_PING 1
54 : #define INST_READ 2
55 : #define INST_WRITE 3
56 : #define INST_REG_WRITE 4
57 : #define INST_ACTION 5
58 : #define INST_FACTORY_RESET 6
59 : #define INST_CLEAR 16
60 : #define INST_SYNC_WRITE 131
61 : #define INST_BULK_READ 146
62 :
63 : // 2.0 protocol commands
64 : #define INST_REBOOT 8
65 : #define INST_STATUS 85
66 : #define INST_SYNC_READ 130
67 : #define INST_BULK_WRITE 147
68 :
69 : // 2.0 protocol packet offsets
70 : #define PKT_HEADER0 0
71 : #define PKT_HEADER1 1
72 : #define PKT_HEADER2 2
73 : #define PKT_RESERVED 3
74 : #define PKT_ID 4
75 : #define PKT_LENGTH_L 5
76 : #define PKT_LENGTH_H 6
77 : #define PKT_INSTRUCTION 7
78 : #define PKT_ERROR 8
79 : #define PKT_PARAMETER0 8
80 :
81 : /* Macro for Control Table Value */
82 : #define DXL_MAKEWORD(a, b) ((uint16_t)(((uint8_t)(((uint64_t)(a)) & 0xff)) | ((uint16_t)((uint8_t)(((uint64_t)(b)) & 0xff))) << 8))
83 : #define DXL_MAKEDWORD(a, b) ((uint32_t)(((uint16_t)(((uint64_t)(a)) & 0xffff)) | ((uint32_t)((uint16_t)(((uint64_t)(b)) & 0xffff))) << 16))
84 : #define DXL_LOWORD(l) ((uint16_t)(((uint64_t)(l)) & 0xffff))
85 : #define DXL_HIWORD(l) ((uint16_t)((((uint64_t)(l)) >> 16) & 0xffff))
86 : #define DXL_LOBYTE(w) ((uint8_t)(((uint64_t)(w)) & 0xff))
87 : #define DXL_HIBYTE(w) ((uint8_t)((((uint64_t)(w)) >> 8) & 0xff))
88 :
89 : // register offsets
90 : #define REG_OPERATING_MODE 11
91 : #define OPMODE_CURR_CONTROL 0
92 : #define OPMODE_VEL_CONTROL 1
93 : #define OPMODE_POS_CONTROL 3
94 : #define OPMODE_EXT_POS_CONTROL 4
95 :
96 : #define REG_TORQUE_ENABLE 64
97 :
98 : #define REG_STATUS_RETURN 68
99 : #define STATUS_RETURN_NONE 0
100 : #define STATUS_RETURN_READ 1
101 : #define STATUS_RETURN_ALL 2
102 :
103 : #define REG_GOAL_POSITION 116
104 :
105 : // how many times to send servo configure msgs
106 : #define CONFIGURE_SERVO_COUNT 4
107 :
108 : // how many times to send servo detection
109 : #define DETECT_SERVO_COUNT 4
110 :
111 : const AP_Param::GroupInfo AP_RobotisServo::var_info[] = {
112 :
113 : // @Param: POSMIN
114 : // @DisplayName: Robotis servo position min
115 : // @Description: Position minimum at servo min value. This should be within the position control range of the servos, normally 0 to 4095
116 : // @Range: 0 4095
117 : // @User: Standard
118 : AP_GROUPINFO("POSMIN", 1, AP_RobotisServo, pos_min, 0),
119 :
120 : // @Param: POSMAX
121 : // @DisplayName: Robotis servo position max
122 : // @Description: Position maximum at servo max value. This should be within the position control range of the servos, normally 0 to 4095
123 : // @Range: 0 4095
124 : // @User: Standard
125 : AP_GROUPINFO("POSMAX", 2, AP_RobotisServo, pos_max, 4095),
126 :
127 : AP_GROUPEND
128 : };
129 :
130 : // constructor
131 2094 : AP_RobotisServo::AP_RobotisServo(void)
132 : {
133 : // set defaults from the parameter table
134 2094 : AP_Param::setup_object_defaults(this, var_info);
135 2094 : }
136 :
137 2056 : void AP_RobotisServo::init(void)
138 : {
139 2056 : AP_SerialManager &serial_manager = AP::serialmanager();
140 2056 : port = serial_manager.find_serial(AP_SerialManager::SerialProtocol_Robotis,0);
141 2056 : if (port) {
142 0 : baudrate = serial_manager.find_baudrate(AP_SerialManager::SerialProtocol_Robotis, 0);
143 0 : us_per_byte = 10 * 1e6 / baudrate;
144 0 : us_gap = 4 * 1e6 / baudrate;
145 : }
146 2056 : }
147 :
148 : /*
149 : addStuffing() from Robotis SDK. This pads the packet as required by the protocol
150 : */
151 0 : void AP_RobotisServo::add_stuffing(uint8_t *packet)
152 : {
153 0 : int packet_length_in = DXL_MAKEWORD(packet[PKT_LENGTH_L], packet[PKT_LENGTH_H]);
154 0 : int packet_length_out = packet_length_in;
155 :
156 0 : if (packet_length_in < 8) {
157 : // INSTRUCTION, ADDR_L, ADDR_H, CRC16_L, CRC16_H + FF FF FD
158 0 : return;
159 : }
160 :
161 : uint8_t *packet_ptr;
162 0 : uint16_t packet_length_before_crc = packet_length_in - 2;
163 0 : for (uint16_t i = 3; i < packet_length_before_crc; i++) {
164 0 : packet_ptr = &packet[i+PKT_INSTRUCTION-2];
165 0 : if (packet_ptr[0] == 0xFF && packet_ptr[1] == 0xFF && packet_ptr[2] == 0xFD) {
166 0 : packet_length_out++;
167 : }
168 : }
169 :
170 0 : if (packet_length_in == packet_length_out) {
171 : // no stuffing required
172 0 : return;
173 : }
174 :
175 0 : uint16_t out_index = packet_length_out + 6 - 2; // last index before crc
176 0 : uint16_t in_index = packet_length_in + 6 - 2; // last index before crc
177 :
178 0 : while (out_index != in_index) {
179 0 : if (packet[in_index] == 0xFD && packet[in_index-1] == 0xFF && packet[in_index-2] == 0xFF) {
180 0 : packet[out_index--] = 0xFD; // byte stuffing
181 0 : if (out_index != in_index) {
182 0 : packet[out_index--] = packet[in_index--]; // FD
183 0 : packet[out_index--] = packet[in_index--]; // FF
184 0 : packet[out_index--] = packet[in_index--]; // FF
185 : }
186 : } else {
187 0 : packet[out_index--] = packet[in_index--];
188 : }
189 : }
190 :
191 0 : packet[PKT_LENGTH_L] = DXL_LOBYTE(packet_length_out);
192 0 : packet[PKT_LENGTH_H] = DXL_HIBYTE(packet_length_out);
193 : }
194 :
195 : /*
196 : send a protocol 2.0 packet
197 : */
198 0 : void AP_RobotisServo::send_packet(uint8_t *txpacket)
199 : {
200 0 : add_stuffing(txpacket);
201 :
202 : // check max packet length
203 0 : uint16_t total_packet_length = DXL_MAKEWORD(txpacket[PKT_LENGTH_L], txpacket[PKT_LENGTH_H]) + 7;
204 :
205 : // make packet header
206 0 : txpacket[PKT_HEADER0] = 0xFF;
207 0 : txpacket[PKT_HEADER1] = 0xFF;
208 0 : txpacket[PKT_HEADER2] = 0xFD;
209 0 : txpacket[PKT_RESERVED] = 0x00;
210 :
211 : // add CRC16
212 0 : uint16_t crc = crc_crc16_ibm(0, txpacket, total_packet_length - 2); // 2: CRC16
213 0 : txpacket[total_packet_length - 2] = DXL_LOBYTE(crc);
214 0 : txpacket[total_packet_length - 1] = DXL_HIBYTE(crc);
215 :
216 0 : port->write(txpacket, total_packet_length);
217 :
218 0 : delay_time_us += total_packet_length * us_per_byte + us_gap;
219 0 : }
220 :
221 : /*
222 : use a broadcast ping to find attached servos
223 : */
224 0 : void AP_RobotisServo::detect_servos(void)
225 : {
226 0 : uint8_t txpacket[10] {};
227 :
228 0 : txpacket[PKT_ID] = BROADCAST_ID;
229 0 : txpacket[PKT_LENGTH_L] = 3;
230 0 : txpacket[PKT_LENGTH_H] = 0;
231 0 : txpacket[PKT_INSTRUCTION] = INST_PING;
232 :
233 0 : send_packet(txpacket);
234 :
235 : // give plenty of time for replies from all servos
236 0 : last_send_us = AP_HAL::micros();
237 0 : delay_time_us += 1000 * us_per_byte;
238 0 : }
239 :
240 : /*
241 : broadcast configure all servos
242 : */
243 0 : void AP_RobotisServo::configure_servos(void)
244 : {
245 : // disable torque control
246 0 : send_command(BROADCAST_ID, REG_TORQUE_ENABLE, 0, 1);
247 :
248 : // disable replies unless we read
249 0 : send_command(BROADCAST_ID, REG_STATUS_RETURN, STATUS_RETURN_READ, 1);
250 :
251 : // use position control mode
252 0 : send_command(BROADCAST_ID, REG_OPERATING_MODE, OPMODE_POS_CONTROL, 1);
253 :
254 : // enable torque control
255 0 : send_command(BROADCAST_ID, REG_TORQUE_ENABLE, 1, 1);
256 0 : }
257 :
258 :
259 : /*
260 : send a command to a single servo, changing a register value
261 : */
262 0 : void AP_RobotisServo::send_command(uint8_t id, uint16_t reg, uint32_t value, uint8_t len)
263 : {
264 0 : uint8_t txpacket[16] {};
265 :
266 0 : txpacket[PKT_ID] = id;
267 0 : txpacket[PKT_LENGTH_L] = 5 + len;
268 0 : txpacket[PKT_LENGTH_H] = 0;
269 0 : txpacket[PKT_INSTRUCTION] = INST_WRITE;
270 0 : txpacket[PKT_INSTRUCTION+1] = DXL_LOBYTE(reg);
271 0 : txpacket[PKT_INSTRUCTION+2] = DXL_HIBYTE(reg);
272 :
273 : // Register values are transmitted as little-endian.
274 0 : value = htole32(value);
275 0 : memcpy(&txpacket[PKT_INSTRUCTION+3], &value, MIN(len,4));
276 :
277 0 : send_packet(txpacket);
278 0 : }
279 :
280 : /*
281 : read response bytes
282 : */
283 0 : void AP_RobotisServo::read_bytes(void)
284 : {
285 0 : uint32_t n = port->available();
286 0 : if (n == 0 && pktbuf_ofs < PKT_INSTRUCTION) {
287 0 : return;
288 : }
289 :
290 0 : if (n > sizeof(pktbuf) - pktbuf_ofs) {
291 0 : n = sizeof(pktbuf) - pktbuf_ofs;
292 : }
293 0 : for (uint8_t i=0; i<n; i++) {
294 0 : pktbuf[pktbuf_ofs++] = port->read();
295 : }
296 :
297 : // discard bad leading data. This should be rare
298 0 : while (pktbuf_ofs >= 4 &&
299 0 : (pktbuf[0] != 0xFF || pktbuf[1] != 0xFF || pktbuf[2] != 0xFD || pktbuf[3] != 0x00)) {
300 0 : memmove(pktbuf, &pktbuf[1], pktbuf_ofs-1);
301 0 : pktbuf_ofs--;
302 : }
303 :
304 0 : if (pktbuf_ofs < 10) {
305 : // not enough data yet
306 0 : return;
307 : }
308 :
309 0 : const uint16_t total_packet_length = DXL_MAKEWORD(pktbuf[PKT_LENGTH_L], pktbuf[PKT_LENGTH_H]) + PKT_INSTRUCTION;
310 0 : if (total_packet_length > sizeof(pktbuf)) {
311 0 : pktbuf_ofs = 0;
312 0 : return;
313 : }
314 0 : if (pktbuf_ofs < total_packet_length) {
315 : // more data needed
316 0 : return;
317 : }
318 :
319 : // check CRC
320 0 : const uint16_t crc = DXL_MAKEWORD(pktbuf[total_packet_length-2], pktbuf[total_packet_length-1]);
321 0 : const uint16_t calc_crc = crc_crc16_ibm(0, pktbuf, total_packet_length - 2);
322 0 : if (calc_crc != crc) {
323 0 : memmove(pktbuf, &pktbuf[total_packet_length], pktbuf_ofs - total_packet_length);
324 0 : pktbuf_ofs -= total_packet_length;
325 0 : return;
326 : }
327 :
328 : // process full packet
329 0 : process_packet(pktbuf, total_packet_length);
330 :
331 0 : memmove(pktbuf, &pktbuf[total_packet_length], pktbuf_ofs - total_packet_length);
332 0 : pktbuf_ofs -= total_packet_length;
333 : }
334 :
335 : /*
336 : process a packet from a servo
337 : */
338 0 : void AP_RobotisServo::process_packet(const uint8_t *pkt, uint8_t length)
339 : {
340 0 : uint8_t id = pkt[PKT_ID];
341 0 : if (id > 16 || id < 1) {
342 : // discard packets from servos beyond max or min. Note that we
343 : // don't allow servo 0, to make mapping to SERVOn_* parameters
344 : // easier
345 0 : return;
346 : }
347 0 : uint32_t id_mask = (1U<<(id-1));
348 0 : if (!(id_mask & servo_mask)) {
349 : // mark the servo as present
350 0 : servo_mask |= id_mask;
351 0 : DEV_PRINTF("Robotis: new servo %u\n", id);
352 : }
353 : }
354 :
355 :
356 41692164 : void AP_RobotisServo::update()
357 : {
358 41692164 : if (!initialised) {
359 2056 : initialised = true;
360 2056 : init();
361 2056 : last_send_us = AP_HAL::micros();
362 2056 : return;
363 : }
364 :
365 41690108 : if (port == nullptr) {
366 41690108 : return;
367 : }
368 :
369 0 : read_bytes();
370 :
371 0 : uint32_t now = AP_HAL::micros();
372 0 : if (last_send_us != 0 && now - last_send_us < delay_time_us) {
373 : // waiting for last send to complete
374 0 : return;
375 : }
376 :
377 0 : if (detection_count < DETECT_SERVO_COUNT) {
378 0 : detection_count++;
379 0 : detect_servos();
380 0 : return;
381 : }
382 :
383 0 : if (servo_mask == 0) {
384 0 : return;
385 : }
386 :
387 0 : if (configured_servos < CONFIGURE_SERVO_COUNT) {
388 0 : configured_servos++;
389 0 : last_send_us = now;
390 0 : configure_servos();
391 0 : return;
392 : }
393 :
394 0 : last_send_us = now;
395 0 : delay_time_us = 0;
396 :
397 : // loop for all 16 channels
398 0 : for (uint8_t i=0; i<NUM_SERVO_CHANNELS; i++) {
399 0 : if (((1U<<i) & servo_mask) == 0) {
400 0 : continue;
401 : }
402 0 : SRV_Channel *c = SRV_Channels::srv_channel(i);
403 0 : if (c == nullptr) {
404 0 : continue;
405 : }
406 0 : const uint16_t pwm = c->get_output_pwm();
407 0 : const uint16_t min = c->get_output_min();
408 0 : const uint16_t max = c->get_output_max();
409 0 : float v = float(pwm - min) / (max - min);
410 0 : uint32_t value = pos_min + v * (pos_max - pos_min);
411 0 : send_command(i+1, REG_GOAL_POSITION, value, 4);
412 : }
413 : }
414 :
415 : #endif // AP_ROBOTISSERVO_ENABLED
|
