迁移分支
This commit is contained in:
zengxiaobin
424
src/pMotionControler/Controler.cpp
Normal file
424
src/pMotionControler/Controler.cpp
Normal file
@@ -0,0 +1,424 @@
|
||||
/*
|
||||
* @Author: zjk 1553836110@qq.com
|
||||
* @Date: 2023-10-16 14:04:53
|
||||
* @LastEditors: zjk 1553836110@qq.com
|
||||
* @LastEditTime: 2023-11-02 15:17:38
|
||||
* @FilePath: /moos-ivp-pi/src/pMotionControler/Controler.cpp
|
||||
* @Description:
|
||||
*
|
||||
* Copyright (c) 2023 by ${git_name_email}, All Rights Reserved.
|
||||
*/
|
||||
#include"Controler.hpp"
|
||||
|
||||
Controler::Controler()
|
||||
{
|
||||
// Error_capacity = 10;
|
||||
initVariable();
|
||||
}
|
||||
|
||||
void Controler::initVariable()
|
||||
{
|
||||
ClearValTim(desired_speed);
|
||||
ClearValTim(desired_heading);
|
||||
ClearValTim(desired_depth);
|
||||
ClearValTim(desired_pitch);
|
||||
|
||||
ClearValTim(current_speed);
|
||||
ClearValTim(current_heading);
|
||||
ClearValTim(current_pitch);
|
||||
ClearValTim(current_depth);
|
||||
current_time = 0;
|
||||
start_time = 0;
|
||||
|
||||
ClearValTim(desired_thrust);
|
||||
ClearValTim(desired_rudder);
|
||||
ClearValTim(desired_elevator);
|
||||
|
||||
iterations = 0;
|
||||
start_time = 0.0;
|
||||
|
||||
tardy_helm_thresh = 2.0;
|
||||
tardy_nav_thresh = 2.0;
|
||||
// current_error = 0;
|
||||
// last_error = 0;
|
||||
// Error.clear();
|
||||
}
|
||||
|
||||
bool Controler::setDesSpeed(double spd, double tim)
|
||||
{
|
||||
desired_speed.value = spd;
|
||||
desired_speed.time = tim;
|
||||
return true;
|
||||
}
|
||||
bool Controler::setDesDepth(double dph, double tim)
|
||||
{
|
||||
desired_depth.value = dph;
|
||||
desired_depth.time = tim;
|
||||
return true;
|
||||
}
|
||||
bool Controler::setDesPitch(double pth, double tim)
|
||||
{
|
||||
desired_pitch.value = pth;
|
||||
desired_pitch.time = tim;
|
||||
return true;
|
||||
}
|
||||
bool Controler::setDesHeading(double hdg, double tim)
|
||||
{
|
||||
if(is_rad)
|
||||
hdg = radToDegrees(hdg);
|
||||
|
||||
desired_heading.value = angle180(hdg);
|
||||
desired_heading.time = tim;
|
||||
return true;
|
||||
}
|
||||
bool Controler::setCurSpeed(double spd, double tim)
|
||||
{
|
||||
current_speed.value = spd;
|
||||
current_speed.time = tim;
|
||||
return true;
|
||||
}
|
||||
bool Controler::setCurDepth(double dph, double tim)
|
||||
{
|
||||
current_depth.value = dph;
|
||||
current_depth.time = tim;
|
||||
return true;
|
||||
}
|
||||
bool Controler::setCurPitch(double pth, double tim)
|
||||
{
|
||||
current_pitch.value = pth;
|
||||
current_pitch.time = tim;
|
||||
return true;
|
||||
}
|
||||
bool Controler::setCurHeading(double hdg, double tim)
|
||||
{
|
||||
current_heading.value = hdg;
|
||||
current_heading.time = tim;
|
||||
return true;
|
||||
}
|
||||
bool Controler::setDesiredValues()
|
||||
{
|
||||
iterations++;
|
||||
|
||||
desired_thrust.value = 0;
|
||||
desired_rudder.value = 0;
|
||||
desired_elevator.value = 0;
|
||||
|
||||
//=============================================================
|
||||
// Part 1: Ensure all nav and helm messages have been received
|
||||
// for first time. If not, we simply wait, without declaring an
|
||||
// override. After all messages have been received at least
|
||||
// once, staleness is checked below.
|
||||
//=============================================================
|
||||
runMsg.push_back("desired_heading-time:" + doubleToString(desired_heading.time));
|
||||
runMsg.push_back("desired_speed-time:" + doubleToString(desired_speed.time));
|
||||
runMsg.push_back("current_heading-time:" + doubleToString(current_heading.time));
|
||||
runMsg.push_back("current_speed-time:" + doubleToString(current_speed.time));
|
||||
runMsg.push_back("desired_depth-time:" + doubleToString(desired_depth.time));
|
||||
runMsg.push_back("current_depth-time:" + doubleToString(current_depth.time));
|
||||
runMsg.push_back("current_pitch-time:" + doubleToString(current_pitch.time));
|
||||
// runMsg.push_back("current_depth-time:" + doubleToString(current_depth.time));
|
||||
|
||||
if(has_speedCtrl)
|
||||
{
|
||||
if((desired_heading.time == 0) || (desired_speed.time == 0))
|
||||
return true;
|
||||
}
|
||||
if(has_headCtrl)
|
||||
{
|
||||
if((current_heading.time == 0) || (current_speed.time == 0))
|
||||
return true;
|
||||
}
|
||||
if(has_depthCtrl)
|
||||
{
|
||||
if(desired_depth.time == 0)
|
||||
return true;
|
||||
if((current_depth.time == 0) || (current_pitch.time == 0))
|
||||
return true;
|
||||
}
|
||||
//=============================================================
|
||||
// Part 2: Critical info staleness (if not in simulation mode)
|
||||
//=============================================================
|
||||
//TODO: 验证信息过时操作
|
||||
if(cheakStalensee)
|
||||
{
|
||||
bool is_stale = checkForStaleness();
|
||||
if(is_stale)
|
||||
{
|
||||
has_override = true;
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
//=============================================================
|
||||
// Part 3: Set the actuator values. Note: If desired thrust is
|
||||
// zero others desired values will automatically be zero too.
|
||||
//=============================================================
|
||||
|
||||
// m_desired_thrust = setDesiredThrust();
|
||||
|
||||
// if(m_desired_thrust > 0)
|
||||
// m_desired_rudder = setDesiredRudder();
|
||||
|
||||
// if(m_desired_thrust > 0 && m_depth_control)
|
||||
// m_desired_elevator = setDesiredElevator();
|
||||
return true;
|
||||
}
|
||||
//TODO： 添加操控权判断
|
||||
bool Controler::overrived(string sval)
|
||||
{
|
||||
if(tolower(sval) == "true")
|
||||
{
|
||||
if(has_override == false)
|
||||
addPosting("HAS_CONTROL", "false");
|
||||
has_override = true;
|
||||
}
|
||||
else if(tolower(sval) == "false")
|
||||
{
|
||||
// Upon lifting an override, the timestamps are reset. New values
|
||||
// for all will need to be received before desired_* outputs will
|
||||
// be produced. If we do not reset, it's possible we may
|
||||
// interpret older pubs as being stale but they may have been
|
||||
// paused also during an override.
|
||||
if(has_override == true)
|
||||
{
|
||||
desired_speed.time = 0;
|
||||
desired_heading.time = 0;
|
||||
desired_depth.time = 0;
|
||||
desired_pitch.time = 0;
|
||||
|
||||
current_speed.time = 0;
|
||||
current_heading.time = 0;
|
||||
current_pitch.time = 0;
|
||||
current_depth.time = 0;
|
||||
}
|
||||
has_override = false;
|
||||
addPosting("HAS_CONTROL", "true");
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
bool Controler::checkForStaleness()
|
||||
{
|
||||
bool is_stale = false;
|
||||
|
||||
// =========================================================
|
||||
// Part 1: Check for Helm staleness
|
||||
// =========================================================
|
||||
double hdg_delta = (current_time - desired_heading.time);
|
||||
if(hdg_delta > tardy_helm_thresh) {
|
||||
string staleness = doubleToStringX(hdg_delta,3);
|
||||
addPosting("PID_STALE", "Stale DesHdg:" + staleness);
|
||||
is_stale = true;
|
||||
}
|
||||
double spd_delta = (current_time - desired_speed.time);
|
||||
if(spd_delta > tardy_helm_thresh) {
|
||||
string staleness = doubleToStringX(spd_delta,3);
|
||||
addPosting("PID_STALE", "Stale DesSpd:" + staleness);
|
||||
is_stale = true;
|
||||
}
|
||||
|
||||
// =========================================================
|
||||
// Part 2B: Check for Nav staleness
|
||||
// =========================================================
|
||||
double nav_hdg_delta = (current_time - current_heading.time);
|
||||
if(nav_hdg_delta > tardy_nav_thresh) {
|
||||
string staleness = doubleToStringX(nav_hdg_delta,3);
|
||||
addPosting("PID_STALE", "Stale NavHdg:" + staleness);
|
||||
is_stale = true;
|
||||
}
|
||||
double nav_spd_delta = (current_time - current_speed.time);
|
||||
if(nav_spd_delta > tardy_nav_thresh) {
|
||||
string staleness = doubleToStringX(nav_spd_delta,3);
|
||||
addPosting("PID_STALE", "Stale NavSpd:" + staleness);
|
||||
is_stale = true;
|
||||
}
|
||||
// =========================================================
|
||||
// Part 2C: If depth control, check for Helm Depth staleness
|
||||
// =========================================================
|
||||
if(has_depthCtrl) {
|
||||
double dep_delta = (current_time - desired_depth.time);
|
||||
if(dep_delta > tardy_helm_thresh) {
|
||||
string staleness = doubleToStringX(dep_delta,3);
|
||||
addPosting("PID_STALE", "Stale DesDepth:" + staleness);
|
||||
is_stale = true;
|
||||
}
|
||||
}
|
||||
|
||||
// =========================================================
|
||||
// Part 2D: If depth control, check for Nav Depth staleness
|
||||
// =========================================================
|
||||
if(has_depthCtrl) {
|
||||
double nav_dep_delta = (current_time - current_depth.time);
|
||||
if(nav_dep_delta > tardy_nav_thresh) {
|
||||
string staleness = doubleToStringX(nav_dep_delta,3);
|
||||
addPosting("PID_STALE", "Stale NavDep:" + staleness);
|
||||
is_stale = true;
|
||||
}
|
||||
double nav_pit_delta = (current_time - current_pitch.time);
|
||||
if(nav_pit_delta > tardy_nav_thresh) {
|
||||
string staleness = doubleToStringX(nav_pit_delta,3);
|
||||
addPosting("PID_STALE", "Stale NavPitch:" + staleness);
|
||||
is_stale = true;
|
||||
}
|
||||
}
|
||||
return(is_stale);
|
||||
}
|
||||
|
||||
// bool Controler::setLimit(double max_thrush, double min_thrush, double max_rudder, double min_rubber, double max_elevator, double min_elevator)
|
||||
// {
|
||||
// limit_thrust[0] = min_thrush;
|
||||
// limit_thrust[1] = max_thrush;
|
||||
|
||||
// limit_rudder[0] = min_rubber;
|
||||
// limit_rudder[1] = max_rudder;
|
||||
|
||||
// limit_elevator[0] = min_elevator;
|
||||
// limit_elevator[1] = max_elevator;
|
||||
// }
|
||||
|
||||
bool Controler::Limit(double &a, double max, double min)
|
||||
{
|
||||
a = (a < min) ? min : a;
|
||||
a = (a > max) ? max : a;
|
||||
return true;
|
||||
}
|
||||
|
||||
bool Controler::setCtrl(bool speed, bool heading, bool depth)
|
||||
{
|
||||
has_speedCtrl = speed;
|
||||
has_headCtrl = heading;
|
||||
has_depthCtrl = depth;
|
||||
return true;
|
||||
}
|
||||
|
||||
void Controler::addPosting(std::string var, std::string sval)
|
||||
{
|
||||
VarDataPair pair(var, sval);
|
||||
postings.push_back(pair);
|
||||
}
|
||||
|
||||
void Controler::addPosting(std::string var, double val)
|
||||
{
|
||||
VarDataPair pair(var, val);
|
||||
postings.push_back(pair);
|
||||
}
|
||||
|
||||
double Controler::getFrequency() const
|
||||
{
|
||||
double elapsed = current_time - start_time;
|
||||
|
||||
double frequency = 0;
|
||||
if(elapsed > 0)
|
||||
frequency = ((double)(iterations)) / elapsed;
|
||||
|
||||
return(frequency);
|
||||
}
|
||||
|
||||
bool Controler::ClearValTim(vlaTim &a)
|
||||
{
|
||||
if(&a == NULL)
|
||||
return false;
|
||||
else
|
||||
{
|
||||
a.value = 0;
|
||||
a.time = 0;
|
||||
}
|
||||
|
||||
}
|
||||
//TODO: 控制器参数配置函数
|
||||
list<std::string> Controler::setConfigParams(std::list<std::string>)
|
||||
{
|
||||
list<string> unhandled_params;
|
||||
list<string>::iterator p;
|
||||
// for(p=param_lines.begin(); p!=param_lines.end(); p++) {
|
||||
// string orig = *p;
|
||||
// string line = tolower(orig);
|
||||
// string param = biteStringX(line, '=');
|
||||
// if(param == "speed_factor")
|
||||
// m_config_params.push_front(orig);
|
||||
// else if(param == "simulation")
|
||||
// m_config_params.push_front(orig);
|
||||
// else if(param == "sim_instability")
|
||||
// m_config_params.push_front(orig);
|
||||
// else if(param == "tardy_helm_threshold")
|
||||
// m_config_params.push_front(orig);
|
||||
// else if(param == "tardy_nav_threshold")
|
||||
// m_config_params.push_front(orig);
|
||||
// else if(param == "yaw_pid_kp")
|
||||
// m_config_params.push_front(orig);
|
||||
// else if(param == "yaw_pid_kd")
|
||||
// m_config_params.push_front(orig);
|
||||
// else if(param == "yaw_pid_ki")
|
||||
// m_config_params.push_front(orig);
|
||||
// else if(param == "yaw_pid_integral_limit")
|
||||
// m_config_params.push_front(orig);
|
||||
// else if(param == "yaw_pid_ki_limit")
|
||||
// m_config_params.push_front(orig);
|
||||
// else if(param == "maxrudder")
|
||||
// m_config_params.push_front(orig);
|
||||
// else if(param == "heading_debug")
|
||||
// m_config_params.push_front(orig);
|
||||
|
||||
// else if(param == "speed_pid_kp")
|
||||
// m_config_params.push_front(orig);
|
||||
// else if(param == "speed_pid_kd")
|
||||
// m_config_params.push_front(orig);
|
||||
// else if(param == "speed_pid_ki")
|
||||
// m_config_params.push_front(orig);
|
||||
// else if(param == "speed_pid_integral_limit")
|
||||
// m_config_params.push_front(orig);
|
||||
// else if(param == "maxthrust")
|
||||
// m_config_params.push_front(orig);
|
||||
// else if(param == "speed_debug")
|
||||
// m_config_params.push_front(orig);
|
||||
|
||||
// else if(param == "depth_control")
|
||||
// m_config_params.push_front(orig);
|
||||
// else if(param == "z_to_pitch_pid_kp")
|
||||
// m_config_params.push_front(orig);
|
||||
// else if(param == "z_to_pitch_pid_kd")
|
||||
// m_config_params.push_front(orig);
|
||||
// else if(param == "z_to_pitch_pid_ki")
|
||||
// m_config_params.push_front(orig);
|
||||
// else if(param == "z_to_pitch_pid_integral_limit")
|
||||
// m_config_params.push_front(orig);
|
||||
// else if(param == "maxpitch")
|
||||
// m_config_params.push_front(orig);
|
||||
// else if(param == "depth_debug")
|
||||
// m_config_params.push_front(orig);
|
||||
|
||||
// else if(param == "pitch_pid_kp")
|
||||
// m_config_params.push_front(orig);
|
||||
// else if(param == "pitch_pid_kd")
|
||||
// m_config_params.push_front(orig);
|
||||
// else if(param == "pitch_pid_ki")
|
||||
// m_config_params.push_front(orig);
|
||||
// else if(param == "pitch_pid_integral_limit")
|
||||
// m_config_params.push_front(orig);
|
||||
// else if(param == "maxelevator")
|
||||
// m_config_params.push_front(orig);
|
||||
|
||||
// else
|
||||
// unhandled_params.push_front(orig);
|
||||
// }
|
||||
return(unhandled_params);
|
||||
}
|
||||
|
||||
Json::Value Controler::getConfig(string path)
|
||||
{
|
||||
ifstream ifs;
|
||||
ifs.open(path, ios::in);
|
||||
Json::Reader taskConfigureReader;
|
||||
Json::Value inputJsonValue;
|
||||
taskConfigureReader.parse(ifs, inputJsonValue);
|
||||
ifs.close();
|
||||
return inputJsonValue;
|
||||
}
|
||||
|
||||
void Controler::setCheakStalensee(string s)
|
||||
{
|
||||
if(s == "true")
|
||||
cheakStalensee = true;
|
||||
else
|
||||
cheakStalensee = false;
|
||||
}
|
||||
Reference in New Issue
Block a user