2 * Copyright Droids Corporation, Microb Technology, Eirbot (2008)
3 * JD Brossillon <jean.damien.brossillon@gmail.com> (main job)
4 * Olivier Matz <zer0@droids-corp.org> (clean-up, fixes)
6 * This program is free software; you can redistribute it and/or modify
7 * it under the terms of the GNU General Public License as published by
8 * the Free Software Foundation; either version 2 of the License, or
9 * (at your option) any later version.
11 * This program is distributed in the hope that it will be useful,
12 * but WITHOUT ANY WARRANTY; without even the implied warranty of
13 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
14 * GNU General Public License for more details.
16 * You should have received a copy of the GNU General Public License
17 * along with this program; if not, write to the Free Software
18 * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
20 * Revision : $Id: ax12.c,v 1.1.4.3 2008-12-26 13:48:37 zer0 Exp $
24 /* See some help in ax12.h */
29 #ifdef CONFIG_MODULE_SCHEDULER
30 #include <scheduler.h>
33 void AX12_init(AX12 *s)
35 s->hardware_send = NULL;
36 s->hardware_recv = NULL;
37 s->hardware_switch = NULL;
40 /*_________________________________________________________*/
41 /*_________________________________________________________*/
43 void AX12_set_hardware_send(AX12 *s, int8_t(*pf)(uint8_t))
45 s->hardware_send = pf;
48 void AX12_set_hardware_recv(AX12 *s, int16_t(*pf)(void))
50 s->hardware_recv = pf;
53 void AX12_set_hardware_switch(AX12 *s, void(*pf)(uint8_t))
55 s->hardware_switch = pf;
58 /*_________________________________________________________*/
59 /*_________________________________________________________*/
61 /* process the cksum of a packet */
62 uint8_t AX12_checksum(AX12_Packet *packet)
67 checksum += packet->id;
68 checksum += packet->instruction;
70 /* Packet also contain length = number of params + 2 */
71 checksum += packet->nparams + 2;
73 for (i=0; i<packet->nparams; i++)
74 checksum += packet->params[i];
79 /* Send a packet to ax12. Return 0 on sucess. */
80 uint8_t AX12_send(AX12 *s, AX12_Packet *packet)
86 #ifdef CONFIG_MODULE_SCHEDULER
87 uint8_t scheduler_prio;
89 scheduler_prio = scheduler_disable_save();
92 if (packet->nparams > AX12_MAX_PARAMS) {
93 err = AX12_ERROR_TYPE_INVALID_PACKET;
97 /* Switch line to write */
98 s->hardware_switch(AX12_STATE_WRITE);
101 if (s->hardware_send(0xFF)) {
102 err = AX12_ERROR_TYPE_XMIT_FAILED;
106 if (s->hardware_send(0xFF)) {
107 err = AX12_ERROR_TYPE_XMIT_FAILED;
112 if (s->hardware_send(packet->id)) {
113 err = AX12_ERROR_TYPE_XMIT_FAILED;
118 if (s->hardware_send(packet->nparams + 2)) {
119 err = AX12_ERROR_TYPE_XMIT_FAILED;
124 if (s->hardware_send(packet->instruction)) {
125 err = AX12_ERROR_TYPE_XMIT_FAILED;
130 for (i=0; i<(packet->nparams); i++) {
131 if (s->hardware_send(packet->params[i])) {
132 err = AX12_ERROR_TYPE_XMIT_FAILED;
137 IRQ_LOCK(flags); /* needed if hardware_switch is
141 if (s->hardware_send(AX12_checksum(packet))) {
143 err = AX12_ERROR_TYPE_XMIT_FAILED;
147 /* Switch line back to read */
148 s->hardware_switch(AX12_STATE_READ);
152 #ifdef CONFIG_MODULE_SCHEDULER
153 scheduler_enable_restore(scheduler_prio);
158 /* Switch line back to read */
159 s->hardware_switch(AX12_STATE_READ);
160 #ifdef CONFIG_MODULE_SCHEDULER
161 scheduler_enable_restore(scheduler_prio);
166 /* Receive a packet from ax12. Return 0 on sucess. */
167 uint8_t AX12_recv(AX12 *s, AX12_Packet *packet)
173 /* Switch line to read */
174 s->hardware_switch(AX12_STATE_READ);
176 c = s->hardware_recv();
178 return AX12_ERROR_TYPE_NO_ANSWER;
180 return AX12_ERROR_TYPE_INVALID_PACKET;
182 c = s->hardware_recv();
184 return AX12_ERROR_TYPE_TIMEOUT;
186 return AX12_ERROR_TYPE_INVALID_PACKET;
188 c = s->hardware_recv();
190 return AX12_ERROR_TYPE_TIMEOUT;
191 packet->id = c; /* we should check id ? */
193 c = s->hardware_recv();
195 return AX12_ERROR_TYPE_TIMEOUT;
197 packet->nparams = length - 2;
198 if (packet->nparams > AX12_MAX_PARAMS)
199 return AX12_ERROR_TYPE_INVALID_PACKET;
201 c = s->hardware_recv();
203 return AX12_ERROR_TYPE_TIMEOUT;
206 return packet->error;
208 for (i=0; i<(length-2); i++) {
209 c = s->hardware_recv();
211 return AX12_ERROR_TYPE_TIMEOUT;
212 packet->params[i] = c;
216 c = s->hardware_recv();
218 if (c != AX12_checksum(packet))
219 return AX12_ERROR_TYPE_BAD_CKSUM;
224 /*_________________________________________________________*/
225 /*_________________________________________________________*/
227 /* Write a byte to an adress. Return 0 on success. */
228 uint8_t AX12_write_byte(AX12 *s, uint8_t id, AX12_ADDRESS address,
234 memset(&p, 0, sizeof(p));
235 memset(&rp, 0, sizeof(rp));
238 p.instruction = AX12_WRITE;
242 p.params[0] = (uint8_t)address;
248 ret = AX12_send(s, &p);
252 /* We talk broadcast, no reply */
253 if (p.id == AX12_BROADCAST_ID)
257 return AX12_recv(s, &rp);
260 /* Write a word to an adress. Return 0 on success. */
261 uint8_t AX12_write_int(AX12 *s, uint8_t id, AX12_ADDRESS address,
267 memset(&p, 0, sizeof(p));
268 memset(&rp, 0, sizeof(rp));
271 p.instruction = AX12_WRITE;
275 p.params[0] = (uint8_t)address;
278 p.params[1] = 0xFF & data;
281 p.params[2] = data>>8;
284 ret = AX12_send(s, &p);
288 /* We talk broadcast, no reply */
289 if (p.id == AX12_BROADCAST_ID)
293 return AX12_recv(s, &rp);
296 /* Read a byte at given adress. On success, fill val and return 0,
297 * else return error and keep val unmodified. */
298 uint8_t AX12_read_byte(AX12 *s, uint8_t id, AX12_ADDRESS address,
304 memset(&p, 0, sizeof(p));
305 memset(&rp, 0, sizeof(rp));
308 p.instruction = AX12_READ;
312 p.params[0] = (uint8_t)address;
318 ret = AX12_send(s, &p);
322 ret = AX12_recv(s, &rp);
330 uint8_t AX12_read_int(AX12 *s, uint8_t id, AX12_ADDRESS address,
336 memset(&p, 0, sizeof(p));
337 memset(&rp, 0, sizeof(rp));
340 p.instruction = AX12_READ;
344 p.params[0] = (uint8_t)address;
350 ret = AX12_send(s, &p);
354 ret = AX12_recv(s, &rp);
358 *val = rp.params[0] + ((rp.params[1])<<8);
362 /*_________________________________________________________*/
363 /*_________________________________________________________*/
365 uint8_t AX12_set_position(AX12 *s,uint8_t id, uint16_t position)
367 return AX12_write_int(s, id, AA_GOAL_POSITION_L, position);
370 uint8_t AX12_set_position2(AX12 *s, uint8_t id, uint16_t position,
377 p.instruction = AX12_WRITE;
381 p.params[0] = AA_GOAL_POSITION_L;
383 p.params[1] = 0xFF & position;
384 p.params[2] = position>>8;
386 p.params[3] = 0xFF & speed;
387 p.params[4] = speed>>8;
390 ret = AX12_send(s, &p);
394 /* We talk broadcast, no reply */
395 if(p.id == AX12_BROADCAST_ID)
399 return AX12_recv(s, &rp);
402 uint8_t AX12_set_position3(AX12*s, uint8_t id, uint16_t position,
403 uint16_t speed, uint16_t torque)
409 p.instruction = AX12_WRITE;
413 p.params[0] = AA_GOAL_POSITION_L;
415 p.params[1] = 0xFF & position;
416 p.params[2] = position>>8;
418 p.params[3] = 0xFF & speed;
419 p.params[4] = speed>>8;
421 p.params[5] = 0xFF & torque;
422 p.params[6] = torque>>8;
425 ret = AX12_send(s, &p);
429 /* We talk broadcast, no reply */
430 if(p.id == AX12_BROADCAST_ID)
434 return AX12_recv(s, &rp);
437 uint8_t AX12_get_position(AX12 *s, uint8_t id, uint16_t *pos)
439 return AX12_read_int(s, id, AA_PRESENT_POSITION_L, pos);
442 uint8_t AX12_get_speed(AX12 *s, uint8_t id, uint16_t *speed)
444 return AX12_read_int(s, id, AA_PRESENT_SPEED_L, speed);
447 uint8_t AX12_get_load(AX12 *s, uint8_t id, uint16_t *load)
449 return AX12_read_int(s, id, AA_PRESENT_LOAD_L, load);
452 uint8_t AX12_ping(AX12 *s, uint8_t id)
458 p.instruction = AX12_PING;
462 ret = AX12_send(s, &p);
466 /* We talk broadcast, no reply */
467 if(p.id == AX12_BROADCAST_ID)
471 return AX12_recv(s, &rp);
474 uint8_t AX12_reset(AX12 *s, uint8_t id)
480 p.instruction = AX12_RESET;
484 ret = AX12_send(s, &p);
488 /* We talk broadcast, no reply */
489 if(p.id == AX12_BROADCAST_ID)
493 return AX12_recv(s, &rp);