From a750fc58646c6f1ab5319d9bcf786be7a525a77c Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Sun, 22 Apr 2012 22:07:20 +0200 Subject: digital/{ai,beacon,io-hub}: add beacon simulation stub --- digital/beacon/src/stub/simu_stub.host.c | 126 +++++++++++++++++++++++++++++++ 1 file changed, 126 insertions(+) create mode 100644 digital/beacon/src/stub/simu_stub.host.c (limited to 'digital/beacon/src/stub/simu_stub.host.c') diff --git a/digital/beacon/src/stub/simu_stub.host.c b/digital/beacon/src/stub/simu_stub.host.c new file mode 100644 index 00000000..cdbacaa3 --- /dev/null +++ b/digital/beacon/src/stub/simu_stub.host.c @@ -0,0 +1,126 @@ +/* simu_stub.c */ +/* lol - Laser Opponent Location finding system. {{{ + * + * Copyright (C) 2012 Nicolas Schodet + * + * APBTeam: + * Web: http://apbteam.org/ + * Email: team AT apbteam DOT org + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation; either version 2 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program; if not, write to the Free Software + * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. + * + * }}} */ +#include "common.h" + +#include "modules/host/host.h" +#include "modules/host/mex.h" +#include "modules/twi/twi.h" +#include "modules/utils/utils.h" +#include "modules/utils/byte.h" +#include "modules/utils/crc.h" + +/** This is a stub only used in simulation to bypass all beacon computations + * and directly provide a position via TWI. */ + +struct position_t +{ + uint16_t x, y; +}; + +struct position_t simu_positions[2]; + +uint8_t simu_seq; + +static void +simu_twi_proto_poll (void) +{ + /* Do nothing interresting with the received value, but check it + * anyway. */ + uint8_t recv_buf[AC_TWI_SLAVE_RECV_BUFFER_SIZE]; + uint8_t recv_size; + while ((recv_size = twi_slave_poll (recv_buf, sizeof (recv_buf)))) + { + if (crc_compute (recv_buf + 1, recv_size - 1) != recv_buf[0]) + continue; + if (recv_buf[1] == simu_seq) + continue; + if (recv_size != 3) + continue; + simu_seq = recv_buf[1]; + } +} + +static void +simu_twi_proto_update (void) +{ + /* Update TWI slave buffer after each position update. */ + uint8_t status[4 + 5 * UTILS_COUNT (simu_positions)]; + uint8_t i, index = 1; + status[index++] = 0; + status[index++] = 0; + status[index++] = simu_seq; + for (i = 0; i < UTILS_COUNT (simu_positions); i++) + { + status[index++] = v16_to_v8 (simu_positions[i].x, 1); + status[index++] = v16_to_v8 (simu_positions[i].x, 0); + status[index++] = v16_to_v8 (simu_positions[i].y, 1); + status[index++] = v16_to_v8 (simu_positions[i].y, 0); + status[index++] = simu_positions[i].x == (uint16_t) -1 ? 0 : 100; + } + status[0] = crc_compute (&status[1], sizeof (status) - 1); + twi_slave_update (status, sizeof (status)); +} + +static void +simu_positions_handle (void *user, mex_msg_t *msg) +{ + uint8_t i; + struct position_t pos; + mex_msg_pop (msg, "BHH", &i, &pos.x, &pos.y); + simu_positions[i] = pos; + simu_twi_proto_update (); +} + +static void +simu_init (void) +{ + const char *mex_instance; + mex_node_connect (); + mex_instance = host_get_instance ("beacon0", 0); + uint8_t mtype = mex_node_reservef ("%s:position", mex_instance); + mex_node_register (mtype, simu_positions_handle, 0); + uint8_t i; + for (i = 0; i < UTILS_COUNT (simu_positions); i++) + { + simu_positions[i].x = (uint16_t) -1; + simu_positions[i].y = (uint16_t) -1; + } +} + +int +main (int argc, char ** argv) +{ + avr_init (argc, argv); + simu_init (); + twi_init (AC_BEACON_TWI_ADDRESS); + simu_twi_proto_update (); + while (1) + { + simu_twi_proto_poll (); + mex_node_wait_date (mex_node_date () + 1); + } + return 0; +} + -- cgit v1.2.3