bool RunRadarDataList() {
const int c_PerComDataSize = 1024;
unsigned char com_data[1024];
int act_com_size;
bool ok_com;
char config_path[255];
int port_num;
int baud_rate;
GetConfigPath(config_path, sizeof(config_path));
GetPortNumOfSerialInterface(config_path, &port_num);
GetBaudRateOfSerialInterface(config_path, &baud_rate);
ok_com = OpenComport(port_num, baud_rate);
if (false == ok_com) {
LOG("OpenComport() error!\n");
return false;
}
else {
while (true) {
act_com_size = ReadComport(com_data, c_PerComDataSize);
if (0 == act_com_size)
continue;
else
AnalyseComData(com_data, act_com_size);
}
CloseComport();
return true;
}
}
FREObject closePort(FREContext ctx, void* funcData, uint32_t argc, FREObject argv[])
{
FREObject result;
CloseComport(comPort);
FRENewObjectFromBool(1, &result);
return result;
}
/*!
@brief Handle interrupt event (SIGINT) so program exits cleanly
*/
void sigint_handler( int32_t sig )
{
// Close the serial port
CloseComport( serial_port_number );
printf("\nExiting...\n");
exit(sig);
}
// Close communication (you can reopen it later)
void CCPCBooster::ClosePort()
{
if (_currentState == PortOpened)
{
#if _WINDOWS
CloseHandle(_COMPortHandle);
#else
CloseComport(_COMPortHandle);
#endif
}
_currentState = PortClosed;
}
void contextFinalizer(FREContext ctx)
{
pthread_cancel(ptrToThread);
CloseComport(comPort);
return;
}
/*
* 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);
}
/*******************************************************************************
* @fn void serial_close()
* @brief Close serial port
* ****************************************************************************/
void serial_close()
{
CloseComport( serial_port_number );
}
int main(int argc,char** argv)
{
ros::init(argc, argv, "servo");
ros::param::get("/port", port);
if(port == "/dev/ttyS0")
Comport = 0;
else if(port == "/dev/ttyS1")
Comport = 1;
else if(port == "/dev/ttyS2")
Comport = 2;
else if(port == "/dev/ttyS3")
Comport = 3;
else if(port == "/dev/ttyS4")
Comport = 4;
else if(port == "/dev/ttyS5")
Comport = 5;
else if(port == "/dev/ttyS6")
Comport = 6;
else if(port == "/dev/ttyS7")
Comport = 7;
else if(port == "/dev/ttyS8")
Comport = 8;
else if(port == "/dev/ttyS9")
Comport = 9;
else if(port == "/dev/ttyS10")
Comport = 10;
else if(port == "/dev/ttyS11")
Comport = 11;
else if(port == "/dev/ttyS12")
Comport = 12;
else if(port == "/dev/ttyS13")
Comport = 13;
else if(port == "/dev/ttyS14")
Comport = 14;
else if(port == "/dev/ttyS15")
Comport = 15;
else if(port == "/dev/ttyUSB0")
Comport = 16;
else if(port == "/dev/ttyUSB1")
Comport = 17;
else if(port == "/dev/ttyUSB2")
Comport = 18;
else if(port == "/dev/ttyUSB3")
Comport = 19;
else if(port == "/dev/ttyUSB4")
Comport = 20;
else if(port == "/dev/ttyUSB5")
Comport = 21;
ros::param::get("/baudrate", baudrate);
ros::param::get("/servo_id", servo_id);
ros::param::get("/servo_speed", servo_speed);
ros::param::get("/max_angle", max_angle);
ros::param::get("/min_angle", min_angle);
OpenComport(Comport, baudrate);
servoAction servo(ros::this_node::getName());
if (!ros::ok())
{
CloseComport(Comport);
}
ros::spin();
return 0;
}
void
dinitSerial() {
CloseComport(SERIAL_PORT);
}
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;
}
void close_com(GtkWidget *window)
{
CloseComport(com_no);
}
