/**
* Opens a serial port with the default configuration
* (115200 bps, 8-N-1), using the device specified in the constructor
*/
int Transport::openComm(const char* device)
{
int tmp = OpenSerial(&(this->serial), device);
if (tmp < 0) {
return -1;
}
tmp = SetupSerial(this->serial);
if (tmp < 0) {
return -2;
}
return 0;
}
int SerialRoutine(int port_num){
SerialClose(port_num);
return(OpenSerial(port_num, 115200, 1, 8, eParityNone));
}
static int SerparOpen(const char *name, const char *arg)
{
char *sername = NULL;
char *parname = NULL;
#ifdef DEBUG
printf("SerparOpen: name %s arg %s\n", name, arg ? arg : "<NULL>");
#endif
#ifdef COMPILING_ON_WINDOWS
if (IsOpenSerial() || IsOpenParallel()) return -1;
#else
if (Unix_IsSerialInUse() || Unix_IsParallelInUse()) return -1;
#endif
#ifdef COMPILING_ON_WINDOWS
if (SerparMatch(name, arg) == -1)
return -1;
#else
Unix_IsValidParallelDevice(name,&sername,&parname);
# ifdef DEBUG
printf("translated %s to serial %s and parallel %s\n",
name==0 ? "NULL" : name,
sername==0 ? "NULL" : sername,
parname==0 ? "NULL" : parname);
# endif
if (sername==NULL || parname==NULL) return -1;
#endif
user_options_set = FALSE;
/* interpret and store the arguments */
if (arg != NULL)
{
unsigned int target_baud_rate;
target_baud_rate = (unsigned int)strtoul(arg, NULL, 10);
if (target_baud_rate > 0)
{
#ifdef DEBUG
printf("user selected baud rate %u\n", target_baud_rate);
#endif
process_baud_rate(target_baud_rate);
}
#ifdef DEBUG
else
printf("could not understand baud rate %s\n", arg);
#endif
}
#ifdef COMPILING_ON_WINDOWS
{
/*
* The serial port number is in name[0] followed by
* the parallel port number in name[1]
*/
int sport = name[0] - '0';
int pport = name[1] - '0';
if (OpenParallel(pport) != COM_OK)
return -1;
if (OpenSerial(sport, FALSE) != COM_OK)
{
CloseParallel();
return -1;
}
}
#else
Unix_OpenParallel(parname);
Unix_OpenSerial(sername);
#endif
serpar_reset();
#if defined(__unix) || defined(__CYGWIN32__)
Unix_ioctlNonBlocking();
#endif
Angel_RxEngineInit(&config, &rxstate);
return 0;
}
static int SerialOpen(const char *name, const char *arg)
{
const char *port_name = name;
#ifdef DEBUG
printf("SerialOpen: name %s arg %s\n", name, arg ? arg : "<NULL>");
#endif
#ifdef COMPILING_ON_WINDOWS
if (IsOpenSerial()) return -1;
#else
if (Unix_IsSerialInUse()) return -1;
#endif
#ifdef COMPILING_ON_WINDOWS
if (SerialMatch(name, arg) != adp_ok)
return adp_failed;
#else
port_name = Unix_MatchValidSerialDevice(port_name);
# ifdef DEBUG
printf("translated port to %s\n", port_name == 0 ? "NULL" : port_name);
# endif
if (port_name == 0) return adp_failed;
#endif
user_options_set = FALSE;
/* interpret and store the arguments */
if ( arg != NULL )
{
unsigned int target_baud_rate;
target_baud_rate = (unsigned int)strtoul(arg, NULL, 10);
if (target_baud_rate > 0)
{
#ifdef DEBUG
printf( "user selected baud rate %u\n", target_baud_rate );
#endif
process_baud_rate( target_baud_rate );
}
#ifdef DEBUG
else
printf( "could not understand baud rate %s\n", arg );
#endif
}
else if (baud_rate > 0)
{
/* If the user specified a baud rate on the command line "-b" or via
the "set remotebaud" command then try to use that one */
process_baud_rate( baud_rate );
}
#ifdef COMPILING_ON_WINDOWS
{
int port = IsValidDevice(name);
if (OpenSerial(port, FALSE) != COM_OK)
return -1;
}
#else
if (Unix_OpenSerial(port_name) < 0)
return -1;
#endif
serial_reset();
#if defined(__unix) || defined(__CYGWIN__)
Unix_ioctlNonBlocking();
#endif
Angel_RxEngineInit(&config, &rxstate);
/*
* DANGER!: passing in NULL as the packet is ok for now as it is just
* IGNOREd but this may well change
*/
Angel_TxEngineInit(&config, NULL, &wstate.txstate);
return 0;
}
int main(int argc, char **argv) {
int i;
pthread_t p_thread;
int thr_id;
int status;
int rc;
void *socket_fd;
struct sockaddr_in servaddr; //server addr
// wifi
struct termios tio, old_tio;
int ret;
unsigned char DataBuf[BUFFER_SIZE];
int ReadMsgSize;
unsigned char receiveBuffer[BUFFER_SIZE];
int receiveSize = 0;
unsigned char remainder[BUFFER_SIZE];
int parsingSize = 0;
char th_data[256];
memset( &pastData, 0, sizeof( MGData ) );
memset( &io, 0, sizeof( UINT8 )*8 );
signal( SIGINT, (void *)sig_handler);
fd = OpenSerial();
if( fd == -1 )
{
writeLog("/work/iot/log", "Uart Open Fail." );
return -1;
}
/******************** DB connect ***********************/
// DB Open
rc = _sqlite3_open( DBPATH, &pSQLite3 );
if( rc != 0 )
{
writeLog( "/work/iot/log", "error DB Open" );
return -1;
}
else
{
writeLog( "/work/iot/log", "DB Open" );
printf("%s OPEN!!\n", DBPATH);
}
//sqlite3_busy_timeout( pSQLite3, 1000);
//sqlite3_busy_timeout( pSQLite3, 5000);
// DB Customize
rc = _sqlite3_customize( &pSQLite3 );
if( rc != 0 )
{
writeLog( "/work/iot/log", "error DB Customize" );
return -1;
}
rc = _sqlite3_nolock( &pSQLite3 );
if( rc != 0 )
{
writeLog( "/work/iot/log", "error set nolock" );
return -1;
}
rc = init();
if( rc == -1 )
{
writeLog( "/work/iot/log", "error init()" );
return -1;
}
/***************** Server Connect **********************/
if( -1 == ( m2mid = msgget( (key_t)2222, IPC_CREAT | 0666)))
{
writeLog( "/work/iot/log", "error msgget() m2mid" );
return -1;
}
if( -1 == ( eventid = msgget( (key_t)3333, IPC_CREAT | 0666)))
{
writeLog( "/work/iot/log", "error msgget() eventid" );
return -1;
}
memcpy( th_data, (void *)&fd, sizeof(fd) );
if( pthread_create(&threads[2], NULL, &thread_main, (void *)th_data) == -1 )
{
writeLog("/work/iot/log", "[Uart] threads[2] thread_main error" );
printf("error thread\n");
}
if( pthread_create(&threads[1], NULL, &ConnectPorcess, NULL ) == -1 )
{
writeLog("/work/iot/log", "[TCP/IP] thread Create error" );
}
usleep(1000);
while(1)
{
// receive Bridge
ReadMsgSize = read(fd, DataBuf, BUFFER_SIZE);
if(ReadMsgSize > 0)
{
if( receiveSize >= BUFFER_SIZE )
continue;
/*
printf("STX ");
for( i = 0; i< ReadMsgSize; i++ )
printf("%02X ", DataBuf[i]);
printf(" ETX\n");
*/
memcpy( receiveBuffer+receiveSize, DataBuf, ReadMsgSize );
receiveSize += ReadMsgSize;
//printf("receiveSize = %d\n", receiveSize );
parsingSize = uartParsingReceiveValue(receiveBuffer, receiveSize, remainder, parsingSize);
//printf("parsingSize(%d) ", parsingSize);
memset( receiveBuffer, 0 , sizeof(BUFFER_SIZE) );
receiveSize = 0;
memcpy( receiveBuffer, remainder, parsingSize );
receiveSize = parsingSize;
parsingSize = 0; // add 20140508
memset( remainder, 0 , sizeof(BUFFER_SIZE) );
}
else
{
//usleep(100);
//printf("Serial timeout %d\n", ReadMsgSize);
}
}
/*******************************************************/
_sqlite3_close( &pSQLite3 );
writeLog( "/work/iot/log", "[IoTManager] END" );
return 0;
}
///////////////////////////////////MAIN FUNCTION//////////////////////////////////////////////
void UserMain( void* pd ){
/////Usual Routine
InitializeStack();
OSChangePrio( MAIN_PRIO );//Change Main Task number to MAIN_PRIO
EnableAutoUpdate();
EnableTaskMonitor();
EnableSmartTraps();
//serviceWatchDog();
//Starting the Code
iprintf("\n\n\n..................Starting Sensor Interface Board.....................\n\n\n");
//Local Variables
//////Scan Flag
/***********Defining Interrupt Timers*****************/
HiResTimer* timer2=0;//50 Hz Interrupt Timer
char m=0;
/***********File Descriptor Variables****************/
int startup_timeout=0;
unsigned char F_range_buff[4]={0};//Radio filtered range
/**********Radio Debug Variable*******************/
double TotalTime=0; char time_ms[2]={0};
/***********Radio control Radiocount and loop counter i********************/
uint8_t Radiocount3=0,Ant_config=0;
uint16_t CRME=0;
/*********Laser Rangefinder Variables***************/
float laser_range=0;
/**********ADC channel Array***************************/
uint16_t ADC_channel[8] = {0};
/**********Navcomp send buffer and other vriables**********************/
char Navcomp_send_buff[48]={0};
Navcomp_send_buff[0]=0x41;
Navcomp_send_buff[1]=0x7A;
Navcomp_send_buff[2]=0x05;
uint16_t netburner_counter=0;//netburner 16 bit counter
//Initialize pins
initPINS();
//Initialize Analog to Digital
InitSingleEndAD();
//Initializing Serial Ports
SerialClose(5);
SerialClose(7);
SerialClose(9);
SerialClose(8);
fdRadio= OpenSerial( 8, 115200, 1, 8, eParityNone );
fdDebug = OpenSerial( 5, 115200, 1, 8, eParityNone );
fdLaser = OpenSerial( 7, 115200, 1, 8, eParityNone );
fdNAVcomp = OpenSerial( 9, 115200, 1, 8, eParityNone );
/*
ReplaceStdio(0,fdDebug);
ReplaceStdio(1,fdDebug);
ReplaceStdio(2,fdDebug);
*/
//Start the Timers and init the DSPI
//DSPIInit(1,2000000,16,0x01,0x01,1,1,0,0,0);//initializing SPI
//printf("Going to wait 3 sec\n");
//OSTimeDly(3*TICKS_PER_SECOND);
initTIMERS(timer2);
J1[7]=0;
/*startup_timeout=ReadWithTimeout(fdDebug,&m,1,2);
if(startup_timeout==-1 || startup_timeout==0){
Start_PAN=StartUpLaserScan(fdLaser);
if(Start_PAN!=11110)
Scan_Status=1;
else
Scan_Status=0;
}
else{
Scan_Status=0;
Start_PAN=11110;
}*/
//printf("Hi\n");
Scan_Complete=1;
/***********packing startup PAN angle*************************/
Navcomp_send_buff[38] = (uint8_t)((Start_PAN & 0xFF00)>>8);
Navcomp_send_buff[37] = (uint8_t)(Start_PAN & 0x00FF);
Navcomp_send_buff[36] = Scan_Status;
OSSimpleTaskCreate(NAVcompData,MAIN_PRIO-1);
OSSimpleTaskCreate(RadioData,MAIN_PRIO-2);
//OSSimpleTaskCreate(MIScompData,MAIN_PRIO+2);
//enableWatchDog( 1, 0x001F );//0x001C
//Creating Data Receiving task from the computer
while(1){
//printf("Hi\n");
TotalTime=timer1->readTime();
//First if statement to command host radio to get ranging data from 101 guest with antenna A
if(FiveHzflag==1 && Radiocount3==0){
//printf("In WHile Loop\n");
CRME=(uint16_t)radio_in_buff1[28]*256+(uint16_t)radio_in_buff1[29];
if((unsigned char)radio_in_buff1[12]==0 && CRME<60){
F_range_buff[0]=radio_in_buff1[24];
F_range_buff[1]=radio_in_buff1[25];
F_range_buff[2]=radio_in_buff1[26];
F_range_buff[3]=radio_in_buff1[27];
F_range_buff[4]=radio_in_buff1[32];
F_range_buff[5]=radio_in_buff1[33];
F_range_buff[6]=radio_in_buff1[12];
Ant_config=radio_in_buff1[11];
}
Radiocount3=1;
FiveHzflag=0;
}//first if bracket
//second if statement to command host radio to get ranging data from 102 guest with antenna A
if(FiveHzflag==1 && Radiocount3==1){
CRME=(uint16_t)radio_in_buff2[28]*256+(uint16_t)radio_in_buff2[29];
if((unsigned char)radio_in_buff2[12]==0 && CRME<60){
F_range_buff[0]=radio_in_buff2[24];
F_range_buff[1]=radio_in_buff2[25];
F_range_buff[2]=radio_in_buff2[26];
F_range_buff[3]=radio_in_buff2[27];
F_range_buff[4]=radio_in_buff2[32];
F_range_buff[5]=radio_in_buff2[33];
Ant_config=radio_in_buff2[11];
}
Radiocount3=0;
FiveHzflag=0;
}//second if bracket
if(FiftyHzflag==1){
if(Scan_Complete==1){
//printf("Start Pan=%d,Start Pan=%d\n",Start_PAN,(int16_t)Navcomp_send_buff[38]*256+(int16_t)Navcomp_send_buff[37]);
laser_range=ReadLaser(fdLaser);
Navcomp_send_buff[36] = Scan_Status;
Navcomp_send_buff[38] = (uint8_t)((Start_PAN & 0xFF00)>>8);
Navcomp_send_buff[37] = (uint8_t)(Start_PAN & 0x00FF);
//printf("laser range=%g\n",laser_range);
//printf("laser range=%g\n",laser_range);
//uint32_t Range=(uint32_t)F_range_buff[0]*16777216+(uint32_t)F_range_buff[1]*65536+(uint32_t)F_range_buff[2]*256+(uint32_t)F_range_buff[3];
//printf("%zu,%u,%u,%u\n",Range,Ant_config,(unsigned char)radio_in_buff[12],(uint16_t)radio_in_buff[32]*256+(uint16_t)radio_in_buff[33]);
StartAD();
while (!ADDone()){}
asm("nop");
for (int i = 0; i < 8; i++)
ADC_channel[i] = (unsigned short int)(1000 * (((double)GetADResult(i)) * 3.3 / (32768.0)));
//printf("%d \n", ADC_channel[1]);
sprintf(time_ms,"%lf",TotalTime);
//send data to the computer
SendtoNAVCOMP(Navcomp_send_buff,ADC_channel,time_ms,netburner_counter,laser_range,F_range_buff,PanAngle,fdNAVcomp,fdDebug,sizeof(Navcomp_send_buff),Ant_config);
netburner_counter ++;
//printf("%g\n",dYaw);
//dYaw=0;
StartAD();
while (!ADDone()){}
asm("nop");
//dYaw=93;
uint16_t ServoPot = GetADResult(0);
////Servo PAN 1 numbers
Pulse=12287-dYaw*20.51;
//printf("%d\n",ServoPot);
if(Pulse<8594 || Pulse==8594)
sim1.mcpwm.sm[1].val[5]=8594;
if(Pulse>15980 || Pulse==15980)
sim1.mcpwm.sm[1].val[5]=15980;
else
sim1.mcpwm.sm[1].val[5]=Pulse;//PAN control
/*////Servo PAN 2 numbers
Pulse=12287-dYaw*20.14;
if(Pulse<8594 || Pulse==8310)
sim1.mcpwm.sm[1].val[5]=8310;
if(Pulse>15980 || Pulse==15560)
sim1.mcpwm.sm[1].val[5]=15560;
else
sim1.mcpwm.sm[1].val[5]=Pulse;//PAN control*/
double cYaw=(ServoPot-12885)/63;//PAN 1
//double cYaw=(ServoPot-14667)/63.05;//PAN 2
//Calibration PAN servo 1
//0-8594
//90-10440
//180-12287 Position in which PAN faces front
//270-14844
//360-15980
//Calibration pot PAN servo 1
//360=1564
//180=12885 //position in which PAN faces front
//270=7270
//90=18560
//0=24290
//Calibration PAN servo 2
//0-8310
//90-10440
//180-12287 Position in which PAN faces front
//270-13900
//360-15560
//Calibration pot PAN servo 2
//360=3264
//180=14667 //position in which PAN faces front
//270=8999
//90=20085
//0=25971
pwmr_comp=sim1.mcpwm.mcr;
sim1.mcpwm.mcr |=LDOK;
PanAngle = cYaw * 10;
//printf("%g\n",cYaw);
}
FiftyHzflag=0;
//serviceWatchDog();//
}//FiftyHzflag bracket
}//Main While loop Bracket
void UserMain( void* pd ){
/////Usual Routine
InitializeStack();
OSChangePrio( MAIN_PRIO );
EnableAutoUpdate();
EnableTaskMonitor();
EnableSmartTraps();
initPINS();
initDSPI();
//Local Variables
/***********Defining Interrupt Timers*****************/
HiResTimer* timer2=0;//50 Hz Interrupt Timer
int32_t IMU_data[6]={0};
char Navcomp_send_buff[64]={0},time_ms[2]={0},G[20]={0};
uint16_t NB_counter=0,sum=0;
double TotalTime=0;
int fdDebug=0;
uint8_t i=0;
BYTE IMU_command[24]={xahigh,0,xalow,0,yahigh,0,yalow,0,zahigh,0,zalow,0,xghigh,0,xglow,0,yghigh,0,yglow,0,zghigh,0,zglow,0};
BYTE IMU3_raw[24]={0};//IMU 3
BYTE IMU4_raw[24]={0};//IMU 4
Navcomp_send_buff[0] = 0x41;
Navcomp_send_buff[1] = 0x7A;
Navcomp_send_buff[2] = 0x04;
SerialClose(0);
SerialClose(2);
SerialClose(7);
SerialClose(9);
fdNavcomp=OpenSerial(7,15200,1,8,eParityNone);
fdcrank=OpenSerial(9,15200,1,8,eParityNone);
fdgrabber=OpenSerial(2,15200,1,8,eParityNone);
fdDebug=OpenSerial(0,15200,1,8,eParityNone);
OSSimpleTaskCreate(NAVcompData,MAIN_PRIO-1);
sprintf(G,"~ECHOF 1\r");
i=0;
for (i=0;i<sizeof(G);i++){
write(fdcrank,&G[i],1);
write(fdgrabber,&G[i],1);
}
initTIMERS(timer2);
while(1){
if (FiftyHzTaskFlag==1){
DSPIStart(1,IMU_command,IMU3_raw,24,NULL);//IMU3
while(!DSPIdone(1)){/*iprintf("DSPI1done state=%s\n",(DSPIdone(1))?"true":"false");*/};
//iprintf("....................................................................\n");
DSPIStart(3,IMU_command,IMU4_raw,24,NULL);//IMU3
while(!DSPIdone(3)){/*iprintf("DSPI3done state=%s\n",(DSPIdone(3))?"true":"false");*/};
//packaging NB counter
Navcomp_send_buff[4]=(uint8_t)((NB_counter & 0xFF00)>>8);
Navcomp_send_buff[3]=(uint8_t)(NB_counter & 0x00FF);
NB_counter++;
//getting time in ms
TotalTime=timer2->readTime();
sprintf(time_ms,"%lf",TotalTime);
Navcomp_send_buff[6]=time_ms[0];
Navcomp_send_buff[5]=time_ms[1];
IMU_data[0]=(((int32_t)IMU3_raw[2]<<24|(int32_t)IMU3_raw[3]<<16|(int32_t)IMU3_raw[4]<<8|(int32_t)IMU3_raw[5])-((int32_t)IMU4_raw[2]<<24|(int32_t)IMU4_raw[3]<<16|(int32_t)IMU4_raw[4]<<8|(int32_t)IMU4_raw[5]))/2;//X-Accel
IMU_data[1]=(((int32_t)IMU3_raw[6]<<24|(int32_t)IMU3_raw[7]<<16|(int32_t)IMU3_raw[8]<<8|(int32_t)IMU3_raw[9])-((int32_t)IMU4_raw[6]<<24|(int32_t)IMU4_raw[7]<<16|(int32_t)IMU4_raw[8]<<8|(int32_t)IMU4_raw[9]))/2;//Y-Accel
IMU_data[2]=(((int32_t)IMU3_raw[10]<<24|(int32_t)IMU3_raw[11]<<16|(int32_t)IMU3_raw[12]<<8|(int32_t)IMU3_raw[13])+((int32_t)IMU4_raw[10]<<24|(int32_t)IMU4_raw[11]<<16|(int32_t)IMU4_raw[12]<<8|(int32_t)IMU4_raw[13]))/2;//Z-Accel
IMU_data[3]=(((int32_t)IMU3_raw[14]<<24|(int32_t)IMU3_raw[15]<<16|(int32_t)IMU3_raw[16]<<8|(int32_t)IMU3_raw[17])-((int32_t)IMU4_raw[14]<<24|(int32_t)IMU4_raw[15]<<16|(int32_t)IMU4_raw[16]<<8|(int32_t)IMU4_raw[17]))/2;//X-Gyro
IMU_data[4]=(((int32_t)IMU3_raw[18]<<24|(int32_t)IMU3_raw[19]<<16|(int32_t)IMU3_raw[20]<<8|(int32_t)IMU3_raw[21])-((int32_t)IMU4_raw[18]<<24|(int32_t)IMU4_raw[19]<<16|(int32_t)IMU4_raw[20]<<8|(int32_t)IMU4_raw[21]))/2;//Y-Gyro
IMU_data[5]=(((int32_t)IMU3_raw[22]<<24|(int32_t)IMU3_raw[23]<<16|(int32_t)IMU3_raw[0]<<8|(int32_t)IMU3_raw[1])+((int32_t)IMU4_raw[22]<<24|(int32_t)IMU4_raw[23]<<16|(int32_t)IMU4_raw[0]<<8|(int32_t)IMU4_raw[1]))/2;//Z-Gyro
i=0;
for(i=0;i<6;i++){
Navcomp_send_buff[4*i+10]=(BYTE)((uint32_t)(IMU_data[i] & 0xFF000000)>>24);
Navcomp_send_buff[4*i+9]=(BYTE)((uint32_t)(IMU_data[i] & 0x00FF0000)>>16);
Navcomp_send_buff[4*i+8]=(BYTE)((uint32_t)(IMU_data[i] & 0x0000FF00)>>8);
Navcomp_send_buff[4*i+7]=(BYTE)((uint32_t)(IMU_data[i] & 0x000000FF));
}
Navcomp_send_buff[47]=statestatus;
//Calculating checksum
sum=0;
i=0;
for(i=3;i<63;i++){
sum +=Navcomp_send_buff[i];
}
Navcomp_send_buff[63]=(uint8_t)(sum % 256);
for(uint8_t j=0;j<sizeof(Navcomp_send_buff);j++){
write(fdNavcomp,&Navcomp_send_buff[j],1);
}
sprintf(G,"!VAR 1 %d\r",commandstatus);
i=0;
for (i=0;i<sizeof(G);i++){
write(fdcrank,&G[i],1);
write(fdgrabber,&G[i],1);
}
//printf("Value:%f MSB:%d LSB:%d\n",0.00025*((int16_t)((uint16_t)send_buff[18]*256 + (uint16_t)send_buff[17])),(uint8_t)send_buff[18],(uint8_t) send_buff[17]);
FiftyHzTaskFlag=0;
}//FiftyHz if
}//Main While loop
