Skip to content

Commit

Permalink
add pos control
Browse files Browse the repository at this point in the history
  • Loading branch information
@tmori
tmori committed Nov 26, 2024
1 parent f55bc04 commit e738677
Show file tree
Hide file tree
Showing 3 changed files with 67 additions and 13 deletions.
44 changes: 43 additions & 1 deletion drone_control/common/include/drone_radio_controller.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -15,15 +15,23 @@ class DroneRadioController {
double delta_time;
double alt_control_cycle;
double head_control_cycle;
double pos_control_cycle;
double alt_delta_value_m;
double pos_delta_value_m;
double yaw_delta_value_deg;
double pos_max_spd;
bool pos_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 @@ -51,6 +59,16 @@ class DroneRadioController {
delta_time = loader.getParameter("SIMULATION_DELTA_TIME");
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;
}
else {
std::cout << "Position 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");
pos_max_spd = loader.getParameter("PID_POS_MAX_SPD");
Expand Down Expand Up @@ -79,7 +97,31 @@ class DroneRadioController {
alt_time += this->delta_time;
return r_altitude;
}

// almost same value of double data
bool is_same_value(double a, double b) {
return fabs(a - b) < 0.0001;
}
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);
}
bool is_pos_control_enable()
{
return pos_control_enable;
}
// Function to update target yaw
double update_target_yaw(double v) {
if (yaw_time >= head_control_cycle) {
Expand Down
18 changes: 10 additions & 8 deletions drone_control/config/param-api-mixer.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -7,10 +7,10 @@ GRAVITY 9.81
PID_ALT_CONTROL_CYCLE 0.0
PID_ALT_MAX_POWER 9.81
PID_ALT_THROTTLE_GAIN 1.0
PID_ALT_MAX_SPD 5.0
PID_ALT_MAX_SPD 10.0

## 高度制御のPIDパラメータ
PID_ALT_Kp 5.0
PID_ALT_Kp 10.0
PID_ALT_Ki 0.0
PID_ALT_Kd 5.0
PID_ALT_SPD_Kp 5.0
Expand All @@ -23,12 +23,12 @@ SPD_CONTROL_CYCLE 0.0
PID_POS_MAX_SPD 20.0

## 水平位置制御のPIDパラメータ
PID_POS_X_Kp 5.0
PID_POS_X_Kp 6.0
PID_POS_X_Ki 0.0
PID_POS_X_Kd 10.0
PID_POS_Y_Kp 5.0
PID_POS_X_Kd 3.0
PID_POS_Y_Kp 6.0
PID_POS_Y_Ki 0.0
PID_POS_Y_Kd 10.0
PID_POS_Y_Kd 3.0

# 水平速度制御のPIDパラメータ
PID_POS_VX_Kp 10.0
Expand Down Expand Up @@ -76,5 +76,7 @@ PID_YAW_RATE_Ki 0.0
PID_YAW_RATE_Kd 0.1

# ラジオコントロール
YAW_DELTA_VALUE_DEG 0.05
ALT_DELTA_VALUE_M 0.002
YAW_DELTA_VALUE_DEG 0.07
ALT_DELTA_VALUE_M 0.003
POS_DELTA_VALUE_M 0.01
POS_CONTROL_ENABLE 0
18 changes: 14 additions & 4 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 @@ -43,8 +43,6 @@ mi_drone_control_out_t hako_module_drone_controller_impl_run(mi_drone_control_in
ctrl->save_for_initial_position(pos.z);
double target_yaw = ctrl->update_target_yaw(in->target.direction_velocity.r);
double target_pos_z = ctrl->update_target_altitude(-in->target.throttle.power);
double target_vx = -in->target.attitude.pitch * ctrl->get_pos_max_spd();
double target_vy = in->target.attitude.roll * ctrl->get_pos_max_spd();
/*
* 高度制御
*/
Expand All @@ -58,8 +56,20 @@ mi_drone_control_out_t hako_module_drone_controller_impl_run(mi_drone_control_in
/*
* 水平制御
*/
DroneVelInputType spd_in(velocity, target_vx, target_vy);
DronePosOutputType pos_out = ctrl->pos->run_spd(spd_in);
DronePosOutputType pos_out = {};
if (ctrl->is_pos_control_enable() == false) {
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);
}
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);
}
/*
* 姿勢角度制御
*/
Expand Down

0 comments on commit e738677

Please sign in to comment.