hongshaorou/YOPO
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Fix control issues during high-speed flight

Browse Source
This commit is contained in:
TJU-Lu 2025-11-19 13:39:15 +08:00
parent 315063477d
commit 4aeb9de9df
3 changed files with 25 additions and 15 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
Controller/src/so3_control/include/so3_control/NetworkControl.h
View File

@ -110,11 +110,11 @@ private:
void recordLog(Eigen::Vector3d &cur_v, Eigen::Vector3d &cur_a, Eigen::Vector3d &des_a, Eigen::Vector3d &dis_a, double cur_yaw, double des_yaw);
Eigen::Vector3d publishHoverSO3Command(Eigen::Vector3d des_pos_, Eigen::Vector3d des_vel_, Eigen::Vector3d des_acc_, double des_yaw_, double des_yaw_dot_);
Eigen::Vector3d publishHoverSO3Command(Eigen::Vector3d des_pos, Eigen::Vector3d des_vel, Eigen::Vector3d des_acc, double des_yaw, double des_yaw_dot);
void get_Q_from_ACC(const Eigen::Vector3d &ref_acc, double ref_yaw, Eigen::Quaterniond &quat_des, Eigen::Vector3d &force_des);
Eigen::Vector3d get_Q_from_ACC(const Eigen::Vector3d &ref_acc, double ref_yaw, Eigen::Quaterniond &quat_des, Eigen::Vector3d &force_des);
void pub_SO3_command(Eigen::Vector3d ref_acc, double ref_yaw, double cur_yaw);
Eigen::Vector3d pub_SO3_command(Eigen::Vector3d ref_acc, double ref_yaw, double cur_yaw);
void limite_acc(Eigen::Vector3d &acc);

29
Controller/src/so3_control/src/NetworkControl.cpp
View File

@ -150,7 +150,7 @@ Eigen::Vector3d NetworkControl::publishHoverSO3Command(Eigen::Vector3d des_pos,
return att_acc;
}
void NetworkControl::get_Q_from_ACC(const Eigen::Vector3d &ref_acc, double ref_yaw, Eigen::Quaterniond &quat_des, Eigen::Vector3d &force_des)
Eigen::Vector3d NetworkControl::get_Q_from_ACC(const Eigen::Vector3d &ref_acc, double ref_yaw, Eigen::Quaterniond &quat_des, Eigen::Vector3d &force_des)
{
Eigen::Vector3d force_ = mass_ * ONE_G * Eigen::Vector3d(0, 0, 1);
force_.noalias() += mass_ * ref_acc;
@ -187,14 +187,16 @@ void NetworkControl::get_Q_from_ACC(const Eigen::Vector3d &ref_acc, double ref_y
quat_des = Eigen::Quaterniond(R);
force_des = force_;
Eigen::Vector3d acc_actual = force_des / mass_ - Eigen::Vector3d(0, 0, ONE_G);
return acc_actual;
}
// 世界系的期望加速度：ref_acc（加上g）、期望yaw：ref_yaw
void NetworkControl::pub_SO3_command(Eigen::Vector3d ref_acc, double ref_yaw, double cur_yaw)
Eigen::Vector3d NetworkControl::pub_SO3_command(Eigen::Vector3d ref_acc, double ref_yaw, double cur_yaw)
{
Eigen::Vector3d force;
Eigen::Quaterniond quat_des;
get_Q_from_ACC(ref_acc, ref_yaw, quat_des, force);
Eigen::Vector3d acc_actual = get_Q_from_ACC(ref_acc, ref_yaw, quat_des, force);
quadrotor_msgs::SO3Command::Ptr so3_command(new quadrotor_msgs::SO3Command);
so3_command->header.stamp = ros::Time::now();
so3_command->force.x = force(0);
@ -217,13 +219,16 @@ void NetworkControl::pub_SO3_command(Eigen::Vector3d ref_acc, double ref_yaw, do
double thrust_norm = force.norm() / (mass_ * ONE_G) * hover_thrust_;
mavros_interface_.pub_att_thrust_cmd(quat_des, thrust_norm);
last_thrust_ = thrust_norm;
return acc_actual;
}
void NetworkControl::limite_acc(Eigen::Vector3d &acc){
return; // limited if needed
acc[0] = std::max(-8.0, std::min(acc[0], 8.0));
acc[1] = std::max(-8.0, std::min(acc[1], 8.0));
acc[2] = std::max(-4.0, std::min(acc[2], 4.0));
return;
double max_norm = 10.0;
double norm = acc.norm();
if (norm > max_norm) {
acc = acc / norm * max_norm;
}
}
void NetworkControl::network_cmd_callback(const quadrotor_msgs::PositionCommand::ConstPtr &cmd)
@ -239,6 +244,8 @@ void NetworkControl::network_cmd_callback(const quadrotor_msgs::PositionCommand:
position_cmd_init_ = true;
des_pos_ = Eigen::Vector3d(cmd->position.x, cmd->position.y, cmd->position.z);
des_vel_ = Eigen::Vector3d(cmd->velocity.x, cmd->velocity.y, cmd->velocity.z);
Eigen::Vector3d des_acc = Eigen::Vector3d(cmd->acceleration.x, cmd->acceleration.y, cmd->acceleration.z);
limite_acc(des_acc);
@ -255,18 +262,15 @@ void NetworkControl::network_cmd_callback(const quadrotor_msgs::PositionCommand:
att_acc = des_acc - dis_acc_;
else
att_acc = des_acc;
pub_SO3_command(att_acc, des_yaw, cur_yaw_);
att_acc = pub_SO3_command(att_acc, des_yaw, cur_yaw_);
// std::cout<<"acc: "<<des_acc.transpose()<<" yaw:"<<des_yaw<<std::endl;
if (record_log_)
recordLog(cur_vel_, cur_acc_, des_acc, dis_acc_, cur_yaw_, des_yaw);
}
else
{
Eigen::Vector3d des_pos = Eigen::Vector3d(cmd->position.x, cmd->position.y, cmd->position.z);
Eigen::Vector3d des_vel = Eigen::Vector3d(cmd->velocity.x, cmd->velocity.y, cmd->velocity.z);
double des_yaw = cmd->yaw;
double des_yaw_dot = cmd->yaw_dot;
att_acc = publishHoverSO3Command(des_pos, des_vel, des_acc, des_yaw, des_yaw_dot);
att_acc = publishHoverSO3Command(des_pos_, des_vel_, des_acc, des_yaw, des_yaw_dot);
if (record_log_)
recordLog(cur_vel_, cur_acc_, att_acc, dis_acc_, cur_yaw_, des_yaw);
}
@ -345,6 +349,7 @@ void NetworkControl::takeoff_land_thread(quadrotor_msgs::SetTakeoffLand::Request
float takeoff_altitude = req.takeoff_altitude;
des_pos_ = cur_pos_;
des_pos_(2) -= 0.2;
des_vel_ = Eigen::Vector3d(0, 0, 0);
des_yaw_ = cur_yaw_;
mutex_.unlock();
ref_valid_ = true;

5
Controller/src/so3_quadrotor_simulator/src/quadrotor_simulator_so3.cpp
View File

@ -282,6 +282,7 @@ main(int argc, char** argv)
ros::Time next_odom_pub_time = ros::Time::now();
while (n.ok())
{
ros::Time t_loop_start = ros::Time::now();
ros::spinOnce();
auto last = control;
@ -317,6 +318,10 @@ main(int argc, char** argv)
}
}
double loop_time = (ros::Time::now() - t_loop_start).toSec();
if (loop_time > dt)
ROS_WARN("Simulator loop overrun: loop_time = %.2f ms > expected %.2f ms!", loop_time * 1000.0, dt * 1000.0);
r.sleep();
}