2 * Copyright Droids Corporation (2009)
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.
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.
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
18 * Revision : $Id: i2c_protocol.c,v 1.8 2009-11-08 17:24:33 zer0 Exp $
25 #include <aversive/pgmspace.h>
26 #include <aversive/wait.h>
27 #include <aversive/error.h>
37 #include <control_system_manager.h>
38 #include <trajectory_manager.h>
39 #include <vect_base.h>
42 #include <obstacle_avoidance.h>
43 #include <blocking_detection_manager.h>
44 #include <robot_system.h>
45 #include <position_manager.h>
49 #include <parse_string.h>
50 #include <parse_num.h>
52 #include "../common/i2c_commands.h"
55 #include "i2c_protocol.h"
57 #define I2C_STATE_MAX 4
59 #define I2C_TIMEOUT 100 /* ms */
60 #define I2C_MAX_ERRORS 40
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;
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 */
70 static volatile uint8_t running_op = OP_READY;
73 static uint8_t error_log = 0;
75 /* used for commands */
76 uint8_t command_buf[I2C_SEND_BUFFER_SIZE];
77 volatile int8_t command_dest=-1;
78 volatile uint8_t command_size=0;
80 static int8_t i2c_req_mechboard_status(void);
81 static int8_t i2c_req_sensorboard_status(void);
83 #define I2C_ERROR(args...) do { \
84 if (error_log < I2C_MAX_LOG) { \
85 ERROR(E_USER_I2C_PROTO, args); \
87 if (error_log == I2C_MAX_LOG) { \
88 ERROR(E_USER_I2C_PROTO, \
89 "i2c logs are now warnings"); \
93 WARNING(E_USER_I2C_PROTO, args); \
96 void i2c_protocol_init(void)
100 void i2c_protocol_debug(void)
102 printf_P(PSTR("I2C protocol debug infos:\r\n"));
103 printf_P(PSTR(" i2c_state=%d\r\n"), i2c_state);
104 printf_P(PSTR(" i2c_errors=%d\r\n"), i2c_errors);
105 printf_P(PSTR(" running_op=%d\r\n"), running_op);
106 printf_P(PSTR(" command_size=%d\r\n"), command_size);
107 printf_P(PSTR(" command_dest=%d\r\n"), command_dest);
108 printf_P(PSTR(" i2c_status=%x\r\n"), i2c_status());
111 static void i2cproto_next_state(uint8_t inc)
114 if (i2c_state >= I2C_STATE_MAX) {
120 void i2cproto_wait_update(void)
123 poll_num = i2c_poll_num;
124 WAIT_COND_OR_TIMEOUT((i2c_poll_num-poll_num) > 1, 150);
127 /* called periodically : the goal of this 'thread' is to send requests
128 * and read answers on i2c slaves in the correct order. */
129 void i2c_poll_slaves(void *dummy)
133 static uint8_t a = 0;
139 /* already running */
141 if (running_op != OP_READY) {
146 /* if a command is ready to be sent, so send it */
149 err = i2c_send(command_dest, command_buf, command_size,
157 /* no command, so do the polling */
158 running_op = OP_POLL;
162 /* poll status of mechboard */
163 #define I2C_REQ_MECHBOARD 0
164 case I2C_REQ_MECHBOARD:
165 if ((err = i2c_req_mechboard_status()))
169 #define I2C_ANS_MECHBOARD 1
170 case I2C_ANS_MECHBOARD:
171 if ((err = i2c_recv(I2C_MECHBOARD_ADDR,
172 sizeof(struct i2c_ans_mechboard_status),
177 /* poll status of sensorboard */
178 #define I2C_REQ_SENSORBOARD 2
179 case I2C_REQ_SENSORBOARD:
180 if ((err = i2c_req_sensorboard_status()))
184 #define I2C_ANS_SENSORBOARD 3
185 case I2C_ANS_SENSORBOARD:
186 if ((err = i2c_recv(I2C_SENSORBOARD_ADDR,
187 sizeof(struct i2c_ans_sensorboard_status),
192 /* nothing, go to the first request */
195 running_op = OP_READY;
202 running_op = OP_READY;
205 if (i2c_errors > I2C_MAX_ERRORS) {
206 I2C_ERROR("I2C send is_cmd=%d proto_state=%d "
207 "err=%d i2c_status=%x", !!command_size, i2c_state, err, i2c_status());
213 /* called when the xmit is finished */
214 void i2c_sendevent(int8_t size)
217 if (running_op == OP_POLL) {
218 i2cproto_next_state(1);
225 NOTICE(E_USER_I2C_PROTO, "send error state=%d size=%d "
226 "op=%d", i2c_state, size, running_op);
227 if (i2c_errors > I2C_MAX_ERRORS) {
228 I2C_ERROR("I2C error, slave not ready");
233 if (running_op == OP_POLL) {
234 /* skip associated answer */
235 i2cproto_next_state(2);
238 running_op = OP_READY;
241 /* called rx event */
242 void i2c_recvevent(uint8_t * buf, int8_t size)
244 if (running_op == OP_POLL)
245 i2cproto_next_state(1);
247 /* recv is only trigged after a poll */
248 running_op = OP_READY;
256 case I2C_ANS_MECHBOARD_STATUS: {
257 struct i2c_ans_mechboard_status * ans =
258 (struct i2c_ans_mechboard_status *)buf;
260 if (size != sizeof (*ans))
264 mechboard.mode = ans->mode;
265 mechboard.status = ans->status;
266 mechboard.lintel_count = ans->lintel_count;
267 mechboard.column_flags = ans->column_flags;
269 mechboard.pump_left1 = ans->pump_left1;
270 mechboard.pump_left2 = ans->pump_left2;
271 mechboard.pump_right1 = ans->pump_right1;
272 mechboard.pump_right2 = ans->pump_right2;
273 pwm_ng_set(LEFT_PUMP1_PWM, mechboard.pump_left1);
274 pwm_ng_set(LEFT_PUMP2_PWM, mechboard.pump_left2);
276 mechboard.pump_right1_current = ans->pump_right1_current;
277 mechboard.pump_right2_current = ans->pump_right2_current;
279 mechboard.servo_lintel_left = ans->servo_lintel_left;
280 mechboard.servo_lintel_right = ans->servo_lintel_right;
281 pwm_ng_set(&gen.servo2, mechboard.servo_lintel_right);
282 pwm_ng_set(&gen.servo3, mechboard.servo_lintel_left);
287 case I2C_ANS_SENSORBOARD_STATUS: {
288 struct i2c_ans_sensorboard_status * ans =
289 (struct i2c_ans_sensorboard_status *)buf;
291 if (size != sizeof (*ans))
293 sensorboard.status = ans->status;
294 sensorboard.opponent_x = ans->opponent_x;
295 sensorboard.opponent_y = ans->opponent_y;
296 sensorboard.opponent_a = ans->opponent_a;
297 sensorboard.opponent_d = ans->opponent_d;
299 sensorboard.scan_status = ans->scan_status;
300 sensorboard.dropzone_h = ans->dropzone_h;
301 sensorboard.dropzone_x = ans->dropzone_x;
302 sensorboard.dropzone_y = ans->dropzone_y;
313 NOTICE(E_USER_I2C_PROTO, "recv error state=%d op=%d",
314 i2c_state, running_op);
315 if (i2c_errors > I2C_MAX_ERRORS) {
316 I2C_ERROR("I2C error, slave not ready");
322 void i2c_recvbyteevent(uint8_t hwstatus, uint8_t i, uint8_t c)
328 /* ******** ******** ******** ******** */
330 /* ******** ******** ******** ******** */
334 i2c_send_command(uint8_t addr, uint8_t * buf, uint8_t size)
337 microseconds us = time_get_us2();
339 while ((time_get_us2() - us) < (I2C_TIMEOUT)*1000L) {
341 if (command_size == 0) {
342 memcpy(command_buf, buf, size);
350 /* this should not happen... except if we are called from an
351 * interrupt context, but it's forbidden */
352 I2C_ERROR("I2C command send failed");
356 static int8_t i2c_req_mechboard_status(void)
358 struct i2c_req_mechboard_status buf;
361 buf.hdr.cmd = I2C_REQ_MECHBOARD_STATUS;
362 buf.pump_left1_current = sensor_get_adc(ADC_CSENSE3);
363 buf.pump_left2_current = sensor_get_adc(ADC_CSENSE4);
364 err = i2c_send(I2C_MECHBOARD_ADDR, (uint8_t*)&buf,
365 sizeof(buf), I2C_CTRL_GENERIC);
370 static int8_t i2c_req_sensorboard_status(void)
372 struct i2c_req_sensorboard_status buf;
374 buf.hdr.cmd = I2C_REQ_SENSORBOARD_STATUS;
376 buf.x = position_get_x_s16(&mainboard.pos);
377 buf.y = position_get_y_s16(&mainboard.pos);
378 buf.a = position_get_a_deg_s16(&mainboard.pos);
380 buf.enable_pickup_wheels = mainboard.enable_pickup_wheels;
382 return i2c_send(I2C_SENSORBOARD_ADDR, (uint8_t*)&buf,
383 sizeof(buf), I2C_CTRL_GENERIC);
386 int8_t i2c_set_color(uint8_t addr, uint8_t color)
388 struct i2c_cmd_generic_color buf;
390 if (addr == I2C_SENSORBOARD_ADDR)
391 return 0; /* XXX disabled for now */
392 buf.hdr.cmd = I2C_CMD_GENERIC_SET_COLOR;
394 return i2c_send_command(addr, (uint8_t*)&buf, sizeof(buf));
397 int8_t i2c_led_control(uint8_t addr, uint8_t led, uint8_t state)
399 struct i2c_cmd_led_control buf;
400 buf.hdr.cmd = I2C_CMD_GENERIC_LED_CONTROL;
403 return i2c_send_command(addr, (uint8_t*)&buf, sizeof(buf));
406 int8_t i2c_mechboard_mode_manual(void)
408 struct i2c_cmd_mechboard_set_mode buf;
409 buf.hdr.cmd = I2C_CMD_MECHBOARD_SET_MODE;
410 buf.mode = I2C_MECHBOARD_MODE_MANUAL;
411 return i2c_send_command(I2C_MECHBOARD_ADDR, (uint8_t*)&buf, sizeof(buf));
414 int8_t i2c_mechboard_mode_harvest(void)
416 struct i2c_cmd_mechboard_set_mode buf;
417 buf.hdr.cmd = I2C_CMD_MECHBOARD_SET_MODE;
418 buf.mode = I2C_MECHBOARD_MODE_HARVEST;
419 return i2c_send_command(I2C_MECHBOARD_ADDR, (uint8_t*)&buf, sizeof(buf));
422 int8_t i2c_mechboard_mode_lazy_harvest(void)
424 struct i2c_cmd_mechboard_set_mode buf;
425 buf.hdr.cmd = I2C_CMD_MECHBOARD_SET_MODE;
426 buf.mode = I2C_MECHBOARD_MODE_LAZY_HARVEST;
427 return i2c_send_command(I2C_MECHBOARD_ADDR, (uint8_t*)&buf, sizeof(buf));
430 int8_t i2c_mechboard_mode_prepare_pickup(uint8_t side)
432 struct i2c_cmd_mechboard_set_mode buf;
433 buf.hdr.cmd = I2C_CMD_MECHBOARD_SET_MODE;
434 buf.mode = I2C_MECHBOARD_MODE_PREPARE_PICKUP;
435 buf.prep_pickup.next_mode = I2C_MECHBOARD_MODE_PREPARE_PICKUP;
436 buf.prep_pickup.side = side;
437 return i2c_send_command(I2C_MECHBOARD_ADDR, (uint8_t*)&buf, sizeof(buf));
440 int8_t i2c_mechboard_mode_push_temple_disc(uint8_t side)
442 struct i2c_cmd_mechboard_set_mode buf;
443 buf.hdr.cmd = I2C_CMD_MECHBOARD_SET_MODE;
444 buf.mode = I2C_MECHBOARD_MODE_PUSH_TEMPLE_DISC;
445 buf.prep_pickup.side = side;
446 return i2c_send_command(I2C_MECHBOARD_ADDR, (uint8_t*)&buf, sizeof(buf));
449 int8_t i2c_mechboard_mode_prepare_pickup_next(uint8_t side, uint8_t next_mode)
451 struct i2c_cmd_mechboard_set_mode buf;
452 buf.hdr.cmd = I2C_CMD_MECHBOARD_SET_MODE;
453 buf.mode = I2C_MECHBOARD_MODE_PREPARE_PICKUP;
454 buf.prep_pickup.next_mode = next_mode;
455 buf.prep_pickup.side = side;
456 return i2c_send_command(I2C_MECHBOARD_ADDR, (uint8_t*)&buf, sizeof(buf));
459 int8_t i2c_mechboard_mode_pickup(void)
461 struct i2c_cmd_mechboard_set_mode buf;
462 buf.hdr.cmd = I2C_CMD_MECHBOARD_SET_MODE;
463 buf.mode = I2C_MECHBOARD_MODE_PICKUP;
464 return i2c_send_command(I2C_MECHBOARD_ADDR, (uint8_t*)&buf, sizeof(buf));
467 int8_t i2c_mechboard_mode_eject(void)
469 struct i2c_cmd_mechboard_set_mode buf;
470 buf.hdr.cmd = I2C_CMD_MECHBOARD_SET_MODE;
471 buf.mode = I2C_MECHBOARD_MODE_EJECT;
472 return i2c_send_command(I2C_MECHBOARD_ADDR, (uint8_t*)&buf, sizeof(buf));
475 int8_t i2c_mechboard_mode_manivelle(void)
477 struct i2c_cmd_mechboard_set_mode buf;
478 buf.hdr.cmd = I2C_CMD_MECHBOARD_SET_MODE;
479 buf.mode = I2C_MECHBOARD_MODE_MANIVELLE;
480 return i2c_send_command(I2C_MECHBOARD_ADDR, (uint8_t*)&buf, sizeof(buf));
483 int8_t i2c_mechboard_mode_push_temple(uint8_t level)
485 struct i2c_cmd_mechboard_set_mode buf;
486 buf.hdr.cmd = I2C_CMD_MECHBOARD_SET_MODE;
487 buf.mode = I2C_MECHBOARD_MODE_PUSH_TEMPLE;
488 buf.push_temple.level = level;
489 return i2c_send_command(I2C_MECHBOARD_ADDR, (uint8_t*)&buf,sizeof(buf));
492 int8_t i2c_mechboard_mode_prepare_build_both(uint8_t level)
494 struct i2c_cmd_mechboard_set_mode buf;
495 buf.hdr.cmd = I2C_CMD_MECHBOARD_SET_MODE;
496 buf.mode = I2C_MECHBOARD_MODE_PREPARE_BUILD;
497 buf.prep_build.level_l = level;
498 buf.prep_build.level_r = level;
499 return i2c_send_command(I2C_MECHBOARD_ADDR, (uint8_t*)&buf, sizeof(buf));
502 int8_t i2c_mechboard_mode_prepare_build_select(int8_t level_l, int8_t level_r)
504 struct i2c_cmd_mechboard_set_mode buf;
505 buf.hdr.cmd = I2C_CMD_MECHBOARD_SET_MODE;
506 buf.mode = I2C_MECHBOARD_MODE_PREPARE_BUILD;
507 buf.prep_build.level_l = level_l;
508 buf.prep_build.level_r = level_r;
509 return i2c_send_command(I2C_MECHBOARD_ADDR, (uint8_t*)&buf, sizeof(buf));
512 int8_t i2c_mechboard_mode_prepare_inside_both(uint8_t level)
514 struct i2c_cmd_mechboard_set_mode buf;
515 buf.hdr.cmd = I2C_CMD_MECHBOARD_SET_MODE;
516 buf.mode = I2C_MECHBOARD_MODE_PREPARE_INSIDE;
517 buf.prep_inside.level_l = level;
518 buf.prep_inside.level_r = level;
519 return i2c_send_command(I2C_MECHBOARD_ADDR, (uint8_t*)&buf, sizeof(buf));
522 int8_t i2c_mechboard_mode_prepare_inside_select(int8_t level_l, int8_t level_r)
524 struct i2c_cmd_mechboard_set_mode buf;
525 buf.hdr.cmd = I2C_CMD_MECHBOARD_SET_MODE;
526 buf.mode = I2C_MECHBOARD_MODE_PREPARE_INSIDE;
527 buf.prep_inside.level_l = level_l;
528 buf.prep_inside.level_r = level_r;
529 return i2c_send_command(I2C_MECHBOARD_ADDR, (uint8_t*)&buf, sizeof(buf));
532 int8_t i2c_mechboard_mode_simple_autobuild(uint8_t level)
534 struct i2c_cmd_mechboard_set_mode buf;
535 buf.hdr.cmd = I2C_CMD_MECHBOARD_SET_MODE;
536 buf.mode = I2C_MECHBOARD_MODE_AUTOBUILD;
537 buf.autobuild.level_left = level;
538 buf.autobuild.level_right = level;
539 buf.autobuild.count_left = 2;
540 buf.autobuild.count_right = 2;
541 buf.autobuild.do_lintel = 1;
542 buf.autobuild.distance_left = 210;
543 buf.autobuild.distance_right = 210;
544 return i2c_send_command(I2C_MECHBOARD_ADDR, (uint8_t*)&buf, sizeof(buf));
547 int8_t i2c_mechboard_mode_autobuild(uint8_t level_l, uint8_t count_l,
549 uint8_t level_r, uint8_t count_r,
553 struct i2c_cmd_mechboard_set_mode buf;
554 buf.hdr.cmd = I2C_CMD_MECHBOARD_SET_MODE;
555 buf.mode = I2C_MECHBOARD_MODE_AUTOBUILD;
556 buf.autobuild.level_left = level_l;
557 buf.autobuild.level_right = level_r;
558 buf.autobuild.count_left = count_l;
559 buf.autobuild.count_right = count_r;
560 buf.autobuild.distance_left = dist_l;
561 buf.autobuild.distance_right = dist_r;
562 buf.autobuild.do_lintel = do_lintel;
563 return i2c_send_command(I2C_MECHBOARD_ADDR, (uint8_t*)&buf, sizeof(buf));
566 int8_t i2c_mechboard_mode_init(void)
568 struct i2c_cmd_mechboard_set_mode buf;
569 buf.hdr.cmd = I2C_CMD_MECHBOARD_SET_MODE;
570 buf.mode = I2C_MECHBOARD_MODE_INIT;
571 return i2c_send_command(I2C_MECHBOARD_ADDR, (uint8_t*)&buf, sizeof(buf));
574 int8_t i2c_mechboard_mode_prepare_get_lintel(void)
576 struct i2c_cmd_mechboard_set_mode buf;
577 buf.hdr.cmd = I2C_CMD_MECHBOARD_SET_MODE;
578 buf.mode = I2C_MECHBOARD_MODE_PREPARE_GET_LINTEL;
579 return i2c_send_command(I2C_MECHBOARD_ADDR, (uint8_t*)&buf, sizeof(buf));
582 int8_t i2c_mechboard_mode_get_lintel(void)
584 struct i2c_cmd_mechboard_set_mode buf;
585 buf.hdr.cmd = I2C_CMD_MECHBOARD_SET_MODE;
586 buf.mode = I2C_MECHBOARD_MODE_GET_LINTEL;
587 return i2c_send_command(I2C_MECHBOARD_ADDR, (uint8_t*)&buf, sizeof(buf));
590 int8_t i2c_mechboard_mode_put_lintel(void)
592 struct i2c_cmd_mechboard_set_mode buf;
593 buf.hdr.cmd = I2C_CMD_MECHBOARD_SET_MODE;
594 buf.mode = I2C_MECHBOARD_MODE_PUT_LINTEL;
595 return i2c_send_command(I2C_MECHBOARD_ADDR, (uint8_t*)&buf, sizeof(buf));
598 int8_t i2c_mechboard_mode_clear(void)
600 struct i2c_cmd_mechboard_set_mode buf;
601 buf.hdr.cmd = I2C_CMD_MECHBOARD_SET_MODE;
602 buf.mode = I2C_MECHBOARD_MODE_CLEAR;
603 return i2c_send_command(I2C_MECHBOARD_ADDR, (uint8_t*)&buf, sizeof(buf));
606 int8_t i2c_mechboard_mode_loaded(void)
608 struct i2c_cmd_mechboard_set_mode buf;
609 buf.hdr.cmd = I2C_CMD_MECHBOARD_SET_MODE;
610 buf.mode = I2C_MECHBOARD_MODE_LOADED;
611 return i2c_send_command(I2C_MECHBOARD_ADDR, (uint8_t*)&buf, sizeof(buf));
614 int8_t i2c_mechboard_mode_store(void)
616 struct i2c_cmd_mechboard_set_mode buf;
617 buf.hdr.cmd = I2C_CMD_MECHBOARD_SET_MODE;
618 buf.mode = I2C_MECHBOARD_MODE_STORE;
619 return i2c_send_command(I2C_MECHBOARD_ADDR, (uint8_t*)&buf, sizeof(buf));
622 int8_t i2c_mechboard_mode_lazy_pickup(void)
624 struct i2c_cmd_mechboard_set_mode buf;
625 buf.hdr.cmd = I2C_CMD_MECHBOARD_SET_MODE;
626 buf.mode = I2C_MECHBOARD_MODE_LAZY_PICKUP;
627 return i2c_send_command(I2C_MECHBOARD_ADDR, (uint8_t*)&buf, sizeof(buf));
630 int8_t i2c_sensorboard_set_beacon(uint8_t enable)
632 struct i2c_cmd_sensorboard_start_beacon buf;
633 buf.hdr.cmd = I2C_CMD_SENSORBOARD_SET_BEACON;
635 return i2c_send_command(I2C_SENSORBOARD_ADDR, (uint8_t*)&buf, sizeof(buf));
638 int8_t i2c_sensorboard_scanner_set(uint8_t mode)
640 struct i2c_cmd_sensorboard_scanner buf;
641 buf.hdr.cmd = I2C_CMD_SENSORBOARD_SET_SCANNER;
643 return i2c_send_command(I2C_SENSORBOARD_ADDR, (uint8_t*)&buf, sizeof(buf));
646 int8_t i2c_sensorboard_scanner_calib(void)
648 struct i2c_cmd_sensorboard_calib_scanner buf;
649 buf.hdr.cmd = I2C_CMD_SENSORBOARD_CALIB_SCANNER;
650 return i2c_send_command(I2C_SENSORBOARD_ADDR, (uint8_t*)&buf, sizeof(buf));
653 int8_t i2c_sensorboard_scanner_algo_column(uint8_t zone,
654 int16_t x, int16_t y)
656 struct i2c_cmd_sensorboard_scanner_algo buf;
657 buf.hdr.cmd = I2C_CMD_SENSORBOARD_SCANNER_ALGO;
658 buf.algo = I2C_SCANNER_ALGO_COLUMN_DROPZONE;
659 buf.drop_zone.working_zone = zone;
660 buf.drop_zone.center_x = x;
661 buf.drop_zone.center_y = y;
662 return i2c_send_command(I2C_SENSORBOARD_ADDR, (uint8_t*)&buf, sizeof(buf));
665 int8_t i2c_sensorboard_scanner_algo_temple(uint8_t zone,
666 int16_t x, int16_t y)
668 struct i2c_cmd_sensorboard_scanner_algo buf;
669 buf.hdr.cmd = I2C_CMD_SENSORBOARD_SCANNER_ALGO;
670 buf.algo = I2C_SCANNER_ALGO_TEMPLE_DROPZONE;
671 buf.drop_zone.working_zone = zone;
672 buf.drop_zone.center_x = x;
673 buf.drop_zone.center_y = y;
674 return i2c_send_command(I2C_SENSORBOARD_ADDR, (uint8_t*)&buf, sizeof(buf));
677 int8_t i2c_sensorboard_scanner_algo_check(uint8_t level,
678 int16_t x, int16_t y)
680 struct i2c_cmd_sensorboard_scanner_algo buf;
681 buf.hdr.cmd = I2C_CMD_SENSORBOARD_SCANNER_ALGO;
682 buf.algo = I2C_SCANNER_ALGO_CHECK_TEMPLE;
683 buf.check_temple.level = level;
684 buf.check_temple.temple_x = x;
685 buf.check_temple.temple_y = y;
686 return i2c_send_command(I2C_SENSORBOARD_ADDR, (uint8_t*)&buf, sizeof(buf));