/* * @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 Controler::setConfigParams(std::list) { list unhandled_params; list::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; }