Skip to content

Commit

Permalink
parameters are tuned
Browse files Browse the repository at this point in the history
  • Loading branch information
@tmori
tmori committed Nov 26, 2024
1 parent e738677 commit 7f3ee3f
Show file tree
Hide file tree
Showing 4 changed files with 69 additions and 76 deletions.
41 changes: 21 additions & 20 deletions drone_control/common/include/drone_angle_controller.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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");
Expand Down Expand Up @@ -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_ */
52 changes: 19 additions & 33 deletions drone_control/common/include/drone_radio_controller.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down Expand Up @@ -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");
Expand Down Expand Up @@ -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<double, double> 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) {
Expand Down
15 changes: 7 additions & 8 deletions drone_control/config/param-api-mixer.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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パラメータ
Expand Down Expand Up @@ -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
37 changes: 22 additions & 15 deletions drone_control/workspace/RadioController/hako_module_drone_controller_impl.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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<double, double> 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);

/*
* 出力
*/
Expand Down

0 comments on commit 7f3ee3f

Please sign in to comment.