projects/microb2010/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
  57 #define I2C_STATE_MAX 4
  58
  59 #define I2C_TIMEOUT 100 /* ms */
  60 #define I2C_MAX_ERRORS 40
  61
  62 static volatile uint8_t i2c_poll_num = 0;
  63 static volatile uint8_t i2c_state = 0;
  64 static volatile uint16_t i2c_errors = 0;
  65
  66 #define OP_READY 0 /* no i2c op running */
  67 #define OP_POLL  1 /* a user command is running */
  68 #define OP_CMD   2 /* a polling (req / ans) is running */
  69
  70 static volatile uint8_t running_op = OP_READY;
  71
  72 #define I2C_MAX_LOG 3
  73 #ifndef HOST_VERSION
  74 static uint8_t error_log = 0;
  75
  76 static int8_t i2c_req_cobboard_status(void);
  77 static int8_t i2c_req_ballboard_status(void);
  78
  79 #endif
  80
  81 /* used for commands */
  82 uint8_t command_buf[I2C_SEND_BUFFER_SIZE];
  83 volatile int8_t command_dest=-1;
  84 volatile uint8_t command_size=0;
  85
  86 #define I2C_ERROR(args...) do {                                         \
  87                 if (error_log < I2C_MAX_LOG) {                          \
  88                         ERROR(E_USER_I2C_PROTO, args);                  \
  89                         error_log ++;                                   \
  90                         if (error_log == I2C_MAX_LOG) {                 \
  91                                 ERROR(E_USER_I2C_PROTO,                 \
  92                                       "i2c logs are now warnings");     \
  93                         }                                               \
  94                 }                                                       \
  95                 else                                                    \
  96                         WARNING(E_USER_I2C_PROTO, args);                \
  97         } while(0)
  98
  99 void i2c_protocol_init(void)
 100 {
 101 }
 102
 103 void i2c_protocol_debug(void)
 104 {
 105 #ifdef HOST_VERSION
 106         return;
 107 #else
 108         printf_P(PSTR("I2C protocol debug infos:\r\n"));
 109         printf_P(PSTR("  i2c_state=%d\r\n"), i2c_state);
 110         printf_P(PSTR("  i2c_errors=%d\r\n"), i2c_errors);
 111         printf_P(PSTR("  running_op=%d\r\n"), running_op);
 112         printf_P(PSTR("  command_size=%d\r\n"), command_size);
 113         printf_P(PSTR("  command_dest=%d\r\n"), command_dest);
 114         printf_P(PSTR("  i2c_status=%x\r\n"), i2c_status());
 115 #endif
 116 }
 117
 118 #ifndef HOST_VERSION
 119 static void i2cproto_next_state(uint8_t inc)
 120 {
 121         i2c_state += inc;
 122         if (i2c_state >= I2C_STATE_MAX) {
 123                 i2c_state = 0;
 124                 i2c_poll_num ++;
 125         }
 126 }
 127
 128 void i2cproto_wait_update(void)
 129 {
 130         uint8_t poll_num;
 131         poll_num = i2c_poll_num;
 132         WAIT_COND_OR_TIMEOUT((i2c_poll_num-poll_num) > 1, 150);
 133 }
 134
 135 /* called periodically : the goal of this 'thread' is to send requests
 136  * and read answers on i2c slaves in the correct order. */
 137 void i2c_poll_slaves(void *dummy)
 138 {
 139         uint8_t flags;
 140         int8_t err;
 141         static uint8_t a = 0;
 142
 143         a++;
 144         if (a & 0x4)
 145                 LED2_TOGGLE();
 146
 147         /* already running */
 148         IRQ_LOCK(flags);
 149         if (running_op != OP_READY) {
 150                 IRQ_UNLOCK(flags);
 151                 return;
 152         }
 153
 154         /* if a command is ready to be sent, so send it */
 155         if (command_size) {
 156                 running_op = OP_CMD;
 157                 err = i2c_send(command_dest, command_buf, command_size,
 158                              I2C_CTRL_GENERIC);
 159                 if (err < 0)
 160                         goto error;
 161                 IRQ_UNLOCK(flags);
 162                 return;
 163         }
 164
 165         /* no command, so do the polling */
 166         running_op = OP_POLL;
 167
 168         switch(i2c_state) {
 169
 170         /* poll status of cobboard */
 171 #define I2C_REQ_COBBOARD 0
 172         case I2C_REQ_COBBOARD:
 173                 if ((err = i2c_req_cobboard_status()))
 174                         goto error;
 175                 break;
 176
 177 #define I2C_ANS_COBBOARD 1
 178         case I2C_ANS_COBBOARD:
 179                 if ((err = i2c_recv(I2C_COBBOARD_ADDR,
 180                                     sizeof(struct i2c_ans_cobboard_status),
 181                                     I2C_CTRL_GENERIC)))
 182                         goto error;
 183                 break;
 184
 185         /* poll status of ballboard */
 186 #define I2C_REQ_BALLBOARD 2
 187         case I2C_REQ_BALLBOARD:
 188                 if ((err = i2c_req_ballboard_status()))
 189                         goto error;
 190                 break;
 191
 192 #define I2C_ANS_BALLBOARD 3
 193         case I2C_ANS_BALLBOARD:
 194                 if ((err = i2c_recv(I2C_BALLBOARD_ADDR,
 195                                     sizeof(struct i2c_ans_ballboard_status),
 196                                     I2C_CTRL_GENERIC)))
 197                         goto error;
 198                 break;
 199
 200         /* nothing, go to the first request */
 201         default:
 202                 i2c_state = 0;
 203                 running_op = OP_READY;
 204         }
 205         IRQ_UNLOCK(flags);
 206
 207         return;
 208
 209  error:
 210         running_op = OP_READY;
 211         IRQ_UNLOCK(flags);
 212         i2c_errors++;
 213         if (i2c_errors > I2C_MAX_ERRORS) {
 214                 I2C_ERROR("I2C send is_cmd=%d proto_state=%d "
 215                       "err=%d i2c_status=%x", !!command_size, i2c_state, err, i2c_status());
 216                 i2c_reset();
 217                 i2c_errors = 0;
 218         }
 219 }
 220
 221 /* called when the xmit is finished */
 222 void i2c_sendevent(int8_t size)
 223 {
 224         if (size > 0) {
 225                 if (running_op == OP_POLL) {
 226                         i2cproto_next_state(1);
 227                 }
 228                 else
 229                         command_size = 0;
 230         }
 231         else {
 232                 i2c_errors++;
 233                 NOTICE(E_USER_I2C_PROTO, "send error state=%d size=%d "
 234                         "op=%d", i2c_state, size, running_op);
 235                 if (i2c_errors > I2C_MAX_ERRORS) {
 236                         I2C_ERROR("I2C error, slave not ready");
 237                         i2c_reset();
 238                         i2c_errors = 0;
 239                 }
 240
 241                 if (running_op == OP_POLL) {
 242                         /* skip associated answer */
 243                         i2cproto_next_state(2);
 244                 }
 245         }
 246         running_op = OP_READY;
 247 }
 248
 249 /* called rx event */
 250 void i2c_recvevent(uint8_t * buf, int8_t size)
 251 {
 252         if (running_op == OP_POLL)
 253                 i2cproto_next_state(1);
 254
 255         /* recv is only trigged after a poll */
 256         running_op = OP_READY;
 257
 258         if (size < 0) {
 259                 goto error;
 260         }
 261
 262         switch (buf[0]) {
 263
 264         case I2C_ANS_COBBOARD_STATUS: {
 265                 struct i2c_ans_cobboard_status * ans =
 266                         (struct i2c_ans_cobboard_status *)buf;
 267
 268                 if (size != sizeof (*ans))
 269                         goto error;
 270
 271                 /* status */
 272                 cobboard.mode = ans->mode;
 273                 cobboard.status = ans->status;
 274                 cobboard.left_cobroller_speed = ans->left_cobroller_speed;
 275                 cs_set_consign(&mainboard.left_cobroller.cs, cobboard.left_cobroller_speed);
 276                 cobboard.right_cobroller_speed = ans->right_cobroller_speed;
 277                 cs_set_consign(&mainboard.right_cobroller.cs, cobboard.right_cobroller_speed);
 278
 279                 break;
 280         }
 281
 282         case I2C_ANS_BALLBOARD_STATUS: {
 283                 struct i2c_ans_ballboard_status * ans =
 284                         (struct i2c_ans_ballboard_status *)buf;
 285
 286                 if (size != sizeof (*ans))
 287                         goto error;
 288                 ballboard.mode = ans->mode;
 289                 ballboard.status = ans->status;
 290                 ballboard.ball_count = ans->ball_count;
 291                 break;
 292         }
 293
 294         default:
 295                 break;
 296         }
 297
 298         return;
 299  error:
 300         i2c_errors++;
 301         NOTICE(E_USER_I2C_PROTO, "recv error state=%d op=%d",
 302                i2c_state, running_op);
 303         if (i2c_errors > I2C_MAX_ERRORS) {
 304                 I2C_ERROR("I2C error, slave not ready");
 305                 i2c_reset();
 306                 i2c_errors = 0;
 307         }
 308 }
 309
 310 void i2c_recvbyteevent(uint8_t hwstatus, uint8_t i, uint8_t c)
 311 {
 312 }
 313 #endif /* !HOST_VERSION */
 314
 315 /* ******** ******** ******** ******** */
 316 /* commands */
 317 /* ******** ******** ******** ******** */
 318
 319
 320 static int8_t
 321 i2c_send_command(uint8_t addr, uint8_t * buf, uint8_t size)
 322 {
 323 #ifdef HOST_VERSION
 324         return 0;
 325 #else
 326         uint8_t flags;
 327         microseconds us = time_get_us2();
 328
 329         while ((time_get_us2() - us) < (I2C_TIMEOUT)*1000L) {
 330                 IRQ_LOCK(flags);
 331                 if (command_size == 0) {
 332                         memcpy(command_buf, buf, size);
 333                         command_size = size;
 334                         command_dest = addr;
 335                         IRQ_UNLOCK(flags);
 336                         return 0;
 337                 }
 338                 IRQ_UNLOCK(flags);
 339         }
 340         /* this should not happen... except if we are called from an
 341          * interrupt context, but it's forbidden */
 342         I2C_ERROR("I2C command send failed");
 343         return -EBUSY;
 344 #endif
 345 }
 346
 347 #ifndef HOST_VERSION
 348 static int8_t i2c_req_cobboard_status(void)
 349 {
 350         struct i2c_req_cobboard_status buf;
 351         int8_t err;
 352
 353         buf.hdr.cmd = I2C_REQ_COBBOARD_STATUS;
 354         err = i2c_send(I2C_COBBOARD_ADDR, (uint8_t*)&buf,
 355                         sizeof(buf), I2C_CTRL_GENERIC);
 356
 357         return err;
 358 }
 359
 360 static int8_t i2c_req_ballboard_status(void)
 361 {
 362         struct i2c_req_ballboard_status buf;
 363
 364         buf.hdr.cmd = I2C_REQ_BALLBOARD_STATUS;
 365         return i2c_send(I2C_BALLBOARD_ADDR, (uint8_t*)&buf,
 366                         sizeof(buf), I2C_CTRL_GENERIC);
 367 }
 368 #endif /* !HOST_VERSION */
 369
 370 int8_t i2c_set_color(uint8_t addr, uint8_t color)
 371 {
 372         struct i2c_cmd_generic_color buf;
 373
 374         if (addr == I2C_BALLBOARD_ADDR)
 375                 return 0; /* XXX disabled for now */
 376         buf.hdr.cmd = I2C_CMD_GENERIC_SET_COLOR;
 377         buf.color = color;
 378         return i2c_send_command(addr, (uint8_t*)&buf, sizeof(buf));
 379 }
 380
 381 int8_t i2c_led_control(uint8_t addr, uint8_t led, uint8_t state)
 382 {
 383         struct i2c_cmd_led_control buf;
 384         buf.hdr.cmd = I2C_CMD_GENERIC_LED_CONTROL;
 385         buf.led_num = led;
 386         buf.state = state;
 387         return i2c_send_command(addr, (uint8_t*)&buf, sizeof(buf));
 388 }
 389
 390 int8_t i2c_cobboard_mode_eject(void)
 391 {
 392         struct i2c_cmd_cobboard_set_mode buf;
 393         buf.hdr.cmd = I2C_CMD_COBBOARD_SET_MODE;
 394         buf.mode = cobboard.mode | I2C_COBBOARD_MODE_EJECT;
 395         return i2c_send_command(I2C_COBBOARD_ADDR, (uint8_t*)&buf, sizeof(buf));
 396 }
 397
 398 int8_t i2c_cobboard_mode_harvest(uint8_t side)
 399 {
 400         struct i2c_cmd_cobboard_set_mode buf;
 401         uint8_t mode = cobboard.mode;
 402
 403         buf.hdr.cmd = I2C_CMD_COBBOARD_SET_MODE;
 404         if (side == I2C_LEFT_SIDE) {
 405                 mode |= I2C_COBBOARD_MODE_L_DEPLOY;
 406                 mode |= I2C_COBBOARD_MODE_L_HARVEST;
 407         }
 408         else {
 409                 mode |= I2C_COBBOARD_MODE_R_DEPLOY;
 410                 mode |= I2C_COBBOARD_MODE_R_HARVEST;
 411         }
 412         buf.mode = mode;
 413         return i2c_send_command(I2C_COBBOARD_ADDR, (uint8_t*)&buf, sizeof(buf));
 414 }
 415
 416 int8_t i2c_cobboard_mode_deploy(uint8_t side)
 417 {
 418         struct i2c_cmd_cobboard_set_mode buf;
 419         uint8_t mode = cobboard.mode;
 420
 421         buf.hdr.cmd = I2C_CMD_COBBOARD_SET_MODE;
 422         if (side == I2C_LEFT_SIDE) {
 423                 mode &= ~(I2C_COBBOARD_MODE_L_DEPLOY | I2C_COBBOARD_MODE_L_HARVEST);
 424                 mode |= I2C_COBBOARD_MODE_L_DEPLOY;
 425         }
 426         else {
 427                 mode &= ~(I2C_COBBOARD_MODE_R_DEPLOY | I2C_COBBOARD_MODE_R_HARVEST);
 428                 mode |= I2C_COBBOARD_MODE_R_DEPLOY;
 429         }
 430         buf.mode = mode;
 431         return i2c_send_command(I2C_COBBOARD_ADDR, (uint8_t*)&buf, sizeof(buf));
 432 }
 433
 434 int8_t i2c_cobboard_mode_pack(uint8_t side)
 435 {
 436         struct i2c_cmd_cobboard_set_mode buf;
 437         uint8_t mode = cobboard.mode;
 438
 439         buf.hdr.cmd = I2C_CMD_COBBOARD_SET_MODE;
 440         if (side == I2C_LEFT_SIDE)
 441                 mode &= ~(I2C_COBBOARD_MODE_L_DEPLOY | I2C_COBBOARD_MODE_L_HARVEST);
 442         else
 443                 mode &= ~(I2C_COBBOARD_MODE_R_DEPLOY | I2C_COBBOARD_MODE_R_HARVEST);
 444         buf.mode = mode;
 445         return i2c_send_command(I2C_COBBOARD_ADDR, (uint8_t*)&buf, sizeof(buf));
 446 }
 447
 448 int8_t i2c_cobboard_mode_init(void)
 449 {
 450         struct i2c_cmd_cobboard_set_mode buf;
 451         buf.hdr.cmd = I2C_CMD_COBBOARD_SET_MODE;
 452         buf.mode = I2C_COBBOARD_MODE_INIT;
 453         return i2c_send_command(I2C_COBBOARD_ADDR, (uint8_t*)&buf, sizeof(buf));
 454 }
 455
 456 int8_t i2c_ballboard_set_mode(uint8_t mode)
 457 {
 458         struct i2c_cmd_ballboard_set_mode buf;
 459         buf.hdr.cmd = I2C_CMD_BALLBOARD_SET_MODE;
 460         buf.mode = mode;
 461         return i2c_send_command(I2C_COBBOARD_ADDR, (uint8_t*)&buf, sizeof(buf));
 462 }
 463