vt100: include pgmspace.h as we use PROGMEM macro
[aversive.git] / mainboard / i2c_protocol.c
1 /*
2  *  Copyright Droids Corporation (2009)
3  *
4  *  This program is free software; you can redistribute it and/or modify
5  *  it under the terms of the GNU General Public License as published by
6  *  the Free Software Foundation; either version 2 of the License, or
7  *  (at your option) any later version.
8  *
9  *  This program is distributed in the hope that it will be useful,
10  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
11  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
12  *  GNU General Public License for more details.
13  *
14  *  You should have received a copy of the GNU General Public License
15  *  along with this program; if not, write to the Free Software
16  *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
17  *
18  *  Revision : $Id: i2c_protocol.c,v 1.8 2009-11-08 17:24:33 zer0 Exp $
19  *
20  */
21
22 #include <stdio.h>
23 #include <string.h>
24
25 #include <aversive/pgmspace.h>
26 #include <aversive/wait.h>
27 #include <aversive/error.h>
28
29 #include <ax12.h>
30 #include <uart.h>
31 #include <pwm_ng.h>
32 #include <i2c.h>
33 #include <clock_time.h>
34
35 #include <pid.h>
36 #include <quadramp.h>
37 #include <control_system_manager.h>
38 #include <trajectory_manager.h>
39 #include <vect_base.h>
40 #include <lines.h>
41 #include <polygon.h>
42 #include <obstacle_avoidance.h>
43 #include <blocking_detection_manager.h>
44 #include <robot_system.h>
45 #include <position_manager.h>
46
47 #include <rdline.h>
48 #include <parse.h>
49 #include <parse_string.h>
50 #include <parse_num.h>
51
52 #include "../common/i2c_commands.h"
53 #include "main.h"
54 #include "sensor.h"
55 #include "i2c_protocol.h"
56 #ifdef HOST_VERSION
57 #include "robotsim.h"
58 #endif
59
60 #define I2C_STATE_MAX 4
61
62 #define I2C_TIMEOUT 100 /* ms */
63 #define I2C_MAX_ERRORS 40
64
65 static volatile uint8_t i2c_poll_num = 0;
66 static volatile uint8_t i2c_state = 0;
67 static volatile uint16_t i2c_errors = 0;
68
69 #define OP_READY 0 /* no i2c op running */
70 #define OP_POLL  1 /* a user command is running */
71 #define OP_CMD   2 /* a polling (req / ans) is running */
72
73 static volatile uint8_t running_op = OP_READY;
74
75 #define I2C_MAX_LOG 3
76 #ifndef HOST_VERSION
77 static uint8_t error_log = 0;
78
79 static int8_t i2c_req_cobboard_status(void);
80 static int8_t i2c_req_ballboard_status(void);
81
82 #endif
83
84 /* used for commands */
85 uint8_t command_buf[I2C_SEND_BUFFER_SIZE];
86 volatile int8_t command_dest=-1;
87 volatile uint8_t command_size=0;
88
89 #define I2C_ERROR(args...) do {                                         \
90                 if (error_log < I2C_MAX_LOG) {                          \
91                         ERROR(E_USER_I2C_PROTO, args);                  \
92                         error_log ++;                                   \
93                         if (error_log == I2C_MAX_LOG) {                 \
94                                 ERROR(E_USER_I2C_PROTO,                 \
95                                       "i2c logs are now warnings");     \
96                         }                                               \
97                 }                                                       \
98                 else                                                    \
99                         WARNING(E_USER_I2C_PROTO, args);                \
100         } while(0)
101
102 void i2c_protocol_init(void)
103 {
104 }
105
106 void i2c_protocol_debug(void)
107 {
108 #ifdef HOST_VERSION
109         return;
110 #else
111         printf_P(PSTR("I2C protocol debug infos:\r\n"));
112         printf_P(PSTR("  i2c_state=%d\r\n"), i2c_state);
113         printf_P(PSTR("  i2c_errors=%d\r\n"), i2c_errors);
114         printf_P(PSTR("  running_op=%d\r\n"), running_op);
115         printf_P(PSTR("  command_size=%d\r\n"), command_size);
116         printf_P(PSTR("  command_dest=%d\r\n"), command_dest);
117         printf_P(PSTR("  i2c_status=%x\r\n"), i2c_status());
118 #endif
119 }
120
121 #ifndef HOST_VERSION
122 static void i2cproto_next_state(uint8_t inc)
123 {
124         i2c_state += inc;
125         if (i2c_state >= I2C_STATE_MAX) {
126                 i2c_state = 0;
127                 i2c_poll_num ++;
128         }
129 }
130
131 void i2cproto_wait_update(void)
132 {
133         uint8_t poll_num;
134         poll_num = i2c_poll_num;
135         WAIT_COND_OR_TIMEOUT((i2c_poll_num-poll_num) > 1, 150);
136 }
137
138 /* called periodically : the goal of this 'thread' is to send requests
139  * and read answers on i2c slaves in the correct order. */
140 void i2c_poll_slaves(void *dummy)
141 {
142         uint8_t flags;
143         int8_t err;
144         static uint8_t a = 0;
145
146         a++;
147         if (a & 0x4)
148                 LED2_TOGGLE();
149
150         /* already running */
151         IRQ_LOCK(flags);
152         if (running_op != OP_READY) {
153                 IRQ_UNLOCK(flags);
154                 return;
155         }
156
157         /* if a command is ready to be sent, so send it */
158         if (command_size) {
159                 running_op = OP_CMD;
160                 err = i2c_send(command_dest, command_buf, command_size,
161                              I2C_CTRL_GENERIC);
162                 if (err < 0)
163                         goto error;
164                 IRQ_UNLOCK(flags);
165                 return;
166         }
167
168         /* no command, so do the polling */
169         running_op = OP_POLL;
170
171         switch(i2c_state) {
172
173         /* poll status of cobboard */
174 #define I2C_REQ_COBBOARD 0
175         case I2C_REQ_COBBOARD:
176                 if ((err = i2c_req_cobboard_status()))
177                         goto error;
178                 break;
179
180 #define I2C_ANS_COBBOARD 1
181         case I2C_ANS_COBBOARD:
182                 if ((err = i2c_recv(I2C_COBBOARD_ADDR,
183                                     sizeof(struct i2c_ans_cobboard_status),
184                                     I2C_CTRL_GENERIC)))
185                         goto error;
186                 break;
187
188         /* poll status of ballboard */
189 #define I2C_REQ_BALLBOARD 2
190         case I2C_REQ_BALLBOARD:
191                 if ((err = i2c_req_ballboard_status()))
192                         goto error;
193                 break;
194
195 #define I2C_ANS_BALLBOARD 3
196         case I2C_ANS_BALLBOARD:
197                 if ((err = i2c_recv(I2C_BALLBOARD_ADDR,
198                                     sizeof(struct i2c_ans_ballboard_status),
199                                     I2C_CTRL_GENERIC)))
200                         goto error;
201                 break;
202
203         /* nothing, go to the first request */
204         default:
205                 i2c_state = 0;
206                 running_op = OP_READY;
207         }
208         IRQ_UNLOCK(flags);
209
210         return;
211
212  error:
213         running_op = OP_READY;
214         IRQ_UNLOCK(flags);
215         i2c_errors++;
216         if (i2c_errors > I2C_MAX_ERRORS) {
217                 I2C_ERROR("I2C send is_cmd=%d proto_state=%d "
218                       "err=%d i2c_status=%x", !!command_size, i2c_state, err, i2c_status());
219                 i2c_reset();
220                 i2c_errors = 0;
221         }
222 }
223
224 /* called when the xmit is finished */
225 void i2c_sendevent(int8_t size)
226 {
227         if (size > 0) {
228                 if (running_op == OP_POLL) {
229                         i2cproto_next_state(1);
230                 }
231                 else
232                         command_size = 0;
233         }
234         else {
235                 i2c_errors++;
236                 NOTICE(E_USER_I2C_PROTO, "send error state=%d size=%d "
237                         "op=%d", i2c_state, size, running_op);
238                 if (i2c_errors > I2C_MAX_ERRORS) {
239                         I2C_ERROR("I2C error, slave not ready");
240                         i2c_reset();
241                         i2c_errors = 0;
242                 }
243
244                 if (running_op == OP_POLL) {
245                         /* skip associated answer */
246                         i2cproto_next_state(2);
247                 }
248         }
249         running_op = OP_READY;
250 }
251
252 /* called rx event */
253 void i2c_recvevent(uint8_t * buf, int8_t size)
254 {
255         if (running_op == OP_POLL)
256                 i2cproto_next_state(1);
257
258         /* recv is only trigged after a poll */
259         running_op = OP_READY;
260
261         if (size < 0) {
262                 goto error;
263         }
264
265         switch (buf[0]) {
266
267         case I2C_ANS_COBBOARD_STATUS: {
268                 struct i2c_ans_cobboard_status * ans =
269                         (struct i2c_ans_cobboard_status *)buf;
270
271                 if (size != sizeof (*ans))
272                         goto error;
273
274                 /* status */
275                 cobboard.mode = ans->mode;
276                 cobboard.status = ans->status;
277                 cobboard.cob_count = ans->cob_count;
278                 cobboard.left_cobroller_speed = ans->left_cobroller_speed;
279                 cs_set_consign(&mainboard.left_cobroller.cs, cobboard.left_cobroller_speed);
280                 cobboard.right_cobroller_speed = ans->right_cobroller_speed;
281                 cs_set_consign(&mainboard.right_cobroller.cs, cobboard.right_cobroller_speed);
282
283                 break;
284         }
285
286         case I2C_ANS_BALLBOARD_STATUS: {
287                 uint8_t tmp;
288                 struct i2c_ans_ballboard_status * ans =
289                         (struct i2c_ans_ballboard_status *)buf;
290
291                 if (size != sizeof (*ans))
292                         goto error;
293                 ballboard.mode = ans->mode;
294                 ballboard.status = ans->status;
295                 ballboard.ball_count = ans->ball_count;
296                 tmp = ans->lcob;
297                 if (tmp != I2C_COB_NONE)
298                         ballboard.lcob = tmp;
299                 tmp = ans->rcob;
300                 if (tmp != I2C_COB_NONE)
301                         ballboard.rcob = tmp;
302                 break;
303         }
304
305         default:
306                 break;
307         }
308
309         return;
310  error:
311         i2c_errors++;
312         NOTICE(E_USER_I2C_PROTO, "recv error state=%d op=%d",
313                i2c_state, running_op);
314         if (i2c_errors > I2C_MAX_ERRORS) {
315                 I2C_ERROR("I2C error, slave not ready");
316                 i2c_reset();
317                 i2c_errors = 0;
318         }
319 }
320
321 void i2c_recvbyteevent(uint8_t hwstatus, uint8_t i, uint8_t c)
322 {
323 }
324 #endif /* !HOST_VERSION */
325
326 /* ******** ******** ******** ******** */
327 /* commands */
328 /* ******** ******** ******** ******** */
329
330
331 static int8_t
332 i2c_send_command(uint8_t addr, uint8_t * buf, uint8_t size)
333 {
334 #ifdef HOST_VERSION
335         return robotsim_i2c(addr, buf, size);
336 #else
337         uint8_t flags;
338         microseconds us = time_get_us2();
339
340         while ((time_get_us2() - us) < (I2C_TIMEOUT)*1000L) {
341                 IRQ_LOCK(flags);
342                 if (command_size == 0) {
343                         memcpy(command_buf, buf, size);
344                         command_size = size;
345                         command_dest = addr;
346                         IRQ_UNLOCK(flags);
347                         return 0;
348                 }
349                 IRQ_UNLOCK(flags);
350         }
351         /* this should not happen... except if we are called from an
352          * interrupt context, but it's forbidden */
353         I2C_ERROR("I2C command send failed");
354         return -EBUSY;
355 #endif
356 }
357
358 #ifndef HOST_VERSION
359 static int8_t i2c_req_cobboard_status(void)
360 {
361         struct i2c_req_cobboard_status buf;
362         int8_t err;
363
364         buf.hdr.cmd = I2C_REQ_COBBOARD_STATUS;
365         buf.lspickle = cobboard.lspickle;
366         buf.rspickle = cobboard.rspickle;
367         err = i2c_send(I2C_COBBOARD_ADDR, (uint8_t*)&buf,
368                         sizeof(buf), I2C_CTRL_GENERIC);
369
370         return err;
371 }
372
373 static int8_t i2c_req_ballboard_status(void)
374 {
375         struct i2c_req_ballboard_status buf;
376
377         buf.hdr.cmd = I2C_REQ_BALLBOARD_STATUS;
378         return i2c_send(I2C_BALLBOARD_ADDR, (uint8_t*)&buf,
379                         sizeof(buf), I2C_CTRL_GENERIC);
380 }
381 #endif /* !HOST_VERSION */
382
383 int8_t i2c_set_color(uint8_t addr, uint8_t color)
384 {
385         struct i2c_cmd_generic_color buf;
386
387         if (addr == I2C_BALLBOARD_ADDR)
388                 return 0; /* XXX disabled for now */
389         buf.hdr.cmd = I2C_CMD_GENERIC_SET_COLOR;
390         buf.color = color;
391         return i2c_send_command(addr, (uint8_t*)&buf, sizeof(buf));
392 }
393
394 int8_t i2c_led_control(uint8_t addr, uint8_t led, uint8_t state)
395 {
396         struct i2c_cmd_led_control buf;
397         buf.hdr.cmd = I2C_CMD_GENERIC_LED_CONTROL;
398         buf.led_num = led;
399         buf.state = state;
400         return i2c_send_command(addr, (uint8_t*)&buf, sizeof(buf));
401 }
402
403 int8_t i2c_cobboard_set_mode(uint8_t mode)
404 {
405 #ifdef HOST_VERSION
406         return robotsim_i2c_cobboard_set_mode(mode);
407 #else
408         struct i2c_cmd_cobboard_set_mode buf;
409         buf.hdr.cmd = I2C_CMD_COBBOARD_SET_MODE;
410         buf.mode = mode;
411         return i2c_send_command(I2C_COBBOARD_ADDR, (uint8_t*)&buf, sizeof(buf));
412 #endif
413 }
414
415 static int8_t i2c_cobboard_set_spickle(uint8_t side, uint8_t flags)
416 {
417         if (side == I2C_LEFT_SIDE)
418                 cobboard.lspickle = flags;
419         else
420                 cobboard.rspickle = flags;
421         return 0;
422 }
423
424 int8_t i2c_cobboard_pack(uint8_t side)
425 {
426         return i2c_cobboard_set_spickle(side, 0);
427 }
428
429 int8_t i2c_cobboard_autoharvest(uint8_t side)
430 {
431         return i2c_cobboard_set_spickle(side,
432                                         I2C_COBBOARD_SPK_DEPLOY |
433                                         I2C_COBBOARD_SPK_AUTOHARVEST);
434 }
435
436 int8_t i2c_cobboard_deploy(uint8_t side)
437 {
438         return i2c_cobboard_set_spickle(side, I2C_COBBOARD_SPK_DEPLOY);
439 }
440
441 int8_t i2c_ballboard_set_mode(uint8_t mode)
442 {
443         struct i2c_cmd_ballboard_set_mode buf;
444         buf.hdr.cmd = I2C_CMD_BALLBOARD_SET_MODE;
445         buf.mode = mode;
446         return i2c_send_command(I2C_BALLBOARD_ADDR, (uint8_t*)&buf, sizeof(buf));
447 }
448