diff options
Diffstat (limited to 'source/ct-Bot/bot-logic/behaviour_remotecall.c')
| -rw-r--r-- | source/ct-Bot/bot-logic/behaviour_remotecall.c | 407 |
1 files changed, 407 insertions, 0 deletions
diff --git a/source/ct-Bot/bot-logic/behaviour_remotecall.c b/source/ct-Bot/bot-logic/behaviour_remotecall.c new file mode 100644 index 0000000..6e0125f --- /dev/null +++ b/source/ct-Bot/bot-logic/behaviour_remotecall.c @@ -0,0 +1,407 @@ +/* + * 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 behaviour_remotecall_behaviour.c + * @brief ruft auf ein Kommando hin andere Verhalten auf und bestaetigt dann ihre Ausfuehrung + * + * @author Benjamin Benz (bbe@heise.de) + * @date 07.12.06 +*/ + +#include "bot-logic/bot-logik.h" +#ifdef BEHAVIOUR_REMOTECALL_AVAILABLE +#include <stdlib.h> +#include <stdio.h> +#include <string.h> +#include "log.h" +#include "command.h" + +#include "bot-logic/remote_calls.h" + +#define REMOTE_CALL_IDLE 0 +#define REMOTE_CALL_SCHEDULED 1 +#define REMOTE_CALL_RUNNING 2 + +/*! Uebergabevariable fuer Remotecall-Verhalten */ +static uint8 running_behaviour =REMOTE_CALL_IDLE; + +static uint8 function_id = 255; +static uint8 parameter_count = 0; /*!< Anzahl der Paramter (ohne Zeiger auf Aufrufer) */ +static uint8 parameter_data[8] = {0}; /*!< Hier liegen die eigentlichen Parameter, derzeit brauchen wir maximal 8 Byte (2 floats, 4 (u)int16 oder 4 (u)int8 */ +#ifdef MCU + static uint8 parameter_length[REMOTE_CALL_MAX_PARAM] = {0}; /*!< Hier speichern wir die Laenge der jeweiligen Parameter */ +#else + static uint8* parameter_length = NULL; /*!< Hier speichern wir die Laenge der jeweiligen Parameter */ +#endif + +#ifdef MCU + #include <avr/pgmspace.h> +#else + #define PROGMEM // Alibideklaration hat keine Funktion, verhindert aber eine Warning + #define strcmp_P strcmp // Auf dem PC gibt es keinen Flash, also auch kein eigenes Compare +#endif + +/*! + * Hier muessen alle Boten-Funktionen rein, die Remote aufgerufen werden sollen + * Diese stoßen dann das zugehoerige Verhalten an + * Ein Eintrag erfolgt so: + * PREPARE_REMOTE_CALL(BOTENFUNKTION,NUMBER_OF_PARAMS, STRING DER DIE PARAMETER BESCHREIBT,laenge der jeweiligen Parameter in Byte) + * + * Alle Botenfunktionen muessen folgendem Schema entsprechen + * void bot_xxx(Behaviour_t * caller, ...); + * + * Erklaerung am Bsp: + * PREPARE_REMOTE_CALL(bot_gotoxy, 2, "float x, float y", 4, 4), + * Name der Botenfunktion --^ ^ ^ ^ ^ + * Anzahl der Parameter --------- | | | + * Beschreibung der Parameter -------- | | + * Anzahl der Bytes Parameter 1 ------------------------ | + * Anzahl der Bytes Parameter 2 --------------------------- + * + * Zur Info: + * 1 Byte brauchen: uint8, int8,char + * 2 Byte brauchen: uint16, int16 + * 4 Byte brauchen: uint32, int32, float + */ +const call_t calls[] PROGMEM = { + #ifdef BEHAVIOUR_DRIVE_DISTANCE_AVAILABLE + PREPARE_REMOTE_CALL(bot_drive_distance,3, "int8 curve, int16 speed, int16 cm", 1,2,2), + #endif + #ifdef BEHAVIOUR_GOTOXY_AVAILABLE + PREPARE_REMOTE_CALL(bot_gotoxy, 2, "float x, float y", 4, 4), + #endif + #ifdef BEHAVIOUR_SOLVE_MAZE_AVAILABLE + PREPARE_REMOTE_CALL(bot_solve_maze,0,""), + #endif + #ifdef BEHAVIOUR_CATCH_PILLAR_AVAILABLE + PREPARE_REMOTE_CALL(bot_catch_pillar,0,""), + #endif + #ifdef BEHAVIOUR_DRIVE_SQUARE_AVAILABLE + PREPARE_REMOTE_CALL(bot_drive_square,0,""), + #endif + #ifdef BEHAVIOUR_FOLLOW_LINE_AVAILABLE + PREPARE_REMOTE_CALL(bot_follow_line,0,""), + #endif + #ifdef BEHAVIOUR_GOTO_AVAILABLE + PREPARE_REMOTE_CALL(bot_goto,2," int16 left, int16 right",2,2), + #endif + #ifdef BEHAVIOUR_OLYMPIC_AVAILABLE + PREPARE_REMOTE_CALL(bot_do_slalom,0,""), + #endif + #ifdef BEHAVIOUR_SCAN_AVAILABLE + PREPARE_REMOTE_CALL(bot_scan,0,""), + #endif + #ifdef BEHAVIOUR_SERVO_AVAILABLE + PREPARE_REMOTE_CALL(bot_servo,2,"uint8 servo, uint8 pos",1,1), + #endif + #ifdef BEHAVIOUR_SIMPLE_AVAILABLE + PREPARE_REMOTE_CALL(bot_simple,0,""), + PREPARE_REMOTE_CALL(bot_simple2,1,"int16 light",2), + #endif + #ifdef BEHAVIOUR_TURN_AVAILABLE + PREPARE_REMOTE_CALL(bot_turn,1,"int16 degrees",2) + #endif +}; + +#define STORED_CALLS (sizeof(calls)/sizeof(call_t)) /*!< Anzahl der Remote calls im Array */ + +/*! + * Sucht den Index des Remote-Calls heraus + * @param call String mit dem namen der gesuchten fkt + * @return Index in das calls-Array. Wenn nicht gefunden, dann 255 + */ +uint8 getRemoteCall(char * call){ +// LOG_DEBUG(("Suche nach Funktion: %s",call)); + + uint8 i; + for (i=0; i< (STORED_CALLS); i++){ + if (!strcmp_P (call, calls[i].name)){ + LOG_DEBUG(("calls[%d].name=%s passt",i,call)); + return i; + } + } + return 255; +} + +#ifdef MCU + /*! + * Hilfsfunktion fuer bot_remotecall() + * Baut einen AVR-kompatiblen Parameterstream aus einem uint32-Parameterarray und einem Infoarray ueber die Parameter + * @param dest Zeiger auf das Ausgabearray (len[0]*2 Byte gross!) + * @param count Anzahl der Parameter + * @param len Zeiger auf ein Array, das die Anzahl der Bytes fuer die jeweiligen Parameter enthaelt + * @param data Zeiger auf die Daten (32 Bit, Laenge 8) + * @author Timo Sandmann (mail@timosandmann.de) + * @date 12.01.2007 + */ + void remotecall_convert_params(uint8* dest, uint8 count, uint8* len, uint8* data){ + uint8 i; + /* jeden Parameter behandeln */ + for (i=0; i<count; i++){ + int8 j; + if (len[i] == 1) *dest++ = 0; // auch (u)int8 beginnen immer in geraden Registern + /* pro Parameter LSB zuerst nach dest kopieren */ + for (j=len[i]-1; j>=0; j--) + *dest++ = data[j]; + data += 4; // data-Array ist immer in 32 Bit + } + } +#endif // MCU + +/*! + * Dieses Verhalten kuemmert sich darum die Verhalten, die von aussen angefragt wurden zu starten und liefert ein feedback zurueck, wenn sie beendet sind. + * @param *data der Verhaltensdatensatz + */ +void bot_remotecall_behaviour(Behaviour_t *data){ + uint8 call_id =255; + +// LOG_DEBUG(("Enter bot_remotecall_behaviour")); + void (* func) (struct _Behaviour_t *data); + + switch (running_behaviour) { + case REMOTE_CALL_SCHEDULED: // Es laueft kein Auftrag, aber es steht ein neuer an +// LOG_DEBUG(("REMOTE_CALL_SCHEDULED")); + + call_id=function_id; + if (call_id >= STORED_CALLS){ +// LOG_DEBUG(("keine Funktion gefunden. Exit")); + running_behaviour=REMOTE_CALL_IDLE; + return; + } + + #ifdef PC + // Auf dem PC liegt die calls-Struktur im RAM + func = (void*) calls[call_id].func; + #else + // Auf dem MCU liegt die calls-Struktur im Flash und muss erst geholt werden + func = (void*) pgm_read_word (& calls[call_id].func); + #endif + + if (parameter_count ==0 ){ // Kommen wir ohne Parameter aus? +// LOG_DEBUG(("call_id=%u",call_id)); + func(data); // Die aufgerufene Botenfunktion starten + running_behaviour=REMOTE_CALL_RUNNING; + } else if (parameter_count <= REMOTE_CALL_MAX_PARAM){ // Es gibt gueltige Parameter + // TODO: Ja hier wird es spannend, denn jetzt muessen die Parameter auf den Stack + LOG_DEBUG(("call_id=%u",call_id)); + LOG_DEBUG(("parameter_count=%u", parameter_count)); + // asm-hacks here ;) + #ifdef PC + /* Prinzip auf dem PC: Wir legen alle Parameter einzeln auf den Stack, springen in die Botenfunktion und raeumen anschliessend den Stack wieder auf */ + uint32 tmp; + uint8 i; + volatile uint8 td=1; // verwenden wir nur, damit der Compiler unsere inline-asm-Bloecke nicht umordnet + for (i=0; i<parameter_count*4 && td>0; i+=4,td++){ // Debug-Info ausgeben +// LOG_DEBUG(("parameter_data[%u-%u] = %lu",i, i+3, *(uint32*)(parameter_data+i))); + } + /* Erster Wert in parameter_length ist die Anzahl der Parameter (ohne Zeiger des Aufrufers) */ + for (i=0; i<parameter_count && td>1; i++,td++){ // Check von td eigentlich sinnlos, aber wir brauchen eine echte Datenabhaengigkeit auf dieses Codestueck + /* cdecl-Aufrufkonvention => Parameter von rechts nach links auf den Stack */ + tmp = *(uint32*)(parameter_data+(parameter_count-i-1)*4); + /* Parameter 2 bis n pushen */ + asm volatile( // IA32-Support only + "pushl %0 # parameter i auf stack " + :: "g" (tmp) + : "memory" + ); + } + /* Parameter 1 (data) und Funktionsaufruf */ + asm volatile( + "pushl %0 # push data \n\t" + "movl %1, %%eax # adresse laden \n\t" + "call *%%eax # jump to callee \n\t" + :: "m" (data), "m" (func) + : "eax", "memory" + ); + /* caller rauemt den Stack wieder auf */ + for (i=0; i<=parameter_count && td>2; i++){ // Check von td erzwingt, dass das Aufraeumen erst jetzt passiert + asm volatile( + "pop %%eax # stack aufraeumen " + ::: "eax", "memory" + ); + } + #else + /* Prinzip auf der MCU: Keine komplizierten Rechnungen, sondern einfach alle Register ueberschreiben. + * Achtung: Derzeit braucht kein Verhalten mehr als 8 Register (2*float oder 4*int16 oder 4*int8), aendert sich das, + * muss man den Code hier erweitern! + * Die AVR-Konvention beim Funktionsaufruf: + * Die Groesse in Byte wird zur naechsten geraden Zahl aufgerundet, falls sie ungerade ist. + * Der Registerort faengt mit 26 an. + * Vom Registerort wird die berechete Groesse abgezogen und das Argument in diesen Registern uebergeben (LSB first). + * In r24/r25 legt der Compiler spaeter den Zeiger des Aufrufers, koennen wir hier also weglassen. */ +// LOG_DEBUG(("r22:r23 = %u:%u", parameter_data[1], parameter_data[0])); +// LOG_DEBUG(("r21:r20 = %u:%u", parameter_data[3], parameter_data[2])); +// LOG_DEBUG(("r18:r19 = %u:%u", parameter_data[5], parameter_data[4])); +// LOG_DEBUG(("r16:r17 = %u:%u", parameter_data[7], parameter_data[6])); + asm volatile( // remotecall_convert_params() hat den Speicher bereits richtig sortiert, nur noch Werte laden + "ld r23, Z+ \n\t" + "ld r22, Z+ \n\t" + "ld r21, Z+ \n\t" + "ld r20, Z+ \n\t" + "ld r19, Z+ \n\t" + "ld r18, Z+ \n\t" + "ld r17, Z+ \n\t" + "ld r16, Z " + :: "z" (parameter_data) + : "r23", "r22", "r21", "r20", "r19", "r18", "r17", "r16" + ); + func(data); // Die aufgerufene Botenfunktion starten + #endif + + running_behaviour=REMOTE_CALL_RUNNING; + return; + } else { +// LOG_DEBUG(("Parameteranzahl unzulaessig!")); + } + break; + + case REMOTE_CALL_RUNNING: // Es lief ein Verhalten und ist nun zuende (sonst waeren wir nicht hier) + { + // Antwort schicken + + char * function_name; + + #ifdef PC + function_name=(char*) &calls[function_id].name; + #else + // Auf dem MCU muessen wir die Daten erstmal aus dem Flash holen + + char tmp[REMOTE_CALL_FUNCTION_NAME_LEN+1]; + function_name=(char*)&tmp; + + uint8* from= (uint8*)& calls[function_id].name; + + uint8 i; + for (i=0; i<REMOTE_CALL_FUNCTION_NAME_LEN+1; i++) + *function_name++ = (uint8) pgm_read_byte ( from++ ); + function_name=(char*)&tmp; + #endif + + #ifdef COMMAND_AVAILABLE + int16 result = data->subResult; + command_write_data(CMD_REMOTE_CALL,SUB_REMOTE_CALL_DONE,&result,&result,function_name); + #endif + +// LOG_DEBUG(("Remote-call %s beendet",function_name)); + + // Aufrauemen + function_id=255; + //parameter_length=NULL; + //parameter_data=NULL; + running_behaviour=REMOTE_CALL_IDLE; + + return_from_behaviour(data); // und Verhalten auch aus + break; + } + default: + return_from_behaviour(data); // und Verhalten auch aus + break; + + } +} + +/*! + * Fuehre einen remote_call aus. Es gibt KEIN aufrufendes Verhalten!! + * @param func Zeiger auf den Namen der Fkt + * @param data Zeiger auf die Daten + */ +void bot_remotecall(char* func, remote_call_data_t* data){ + + function_id= getRemoteCall(func); + if (function_id >= STORED_CALLS){ +// LOG_DEBUG(("Funktion %s nicht gefunden. Exit!",func)); + return; + } + + // parameter_length: Zeiger auf ein Array, das zuerst die Anzahl der Parameter und danach die Anzahl der Bytes fuer die jeweiligen Parameter enthaelt + #ifdef PC + parameter_count = calls[function_id].param_count; + parameter_length = (uint8*)calls[function_id].param_len; + #else + // Auf dem MCU muessen wir die Daten erstmal aus dem Flash holen + uint8* from= (uint8*)& calls[function_id].param_len; + uint8 i; + parameter_count = pgm_read_byte(&calls[function_id].param_count); + for (i=0; i<REMOTE_CALL_MAX_PARAM; i++) + parameter_length[i] = (uint8) pgm_read_byte ( from++ ); + #endif +// LOG_DEBUG(("func=%s param_count=%d Len= %u %u %u %u",func,parameter_count,parameter_length[0],parameter_length[1],parameter_length[2])); +// if (data != NULL){ +// LOG_DEBUG(("data= %u %u %u %u",data[0],data[1],data[2],data[3])); +// } + + #ifdef MCU // Die MCU legt die Parameter nach einem anderen Verfahren ab, diese Funktion konvertiert sie deshalb + remotecall_convert_params(parameter_data, parameter_count, parameter_length, (uint8*)data); + #else // Auf dem PC kopieren wir die Daten einfach + memcpy(parameter_data, data, parameter_count*4); + #endif + + running_behaviour=REMOTE_CALL_SCHEDULED; + activateBehaviour(bot_remotecall_behaviour); +} + +/*! + * Fuehre einen remote_call aus. Es gibt KEIN aufrufendes Verhalten!! + * @param data Zeiger die Payload eines Kommandos. Dort muss zuerst ein String mit dem Fkt-Namen stehen. ihm folgen die Nutzdaten + */ +void bot_remotecall_from_command(uint8 * data){ + char * function_name = (char*)data; + remote_call_data_t * params = (remote_call_data_t *)(data+ strlen(function_name)+1); + bot_remotecall(function_name,params); +} + + +/*! + * Listet alle verfuegbaren Remote-Calls auf und verschickt sie als einzelne Kommanods + */ +void remote_call_list(void){ + #ifdef MCU + call_t call_storage; + uint8* to; + uint8* from; + #endif + call_t* call; + + int16 i; + for (i=0; i< (STORED_CALLS); i++){ + #ifdef MCU + // Auf dem MCU muessen die Daten erstmal aus dem Flash ins RAM + from= (uint8*)&calls[i]; + to= (uint8*)&call_storage; + uint8 j; + for (j=0; j< sizeof(call_t); j++){ + *to = (uint8) pgm_read_byte ( from++ ); + to++; + } + call = &call_storage; + #else + call = (call_t*)&calls[i]; + #endif + + #ifdef COMMAND_AVAILABLE + // und uebertragen + command_write_rawdata(CMD_REMOTE_CALL,SUB_REMOTE_CALL_ENTRY,&i,&i, sizeof(call_t),(uint8*)call); + #endif + +// LOG_DEBUG(("%s(%s)",call->name,call->param_info)); + } +} + +#endif |