/* * c't-Sim - Robotersimulator fuer den c't-Bot * * 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. * */ /*! @file command.c * @brief Kommando-Management * @author Benjamin Benz (bbe@heise.de) * @date 20.12.05 */ #include "ct-Bot.h" #include "led.h" #include "log.h" #include "uart.h" #include "adc.h" #include "timer.h" #include "mouse.h" #include "command.h" #include "display.h" #include "sensor.h" #include "motor.h" #include "rc5.h" #include "ir-rc5.h" #include "bot-logic/bot-logik.h" #include "bot-2-pc.h" #include #include #ifdef PC #include "tcp.h" #include #endif #define COMMAND_TIMEOUT 10 /*!< Anzahl an ms, die maximal auf fehlende Daten gewartet wird */ #ifdef COMMAND_AVAILABLE #define RCVBUFSIZE (sizeof(command_t)*2) /*!< Groesse des Empfangspuffers */ command_t received_command; /*!< Puffer fuer Kommandos */ #ifdef PC // Auf dword alignment bestehen, wird fuer MacOS X benoetigt pthread_mutex_t command_mutex __attribute__ ((aligned (4))) = PTHREAD_MUTEX_INITIALIZER; #endif /*! * Liest ein Kommando ein, ist blockierend! * Greift auf low_read() zurueck * Achtung, die Payload wird nicht mitgelesen!!! * @see low_read() */ int8 command_read(void){ int bytesRcvd; int start=0; // start des Kommandos int i; command_t * command; // Pointer zum Casten der empfangegen Daten char * ptr; // Nur zu Hilfszwecken char buffer[RCVBUFSIZE]; // Buffer #if BYTE_ORDER == BIG_ENDIAN uint16 store; //Puffer für die Endian-Konvertierung #endif uint16 old_ticks; // alte Systemzeit buffer[0]=0; // Sicherheitshalber mit sauberem Puffer anfangen // Daten holen, maximal soviele, wie ein Kommando lang ist bytesRcvd=low_read(buffer,sizeof(command_t)); // LOG_DEBUG(("%d/%d read/av",bytesRcvd,tmp)); // LOG_DEBUG(("%x %x %x",buffer[0],buffer[1],buffer[2])); // Suche nach dem Beginn des Frames while ((start Daten verwerfen if (buffer[start] != CMD_STARTCODE){ // LOG_DEBUG(("start not found")); return -1; } // LOG_DEBUG(("Start @%d",start)); // haben wir noch genug Platz im Puffer, um das Packet ferig zu lesen? if ((RCVBUFSIZE-start) < sizeof(command_t)){ // LOG_DEBUG(("not enough space")); // printf("not enough space"); return -1; // nein? ==> verwerfen } i=sizeof(command_t) - (bytesRcvd-start)-1; if (i> 0) { // Fehlen noch Daten ? LOG_DEBUG(("command.c: Start @ %d es fehlen %d bytes ",start,i)); // Systemzeit erfassen old_ticks = TIMER_GET_TICKCOUNT_16; // So lange Daten lesen, bis das Packet vollstaendig ist, oder der Timeout zuschlaegt while (i > 0){ // Wenn der Timeout ueberschritten ist if (TIMER_GET_TICKCOUNT_16-old_ticks > MS_TO_TICKS(COMMAND_TIMEOUT)) return -1; // ==> Abbruch // LOG_DEBUG(("%d bytes missing",i)); i= low_read(buffer+bytesRcvd,i); // LOG_DEBUG(("%d read",i)); bytesRcvd+=i; i=sizeof(command_t) - (bytesRcvd-start); } } // LOG_DEBUG(("%d/%d read/start",bytesRcvd,start)); // LOG_DEBUG(("%x %x %x",buffer[start],buffer[start+1],buffer[start+2])); // Cast in command_t command= (command_t *) ( buffer +start); // LOG_DEBUG(("start: %x ",command->startCode)); // command_display(command); // validate (startcode ist bereits ok, sonst waeren wir nicht hier ) if (command->CRC==CMD_STOPCODE){ // LOG_DEBUG(("Command is valid")); // Transfer #ifdef PC command_lock(); // on PC make storage threadsafe #endif ptr = (char *) &received_command; for (i=0; i> 8) & 0xff; store = received_command.data_r; received_command.data_r = store << 8; received_command.data_r |= (store >> 8) & 0xff; store = received_command.seq; received_command.seq = store << 8; received_command.seq |= (store >> 8) & 0xff; /* "Umdrehen" des Bitfields */ store = received_command.request.subcommand; received_command.request.subcommand = store << 1; received_command.request.direction = store >> 7; #endif #ifdef PC command_unlock(); // on PC make storage threadsafe #endif return 0; } else { // Command not valid // LOG_DEBUG(("Invalid Command:")); // LOG_DEBUG(("%x %x %x",command->startCode,command->request.command,command->CRC)); return -1; } } static uint16 count=1; /*!< Zaehler fuer Paket-Sequenznummer*/ /*! * Uebertraegt ein Kommando und wartet nicht auf eine Antwort * @param command Kennung zum Command * @param subcommand Kennung des Subcommand * @param data_l Daten fuer den linken Kanal * @param data_r Daten fuer den rechten Kanal * @param payload Anzahl der Bytes, die diesem Kommando als Payload folgen */ void command_write(uint8 command, uint8 subcommand, int16* data_l,int16* data_r,uint8 payload){ command_t cmd; cmd.startCode=CMD_STARTCODE; cmd.request.direction=DIR_REQUEST; // Anfrage cmd.request.command= command; cmd.request.subcommand= subcommand; cmd.payload=payload; if (data_l != NULL) cmd.data_l = *data_l; else cmd.data_l = 0; if (data_r != NULL) cmd.data_r = *data_r; else cmd.data_r = 0; cmd.seq=count++; cmd.CRC=CMD_STOPCODE; low_write(&cmd); } /*! * Uebertraegt ein Kommando und wartet nicht auf eine Antwort * @param command Kennung zum Command * @param subcommand Kennung des Subcommand * @param data_l Daten fuer den linken Kanal * @param data_r Daten fuer den rechten Kanal */ //void command_write(uint8 command, uint8 subcommand, int16* data_l,int16* data_r){ //} /*! * Gibt dem Simulator Daten mit Anhang und wartet nicht auf Antwort * @param command Kennung zum Command * @param subcommand Kennung des Subcommand * @param data_l Daten fuer den linken Kanal * @param data_r Daten fuer den rechten Kanal * @param payload Anzahl der Bytes im Anhang * @param data Datenanhang an das eigentliche Command */ void command_write_rawdata(uint8 command, uint8 subcommand, int16* data_l, int16* data_r, uint8 payload, uint8* data){ command_write(command, subcommand, data_l, data_r,payload); low_write_data(data, payload); } /*! * Gibt dem Simulator Daten mit String-Anhang und wartet nicht auf Antwort * @param command Kennung zum Command * @param subcommand Kennung des Subcommand * @param data_l Daten fuer den linken Kanal * @param data_r Daten fuer den rechten Kanal * @param data Datenanhang an das eigentliche Command */ void command_write_data(uint8 command, uint8 subcommand, int16* data_l, int16* data_r, const char* data){ size_t len; uint8 payload; if (data != NULL) { len = strlen(data); if (len > MAX_PAYLOAD) { payload = MAX_PAYLOAD; } else { payload = len; } } else { payload = 0; } command_write(command, subcommand, data_l, data_r,payload); low_write_data((uint8 *)data, payload); } #ifdef MAUS_AVAILABLE /*! * Uebertraegt ein Bild vom Maussensor an den PC */ void transmit_mouse_picture(void){ int16 dummy,i; int16 pixel; uint8 data; maus_image_prepare(); for (i=0; i<6; i++) { dummy= i*54 +1; command_write(CMD_SENS_MOUSE_PICTURE, SUB_CMD_NORM, &dummy , &dummy,54); for (pixel=0; pixel <54; pixel++){ data= maus_image_read(); low_write_data((uint8 *)&data,1); } } } #endif /*! * Wertet das Kommando im Puffer aus * return 1, wenn Kommando schon bearbeitet wurde, 0 sonst */ int command_evaluate(void){ uint8 analyzed = 1; #ifdef LOG_AVAILABLE // command_display(&received_command); #endif // LOG_AVAILABLE switch (received_command.request.command) { #ifdef IR_AVAILABLE case CMD_SENS_RC5: ir_data=received_command.data_l; break; #endif case CMD_AKT_LED: // LED-Steuerung LED_set(received_command.data_l & 255); break; // Einige Kommandos ergeben nur fuer reale Bots Sinn case CMD_WELCOME: #ifdef MCU command_write(CMD_WELCOME, SUB_WELCOME_REAL,0,0,0); #else command_write(CMD_WELCOME, SUB_WELCOME_SIM,0,0,0); #endif break; #ifdef MAUS_AVAILABLE case CMD_SENS_MOUSE_PICTURE: // PC fragt nach dem Bild transmit_mouse_picture(); break; #endif #ifdef BEHAVIOUR_REMOTECALL_AVAILABLE case CMD_REMOTE_CALL: switch (received_command.request.subcommand) { case SUB_REMOTE_CALL_LIST: remote_call_list(); break; case SUB_REMOTE_CALL_ORDER: { uint8 buffer[REMOTE_CALL_BUFFER_SIZE]; low_read(buffer,received_command.payload); bot_remotecall_from_command((uint8 *)&buffer); break; } default: LOG_DEBUG(("unbekanntes Subkommando: %c",received_command.request.subcommand)); break; } break; #endif // Einige Kommandos ergeben nur fuer simulierte Bots Sinn #ifdef PC case CMD_SENS_IR: sensor_abstand(received_command.data_l,received_command.data_r); break; case CMD_SENS_ENC: sensEncL+=received_command.data_l; sensEncR+=received_command.data_r; break; case CMD_SENS_BORDER: sensBorderL=received_command.data_l; sensBorderR=received_command.data_r; break; case CMD_SENS_LINE: sensLineL=received_command.data_l; sensLineR=received_command.data_r; break; case CMD_SENS_LDR: sensLDRL=received_command.data_l; sensLDRR=received_command.data_r; break; case CMD_SENS_TRANS: sensTrans=(char)received_command.data_l; break; #ifdef MAUS_AVAILABLE case CMD_SENS_MOUSE: sensMouseDX=received_command.data_l; sensMouseDY=received_command.data_r; break; #endif case CMD_SENS_ERROR: sensError=(char)received_command.data_l; sensor_update(); /* Error ist der letzte uebertragene Sensorwert, danach koennen wir uns um allgemeine updates kümmern*/ break; case CMD_DONE: simultime=received_command.data_l; system_time_isr(); /* Einmal pro Update-Zyklus aktualisieren wir die Systemzeit */ // printf("X-Frame for Simultime = %d received ",simultime); break; #endif default: analyzed=0; // Command was not analysed yet break; } return analyzed; } #ifdef LOG_AVAILABLE /*! * Gibt ein Kommando auf dem Bildschirm aus */ void command_display(command_t * command){ #ifdef PC /* printf("START= %d\nDIR= %d CMD= %d SUBCMD= %d\nPayload= %d\nDATA = %d %d\nSeq= %d\nCRC= %d", (*command).startCode, (*command).request.direction, (*command).request.command, (*command).request.subcommand, (*command).payload, (*command).data_l, (*command).data_r, (*command).seq, (*command).CRC); */ LOG_DEBUG(("CMD: %c\tData L: %d\tSeq: %d\n", (*command).request.command, (*command).data_l, (*command).seq)); #else LOG_DEBUG(("%x %x %x", (*command).request.command, (*command).data_l, (*command).seq)); /* char* raw= (char*)command; unsigned char i=0; char hex[6]; display_cursor(4,1); display_string("0x"); to_hex(*raw,hex); display_string(hex); display_string(" "); raw++; for (i=1; i