迁移分支
This commit is contained in:
zengxiaobin
242
src/pMotionControler/MotionControler.cpp
Normal file
242
src/pMotionControler/MotionControler.cpp
Normal file
@@ -0,0 +1,242 @@
|
||||
/*
|
||||
* @Author: zjk 1553836110@qq.com
|
||||
* @Date: 2023-10-12 09:52:27
|
||||
* @LastEditors: zjk 1553836110@qq.com
|
||||
* @LastEditTime: 2023-11-07 12:01:57
|
||||
* @FilePath: /moos-ivp-pi/src/pMotionControler/MotionControler.cpp
|
||||
* @Description:
|
||||
*
|
||||
* Copyright (c) 2023 by ${git_name_email}, All Rights Reserved.
|
||||
*/
|
||||
// #define DEBUG
|
||||
#include"MotionControler.hpp"
|
||||
//TODO：增加启用哪个控制器功能
|
||||
bool MotionControler::OnNewMail(MOOSMSG_LIST &NewMail)
|
||||
{
|
||||
AppCastingMOOSApp::OnNewMail(NewMail);
|
||||
MOOSMSG_LIST::iterator p;
|
||||
for(p=NewMail.begin(); p!=NewMail.end(); p++)
|
||||
{
|
||||
CMOOSMsg &msg = *p;
|
||||
|
||||
string key = msg.m_sKey;
|
||||
string sval = msg.m_sVal;
|
||||
double dval = msg.m_dfVal;
|
||||
double dfT;
|
||||
msg.IsSkewed(m_curr_time, &dfT);
|
||||
if(fabs(dfT) < ok_skew)
|
||||
{
|
||||
if((key == "MOOS_MANUAL_OVERIDE") || (key == "MOOS_MANUAL_OVERRIDE"))
|
||||
pengine.overrived(sval);
|
||||
else if(key == "DESIRED_HEADING")
|
||||
pengine.setDesHeading(dval, MOOSTime());
|
||||
else if(key == "DESIRED_SPEED")
|
||||
pengine.setDesSpeed(dval, MOOSTime());
|
||||
else if(key == "DESIRED_DEPTH")
|
||||
pengine.setDesDepth(dval, MOOSTime());
|
||||
else if(key == "NAV_HEADING")
|
||||
pengine.setCurHeading(angle360(dval), MOOSTime());
|
||||
else if(key == "NAV_SPEED")
|
||||
pengine.setCurSpeed(dval, MOOSTime());
|
||||
else if(key == "NAV_DEPTH")
|
||||
pengine.setCurDepth(dval, MOOSTime());
|
||||
else if(key == "NAV_PITCH")
|
||||
pengine.setCurPitch(dval, MOOSTime());
|
||||
else if(key == MSG_ReadConfig) //重新读取配置参数可以清除故障码
|
||||
{
|
||||
int e = pengine.setParam(configFilePath);
|
||||
pengine.setOverride(true);
|
||||
if(e != 0)
|
||||
faultCode = 10 + e;
|
||||
else
|
||||
faultCode = 0;
|
||||
}
|
||||
}
|
||||
}
|
||||
return true;
|
||||
}
|
||||
bool MotionControler::Iterate()
|
||||
{
|
||||
bool a;
|
||||
AppCastingMOOSApp::Iterate();
|
||||
pengine.updateTime(m_curr_time);
|
||||
int i = pengine.step();
|
||||
switch (i)
|
||||
{
|
||||
case 0:
|
||||
RepList["State : "] = "Run";
|
||||
break;
|
||||
case 1:
|
||||
RepList["State : "] = "Ready";
|
||||
break;
|
||||
case -1:
|
||||
RepList["State : "] = "Fault";
|
||||
faultCode = 1;//信息超时
|
||||
break;
|
||||
default:
|
||||
break;
|
||||
}
|
||||
postPengineResults();
|
||||
postPenginePostings();
|
||||
postCharStatus();
|
||||
Notify(MSG_FALUT,faultCode);
|
||||
AppCastingMOOSApp::PostReport();
|
||||
return true;
|
||||
}
|
||||
bool MotionControler::OnConnectToServer()
|
||||
{
|
||||
registerVariables();
|
||||
return true;
|
||||
}
|
||||
bool MotionControler::OnStartUp()
|
||||
{
|
||||
AppCastingMOOSApp::OnStartUp();
|
||||
pengine.setStartTime(MOOSTime());
|
||||
STRING_LIST sParams;
|
||||
m_MissionReader.GetConfiguration(GetAppName(), sParams);
|
||||
//pengine.setConfigParams(sParams);
|
||||
//bool handled = true;
|
||||
STRING_LIST::iterator p;
|
||||
for(p=sParams.begin(); p!=sParams.end(); p++) {
|
||||
string orig = *p;
|
||||
string line = (orig); //识别大小写
|
||||
string param = biteStringX(line, '=');
|
||||
string value = line;
|
||||
double dval = atof(value.c_str());
|
||||
if(param == "config_file")
|
||||
configFilePath = value;
|
||||
else if(param == "tardy_helm_thresh")
|
||||
pengine.setTardyHelm(dval);
|
||||
else if(param == "tardy_nav_thresh")
|
||||
pengine.setTardyNav(dval);
|
||||
else if(param == "cheak_stalensee")
|
||||
pengine.setCheakStalensee(value);
|
||||
else if(param == "AppTick")
|
||||
setFrequency = dval;
|
||||
else if(param == "delta_freqency")
|
||||
frequency_delta = dval;
|
||||
else
|
||||
reportUnhandledConfigWarning(orig);
|
||||
}
|
||||
int e = pengine.setParam(configFilePath);
|
||||
if(e != 0)
|
||||
{
|
||||
faultCode = 10 + e;
|
||||
return false;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
void MotionControler::registerVariables()
|
||||
{
|
||||
AppCastingMOOSApp::RegisterVariables();
|
||||
|
||||
Register("NAV_HEADING", 0);
|
||||
Register("NAV_SPEED", 0);
|
||||
Register("NAV_DEPTH", 0);
|
||||
Register("NAV_PITCH", 0);
|
||||
Register("DESIRED_HEADING", 0);
|
||||
Register("DESIRED_SPEED", 0);
|
||||
Register("DESIRED_DEPTH", 0);
|
||||
Register("PID_VERBOSE", 0);
|
||||
Register("SPEED_FACTOR", 0);
|
||||
Register("MOOS_MANUAL_OVERIDE", 0);
|
||||
Register("MOOS_MANUAL_OVERRIDE", 0);
|
||||
Register(MSG_ReadConfig,0);
|
||||
}
|
||||
bool MotionControler::buildReport()
|
||||
{
|
||||
double frequency = pengine.getFrequency();
|
||||
double delta_freq = 100.0*(setFrequency - frequency) / setFrequency;
|
||||
if(abs(delta_freq) > frequency_delta)
|
||||
faultCode = 2;
|
||||
|
||||
m_msgs << "Frequency Delta : " << frequency << endl;
|
||||
m_msgs << "PID has_control : " << boolToString(pengine.hasControl()) << endl;
|
||||
m_msgs << "Config File Path : " << configFilePath << endl;
|
||||
m_msgs << "S : H : D : | " << intToString(pengine.hasSpdCtrl())+ " | "
|
||||
<< intToString(pengine.hasHdgCtrl())+" | "
|
||||
<< intToString(pengine.hasDphCtrl())+" |" << endl;
|
||||
|
||||
RepList["to BS"] = colVar;
|
||||
RepList["PID"] = pengine.getReport()["W"];
|
||||
string rep = Json::writeString(RepJsBuilder, RepList);
|
||||
m_msgs << rep << endl;
|
||||
|
||||
return(true);
|
||||
}
|
||||
|
||||
void MotionControler::postPenginePostings()
|
||||
{
|
||||
vector<VarDataPair> m_postings = pengine.getPostings();
|
||||
for(unsigned int i=0; i<m_postings.size(); i++)
|
||||
{
|
||||
VarDataPair pair = m_postings[i];
|
||||
string var = pair.get_var();
|
||||
if(pair.is_string() && pair.get_sdata_set())
|
||||
Notify(var, pair.get_sdata());
|
||||
else if(!pair.is_string() && pair.get_ddata_set())
|
||||
Notify(var, pair.get_ddata());
|
||||
}
|
||||
pengine.clearPostings();
|
||||
}
|
||||
void MotionControler::postPengineResults()
|
||||
{
|
||||
bool all_stop = true;
|
||||
if(pengine.hasControl())
|
||||
all_stop = false;
|
||||
|
||||
if(all_stop)
|
||||
{
|
||||
if(allstop_posted)
|
||||
return;
|
||||
Notify("DESIRED_RUDDER", 0.0);
|
||||
Notify("DESIRED_THRUST", 0.0);
|
||||
// if(pengine.hasDphCtrl())
|
||||
Notify("DESIRED_ELEVATOR", 0.0);
|
||||
postColVarToBS(0,0,0);
|
||||
allstop_posted = true;
|
||||
}
|
||||
else
|
||||
{
|
||||
int T=0,S=0,R=0;
|
||||
if(pengine.hasHdgCtrl())
|
||||
{
|
||||
Notify("DESIRED_RUDDER", pengine.getDesiredRudder());
|
||||
R = (int)pengine.getDesiredRudder();
|
||||
}
|
||||
if(pengine.hasSpdCtrl())
|
||||
{
|
||||
Notify("DESIRED_THRUST", pengine.getDesiredThrust());
|
||||
T = (int)pengine.getDesiredThrust();
|
||||
}
|
||||
|
||||
if(pengine.hasDphCtrl())
|
||||
{
|
||||
Notify("DESIRED_ELEVATOR", pengine.getDesiredElevator());
|
||||
S = pengine.getDesiredElevator();
|
||||
}
|
||||
postColVarToBS(T,S,R);
|
||||
allstop_posted = false;
|
||||
}
|
||||
}
|
||||
void MotionControler::postCharStatus()
|
||||
{
|
||||
if(!verbose)
|
||||
return;
|
||||
|
||||
if(pengine.hasControl())
|
||||
cout << "$" << flush;
|
||||
else
|
||||
cout << "*" << flush;
|
||||
}
|
||||
void MotionControler::postColVarToBS(int T, int S, int R)
|
||||
{
|
||||
string s_msg;
|
||||
colVar["mode"] = 2;
|
||||
colVar["thrust"] = T;
|
||||
colVar["rudder"] = R;
|
||||
colVar["elevator"] = S;
|
||||
s_msg = Json::writeString(RepJsBuilder,colVar);
|
||||
Notify(MSG_TO_BS, s_msg);
|
||||
}
|
||||
Reference in New Issue
Block a user