sem/5g_drone_ROS2
1
0
Fork 0
Code Issues 1 Pull Requests 1 Actions Packages Projects Releases Wiki Activity

remove checking of nan

Browse Source
This commit is contained in:
Sem van der Hoeven
2023-05-15 15:47:45 +02:00
parent da5ee7fc88
commit 634a00a837
1 changed files with 20 additions and 20 deletions
Download Patch File Download Diff File Expand all files Collapse all files

40
src/px4_connection/src/px4_controller.cpp
View File

@@ -312,32 +312,32 @@ private:
{ {
auto msg = px4_msgs::msg::TrajectorySetpoint(); auto msg = px4_msgs::msg::TrajectorySetpoint();
if (trajectory_valid_x) // if (trajectory_valid_x)
{ // {
msg.position[0] = position[0]; msg.position[0] = position[0];
trajectory_valid_x = false; trajectory_valid_x = false;
} // }
if (trajectory_valid_y) // if (trajectory_valid_y)
{ // {
msg.position[1] = position[1]; msg.position[1] = position[1];
trajectory_valid_y = false; trajectory_valid_y = false;
} // }
if (trajectory_valid_z) // if (trajectory_valid_z)
{ // {
msg.position[2] = position[2]; msg.position[2] = position[2];
trajectory_valid_z = false; trajectory_valid_z = false;
} // }
publish_trajectory_setpoint(msg); publish_trajectory_setpoint(msg);
} }
void publish_trajectory_setpoint(px4_msgs::msg::TrajectorySetpoint msg) void publish_trajectory_setpoint(px4_msgs::msg::TrajectorySetpoint msg)
{ {
if (trajectory_valid_yaw) // if (trajectory_valid_yaw)
{ // {
msg.yaw = last_angle; msg.yaw = last_angle;
trajectory_valid_yaw = false; trajectory_valid_yaw = false;
} // }
msg.yawspeed = DEFAULT_YAW_SPEED; msg.yawspeed = DEFAULT_YAW_SPEED;
msg.timestamp = this->get_clock()->now().nanoseconds() / 1000; msg.timestamp = this->get_clock()->now().nanoseconds() / 1000;
trajectory_setpoint_publisher->publish(msg); trajectory_setpoint_publisher->publish(msg);