424 lines
13 KiB
C++
Executable File
424 lines
13 KiB
C++
Executable File
/*
|
||
* @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;
|
||
} |