bool OnStartUp(){
m11=190;
m22=240;
m33=82;
d11=75;
d22=105;
d33=55;
D11=0;
D22=0;
D33=0;
L=1; //it should be the same as that in the controller
//here we extract the vehicle name..
m_sVehicleName = "UnNamed";
if(!m_MissionReader.GetConfigurationParam("VehicleName",m_sVehicleName))
MOOSTrace("Warning parameter \"VechicleName\" not specified. Using default of \"%s\"\n",m_sVehicleName.c_str());
x=0; //default value
//here we extract a double from the configuration file
if(!m_MissionReader.GetConfigurationParam("InitialLocation_x",x))
MOOSTrace("Warning parameter \"InitialLocation\" not specified. Using default of \"%f\"\n",x);
y=0; //default value
//here we extract a double from the configuration file
if(!m_MissionReader.GetConfigurationParam("InitialLocation_y",y))
MOOSTrace("Warning parameter \"InitialLocation\" not specified. Using default of \"%f\"\n",y);
psi=0; //default value
//here we extract a double from the configuration file
if(!m_MissionReader.GetConfigurationParam("InitialLocation_psi",psi))
MOOSTrace("Warning parameter \"InitialLocation\" not specified. Using default of \"%f\"\n",psi);
u=0; //default value
//here we extract a double from the configuration file
if(!m_MissionReader.GetConfigurationParam("InitialLocation_u",u))
MOOSTrace("Warning parameter \"InitialLocation\" not specified. Using default of \"%f\"\n",u);
v=0; //default value
//here we extract a double from the configuration file
if(!m_MissionReader.GetConfigurationParam("InitialLocation_v",v))
MOOSTrace("Warning parameter \"InitialLocation\" not specified. Using default of \"%f\"\n",v);
r=0; //default value
//here we extract a double from the configuration file
if(!m_MissionReader.GetConfigurationParam("InitialLocation_r",r))
MOOSTrace("Warning parameter \"InitialLocation\" not specified. Using default of \"%f\"\n",r);
DESIRED_PORTTHRUSTER=0; //default value
//here we extract a double from the configuration file
if(!m_MissionReader.GetConfigurationParam("DESIRED_PORTTHRUSTER",DESIRED_PORTTHRUSTER))
MOOSTrace("Warning parameter \"InitialLocation\" not specified. Using default of \"%f\"\n",DESIRED_PORTTHRUSTER);
DESIRED_STARBOARDTHRUSTER=0; //default value
//here we extract a double from the configuration file
if(!m_MissionReader.GetConfigurationParam("DESIRED_STARBOARDTHRUSTER", DESIRED_STARBOARDTHRUSTER))
MOOSTrace("Warning parameter \"InitialLocation\" not specified. Using default of \"%f\"\n",DESIRED_STARBOARDTHRUSTER);
//MOOSTrace("input_FL=%f, input_FR=%f, time=%f\n",input_FL,input_FR,MOOSTime());
t_r=MOOSTime();
return true;
}
/** Create and set up the port */
bool CMOOSLinuxSerialPort::Create(const char * sPort, int nBaudRate)
{
if (m_nPortFD >= 0)
{
MOOSTrace("Serial Port already open.\n");
return false;
}
#ifndef _WIN32
int nLinuxBaudRate = B9600;
switch(nBaudRate)
{
#ifdef B500000
case 500000: nLinuxBaudRate = B500000; break;
#endif
case 115200: nLinuxBaudRate = B115200; break;
case 57600: nLinuxBaudRate = B57600; break;
case 38400: nLinuxBaudRate = B38400; break;
case 19200: nLinuxBaudRate = B19200; break;
case 9600: nLinuxBaudRate = B9600; break;
case 4800: nLinuxBaudRate = B4800; break;
case 2400: nLinuxBaudRate = B2400; break;
case 1200: nLinuxBaudRate = B1200; break;
case 600: nLinuxBaudRate = B600; break;
case 300: nLinuxBaudRate = B300; break;
default :
printf("Unsupported baud rate\n");
return false;
break;
}
// open and configure the serial port
m_nPortFD = open(sPort, O_RDWR | O_NOCTTY | O_NDELAY);
if (m_nPortFD <0)
{
perror(sPort);
return false;
}
//save the current configuration
tcgetattr(m_nPortFD,&m_OldPortOptions);
//zero the buffers
//bzero(&m_PortOptions, sizeof(m_PortOptions));
memset(&m_PortOptions,0,sizeof(m_PortOptions));
m_PortOptions.c_cflag = nLinuxBaudRate | CS8 | CLOCAL | CREAD;
m_PortOptions.c_iflag = IGNPAR;
m_PortOptions.c_oflag = 0;
/* set input mode (non-canonical, no echo,...) */
m_PortOptions.c_lflag = 0;
// inter-character timer unused
m_PortOptions.c_cc[VTIME] = 0;
// blocking read until 0 chars received, i.e. don't block
m_PortOptions.c_cc[VMIN] = 0;
//save the new settings
tcflush(m_nPortFD, TCIFLUSH);
tcsetattr(m_nPortFD,TCSANOW,&m_PortOptions);
#else
MOOS::DeliberatelyNotUsed(nBaudRate);
#endif
if(m_nPortFD!=0)
m_sPort = sPort;
return m_nPortFD!=0;
}
