bool CMOOSNavTopDownCalEngine::Initialise(STRING_LIST sParams)
{
if(CMOOSNavLSQEngine::Initialise(sParams))
{
m_pStore->SetSpan(200);
}
//we never use heading bias state in TDC
m_bEstimateHeadingBias = false;
string sVal;
if(MOOSGetValueFromToken(sParams,"TDC_TIDE",sVal))
{
double dfVal = atof(sVal.c_str());
m_dfTide = dfVal;
}
if(MOOSGetValueFromToken(sParams,"TDC_SPACING",sVal))
{
double dfVal = atof(sVal.c_str());
if(dfVal>0)
{
m_dfSpacing = dfVal;
}
}
if(MOOSGetValueFromToken(sParams,"TDC_TRIAL_RATE",sVal))
{
double dfVal = atof(sVal.c_str());
if(dfVal>0)
{
m_dfTrialRate = dfVal;
}
}
//initial guesses for top down
if(MOOSGetValueFromToken(sParams,"TDC_GUESS_DEPTHS",sVal))
{
//TDC_GUESS_DEPTHS = [email protected] , [email protected] etc
while(!sVal.empty())
{
string sBlock = MOOSChomp(sVal,",");
string sChan = MOOSChomp(sBlock,"@");
//store this guess depth...
m_GuessedDepths[atoi(sChan.c_str())] = atoi(sBlock.c_str());
}
}
else
{
AddToOutput("Warning now beacons guess depths specified\n");
}
return true;
}
bool CProcessConfigReader::GetConfigurationAndPreserveSpace(std::string sAppName, STRING_LIST &Params)
{
Params.clear();
int nBrackets = 0;
Params.clear();
Reset();
std::string sKey = "PROCESSCONFIG="+sAppName;
if(GoTo(sKey))
{
std::string sBracket = GetNextValidLine();
if(sBracket.find("{")==0)
{
nBrackets++;
while(!GetFile()->eof())
{
std::string sLine = GetNextValidLine();
MOOSTrimWhiteSpace(sLine);
if(sLine.find("}")!=0)
{
std::string sVal(sLine);
std::string sTok = MOOSChomp(sVal, "=");
MOOSTrimWhiteSpace(sTok);
MOOSTrimWhiteSpace(sVal);
if (!sTok.empty())
{
if (!sVal.empty())
{
Params.push_back(sTok+"="+sVal);
}
else if(sLine.find("[")!=std::string::npos || sLine.find("]")!=std::string::npos)
{
Params.push_back(sLine);
}
}
}
else
{
return true;
}
//quick error check - we don't allow nested { on single lines
if(sLine.find("{")==0)
{
MOOSTrace("CProcessConfigReader::GetConfiguration() missing \"}\" syntax error in mission file\n");
}
}
}
}
return false;
}
/// READ STRINGS
bool CProcessConfigReader::GetConfigurationParam(std::string sAppName,std::string sParam, std::string &sVal)
{
Reset();
//remember all names we were asked for....
std::string sl = sParam;
MOOSToLower(sl);
m_Audit[sAppName].insert(sl);
STRING_LIST sParams;
if(GetConfigurationAndPreserveSpace( sAppName, sParams))
{
STRING_LIST::iterator p;
for(p = sParams.begin(); p!=sParams.end(); p++)
{
std::string sTmp = *p;
std::string sTok = MOOSChomp(sTmp,"=");
MOOSTrimWhiteSpace(sTok);
if (sTmp.empty())
return false;
if(MOOSStrCmp(sTok,sParam))
{
MOOSTrimWhiteSpace(sTmp);
sVal=sTmp;
return true;
}
}
}
return false;
}
bool CSimEntity::IsLocalSource(std::string sSrc)
{
//this is a catch for auto-excitation
std::string sSrcVeh = MOOSChomp(sSrc,"/");
if(GetName()==sSrcVeh)
return true;
return false;
}
bool CSingleAction::SetParam(string sParam, string sVal)
{
MOOSToUpper(sParam);
if(sParam == "ACTION")
{
m_sVarName = MOOSChomp(sVal,":");
m_sValue = sVal;
return true;
}
return CSGMOOSBehaviour::SetParam(sParam, sVal);
}
bool CMOOSNavEngine::SetUpSensorChannels(STRING_LIST sParams,string sToken)
{
//ok we need to load up rejection settings
STRING_LIST::iterator p;
for(p = sParams.begin();p!=sParams.end();p++)
{
string sLine = *p;
if(sLine.find(sToken)!=string::npos)
{
MOOSRemoveChars(sLine," \t");
MOOSChomp(sLine,"=");
//LSQ_REJECTION = TheAvtrak : Reject = 3, History = 5,Fail = 0.001
CMOOSSensorChannel NewChannel;
string sSensor = MOOSChomp(sLine,":");
string sHistory = MOOSChomp(sLine,",");
string sFail = MOOSChomp(sLine,",");
MOOSChomp(sHistory,"=");
MOOSChomp(sFail,"=");
if(sFail.empty() ||sHistory.empty())
{
MOOSTrace("error in %s line!\n",sToken.c_str());
return false;
}
int nDepth = atoi(sHistory.c_str());
if(nDepth>0)
{
NewChannel.SetHistoryDepth(nDepth);
}
double dfFail = atof(sFail.c_str());
if(dfFail>0)
{
NewChannel.SetNoiseLimit(dfFail);
}
NewChannel.SetName(sSensor);
m_SensorChannelMap[sSensor]=NewChannel;
}
}
return true;
}
bool CMOOSNavTopDownCalEngine::OnRxTopDownControl(string sInstruction)
{
MOOSToUpper(sInstruction);
if(MOOSStrCmp(sInstruction,"STOP"))
{
SetOnline(false);
SetState(OFFLINE);
return true;
}
else if(MOOSStrCmp(sInstruction,"START"))
{
OnStart();
return true;
}
else if(MOOSStrCmp(sInstruction,"CALCULATE"))
{
//ok try and calculate now....
return Calculate(MOOSTime());
}
else if(sInstruction.find("FOCUS")!=string::npos)
{
MOOSChomp(sInstruction,"=");
int nNew = atoi(sInstruction.c_str());
if(nNew!=0)
{
m_nActiveChannel = nNew;
AddToOutput("Manual set of TDC focus to channel %d",nNew);
SetFocus(nNew);
}
}
else
{
AddToOutput("Unknown top down cal control command");
return false;
}
return true;
}
bool CThirdPartyTask::OnActuationInstruction(string sInstruction, double dfTimeNow)
{
//instruction is of form
//"RUDDER=9,ELEVATOR = 0,THRUST = 90"
string sCopy = sInstruction;
MOOSChomp(sCopy,"RUDDER=");
if(!sCopy.empty())
{
string sRudder = MOOSChomp(sCopy,",");
if(!sRudder.empty())
{
double dfRudder = atof(sRudder.c_str());
m_Rudder.Set(dfRudder,dfTimeNow);
}
}
sCopy = sInstruction;
MOOSChomp(sCopy,"ELEVATOR=");
if(!sCopy.empty())
{
string sElevator = MOOSChomp(sCopy,",");
if(!sElevator.empty())
{
double dfElevator = atof(sElevator.c_str());
m_Elevator.Set(dfElevator,dfTimeNow);
}
}
sCopy = sInstruction;
MOOSChomp(sCopy,"THRUST=");
if(!sCopy.empty())
{
string sThrust = MOOSChomp(sCopy,",");
if(!sThrust.empty())
{
double dfThrust = atof(sThrust.c_str());
m_Thrust.Set(dfThrust,dfTimeNow);
}
}
return true;
}
bool CProcessConfigReader::GetConfiguration(std::string sAppName, STRING_LIST &Params)
{
int nBrackets = 0;
Params.clear();
Reset();
std::string sKey = "PROCESSCONFIG="+sAppName;
if(GoTo(sKey))
{
std::string sBracket = GetNextValidLine();
if(sBracket.find("{")==0)
{
nBrackets++;
while(!GetFile()->eof())
{
std::string sLine = GetNextValidLine();
MOOSRemoveChars(sLine," \t\r");
if(sLine.find("}")!=0)
{
#if(1)
// jckerken 8-12-2004
// ignore if param = <empty string>
std::string sTmp(sLine);
std::string sTok = MOOSChomp(sTmp, "=");
MOOSTrimWhiteSpace(sTok); // Handle potential whitespaces.
MOOSTrimWhiteSpace(sTmp);
if (sTok.size() > 0)
{
MOOSTrimWhiteSpace(sTmp);
if (!sTmp.empty())
{
Params.push_front(sTok+std::string("=")+sTmp); // Was: sLine
}
else if(sLine.find("[")!=std::string::npos || sLine.find("]")!=std::string::npos)
{
Params.push_front(sTok+std::string("=")+sTmp); // Was: sLine
}
}
else
{
Params.push_front(sTok+std::string("=")+sTmp); // Was: sLine
}
#else
Params.push_front(sLine);
#endif
}
else
{
return true;
}
//quick error check - we don't allow nested { on single lines
if(sLine.find("{")==0)
{
MOOSTrace("CProcessConfigReader::GetConfiguration() missing \"}\" syntax error in mission file\n");
}
}
}
}
return false;
}
bool CMOOSSerialPort::Configure(STRING_LIST sParams)
{
MOOSTrace("CMOOSSerialPort::Configure() : ");
STRING_LIST::iterator p;
for(p=sParams.begin();p!=sParams.end();++p)
{
std::string sLine = *p;
std::string sTok = MOOSChomp(sLine,"=");
std::string sVal = sLine;
if(MOOSStrCmp(sTok,"PORT"))
{
m_sPort = sVal;
MOOSTrace("%s,",m_sPort.c_str());
}
else if(MOOSStrCmp(sTok,"BAUDRATE"))
{
m_nBaudRate = atoi(sVal.c_str());
if(m_nBaudRate==0)
{
m_nBaudRate = DEFAULT_BAUDRATE;
}
MOOSTrace("%d,",m_nBaudRate);
}
else if(MOOSStrCmp(sTok,"HANDSHAKING"))
{
if(MOOSStrCmp(sVal,"TRUE"))
{
m_bHandShaking = true;
}
else
{
m_bHandShaking = false;
}
}
else if(MOOSStrCmp(sTok,"VERBOSE"))
{
if(MOOSStrCmp(sVal,"TRUE"))
{
m_bVerbose = true;
}
else
{
m_bVerbose = false;
}
}
else if(MOOSStrCmp(sTok,"STREAMING"))
{
if(MOOSStrCmp(sVal,"TRUE"))
{
m_bStreaming = true;
}
else
{
m_bStreaming = false;
}
MOOSTrace("%s,",m_bStreaming?"streaming":"standard");
}
// ARH 14/05/2005 Added to allow use of the 500kbaud CSM PCMCIA card
else if (MOOSStrCmp(sTok, "USECSMEXT"))
{
if (MOOSStrCmp(sVal, "TRUE"))
{
m_bUseCsmExt = true;
}
else
{
m_bUseCsmExt = false;
}
}
}
bool bSuccess = Create(m_sPort.c_str(),m_nBaudRate);
if(bSuccess)
{
Flush();
if(m_bStreaming)
{
bSuccess = StartThreads();
}
}
MOOSTrace("%s\n",bSuccess?"OK":"FAILED");
return bSuccess;
}
//heres the money.....
int main(int argc, char* argv[])
{
//make the frame
Fl_Double_Window *w = new Fl_Double_Window( 820, 460, "uMS" );
w->size_range(DEFAULT_WIDTH,DEFAULT_HEIGHT);
//OK - lets build a whole set of panes
std::string sWho;
{
//note scoping
Fl_Preferences app( Fl_Preferences::USER, "MOOS", "uMS" );
char Space[2048];
app.get("Communities",Space,"Unnamed:9000@LOCALHOST",sizeof(Space));
sWho = std::string(Space);
}
Fl_Tabs* pTab = new Fl_Tabs(10, 10, 800, 400);
while(!sWho.empty())
{
std::string sChunk = MOOSChomp(sWho,",");
std::string sName = MOOSChomp(sChunk,":");
int nPort = atoi(MOOSChomp(sChunk,"@").c_str());
std::string sHost = sChunk;
if(!sChunk.empty() && nPort>0)
{
Fl_Group *pG = MakeCommunityPane(10,30,800,390,(char*)sName.c_str());
CScopeTabPane * pTabPane = (CScopeTabPane*)(pG);
pTabPane->SetMOOSInfo(sHost,nPort);
Fl_Group::current()->resizable(pG);
}
}
if(pTab->children()==0)
{
MakeCommunityPane(10,30,800,390,"Unnamed");
}
w->end();
//and add some buttons to control them....
w->begin();
{
//add
Fl_Button * pNewCommunityButton = new Fl_Button(10,420,160,30,"Add Community");
pNewCommunityButton->callback(OnAddCommunity,pTab);
//delete
Fl_Button * pDelCommunityButton = new Fl_Button(180,420,160,30,"Remove Community");
pDelCommunityButton->callback(OnRemoveCommunity,pTab);
//save
Fl_Button * pSaveButton = new Fl_Button(350,420,160,30,"Save Layout");
pSaveButton->callback(OnSave,pTab);
pSaveButton->callback();
//rename
Fl_Button * pRenameButton = new Fl_Button(520,420,160,30,"Rename");
pRenameButton->callback(OnRenameCommunity,pTab);
pRenameButton->callback();
}
w->end();
//need to do more work on resizing - maybe even read the manual
w->resizable(pTab);
//and GO!
w->show(argc, argv);
return Fl::run();
}
bool CMOOSNavEngine::MakeLBLObservations(const CMOOSMsg &Msg, OBSLIST &ObsList)
{
//this is LBL data
string sData = Msg.m_sVal;
//what time was interrogation sent?
MOOSChomp(sData,"Tx=");
double dfTxTime = atof(MOOSChomp(sData,",").c_str());
if(dfTxTime<=0)
{
MOOSTrace("Tx time of LBL is not positive\n");
return false;
}
while(!sData.empty())
{
//for each reply...
CMOOSObservation NewObs;
string sChunk = MOOSChomp(sData,",");
MOOSChomp(sChunk,"Ch[");
int nChan = atoi(sChunk.c_str());
if(nChan<=0 || nChan > 14)
{
MOOSTrace("Channel of LBL obs is not valid\n");
continue;
}
NewObs.m_nChan = nChan;
//figure out the beacon from the channel....
CMOOSNavBeacon * pBeacon = GetBeaconByChannel(nChan);
if(pBeacon==NULL)
{
MOOSTrace("Warning: No known Beacon with Channel[%d]\n",nChan);
}
//so what was the observation - the TOF?
MOOSChomp(sChunk,"=");
double dfTOF = atof(sChunk.c_str());
if(dfTOF<=0)
{
MOOSTrace("TOF of LBL obs is not valid\n");
return false;
}
NewObs.m_dfData = dfTOF;
//set obs time Tx Time plus in water time
NewObs.m_dfTime = dfTxTime+dfTOF;
// MOOSTrace("Making LBL Obs.dfTime = %f and now = %f\n",NewObs.m_dfTime,MOOSTime());
//set obs type
NewObs.m_eType = CMOOSObservation::LBL_BEACON_2WR;
//give it a unique id
NewObs.m_nID = GetNextID();
//use the message source to figure out the sensor this
//corresponds to
NewObs.m_pInterrogateSensor = GetSensorBySource(Msg.m_sSrc,Msg.m_sKey);
if(NewObs.m_pInterrogateSensor==NULL)
{
return false;
}
//set observation noise
NewObs.m_dfDataStd = LBL_2WR_STD;
if(NewObs.m_pInterrogateSensor->GetNoise()>=0)
{
NewObs.m_dfDataStd = NewObs.m_pInterrogateSensor->GetNoise();
}
//what other responding sensor was involved?
if(pBeacon!=NULL)
{
NewObs.m_pRespondingSensor = pBeacon->GetSensorByType(CMOOSNavSensor::LBL);
}
else
{
NewObs.m_pRespondingSensor=NULL;
}
//this is an acoustic obs so set the sound velocity
NewObs.m_dfSV = m_dfSV;
//finally add the obs to the list
if(NewObs.m_pRespondingSensor!=NULL)
{
ObsList.push_back(NewObs);
}
/* MOOSTrace("NewLBL: MOOSTime = %.3f,msgtime= %.3f,datatime = %.3f,TxTime = %.3f, cTOF= %.3f\n",
MOOSTime(),
Msg.m_dfTime,
NewObs.m_dfTime,
dfTxTime,
NewObs.m_dfTime-dfTxTime);*/
}
return true;
}
