From 7f3ee3f5138a461d69ac6ca69877c09357036403 Mon Sep 17 00:00:00 2001 From: Takashi Mori Date: Tue, 26 Nov 2024 15:10:46 +0900 Subject: [PATCH] parameters are tuned --- .../common/include/drone_angle_controller.hpp | 41 ++++++++------- .../common/include/drone_radio_controller.hpp | 52 +++++++------------ drone_control/config/param-api-mixer.txt | 15 +++--- .../hako_module_drone_controller_impl.cpp | 37 +++++++------ 4 files changed, 69 insertions(+), 76 deletions(-) diff --git a/drone_control/common/include/drone_angle_controller.hpp b/drone_control/common/include/drone_angle_controller.hpp index b4bd3694..f2afd9ed 100644 --- a/drone_control/common/include/drone_angle_controller.hpp +++ b/drone_control/common/include/drone_angle_controller.hpp @@ -91,27 +91,7 @@ class DroneAngleController { return out; } - DroneAngleOutputType run_rate(DroneAngleRateInputType& in) { - DroneAngleOutputType out = rate_prev_out; - if (angular_rate_simulation_time >= angular_rate_control_cycle) { - //roll rate control - out.p = roll_rate_control->calculate(in.target_roll_rate, NORMALIZE_RADIAN(in.angular_rate.p)); - out.p = flight_controller_get_limit_value(out.p, 0, -roll_torque_max, roll_torque_max); - - //pitch rate control - out.q = pitch_rate_control->calculate(in.target_pitch_rate, NORMALIZE_RADIAN(in.angular_rate.q)); - out.q = flight_controller_get_limit_value(out.q, 0, -pitch_torque_max, pitch_torque_max); - //yaw rate control - out.r = roll_rate_control->calculate(in.target_yaw_rate, NORMALIZE_RADIAN(in.angular_rate.r)); - out.r = flight_controller_get_limit_value(out.r, 0, -yaw_torque_max, yaw_torque_max); - - angular_rate_simulation_time = 0; - rate_prev_out = out; - } - this->angular_rate_simulation_time += this->delta_time; - return out; - } public: DroneAngleController(const HakoControllerParamLoader& loader) { delta_time = loader.getParameter("SIMULATION_DELTA_TIME"); @@ -162,5 +142,26 @@ class DroneAngleController { DroneAngleRateInputType vel = run_angle(in); return run_rate(vel); } + DroneAngleOutputType run_rate(DroneAngleRateInputType& in) { + DroneAngleOutputType out = rate_prev_out; + if (angular_rate_simulation_time >= angular_rate_control_cycle) { + //roll rate control + out.p = roll_rate_control->calculate(in.target_roll_rate, NORMALIZE_RADIAN(in.angular_rate.p)); + out.p = flight_controller_get_limit_value(out.p, 0, -roll_torque_max, roll_torque_max); + + //pitch rate control + out.q = pitch_rate_control->calculate(in.target_pitch_rate, NORMALIZE_RADIAN(in.angular_rate.q)); + out.q = flight_controller_get_limit_value(out.q, 0, -pitch_torque_max, pitch_torque_max); + + //yaw rate control + out.r = roll_rate_control->calculate(in.target_yaw_rate, NORMALIZE_RADIAN(in.angular_rate.r)); + out.r = flight_controller_get_limit_value(out.r, 0, -yaw_torque_max, yaw_torque_max); + + angular_rate_simulation_time = 0; + rate_prev_out = out; + } + this->angular_rate_simulation_time += this->delta_time; + return out; + } }; #endif /* _DRONE_ANGLE_CONTROLLER_HPP_ */ \ No newline at end of file diff --git a/drone_control/common/include/drone_radio_controller.hpp b/drone_control/common/include/drone_radio_controller.hpp index 116142a7..c229095a 100644 --- a/drone_control/common/include/drone_radio_controller.hpp +++ b/drone_control/common/include/drone_radio_controller.hpp @@ -17,21 +17,17 @@ class DroneRadioController { double head_control_cycle; double pos_control_cycle; double alt_delta_value_m; - double pos_delta_value_m; double yaw_delta_value_deg; + double max_roll_deg; + double max_pitch_deg; double pos_max_spd; - bool pos_control_enable = false; + bool angle_control_enable = false; //altitude control bool r_altitude_initialized = false; double r_altitude = 0; double alt_time = 0; - //position control - double r_position_x = 0; - double r_position_y = 0; - double pos_time = 0; - //yaw control double yaw_time = 0; double r_yaw = 0; @@ -60,14 +56,15 @@ class DroneRadioController { head_control_cycle = loader.getParameter("HEAD_CONTROL_CYCLE"); alt_control_cycle = loader.getParameter("PID_ALT_CONTROL_CYCLE"); pos_control_cycle = loader.getParameter("POS_CONTROL_CYCLE"); - pos_delta_value_m = loader.getParameter("POS_DELTA_VALUE_M"); - int p_ctrl_enable = loader.getParameter("POS_CONTROL_ENABLE"); - if (p_ctrl_enable == 1) { - std::cout << "Position control is enabled" << std::endl; - pos_control_enable = true; + max_roll_deg = loader.getParameter("PID_POS_MAX_ROLL"); + max_pitch_deg = loader.getParameter("PID_POS_MAX_PITCH"); + int a_ctrl_enable = loader.getParameter("ANGLE_CONTROL_ENABLE"); + if (a_ctrl_enable == 1) { + std::cout << "Angle control is enabled" << std::endl; + angle_control_enable = true; } else { - std::cout << "Position control is disabled" << std::endl; + std::cout << "Angle control is disabled" << std::endl; } alt_delta_value_m = loader.getParameter("ALT_DELTA_VALUE_M"); yaw_delta_value_deg = loader.getParameter("YAW_DELTA_VALUE_DEG"); @@ -98,29 +95,18 @@ class DroneRadioController { return r_altitude; } // almost same value of double data - bool is_same_value(double a, double b) { - return fabs(a - b) < 0.0001; + + bool is_angle_control_enable() + { + return angle_control_enable; } - std::pair update_target_position(double x, double y, double yaw_rad, double current_x, double current_y) { - if (pos_time >= pos_control_cycle) { - pos_time = 0; - double delta_x = x * cos(yaw_rad) - y * sin(yaw_rad); - double delta_y = x * sin(yaw_rad) + y * cos(yaw_rad); - r_position_x += delta_x * pos_delta_value_m; - r_position_y += delta_y * pos_delta_value_m; - if (is_same_value(x, 0.0) && is_same_value(y, 0.0)) { - r_position_x = current_x; - r_position_y = current_y; - } - //std::cout << "yaw_rad: " << yaw_rad << " delta_x: " << delta_x << " delta_y: " << delta_y << std::endl; - //std::cout << "x: " << x << " y: " << y << " r_position_x: " << r_position_x << " r_position_y: " << r_position_y << std::endl; - } - pos_time += this->delta_time; - return std::make_pair(r_position_x, r_position_y); + double get_max_roll_deg() + { + return max_roll_deg; } - bool is_pos_control_enable() + double get_max_pitch_deg() { - return pos_control_enable; + return max_pitch_deg; } // Function to update target yaw double update_target_yaw(double v) { diff --git a/drone_control/config/param-api-mixer.txt b/drone_control/config/param-api-mixer.txt index d5688ede..1c2af1a3 100644 --- a/drone_control/config/param-api-mixer.txt +++ b/drone_control/config/param-api-mixer.txt @@ -42,12 +42,12 @@ PID_POS_VY_Kd 1.0 HEAD_CONTROL_CYCLE 0.0 ANGULAR_CONTROL_CYCLE 0.0 ANGULAR_RATE_CONTROL_CYCLE 0.0 -PID_POS_MAX_ROLL 20.0 -PID_POS_MAX_PITCH 20.0 -PID_ROLL_RPM_MAX 20.0 -PID_PITCH_RPM_MAX 20.0 -PID_ROLL_TORQUE_MAX 10.0 -PID_PITCH_TORQUE_MAX 10.0 +PID_POS_MAX_ROLL 30.0 +PID_POS_MAX_PITCH 30.0 +PID_ROLL_RPM_MAX 200.0 +PID_PITCH_RPM_MAX 200.0 +PID_ROLL_TORQUE_MAX 100.0 +PID_PITCH_TORQUE_MAX 100.0 PID_YAW_TORQUE_MAX 10.0 ## ロール角度とロール角速度制御のPIDパラメータ @@ -78,5 +78,4 @@ PID_YAW_RATE_Kd 0.1 # ラジオコントロール YAW_DELTA_VALUE_DEG 0.07 ALT_DELTA_VALUE_M 0.003 -POS_DELTA_VALUE_M 0.01 -POS_CONTROL_ENABLE 0 +ANGLE_CONTROL_ENABLE 0 \ No newline at end of file diff --git a/drone_control/workspace/RadioController/hako_module_drone_controller_impl.cpp b/drone_control/workspace/RadioController/hako_module_drone_controller_impl.cpp index 3c99f7ea..4a0165e1 100644 --- a/drone_control/workspace/RadioController/hako_module_drone_controller_impl.cpp +++ b/drone_control/workspace/RadioController/hako_module_drone_controller_impl.cpp @@ -53,28 +53,35 @@ mi_drone_control_out_t hako_module_drone_controller_impl_run(mi_drone_control_in */ DroneHeadingControlInputType head_in(euler, target_yaw); DroneHeadingControlOutputType head_out = ctrl->head->run(head_in); - /* - * 水平制御 - */ - DronePosOutputType pos_out = {}; - if (ctrl->is_pos_control_enable() == false) { + + DroneAngleOutputType angle_out = {}; + if (ctrl->is_angle_control_enable() == false) { + /* + * 水平制御 + */ + DronePosOutputType pos_out = {}; double target_vx = -in->target.attitude.pitch * ctrl->get_pos_max_spd(); double target_vy = in->target.attitude.roll * ctrl->get_pos_max_spd(); DroneVelInputType spd_in(velocity, target_vx, target_vy); pos_out = ctrl->pos->run_spd(spd_in); + /* + * 姿勢角度制御 + */ + DroneAngleInputType angle_in(euler, angular_rate, pos_out.target_roll, pos_out.target_pitch, head_out.target_yaw_rate); + angle_out = ctrl->angle->run(angle_in); } else { - std::pair target_pos = ctrl->update_target_position(-in->target.attitude.pitch, in->target.attitude.roll, euler.z, pos.x, pos.y); - double target_x = target_pos.first; - double target_y = target_pos.second; - DronePosInputType pos_in(pos, velocity, euler, target_x, target_y, ctrl->get_pos_max_spd()); - pos_out = ctrl->pos->run(pos_in); + /* + * 姿勢角速度制御 + */ + //std::cout << "roll: " << in->target.attitude.roll << " pitch: " << in->target.attitude.pitch << " yaw: " << head_out.target_yaw_rate << std::endl; + DroneAngleInputType angle_in(euler, angular_rate, + in->target.attitude.roll * ctrl->get_max_roll_deg(), + in->target.attitude.pitch * ctrl->get_max_pitch_deg(), + head_out.target_yaw_rate); + angle_out = ctrl->angle->run(angle_in); } - /* - * 姿勢角度制御 - */ - DroneAngleInputType angle_in(euler, angular_rate, pos_out.target_roll, pos_out.target_pitch, head_out.target_yaw_rate); - DroneAngleOutputType angle_out = ctrl->angle->run(angle_in); + /* * 出力 */