CopterExpress/clover
1
0
Fork 0
mirror of https://github.com/CopterExpress/clover.git synced 2026-05-26 11:43:25 +00:00
Code Issues Packages Projects Releases Wiki Activity

Head target frame forward when yaw forward

Browse Source
This commit is contained in:
Oleg Kalachev
2022-12-24 00:32:42 +03:00
parent 90620d148f
commit d66eda5cd2
1 changed files with 5 additions and 6 deletions
Download Patch File Download Diff File Expand all files Collapse all files

11
clover/src/simple_offboard.cpp
View File

@@ -489,14 +489,13 @@ void publish(const ros::Time stamp)
// compute navigate setpoint
if (setpoint_type == NAVIGATE || setpoint_type == NAVIGATE_GLOBAL) {
position_msg.pose.orientation = tf::createQuaternionMsgFromYaw(yaw_local); // copy yaw
getNavigateSetpoint(stamp, nav_speed, position_msg.pose.position);
if (setpoint_yaw_type == TOWARDS) {
double yaw_towards = atan2(position_msg.pose.position.y - nav_start.pose.position.y,
position_msg.pose.position.x - nav_start.pose.position.x);
position_msg.pose.orientation = tf::createQuaternionMsgFromYaw(yaw_towards);
yaw_local = atan2(position_msg.pose.position.y - nav_start.pose.position.y,
position_msg.pose.position.x - nav_start.pose.position.x);
}
position_msg.pose.orientation = tf::createQuaternionMsgFromYaw(yaw_local);
getNavigateSetpoint(stamp, nav_speed, position_msg.pose.position);
}
if (setpoint_type == POSITION) {