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; }