/*
* Add ''len'' bytes from ''buf'' to the command buffer for the socket ''cmd''
* If we have a full line, then send the line to ''file''.
*/
static void command_add_frag(command* cmd, int file, const char* buf, int len)
{
int i;
for(i=0; i<len; i++)
{
switch(buf[i])
{
case 0:
printf("read a '0' ?\n");
case '\n':
case '\r':
command_write(cmd, file);
break;
default:
cmd->buf[cmd->ptr++] = buf[i];
if(cmd->ptr >= sizeof(cmd->buf)-1)
{
printf("buf overflow!\n");
command_write(cmd, file);
}
break;
}
}
}
/* ************************************************************************* *
* コマンド分岐関数
* ************************************************************************* */
void
exec_command (char command,
char *parameter, char *str) {
switch (command) {
case 'Q':
command_quit(parameter);
break;
case 'C':
command_check(parameter);
break;
case 'P':
command_print(profile_data, atoi(parameter), str);
break;
case 'R':
command_read(profile_data, parameter);
break;
case 'W':
command_write(profile_data, parameter);
break;
case 'F':
command_find(profile_data, parameter, str);
break;
case 'S':
command_sort(profile_data, atoi(parameter), str);
break;
default:
printf("Invalid command '%c' was found.\n", command);
break;
}
}
/**
* Initialisiert die (High-Level-)Kommunikation
*/
void command_init(void) {
/* eigene Adresse checken */
uint8_t addr = get_bot_address();
if (addr != CMD_BROADCAST && addr > 127) {
/* gespeicherte Adresse ist eine vom Sim Vergebene,
* schalte auf Adressevergabemodus um */
addr = CMD_BROADCAST;
set_bot_address(addr);
}
#ifdef ARM_LINUX_BOARD
/* Start-Synchronisation mit ATmega */
LOG_INFO("Establishing connection to ATmega...");
fflush(stdout);
LOG_DEBUG("command_init(): Sending CMD_DONE to ATmega...");
set_bot_2_atmega();
command_write(CMD_DONE, SUB_CMD_NORM, simultime, 0, 0);
LOG_DEBUG("command_init(): Waiting for CMD_DONE from ATmega...");
const int8_t r = receive_until_frame(CMD_DONE);
if (r != 0) {
LOG_ERROR("command_init(): received invalid command: %d", r);
} else {
LOG_INFO("Connection with ATmega up.");
}
#endif // ARM_LINUX_BOARD
#ifdef MCU
cmd_functions.write = uart_write;
cmd_functions.read = uart_read;
cmd_functions.crc_check = uart_check_crc;
cmd_functions.crc_calc = uart_calc_crc;
#endif // MCU
LOG_DEBUG("command_init() done.");
}
void uart_test(uint32_t runs) {
puts("Initializing...");
if (bot_2_atmega_init() != 0) {
exit(1);
}
set_bot_2_atmega();
memset(&cmd_to_send, 0, sizeof(command_t));
memset(&received_command, 1, sizeof(command_t));
printf("Sending %u CMD_DONE packages...\n", runs);
struct timeval start, end;
gettimeofday(&start, NULL);
uint32_t i;
for (i = 0; i < runs; ++i) {
// uint8_t stop = 0;
command_write(CMD_DONE, SUB_CMD_NORM, i, 0, 0);
int8_t r = receive_until_frame(CMD_DONE);
if (r != 0) {
LOG_ERROR("uart_test(): %u: received invalid command: %d", i, r);
unsigned j;
uint8_t * rx = (uint8_t *) &received_command;
for (j = 0; j < sizeof(command_t); ++j) {
printf("0x%02x ", *(rx++));
}
puts("");
// stop = 1;
}
// puts("tx:");
// command_display(&cmd_to_send);
// puts("rx:");
// command_display(&received_command);
// unsigned j;
// uint8_t * tx = (uint8_t *) &cmd_to_send;
// uint8_t * rx = (uint8_t *) &received_command;
// for (j = 0; j < sizeof(command_t); ++j) {
// if (*rx != *tx && j != 3) {
// printf("uart_test(): ERROR on cmd %u: byte 0x%x differs: 0x%x/0x%x\n", i, j, *tx, *rx);
// stop = 1;
// }
// ++rx;
// ++tx;
// }
// if (stop) {
// exit(1);
// }
memset(&cmd_to_send, 0, sizeof(command_t));
memset(&received_command, 1, sizeof(command_t));
}
gettimeofday(&end, NULL);
uint64_t t = (end.tv_sec - start.tv_sec) * 1000000UL + end.tv_usec - start.tv_usec;
printf("done (%u commands in %llu us).\n", runs, t);
printf("%llu us / cmd.\n", t / runs);
exit(0);
}
static int ssh_stream_write(
git_smart_subtransport_stream *stream,
const char *buffer,
size_t len)
{
ssh_stream *s = (ssh_stream *)stream;
if (command_write(&s->commandHandle, buffer, len))
return -1;
return 0;
}
/**
* Uebertraegt ein Bild vom Maussensor an den PC
* Insgesamt gibt es 324 Pixel
* <pre>
* 18 36 ... 324
* .. .. ... ..
* 2 20 ... ..
* 1 19 ... 307
* </pre>
* Gesendet weren: Pixeldaten (Bit 0 bis Bit 5), Pruefbit, ob Daten gueltig (Bit 6), Markierung fuer den Anfang eines Frames (Bit 7)
*/
void mouse_transmit_picture(void) {
int16_t dummy = 1;
uint8_t data, i, pixel;
mouse_sens_write(MOUSE_PIXEL_DATA_REG, 0x00); // Frame grabben anstossen
for (i = 0; i < 6; i++, dummy += 54) {
command_write(CMD_SENS_MOUSE_PICTURE, SUB_CMD_NORM, dummy, 0, 54);
for (pixel = 0; pixel < 54; pixel++) {
do {
data = mouse_sens_read(MOUSE_PIXEL_DATA_REG);
} while ((data & 0x40) != 0x40);
cmd_functions.write(&data, 1);
}
}
}
int ALU(int command, int operand)
{
trace;
if (command == 10) {
return command_read(operand);
} else if (command == 11) {
return command_write(operand);
} else if (command == 20) {
return command_load(operand);
} else if (command == 21) {
return command_store(operand);
} else if (command == 30) {
return command_add(operand);
} else if (command == 31) {
return command_sub(operand);
} else if (command == 32) {
return command_divide(operand);
} else if (command == 33) {
return command_mul(operand);
} else if (command == 40) {
return command_jump(operand);
} else if (command == 41) {
return command_jneg(operand);
} else if (command == 42) {
return command_jz(operand);
} else if (command == 43) {
return command_halt();
} else if (command == 51) {
return command_not(operand);
} else if (command == 52) {
return command_and(operand);
} else if (command == 53) {
return command_or(operand);
} else if (command == 54) {
return command_xor(operand);
} else if (command == 55) {
return command_jns(operand);
} else if (command == 56) {
return command_jc(operand);
} else if (command == 57) {
return command_jnc(operand);
} else if (command == 58) {
return command_jp(operand);
} else if (command == 59) {
return command_jnp(operand);
} else if (command == 60) {
return command_chl(operand);
} else if (command == 61) {
return command_shr(operand);
} else if (command == 62) {
return command_rcl(operand);
} else if (command == 63) {
return command_rcr(operand);
} else if (command == 64) {
return command_neg(operand);
} else if (command == 65 || command == 75) {
return command_addc(operand);
} else if (command == 66 || command == 76) {
return command_subc(operand);
} else if (command == 67) {
return command_loglc(operand);
} else if (command == 68) {
return command_logrc(operand);
} else if (command == 69) {
return command_rccl(operand);
} else if (command == 70) {
return command_rccr(operand);
} else if (command == 71) {
return command_mova(operand);
} else if (command == 72) {
return command_movr(operand);
} else if (command == 73) {
return command_movca(operand);
} else if (command == 74) {
return command_movcr(operand);
}
sc_regSet(COMMAND_ERROR, 1);
return -1;
}
/**
* Fuehrt die Verarbeitung in der Hauptschleife nach dem
* Verhaltenscode durch. Dazu gehoert beispielsweise, die
* Bildschirmanzeige zu steuern und den Simulator ueber den aktuellen
* Zustand zu informieren.
*/
void post_behaviour(void) {
static uint16_t comm_ticks = 0;
static uint8_t uart_gui = 0;
#if defined MCU && defined DEBUG_TIMES
static uint32_t log_ticks = 0;
#endif
#ifdef BOT_2_RPI_AVAILABLE
bot_2_linux_listen();
#endif // BOT_2_RPI_AVAILABLE
#ifdef CREATE_TRACEFILE_AVAILABLE
trace_add_sensors();
#endif // CREATE_TRACEFILE_AVAILABLE
/* jeweils alle 100 ms kommunizieren Bot, User und Sim */
if (timer_ms_passed_16(&comm_ticks, 50)
#ifdef RC5_AVAILABLE
|| RC5_Code != 0
#endif
) {
if (uart_gui == 0
#ifdef RC5_AVAILABLE
|| RC5_Code != 0
#endif
) {
#ifdef DISPLAY_AVAILABLE
/* GUI-Behandlung starten */
gui_display(display_screen);
#endif // DISPLAY_AVAILABLE
uart_gui = 1; // bot-2-sim ist erst beim naechsten Mal dran
} else {
#ifdef BOT_2_SIM_AVAILABLE
/* Den Sim ueber Sensoren und Aktuatoren informieren */
bot_2_sim_inform(); // NOP auf PC
#endif // BOT_2_SIM_AVAILABLE
uart_gui = 0; // naechstes Mal wieder mit GUI anfangen
}
}
#if defined MCU && defined DEBUG_TIMES
if (timer_ms_passed_32(&log_ticks, 1000UL)) {
uint8_t cpu, uart_in, uart_out;
os_get_utilizations(&cpu, &uart_in, &uart_out);
LOG_INFO("CPU=%3u%% UART_IN=%3u%% UART_OUT=%3u%%", cpu, uart_in, uart_out);
}
#endif // MCU && DEBUG_TIMES
#ifdef PC
#ifdef CREATE_TRACEFILE_AVAILABLE
trace_add_actuators();
#endif // CREATE_TRACEFILE_AVAILABLE
/* Sim oder ATmega ueber naechsten Schleifendurchlauf / Bot-Zyklus informieren */
#ifdef ARM_LINUX_BOARD
set_bot_2_atmega();
#endif // ARM_LINUX_BOARD
command_write(CMD_DONE, SUB_CMD_NORM, simultime, 0, 0); // flusht auch den Sendepuffer
#ifdef DEBUG_TIMES
/* Zum Debuggen der Zeiten */
GETTIMEOFDAY(&init_stop, NULL);
int t2 = (init_stop.tv_sec - init_start.tv_sec) * 1000000 + init_stop.tv_usec - init_start.tv_usec;
LOG_DEBUG("Done-Token (%d) out after %d usec", simultime, t2);
#endif // DEBUG_TIMES
#endif // PC
#if defined OS_AVAILABLE
/* Rest der Zeitscheibe (OS_TIME_SLICE ms) schlafen legen */
os_thread_yield();
#endif // OS_AVAILABLE
}
int command_write_gitbuf(COMMAND_HANDLE *commandHandle, const git_buf *buf)
{
return command_write(commandHandle, buf->ptr, buf->size);
}
int main(int argc, const char **argv)
{
po::options_description options("usage");
int fd = -1;
try
{
std::string host;
std::string port;
std::string filename;
std::string start_string;
std::string length_string;
std::string chunk_size_string;
unsigned int start, length, chunk_size;
bool use_udp = false;
bool verbose = false;
bool verbose2 = false;
bool nocommit = false;
bool noreset = false;
bool notemp = false;
bool otawrite = false;
bool use_force = false;
bool erase_before_write = false;
bool cmd_write = false;
bool cmd_simulate = false;
bool cmd_verify = false;
bool cmd_checksum = false;
bool cmd_read = false;
bool force_used;
action_t action;
options.add_options()
("checksum,C", po::bool_switch(&cmd_checksum)->implicit_value(true), "CHECKSUM")
("chunksize,c", po::value<std::string>(&chunk_size_string)->default_value("0"), "send/receive chunk size")
("erase,e", po::bool_switch(&erase_before_write)->implicit_value(true), "erase before write (instead of during write)")
("filename,f", po::value<std::string>(&filename), "file name")
("force,F", po::bool_switch(&use_force)->implicit_value(true), "use force if image seems to be incompatible")
("host,h", po::value<std::string>(&host)->required(), "host to connect to")
("length,l", po::value<std::string>(&length_string)->default_value("0x1000"), "read length")
("nocommit,n", po::bool_switch(&nocommit)->implicit_value(true), "don't commit after writing")
("noreset,N", po::bool_switch(&noreset)->implicit_value(true), "don't reset after commit")
("notemp,t", po::bool_switch(¬emp)->implicit_value(true), "don't commit temporarily, commit to flash")
("port,p", po::value<std::string>(&port)->default_value("24"), "port to connect to")
("start,s", po::value<std::string>(&start_string)->default_value("2147483647"), "send/receive start address")
("read,R", po::bool_switch(&cmd_read)->implicit_value(true), "READ")
("simulate,S", po::bool_switch(&cmd_simulate)->implicit_value(true), "WRITE simulate")
("udp,u", po::bool_switch(&use_udp)->implicit_value(true), "use UDP instead of TCP")
("verbose,v", po::bool_switch(&verbose)->implicit_value(true), "verbose output")
("verbose2,x", po::bool_switch(&verbose2)->implicit_value(true), "less verbose output")
("verify,V", po::bool_switch(&cmd_verify)->implicit_value(true), "VERIFY")
("write,W", po::bool_switch(&cmd_write)->implicit_value(true), "WRITE");
po::positional_options_description positional_options;
po::variables_map varmap;
po::store(po::parse_command_line(argc, argv, options), varmap);
po::notify(varmap);
if(cmd_checksum)
action = action_checksum;
else
if(cmd_verify)
action = action_verify;
else
if(cmd_simulate)
action = action_simulate;
else
if(cmd_write)
action = action_write;
else
if(cmd_read)
action = action_read;
else
action = action_none;
start = 0;
chunk_size = 0;
try
{
chunk_size = std::stoi(chunk_size_string, 0, 0);
}
catch(...)
{
throw(std::string("invalid value for chunk size argument"));
}
try
{
start = std::stoi(start_string, 0, 0);
}
catch(...)
{
throw(std::string("invalid value for start argument"));
}
try
{
length = std::stoi(length_string, 0, 0);
}
catch(...)
{
throw(std::string("invalid value for length argument"));
}
std::string reply;
std::vector<int> int_value;
std::vector<std::string> string_value;
unsigned int flash_sector_size, flash_ota, flash_slots, flash_slot;
unsigned int flash_address[4];
unsigned int preferred_chunk_size;
GenericSocket channel(host, port, use_udp, verbose);
force_used = false;
try
{
process(channel, "flash-info", reply, "OK [^,]+, sector size: ([0-9]+)[^,]+, OTA update available: ([0-9]+), "
"slots: ([0-9]+), slot: ([0-9]+), "
"address: ([0-9]+), address: ([0-9]+), address: ([0-9]+), address: ([0-9]+)"
"(?:, preferred chunk size: ([0-9]+))?"
"\\s*",
string_value, int_value, verbose);
}
catch(std::string &e)
{
if(use_force)
{
std::cout << "OTA incompatible image, trying to continue due to force flag: " << e << std::endl;
force_used = true;
}
else
throw(std::string("OTA incompatible image: ") + e);
}
if(force_used)
{
flash_sector_size = 4096;
flash_ota = 0;
flash_slots = 0;
flash_slot = 0;
flash_address[0] = 0;
flash_address[1] = 0;
flash_address[2] = 0;
flash_address[3] = 0;
preferred_chunk_size = 512;
}
else
{
flash_sector_size = int_value[0];
flash_ota = int_value[1];
flash_slots = int_value[2];
flash_slot = int_value[3];
flash_address[0] = int_value[4];
flash_address[1] = int_value[5];
flash_address[2] = int_value[6];
flash_address[3] = int_value[7];
preferred_chunk_size = int_value[8];
}
std::cout << "flash operations available, sector size: " << flash_sector_size;
if(flash_ota)
std::cout << ", OTA update available, slots: " << flash_slots << ", current slot: " << flash_slot
<< ", address[0]: 0x" << std::setw(6) << std::setfill('0') << std::hex << flash_address[0]
<< ", address[1]: 0x" << std::setw(6) << std::setfill('0') << std::hex << flash_address[1]
<< ", address[2]: 0x" << std::setw(6) << std::setfill('0') << std::hex << flash_address[2]
<< ", address[3]: 0x" << std::setw(6) << std::setfill('0') << std::hex << flash_address[3]
<< ", preferred chunk size: " << std::setw(0) << std::setfill(' ') << std::dec << preferred_chunk_size << std::endl;
else
std::cout << ", OTA update NOT available" << std::endl;
if(chunk_size == 0)
chunk_size = preferred_chunk_size;
if(chunk_size == 0)
chunk_size = 512;
if((flash_sector_size % chunk_size) != 0)
throw(std::string("chunk size should be dividable by flash sector size"));
if(start == 2147483647)
{
if(flash_ota)
{
if(action == action_write)
{
flash_slot++;
if(flash_slot >= flash_slots)
flash_slot = 0;
}
start = flash_address[flash_slot];
otawrite = true;
}
else
throw(std::string("no start address supplied and image does not support OTA updating"));
}
if((start % flash_sector_size) != 0)
throw(std::string("start address should be dividable by flash sector size"));
int64_t file_length = 0;
if((action != action_none) && (action != action_read))
{
struct stat stat;
if(filename.empty())
throw(std::string("file name required"));
if((fd = open(filename.c_str(), O_RDONLY, 0)) < 0)
throw(std::string("file not found"));
fstat(fd, &stat);
file_length = stat.st_size;
}
if(action == action_read)
{
if(filename.empty())
throw(std::string("file name required"));
if((fd = open(filename.c_str(), O_WRONLY | O_TRUNC | O_CREAT, 0777)) < 0)
throw(std::string("can't create file"));
file_length = 0;
}
switch(action)
{
case(action_read):
{
command_read(channel, fd, start, length, flash_sector_size, chunk_size, verbose);
break;
}
case(action_checksum):
{
command_checksum(channel, fd, file_length, start, flash_sector_size, verbose);
break;
}
case(action_write):
case(action_simulate):
case(action_verify):
{
command_write(channel, fd, file_length, start, flash_sector_size, chunk_size, verbose, action, erase_before_write);
break;
}
case(action_none):
{
break;
}
}
if((action == action_write) && otawrite)
{
if(!nocommit)
{
std::string send_string;
std::string reply;
if(notemp)
{
send_string = std::string("flash-select ") + std::to_string(flash_slot);
process(channel, send_string, reply, "OK flash-select: slot ([0-9]+) selected, address ([0-9]+)\\s*", string_value, int_value, verbose || verbose2);
}
else
{
send_string = std::string("flash-select-once ") + std::to_string(flash_slot);
process(channel, send_string, reply, "OK flash-select-once: slot ([0-9]+) selected, address ([0-9]+)\\s*", string_value, int_value, verbose ||verbose2);
}
if((unsigned int)int_value[0] != flash_slot)
throw(std::string("flash-select failed, local slot (") + std::to_string(flash_slot) + ") != remote slot (" + std::to_string(int_value[0]) + ")");
if((unsigned int)int_value[1] != start)
throw(std::string("flash-select failed, local address (") + std::to_string(flash_slot) + ") != remote address (" + std::to_string(int_value[0]) + ")");
if(notemp)
std::cout << "selected boot slot";
else
std::cout << "selected one time boot slot";
std::cout << ": " << flash_slot << ", address: 0x" << std::hex << std::setw(6) << std::setfill('0') << start << std::dec << std::setw(0) << std::endl;
if(!noreset)
{
std::cout << "rebooting" << std::endl;
channel.send(1000, std::string("reset"));
sleep(2);
channel.reconnect();
std::cout << "reboot finished" << std::endl;
if(!notemp)
{
process(channel, "flash-info", reply, "OK [^,]+, sector size: ([0-9]+)[^,]+, OTA update available: ([0-9]+), "
"slots: ([0-9]+), slot: ([0-9]+), "
"address: ([0-9]+), address: ([0-9]+), address: ([0-9]+), address: ([0-9]+)"
"(?:, preferred chunk size: ([0-9]+))?"
"\\s*",
string_value, int_value, verbose);
if(int_value[3] != (int)flash_slot)
std::cout << "boot failed, requested slot: " << flash_slot << ", active slot: " << int_value[3] << std::endl;
else
{
std::cout << "boot succeeded, permanently selecting boot slot: " << flash_slot << ", address: 0x" << std::hex << std::setw(6) << std::setfill('0') << start << std::dec << std::setw(0) << std::endl;
std::string send_string;
std::string reply;
send_string = std::string("flash-select ") + std::to_string(flash_slot);
process(channel, send_string, reply,
"OK flash-select: slot ([0-9]+) selected, address ([0-9]+)\\s*",
string_value, int_value, verbose || verbose2);
if((unsigned int)int_value[0] != flash_slot)
throw(std::string("flash-select failed, local slot (") + std::to_string(flash_slot) + ") != remote slot (" + std::to_string(int_value[0]) + ")");
if((unsigned int)int_value[1] != start)
throw(std::string("flash-select failed, local address (") + std::to_string(flash_slot) + ") != remote address (" + std::to_string(int_value[0]) + ")");
}
}
}
process(channel, "stats", reply, "> firmware version date: ([a-zA-Z0-9: ]+).*", string_value, int_value, verbose, 1000);
std::cout << "firmware version: " << string_value[0] << std::endl;
}
}
}
catch(const po::error &e)
{
std::cerr << std::endl << "espflash: " << e.what() << std::endl << options;
goto error;
}
catch(const std::exception &e)
{
std::cerr << std::endl << "espflash: " << e.what() << std::endl;
goto error;
}
catch(const std::string &e)
{
std::cerr << std::endl << "espflash: " << e << std::endl;
goto error;
}
catch(...)
{
std::cerr << std::endl << "espflash: unknown exception caught" << std::endl;
goto error;
}
if(fd >= 0)
close(fd);
return(0);
error:
if(fd >= 0)
close(fd);
return(1);
}
/*!
* Verhalten um sich entlang des Fahrweges relevante Koordinaten auf dem Stack zu merken; Verhalten ist nach Aktivierung via Botenroutine
* ein Endlosverhalten und sammelt bis zur Deaktivierung die Wegepunkte; deaktiviert wird es via Notaus oder direkt mit Befehl zum Zurueckfahren und
* damit Start des Stack-Fahrverhaltens
* @param *data Der Verhaltensdatensatz
*/
void bot_save_waypos_behaviour(Behaviour_t * data) {
static uint8_t skip_count = 0;
switch (waypos_state) {
case 0:
pos_store = pos_store_new_size(data, STACK_SIZE);
if (pos_store == NULL) {
exit_behaviour(data, BEHAVIOUR_SUBFAIL);
return;
}
set_pos_to_last(); // aktuelle Botposition wird zum ersten Stackeintrag und merken der Position
bot_push_pos(last_pos.x, last_pos.y);
waypos_state = 1;
break;
case 1: {
// Botpos wird in den Stack geschrieben zum Startzeitpunkt des Verhaltens (Stack noch leer) oder
// wenn die Entfernung zur zuletzt gemerkten Koordinate gewissen Abstand ueberschreitet oder
// wenn Entfernung noch nicht erreicht ist aber ein gewisser Drehwinkel erreicht wurde
uint8_t update = False;
// Abstand zur letzten Position ueberschritten
if (get_dist(last_pos.x, last_pos.y, x_pos, y_pos) > DIST_FOR_PUSH) {
// kein Push notwendig bei gerader Fahrt voraus zum Sparen des Stack-Speicherplatzes
if ((int16_t) heading != last_heading) {
update = True;
}
set_pos_to_last(); // Position jedenfalls merken
}
// bei Drehwinkelaenderung und Uberschreitung einer gewissen Groesse mit geringer Abstandsentfernung zum letzten Punkt kommt er in den Stack
if (turned_angle(last_heading) > ANGLE_FOR_PUSH && get_dist(last_pos.x, last_pos.y, x_pos, y_pos) > DIST_FOR_PUSH_TURN) {
set_pos_to_last();
update = True;
}
if (update) {
LOG_DEBUG("Position (%d|%d) kommt in den Stack", x_pos, y_pos);
position_t pos_0;
pos_0.x = x_pos / 16;
pos_0.y = y_pos / 16;
if (optimized_push >= 1) {
/* Positionen entfernen, die auf einer Linie liegen */
position_t pos_1, pos_2;
if (pos_store_top(pos_store, &pos_1, 1) == True && pos_store_top(pos_store, &pos_2, 2) == True) {
pos_1.x /= 16;
pos_1.y /= 16;
if (abs(pos_1.x - pos_0.x) <= 1) {
pos_0.x = pos_1.x;
}
pos_2.x /= 16;
pos_2.y /= 16;
if (abs(pos_2.x - pos_1.x) <= 1) {
pos_2.x = pos_1.x;
}
int8_t m_1 = (int8_t) (pos_1.x == pos_2.x ? 100 : abs((pos_1.y - pos_2.y) / (pos_1.x - pos_2.x)));
int8_t m = (int8_t) (pos_0.x == pos_1.x ? 100 : abs((pos_0.y - pos_1.y) / (pos_0.x - pos_1.x)));
LOG_DEBUG(" pos_2=(%d|%d)", pos_2.x, pos_2.y);
LOG_DEBUG(" pos_1=(%d|%d)", pos_1.x, pos_1.y);
LOG_DEBUG(" pos =(%d|%d)", pos_0.x, pos_0.y);
LOG_DEBUG(" m_1=%3d\tm=%3d", m_1, m);
LOG_DEBUG(" skip_count=%u", skip_count);
if (abs(m_1 - m) < MAX_GRADIENT) {
LOG_DEBUG("Neue Position auf einer Linie mit beiden Letzten");
if (skip_count < 3) {
LOG_DEBUG(" Verwerfe letzten Eintrag (%d|%d)", pos_1.x, pos_1.y);
pos_store_pop(pos_store, &pos_1);
skip_count++;
} else {
LOG_DEBUG(" Verwerfe Eintrag NICHT, skip_count=%u", skip_count);
skip_count = 0;
}
} else {
skip_count = 0;
}
}
if (optimized_push >= 2) {
/* Schleifen entfernen */
uint8_t i;
for (i=2; pos_store_top(pos_store, &pos_1, i); ++i) {
int32_t diff = get_dist(x_pos, y_pos, pos_1.x, pos_1.y);
LOG_DEBUG(" diff=%d", diff);
if (diff < MAX_POS_DIFF) {
LOG_DEBUG(" Position (%d|%d)@%u liegt in der Naehe", pos_1.x, pos_1.y, i);
uint8_t k;
for (k=i; k>1; --k) {
pos_store_pop(pos_store, &pos_1); // die Positionen bis zur i-ten zurueck loeschen
LOG_DEBUG(" Loesche (%d|%d) vom Stack", pos_1.x, pos_1.y);
}
i = 2;
}
}
}
}
bot_push_pos(x_pos, y_pos);
#if defined PC && defined DEBUG
pos_store_dump(pos_store);
#endif
#ifdef DEBUG
#ifdef MAP_2_SIM_AVAILABLE
command_write(CMD_MAP, SUB_MAP_CLEAR_LINES, 0, 0, 0);
position_t pos_1, pos_2;
pos_store_top(pos_store, &pos_0, 1);
uint16_t i;
for (i = 2; pos_store_top(pos_store, &pos_1, (uint8_t) i); ++i) {
map_draw_line_world(pos_0, pos_1, 0);
pos_0 = pos_1;
pos_1.x = pos_0.x - 16;
pos_1.y = pos_0.y;
pos_2.x = pos_0.x + 16;
pos_2.y = pos_0.y;
map_draw_line_world(pos_1, pos_2, 1);
pos_1.x = pos_0.x;
pos_1.y = pos_0.y - 16;
pos_2.x = pos_0.x;
pos_2.y = pos_0.y + 16;
map_draw_line_world(pos_1, pos_2, 1);
}
#endif // MAP_2_SIM_AVAILABLE
#endif // DEBUG
}
break;
} // case 1
} // switch
}
