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

simple_offboard: fix handling set_attitude service

Browse Source
This commit is contained in:
Oleg Kalachev
2022-06-02 09:48:29 +03:00
parent c4cd157f7c
commit cbdc93d1c3
1 changed files with 7 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files

9
clover/src/simple_offboard.cpp
View File

@@ -691,7 +691,7 @@ bool serve(enum setpoint_type_t sp_type, float x, float y, float z, float vx, fl
// }
if (sp_type == POSITION || sp_type == NAVIGATE || sp_type == NAVIGATE_GLOBAL || sp_type == VELOCITY || sp_type == ATTITUDE) {
// destination point and/or yaw
// destination point and/or attitude
PoseStamped ps;
ps.header.frame_id = frame_id;
ps.header.stamp = stamp;
@@ -700,7 +700,12 @@ bool serve(enum setpoint_type_t sp_type, float x, float y, float z, float vx, fl
ps.pose.position.z = z;
ps.pose.orientation.w = 1.0; // Ensure quaternion is always valid
if (std::isnan(yaw)) {
if (sp_type == ATTITUDE) {
ps.pose.position.x = 0;
ps.pose.position.y = 0;
ps.pose.position.z = 0;
ps.pose.orientation = tf::createQuaternionMsgFromRollPitchYaw(roll, pitch, yaw);
} else if (std::isnan(yaw)) {
setpoint_yaw_type = YAW_RATE;
setpoint_yaw_rate = yaw_rate;
} else if (std::isinf(yaw) && yaw > 0) {