int readArduinoInput(int cport_nr, char shellFilename[], char bufTest[]) {
int i, n;
unsigned char buf[4096];
while(sentinel == 1)
{
n = PollComport(cport_nr, buf, 4095);
if(n > 0)
{
buf[n] = 0; /* always put a "null" at the end of a string! */
for(i=0; i < n; i++)
{
if(buf[i] < 32) /* replace unreadable control-codes by dots */
{
buf[i] = '.';
}
}
int comp = strncmp((char *)buf, (char *)bufTest, 4);
if(comp == 0) {
execv(shellFilename, &shellFilename);
sentinel = 0;
}
printf("received %i bytes: %s\n", n, (char *)buf);
}
#ifdef _WIN32
Sleep(100); /* it's ugly to use a sleeptimer, in a real program, change the while-loop into a (interrupt) timerroutine */
#else
usleep(100000); /* sleep for 100 milliSeconds */
#endif
}
return 0;
}
int CiCreateDef::ReceivePacketFromCreate(CreateSensors &IO_SENSORS_CREATE, unsigned char packet) {
unsigned char *dataR;
DWORD pocet;
dataR = (unsigned char*) malloc(500);
pocet = PollComport(dataR, 500);
//pocet = ReadNBytes(dataR, OI_PacketSize[packet]+COMMAND_CREATE_SIZE);
//std::cout << "ma byt" << OI_PacketSize[packet] << " prislo" << pocet << "\n";
/*union pomoc_t {
WORD data;
unsigned char datab[2];
} pomoc1;
pomoc1.datab[1] = *(dataR + 13);
pomoc1.datab[0] = *(dataR + 14);
std::cout << "zmena polohy" << pomoc1.data << "\n";*/
if (pocet == OI_PacketSize[packet]) {
//std::cout << "prvy" << static_cast<int>(*(dataR)) << "\n";
DecodeSensorsFromPacket(IO_SENSORS_CREATE, packet, dataR);
return pocet;
} else {
std::cout << "došiel chybný packet\n";
}
return -1;
}
//move servo to a position with a certain speed
void servo_move(int comport, int ID, int angle, int rpm)
{
byte servo_id = ID;
if (angle > upper_limit)
angle = upper_limit;
else if (angle < lower_limit)
angle = lower_limit;
byte angle_h = (angle * 0X3FF) / (256 * 360);
byte angle_l = ((angle * 0X3FF) / 360) % 256;
byte rpm_h = (rpm * 0X3FF) / (256 * 114);
byte rpm_l = ((rpm * 0X3FF) / 114) % 256;
byte chksum = 0XFF - servo_id - 0X07 - 0X03 - 0X1E - angle_l - angle_h - rpm_l - rpm_h;
byte inst[11] = {0XFF, 0XFF, servo_id, 0X07, 0X03, 0X1E, angle_l, angle_h, rpm_l, rpm_h, chksum};
send_inst(comport, inst, 11);
usleep(ret_delay);
byte buf[20] = {0};
PollComport(comport, buf, 20);
return;
}
// read multiple bytes, non-blocking.
// TODO : there is no way to know how many bytes were actually read !
bool CCPCBooster::ReadBuffer(unsigned char *buffer, const long nbBytes)
{
#if _WINDOWS
unsigned long nbBytesReceived = 0;
BOOL fSuccess = ReadFile (_COMPortHandle, buffer, nbBytes, &nbBytesReceived, NULL);
return ((nbBytesReceived == nbBytes) && fSuccess);
#else
/*
for (long i=0 ; i< nbBytes ; i++)
buffer[i] =_COMPortHandle.ReadByte( 100 );
*/
long bytesLeft = nbBytes;
while(bytesLeft != 0)
{
int i = PollComport(_COMPortHandle, buffer, bytesLeft) ;
if (i > 0) {
bytesLeft -= i;
buffer += i;
}
}
return true ;
#endif
}
int
GetPacerParamValue(int paramType) {
char buff[1024];
int sendIndex = 0;
int raspunsOk = 0;
char *indexK = NULL;
int timeout = 0;
PollComport(SERIAL_PORT, buff, sizeof(buff));
sendPacketToPacer(FTYPE_GET_PACER_PARAM, 0x00, paramType);
while((sendIndex = PollComport(SERIAL_PORT, buff, sizeof(buff))) == 0 && ++timeout < 5000);
if((indexK = strchr(buff, 'K')) != NULL)
return (int)(*(indexK + 1));
return -1;
}
//read temperature of servo in celsius
int read_temp(int comport, int ID)
{
byte buf[10] = {0};
byte chksum = 0XFF - ID - 0X32;
byte inst[8] = {0XFF, 0XFF, ID, 0X04, 0X02, 0X2B, 0X01, chksum};
send_inst(comport, inst, 8);
usleep(ret_delay);
PollComport(comport, buf, 10);
return buf[5];
}
//read voltage of servo
int read_voltage(int comport, int ID)
{
byte buf[10] = {0};
byte chksum = 0XFF - ID - 0X31;
byte inst[8] = {0XFF, 0XFF, ID, 0X04, 0X02, 0X2A, 0X01, chksum};
send_inst(comport, inst, 8);
usleep(ret_delay);
PollComport(comport, buf, 10);
return (buf[5]/10);
}
int main()
{
int i, n,cport_nr=17, /* /dev/ttyS0 (COM1 on windows) */bdrate=9600; /* 9600 baud */
unsigned char buf[4096];
char temp ;
// printf("I am here on 31\n");
if(OpenComport(cport_nr, bdrate))
{
printf("Can not open comport\n");
return(0);
}
char str[13];
int count = 0 ;
int j;
while(1)
{
n = PollComport(cport_nr, buf, 1);
if(n > 0)
{
str[count ++ ] = buf[0] ;
if(str[count-1] == '#')
{
for(j = 0 ; str[j] != '#'; j++)
printf("%c",str[j]);
printf("\n");
count = 0;
}
buf[n] = 0;
for(i=0; i < n; i++)
{
if(buf[i] < 32) /* replace unreadable control-codes by dots */
{
buf[i] = '.';
}
}
// printf("received %i bytes: %c , %d \n", n, buf[0],strlen((char *)buf));
}
}
return(0);
}
//read the speed of the servo
double read_speed(int comport, int ID)
{
byte buf[10] = {0};
byte chksum = 0XFF - ID - 0X2A;
byte inst[8] = {0XFF, 0XFF, ID, 0X04, 0X02, 0X26, 0X02, chksum};
send_inst(comport, inst, 8);
usleep(ret_delay);
PollComport(comport, buf, 10);
double speed = ((double)buf[6] * 256 + (double)buf[5]) * 114 / 0X3FF;
return speed;
}
int CiCreateDef::ConnectToPort() {
int pom = OpenComport();
if (pom == 0) {
// precitame co tam caka
unsigned char *dataR;
dataR = (unsigned char*) malloc(500);
PollComport(dataR, 500);
}
return pom;
}
QString SerialPort::read() {
unsigned char *buffer = (unsigned char*)malloc(sizeof(unsigned char) * 4096);
int charCount = PollComport(portNumber, buffer, 4096);
QString msg("Empty port buffer!");
if(charCount != 0) {
msg = ((const char *)buffer);
msg.chop(msg.size() - charCount);
}
free(buffer);
return msg;
}
// Non-blocking read of a byte. If the buffer is empty, retunrs false.
bool CCPCBooster::ReadByte(unsigned char &val)
{
#if _WINDOWS
unsigned long nbBytesReceived = 0;
BOOL fSuccess = ReadFile (_COMPortHandle, &val, 1, &nbBytesReceived, NULL);
return nbBytesReceived == 1 ;
#else
return PollComport(_COMPortHandle, &val, 1) == 1;
#endif
}
//read the position of the servo
double read_angle(int comport, int ID)
{
byte buf[20] = {0};
byte chksum = 0XFF - ID - 0X2C;
byte inst[8] = {0XFF, 0XFF, ID, 0X04, 0X02, 0X24, 0X02, chksum};
send_inst(comport, inst, 8);
usleep(ret_delay);
PollComport(comport, buf, 20);
int i = 0;
while (buf[i]!=0XFF && buf[i + 1]!=0XFF)
i++;
double ang = ((double)buf[i + 6] * 256 + (double)buf[i + 5]) * 360 / 0X3FF;
return ang;
}
// Read a byte. If the buffer is empty, wait for a byte to come
bool CCPCBooster::ReadWaitByte(unsigned char &val)
{
#if _WINDOWS
unsigned long nbBytesReceived = 0;
BOOL fSuccess = TRUE;
while ( nbBytesReceived != 1 && fSuccess == TRUE)
{
fSuccess = ReadFile (_COMPortHandle, &val, 1, &nbBytesReceived, NULL);
}
return ((nbBytesReceived == 1) && fSuccess);
#else
int ret;
do {
ret = PollComport(_COMPortHandle, &val, 1);
} while (ret != 1);
return true ;
#endif
}
void *pollForData(void *)
{
unsigned char incomingBuffer[4096];
int incomingBufferSize = 0;
uint8_t prevCollection = 0;
while (1)
{
multiplatformSleep(2); // used only for testing. I want manageable loops, not crazy ones.
//从10ms缩短到2ms，可以提高通信速率_wh
incomingBufferSize = PollComport(comPort, incomingBuffer, 4095);
if (incomingBufferSize > 0)
{
pthread_mutex_lock(&safety);
memcpy(buffer + bufferSize, incomingBuffer, incomingBufferSize);
bufferSize = bufferSize + incomingBufferSize;
buffer[bufferSize] = 0;
pthread_mutex_unlock(&safety);
prevCollection = 1;
}
else
{
prevCollection = 0;
}
if ((sentEvent == 0) && (((prevCollection == 0) && (bufferSize > 0)) || (bufferSize > 1024)))
{
sentEvent = 1;
FREDispatchStatusEventAsync(dllContext, (uint8_t*) "bufferHasData", (const uint8_t*) "INFO");
}
else
if (sentEvent == 1)
{
multiplatformSleep(200);//从500ms减到200ms_wh
sentEvent = 0;
}
}
return NULL;
}
int main()
{
int i, n, cport_nr=0, bdrate=9600;
unsigned char buf[4096];
if(OpenComport(cport_nr, bdrate))
{
printf("Cannot open connection\n");
return(0);
}
while(1)
{
n = PollComport(cport_nr, buf, 4095);
if(n > 0)
{
buf[n] = 0; //Always put a "null" at the end of a string
for(i=0; i < n; i++)
{
if(buf[i] < 32)
{
buf[i] = '.';
}
}
printf("received %i bytes: %s\n", n, (char *)buf);
}
usleep(100000);
}
return(0);
}
static gboolean time_handler(GtkWidget *widget)
{
//if (widget->window == NULL) return FALSE; //prevents the timer from running if the window has been closed. //removed because it causes compile error
SendByte(com_no, read_v);
PollComport(com_no, serbuf, SER_IN_BUFLEN);
raw_v = (gdouble)serbuf[0];
v_out = raw_v/10;
i_out = v_out/load;
adc = (v_out*0.1760299*1023)/3.3;
sprintf(buff, "%3.0f", adc);
gtk_label_set_text(GTK_LABEL(label5), buff);
sprintf(buff, "%3.2f V", v_out);
gtk_label_set_text(GTK_LABEL(label6), buff);
sprintf(buff, "%3.3f A", i_out);
gtk_label_set_text(GTK_LABEL(label7), buff);
return TRUE; //alows the timer to run again
}
void kalmanFilterResults(int trash) {
char line[128];
float token[5];
int send_back_status;
int data_correct;
char num_byte_char;
int num_byte_int;
int j, k, l;
int i, n,
cport_nr=16, // /dev/ttyS0 (COM1 on windows)
bdrate=57600; // 9600 baud
unsigned char buf[4096];
char in_size;
TRANSMIT_PACKAGE tp;
data_correct = XBEE_FAIL;
//send_back_status = SendByte(cport_nr, 'a');
n = PollComport(cport_nr, buf, 5);
//Check for start sequence
if(n == 5)
{
buf[n] = 0; /* always put a "null" at the end of a string! */
if (strcmp(buf, start_sequence) == 0)
{
printf("Start sequence obtained\n");
data_correct = XBEE_SUCCESS;
}
else
{
printf("Wating for start sequence, start sequence doesn't match\n");
data_correct = XBEE_FAIL;
}
printf("received start sequence %i bytes: %s\n", n, (char *)buf);
}
else
{
data_correct= XBEE_FAIL;
printf("Hello\n");
}
if(data_correct == XBEE_SUCCESS)
{
n = PollComport(cport_nr, &in_size, 1);
//printf("Reading %d bytes\n", (int)in_size);
n = PollComport(cport_nr, buf, (int)in_size);
if (n == (int)in_size){
memcpy((void*)&tp, buf,n);
printf("IMU ID: %d\n", tp.id);
printf("IMU Qs: q1 = %f,q2 = %f, q3 = %f, q4 = %f\n", tp.q.q0, tp.q.q1, tp.q.q2, tp.q.q3);
printf("received %d bytes: %s\n", n, buf);
if (tp.id == 0) { //shoulder
//arm[0]=quaternionToPitch(tp.q);
//arm[1]=quaternionToYaw(tp.q);
} else if (tp.id == 1) { //elbow
//arm[2] = quaternionToPitch(tp.q);
//printf("HERE");
} else if (tp.id == 2) { //wrist
//arm[5] = 45//quaternionToPitch(tp.q); //updown
//arm[4] = 45//quaternionToRoll(tp.q);
} else {
perror ("Error reading device id");
}
data_correct = XBEE_SUCCESS;
glutPostRedisplay();
}
else{
printf("read %d bytes, not enough bytes read\n", n);
data_correct = XBEE_FAIL;
}
}
glutTimerFunc(1,kalmanFilterResults,0);
}
/*
* main program
*/
int main(int argc, char** argv) {
//global var
time_t sec0, sec1;
unsigned int delayPixel= 120000, delayGyrodataSending = 25000;
unsigned int idPayload = 303, useKeyboardCmd = 0, buffer_max_num = 64;
unsigned char keyChar;
int gpioOK = 0;
dictionary *iniDict; //ini dictionery
char strPathCfg[255];
GPS_DATA last_posx;
//init last gps pos
last_posx.gps_status = last_posx.gps_loc.lat = last_posx.gps_loc.lon = 0;
last_posx.gps_speed = last_posx.gps_altitude = last_posx.gps_time = 0;
//show titel
#if VERBOSE==1
printf("%s", APP_TITEL);
#endif
//init pin switch
gpioOK = wiringPiSetup();
if(gpioOK != -1)
{
pinMode(6, INPUT);
pullUpDnControl(6, PUD_UP);
}
else
printf("gpio init eror\r\n");
//parsing argumen jika ada
if(argc>2)
{
//default cfg file
getcwd(strPathCfg, sizeof(strPathCfg));
strcat(strPathCfg, "/");
strcat(strPathCfg, CFG_NAME);
int i;
for(i=1;i<(argc-1);i++)
{
//delay pixel
if(strcmp("-d",argv[i])==0)
delayPixel = atoi(argv[++i]);
//id payload
if(strcmp("-i",argv[i])==0)
idPayload = atoi(argv[++i]);
//pake keyboard ato tidak
if(strcmp("-k",argv[i])==0)
useKeyboardCmd = atoi(argv[++i]);
//gyro send
if(strcmp("-g",argv[i])==0)
delayGyrodataSending = atoi(argv[++i]);
//buffer size
if(strcmp("-b",argv[i])==0)
buffer_max_num = atoi(argv[++i]);
}
}
else //baca dari cfg file
{
//load ini file
//char strCfg[80];
if(argc==2)
{
strcpy(strPathCfg, argv[1]);
}
else
{
//get cwd
if (getcwd(strPathCfg, sizeof(strPathCfg)) != NULL)
printf("Current working dir: %s\r\n", strPathCfg);
else
{
strcpy(strPathCfg, argv[1]);
}
}
//check file exists
strcat(strPathCfg, "/");
strcat(strPathCfg, CFG_NAME);
if(!file_exist(strPathCfg))
{
printf("Configuration file %s not found!!!\r\n", strPathCfg );
return(EXIT_FAILURE);
}
//load from config file
printf("Loading configuration file from %s\r\n", strPathCfg);
//load ini parser
iniDict = iniparser_load(strPathCfg);
//read cfg value
if(iniDict)
{
idPayload = iniparser_getint(iniDict,"payload:id",100);
delayPixel = iniparser_getint(iniDict,"payload:cam_delay",120000);
delayGyrodataSending = iniparser_getint(iniDict,"payload:g_delay",20000);
buffer_max_num = iniparser_getint(iniDict,"payload:buffer",64);
}
}
//show config setup
printf("======================================\r\n");
printf("Configuration :\r\n");
printf("Delay Pixel = %d uS\r\n", delayPixel);
printf("ID Payload = %d\r\n", idPayload);
printf("Use Keyboard = %d\r\n", useKeyboardCmd);
printf("Gyro Delay = %d uS\r\n", delayGyrodataSending);
printf("Buffer = %d byte\r\n", buffer_max_num);
printf("======================================\r\n");
//init bus i2c
int i2c_hdl = I2C_Init_Bus(i2c_dev, &statusPeripheral);
//init start adxl345
ADXL345_Init_Start(i2c_hdl, adxl345_addr, &statusPeripheral);
//init itg3200
ITG3200_Init_Start(i2c_hdl, itg3200_addr, &statusPeripheral);
//open port
unsigned char buffRX[COM_BUFF_NUM];
int buffNumRX;
//make it as null string
buffRX[COM_BUFF_NUM-1] = 0;
if (OpenComport(COM_PORT, COM_SPEED)) {
fprintf(stderr, "Open port %d failed : %s\r\n!!!", COM_PORT, strerror(errno));
return errno;
}
#if VERBOSE==1
printf("communication port opened\r\n");
#endif
//tes port com
//while(1){
////ambil perintah dari comport
//buffNumRX = PollComport(COM_PORT, buffRX, (COM_BUFF_NUM - 2));
//if (buffNumRX > 0) {
////printf("%d = %s\n", buffNumRX, buffRX);
//int i;
//for(i=0;i<buffNumRX;i++)
//{
//printf("%X %s", buffRX[i], (i<buffNumRX-1) ? "":"\n");
//}
//char ss[10];
//snprintf(ss,6,"%d",(buffRX[1]<<8)+buffRX[2]);
//printf("id = %s\n", ss);
//}
//}
//open cam
CvCapture *camHdl = cvCaptureFromCAM(CV_CAP_ANY);
Cam_Init_Start(camHdl, &statusPeripheral);
//main loop
while (1) {
if(useKeyboardCmd)
{
//dummy keyboard input
printf("press command key\r\n");
keyChar = getchar();
//if (keyChar == cmdList[3][0])
//opsState = STATE_EXIT;
//if (keyChar == cmdList[2][0])
//opsState = STATE_GET_CAM_DATA;
//if (keyChar == cmdList[1][0])
//opsState = STATE_GET_G_DATA;
if (keyChar == cmdList[3])
opsState = STATE_EXIT;
if (keyChar == cmdList[2])
opsState = STATE_GET_CAM_DATA;
if (keyChar == cmdList[1])
opsState = STATE_GET_G_DATA;
}
//ambil perintah dari comport
buffNumRX = PollComport(COM_PORT, buffRX, (COM_BUFF_NUM - 2));
//data diterima -> cek printah
if (buffNumRX > 0) {
//int i;
//unsigned char *chrPos;
////make it null string
//if (buffRX[buffNumRX] != 0)
//buffRX[buffNumRX] = 0;
////default ops state = idle
//opsState = STATE_IDLE;
////add new state
//for (i = 0; i < CMD_COUNTER; i++) {
//chrPos = strstr(buffRX,cmdList[i]);
//if ((chrPos != NULL) && ((chrPos - buffRX + 1) == 1)) { //got a match?
////proses state
//opsState = i;
//break;
//}
//}
opsState = CheckCommand(cmdList, buffNumRX, buffRX);
}
//cek tobmol
if(gpioOK != -1)
{
//printf("%d\r\n", digitalRead(6));
//SendByte(COM_PORT, digitalRead(6)+0x30);
//usleep(1e5);
if(digitalRead(6) == 0)
{
usleep(5e5);
if(digitalRead(6) == 0)
opsState = STATE_SHUTDOWN_OS;
}
}
//lakukan proses seuai ops state
if (opsState == STATE_EXIT) {
#if VERBOSE==1
printf("exiting...\r\n");
//fflush(stdout);
#endif
break;
}
//shutdown os
if (opsState == STATE_SHUTDOWN_OS)
{
printf("shutdown...\r\n");
break;
}
//restart os
if (opsState == STATE_RESTART_OS)
{
printf("restart...\r\n");
break;
}
//other state
switch (opsState) {
case STATE_IDLE: //idle state
//SendByte(COM_PORT, 's');
break;
case STATE_GET_G_DATA: //ambil data g
//SendByte(COM_PORT, 'g');
GetAndSendAccGyro(i2c_hdl, adxl345_addr, itg3200_addr, idPayload, delayGyrodataSending, &last_posx, buffer_max_num);
break;
case STATE_GET_CAM_DATA://ambil data cam
//SendByte(COM_PORT, 'c');
#if VERBOSE==1
printf("grab camera start\r\n");
sec0 = time(NULL);
#endif
GrabAndSendCameraData(camHdl, i2c_hdl, 5, delayPixel, idPayload, &last_posx, buffer_max_num);
#if VERBOSE==1
sec1 = time(NULL);
printf("grab camera finish in %lds\r\n", (sec1 - sec0));
#endif
opsState = STATE_IDLE;
//usleep(5e4);
//ComportFlush(COM_PORT);
//usleep(1.5e6);
break;
case STATE_GET_STATUS: //ambil status payload
snprintf((char*)buffRX, 16, "s,%.3d,%d,%d\r", idPayload, 990, statusPeripheral);
//SendBuf(COM_PORT, buffRX, 10);
//snprintf((char*)buffRX, 16, "s,%.3d,%d\r", idPayload, 990);
cprintf(COM_PORT, buffRX);
opsState = STATE_IDLE; //go back to idle
#if VERBOSE==1
printf("sendstatus reply = %s\r\n", buffRX);
#endif
break;
case STATE_REINIT_PERIPHERAL: //reinit peripheral
//init cam
Cam_Init_Start(camHdl, &statusPeripheral);
//init bus i2c
i2c_hdl = I2C_Init_Bus(i2c_dev, &statusPeripheral);
//init start adxl345
ADXL345_Init_Start(i2c_hdl, adxl345_addr, &statusPeripheral);
//init itg3200
ITG3200_Init_Start(i2c_hdl, itg3200_addr, &statusPeripheral);
opsState = STATE_IDLE; //go back to idle
break;
case STATE_READ_WRITE_CONFIGURATION: //read/write config file
printf("Opening configuration file %s\r\n", strPathCfg);
//load ini file
iniDict = iniparser_load(strPathCfg);
//write if neccessary
if(buffRX[1]==1) //write setting
{
char stmp[10];
//id
snprintf(stmp,10,"%d", (buffRX[2]<<8)+buffRX[3]);
iniparser_set(iniDict,"payload:id",stmp);
//cam delay in ms
snprintf(stmp,10,"%d", buffRX[4]*1000);
iniparser_set(iniDict,"payload:cam_delay",stmp);
//g delay in ms
snprintf(stmp,10,"%d", buffRX[5]*1000);
iniparser_set(iniDict,"payload:g_delay",stmp);
//buffer max
snprintf(stmp,10,"%d", buffRX[6]);
iniparser_set(iniDict,"payload:buffer",stmp);
Save_INI_File(iniDict, strPathCfg);
}
//reload ini file
iniDict = iniparser_load(strPathCfg);
//read cfg value
if(iniDict)
{
idPayload = iniparser_getint(iniDict,"payload:id",100);
delayPixel = iniparser_getint(iniDict,"payload:cam_delay",120000);
delayGyrodataSending = iniparser_getint(iniDict,"payload:g_delay",20000);
buffer_max_num = iniparser_getint(iniDict,"payload:buffer",64);
}
//send reply always
snprintf((char*)buffRX, 25, "f,%d,%d,%d,%d\r", idPayload, delayPixel, delayGyrodataSending,buffer_max_num);
cprintf(COM_PORT, buffRX);
opsState = STATE_IDLE; //go back to idle
#if VERBOSE==1
printf("send config reply = %s\r\n", buffRX);
#endif
break;
default:
break;
}
}
//release cam
cvReleaseCapture(&camHdl);
//release port
CloseComport(COM_PORT);
//close bus
closeBus(i2c_hdl);
//free ini handler
if(iniDict)
iniparser_freedict(iniDict);
//cek shutdown atau restart
switch (opsState)
{
case STATE_SHUTDOWN_OS:
system("sudo shutdown now");
break;
case STATE_RESTART_OS:
system("sudo shutdown -r now");
break;
default:
break;
}
//return
return (EXIT_SUCCESS);
}
int main(int argc, char *argv[])
{
MYSQL* con ;
MYSQL_RES * res ;
MYSQL_ROW row ;
con = mysql_init(NULL);
char *server = "localhost";
char *user = "******";
char *password = "******"; /* set me first */
char *database = "zandu";
int i, n,cport_nr=16, /* /dev/ttyS0 (COM1 on windows) */bdrate=9600; /* 9600 baud */
unsigned char buf[4096];
char sensorValueQuery[200], registerQuery[200];
char temp ;
char sensorValues[100];
int count = 0 ;
int j;
int gotNewBot = 0, regNewBot = 0, botId = -1;
if(argc == 2){
cport_nr = atoi(argv[1]);
}
printf("I am here on 31\n");
if(OpenComport(cport_nr, bdrate))
{
printf("Can not open comport\n");
return(0);
}
// initiate the mysql connection
if (!mysql_real_connect(con, server,user, password, database, 0, NULL, 0))
{
fprintf(stderr, "%s\n", mysql_error(con));
exit(1);
}
/*queries for different works */
/**/
while(1)
{
n = PollComport(cport_nr, buf, 1);
if(n > 0)
{
/* Registration logic */
if(gotNewBot && n == 1){
botId = (int) buf[0];
regNewBot = 1;
}
else if(n == 2){
if(buf[0] == '@'){
botId = (int) buf[1];
regNewBot = 1;
}
}
else if(n == 1){
if(buf[0] == '@'){
gotNewBot = 1;
continue;
}
}
if(regNewBot){
sprintf(registerQuery,"insert into bot values (\'%d\',CURRENT_TIMESTAMP,\'%d\')",botId,0);
if (mysql_query(con,registerQuery))
{
fprintf(stderr, "%s\n", mysql_error(con));
gotNewBot = 0;regNewBot = 0;
continue;
//exit(1);
}
//mysql_free_result(res);
sprintf(registerQuery,"insert into sendtobot values (\'%d\',\'%s\',CURRENT_TIMESTAMP,\'%c\')",botId,"server",'Y');
if (mysql_query(con,registerQuery))
{
fprintf(stderr, "%s\n", mysql_error(con));
//exit(1);
}
sprintf(registerQuery,"insert into botinfo values (\'%d\',\'%d\',\'%s\',\'%s\',\'%s\',\'%s\',\'%s\',\'%s\',\'%s\',\'%s\',\'%s\')",botId,1101, "0:0:0:0:0:0:0:0:0", "0:0:0:0:0:0:0:0:0:0:0:0", "0","0", "0","10", "10", "0", "0");
if (mysql_query(con,registerQuery))
{
fprintf(stderr, "%s\n", mysql_error(con));
//exit(1);
}
gotNewBot = 0;regNewBot = 0;
continue;
}
/* Registration logic ends*/
/*sensor wala logic*/
sensorValues[count ++ ] = buf[0] ;
if(sensorValues[count-1] == '#')
{
for(j = 0 ; sensorValues[j] != '#'; j++)
{
printf("%c",sensorValues[j]);
}
sensorValues[j] = '\0'; //else garbage values can go into a database
if(sensorValues[2] == 'S'){
sprintf(sensorValueQuery,"update botinfo set sensor=\'%s\' where bot_id = \'%d\' ;",sensorValues+4,sensorValues[0]);
//puts(sensorValues);
}
else if(sensorValues[2] == 'P'){
sprintf(sensorValueQuery,"update botinfo set port_io=\'%s\' where bot_id = \'%d\' ;",sensorValues+4,sensorValues[0]);
}
// 0 replaced by 2||sensorValues + 2 repalce by sensorValues + 4 || botID replaced by sensorValues[0] || %d replaced by %c
if (mysql_query(con,sensorValueQuery))
{
printf("%s\n",sensorValueQuery);
fprintf(stderr, "%s\n", mysql_error(con));
//exit(1);
}
mysql_free_result(res);
printf("\n");
count = 0;
}
buf[n] = 0;
for(i=0; i < n; i++)
{
if(buf[i] < 32) /* replace unreadable control-codes by dots */
{
buf[i] = '.';
}
}
}
}
return(0);
}
int main(int argc, char *argv[])
{
an_decoder_t an_decoder;
an_packet_t *an_packet;
system_state_packet_t system_state_packet;
raw_sensors_packet_t raw_sensors_packet;
int bytes_received;
if (argc != 3)
{
printf("Usage - program com_port baud_rate\nExample - packet_example.exe COM1 115200\n");
exit(EXIT_FAILURE);
}
/* open the com port */
if (OpenComport(argv[1], atoi(argv[2])))
{
exit(EXIT_FAILURE);
}
an_decoder_initialise(&an_decoder);
while (1)
{
if ((bytes_received = PollComport(an_decoder_pointer(&an_decoder), an_decoder_size(&an_decoder))) > 0)
{
/* increment the decode buffer length by the number of bytes received */
an_decoder_increment(&an_decoder, bytes_received);
/* decode all the packets in the buffer */
while ((an_packet = an_packet_decode(&an_decoder)) != NULL)
{
if (an_packet->id == packet_id_system_state) /* system state packet */
{
/* copy all the binary data into the typedef struct for the packet */
/* this allows easy access to all the different values */
if(decode_system_state_packet(&system_state_packet, an_packet) == 0)
{
printf("System State Packet:\n");
printf("\tLatitude = %f, Longitude = %f, Height = %f\n", system_state_packet.latitude * RADIANS_TO_DEGREES, system_state_packet.longitude * RADIANS_TO_DEGREES, system_state_packet.height);
printf("\tRoll = %f, Pitch = %f, Heading = %f\n", system_state_packet.orientation[0] * RADIANS_TO_DEGREES, system_state_packet.orientation[1] * RADIANS_TO_DEGREES, system_state_packet.orientation[2] * RADIANS_TO_DEGREES);
}
}
else if (an_packet->id == packet_id_raw_sensors) /* raw sensors packet */
{
/* copy all the binary data into the typedef struct for the packet */
/* this allows easy access to all the different values */
if(decode_raw_sensors_packet(&raw_sensors_packet, an_packet) == 0)
{
printf("Raw Sensors Packet:\n");
printf("\tAccelerometers X: %f Y: %f Z: %f\n", raw_sensors_packet.accelerometers[0], raw_sensors_packet.accelerometers[1], raw_sensors_packet.accelerometers[2]);
printf("\tGyroscopes X: %f Y: %f Z: %f\n", raw_sensors_packet.gyroscopes[0] * RADIANS_TO_DEGREES, raw_sensors_packet.gyroscopes[1] * RADIANS_TO_DEGREES, raw_sensors_packet.gyroscopes[2] * RADIANS_TO_DEGREES);
}
}
else
{
printf("Packet ID %u of Length %u\n", an_packet->id, an_packet->length);
}
/* Ensure that you free the an_packet when your done with it or you will leak memory */
an_packet_free(&an_packet);
}
}
#ifdef _WIN32
Sleep(10);
#else
usleep(10000);
#endif
}
}
int main( int argc, char *argv[] )
{
uint8_t serial_buffer[BUFFER_SIZE];
uint8_t packet_buffer[BUFFER_SIZE];
uint8_t final_buffer[BUFFER_SIZE];
//uint8_t adc_sample_buffer[ADC_MAX_SAMPLES];
uint16_t buffer_offset = 0;
uint8_t new_packet = 0;
uint16_t bytes_read = 0;
// Handle interrupt events to make sure files are closed before exiting
(void) signal( SIGINT, sigint_handler );
tx_power = dbm_to_watt(1l);
target_rssi = dbm_to_watt(-60l);
// Make sure input is correct
if( argc < 2 )
{
printf("Usage: %s port baudrate\n", argv[0]);
return 0;
}
printf("Power control test\r\n");
// Convert string serial number to integer
serial_port_number = atoi( argv[1] );
// Use RS232 library to open serial port
if ( OpenComport( serial_port_number, atoi(argv[2]) ) )
{
printf("Error opening serial port.\n");
return 1;
}
// Flush the port
while( PollComport( serial_port_number, serial_buffer, BUFFER_SIZE ) );
memset( serial_buffer, 0x00, sizeof(serial_buffer) );
// Run forever
for(;;)
{
// Read a buffer full of data (if available)
bytes_read = PollComport( serial_port_number, serial_buffer, BUFFER_SIZE );
if( bytes_read > 0 )
{
if( bytes_read >= BUFFER_SIZE )
{
printf("Uh oh!\n");
}
else
{
memcpy( packet_buffer + buffer_offset, serial_buffer, bytes_read );
if ( packet_in_buffer( packet_buffer ) )
{
buffer_offset = 0;
new_packet = 1;
bytes_read = find_and_escape_packet( packet_buffer, final_buffer );
}
else
{
buffer_offset += bytes_read;
if( buffer_offset > BUFFER_SIZE )
{
printf("damn...\n");
memset( packet_buffer, 0x00, sizeof(packet_buffer) );
memset( serial_buffer, 0x00, sizeof(serial_buffer) );
buffer_offset = 0;
}
}
if( new_packet == 1 )
{
process_packet( final_buffer );
new_packet = 0;
memset( packet_buffer, 0x00, sizeof(packet_buffer) );
}
}
}
memset( serial_buffer, 0x00, sizeof(serial_buffer) );
// Don't take up all the processor time
usleep(20000);
if( 0 == send_message-- )
{
SendByte( serial_port_number, next_power ); // Send initial packet
send_message = 1;
if(next_power == 0xff)
{
printf("*");
}
// If no message is received before the next round, use full power
next_power = 0xff;
}
}
return 0;
}
int main(int argc, char *argv[])
{
unsigned char buffer[4096] = {0};
int bufferLen = 0;
int bytesReceived = 0;
int bytesWritten = 0;
int timer = 0;
unsigned int deviceId[3];
Command command;
int extra = -1;
if (argc < 3)
{
print_error("Usage: insteon deviceId [on|off|status|temp_ambient]\n");
return 1;
}
// Parse arguments
parse_device(argv[1], deviceId);
command = parse_command(argv[2]);
if (argc > 3)
extra = parse_extra(argv[3]);
if (command < 0)
{
print_error("Invalid command name!\n");
return 1;
}
print_debug("*****************\n");
print_debug("Device: %s\n", argv[1]);
print_debug("Command: %s\n", argv[2]);
if (argc > 3)
print_debug("Extra: %s\n", argv[3]);
print_debug("*****************\n");
if (acquire_lock())
{
print_error("Timeout while acquiring lock!\n");
return 1;
}
print_debug("Opening comport\n");
if(OpenComport(comport, 19200))
{
print_error("Can not open comport\n");
release_lock();
return 1;
}
// Send command
bytesWritten = send_command(deviceId, command, extra);
if (bytesWritten < 0)
{
print_error("Invalid command or internal error!\n");
goto return_error;
}
// Wait for response
while(1)
{
bytesReceived = PollComport(16, buffer + bufferLen, 4095);
//print_debug("br=%d bl=%d ", bytesReceived, bufferLen);
bufferLen += bytesReceived;
//print_debug("br=%d bl=%d\n", bytesReceived, bufferLen);
if(bytesReceived > 0)
{
int i;
timer = 0;
print_debug("received %i bytes: (%i total)", bytesReceived, bufferLen);
for(i=0; i < bufferLen; i++)
{
print_debug("%02X ", buffer[i]);
}
print_debug("\n");
if (bufferLen > bytesWritten)
{
unsigned int ack = *(buffer + bytesWritten);
unsigned char* message = buffer + bytesWritten + 1;
int messageLen = bufferLen - bytesWritten - 1;
if (ack != CMD_ACK)
{
print_error("Message not acknowledged!\n");
goto return_error;
}
if (messageLen > 1)
{
if (message[0] == CMD_PREFIX)
{
if (message[1] == CMD_INSTEON_STD_RECEIVED)
{
if (messageLen >= 11)
{
static int resultCounter = 0;
print_debug("Insteon standard message received!\n");
if(parse_message(command, message))
{
goto return_error;
}
if(command == CMD_TEMP_GET_SETPOINT && resultCounter == 0 && extra == 3)
{
resultCounter++;
bufferLen -= 11;
}
else
{
goto return_success;
}
}
}
else if (message[1] == CMD_INSTEON_EXT_RECEIVED)
{
if (messageLen >= 25)
{
print_debug("Insteon extended message received!\n");
if(parse_message(command, message))
{
goto return_error;
}
goto return_success;
}
}
else if (message[1] == CMD_X10_RECEIVED)
{
if (messageLen >= 4)
{
print_debug("X10 message received!\n");
if(parse_message(command, message))
{
goto return_error;
}
goto return_success;
}
}
else
{
print_error("Unexpected message received! '");
for(i=0; i < messageLen; i++)
{
print_error("%02X ", message[i]);
}
print_error("'\n");
goto return_error;
}
}
else
{
print_error("Unexpected message header received! '");
for(i=0; i < messageLen; i++)
{
print_error("%02X ", message[i]);
}
print_error("'\n");
goto return_error;
}
}
}
}
usleep(250);
timer++;
if (timer > 4000)
{
print_error("Timeout while waiting for bytes!\n");
goto return_error;
}
}
return_success:
CloseComport(16);
release_lock();
return 0;
return_error:
CloseComport(16);
release_lock();
return 1;
}
static ssize_t
readFromSerial(char *buff, size_t bytcnt) {
return PollComport(SERIAL_PORT, buff, bytcnt);
}
/*******************************************************************************
* @fn void *serial_read_thread()
* @brief Run as thread. Polls serial port and builds packets. Calls
* serial_callback function with each new packet
* ****************************************************************************/
void *serial_read_thread()
{
uint8_t serial_buffer[BUFFER_SIZE];
uint8_t packet_buffer[BUFFER_SIZE];
uint8_t final_buffer[BUFFER_SIZE];
uint16_t buffer_offset = 0;
uint8_t new_packet = 0;
uint16_t bytes_read = 0;
printf("Starting serial read.\r\n");
// Flush the port
while( PollComport( serial_port_number, serial_buffer, BUFFER_SIZE ) );
memset( serial_buffer, 0x00, sizeof(serial_buffer) );
// Run forever
for(;;)
{
// Read a buffer full of data (if available)
bytes_read = PollComport( serial_port_number, serial_buffer, BUFFER_SIZE );
if( bytes_read > 0 )
{
if( bytes_read >= BUFFER_SIZE )
{
printf("Read more bytes than buffer can handle.\n");
}
else
{
memcpy( packet_buffer + buffer_offset, serial_buffer, bytes_read );
if ( packet_in_buffer( packet_buffer ) )
{
buffer_offset = 0;
new_packet = 1;
bytes_read = find_and_escape_packet( packet_buffer, final_buffer );
}
else
{
buffer_offset += bytes_read;
if( buffer_offset > BUFFER_SIZE )
{
printf("Buffer has overflowed. Flushing.\n");
memset( packet_buffer, 0x00, sizeof(packet_buffer) );
memset( serial_buffer, 0x00, sizeof(serial_buffer) );
buffer_offset = 0;
}
}
if( new_packet == 1 )
{
// Call serial callback. If it returns 0, flush the buffer
if ( !serial_read_callback( final_buffer, (bytes_read-1) ) )
{
// Wait ~250ms
usleep(250000);
// Flush the port
while( PollComport( serial_port_number, serial_buffer,
BUFFER_SIZE ) );
memset( serial_buffer, 0x00, sizeof(serial_buffer) );
}
new_packet = 0;
memset( packet_buffer, 0x00, sizeof(packet_buffer) );
}
}
}
memset( serial_buffer, 0x00, sizeof(serial_buffer) );
// Don't take up all the processor time
usleep(2000);
}
}
