diff --git a/px4_ros2_cpp/include/px4_ros2/control/setpoint_types/experimental/attitude.hpp b/px4_ros2_cpp/include/px4_ros2/control/setpoint_types/experimental/attitude.hpp index 5ff57dc..0c0dd17 100644 --- a/px4_ros2_cpp/include/px4_ros2/control/setpoint_types/experimental/attitude.hpp +++ b/px4_ros2_cpp/include/px4_ros2/control/setpoint_types/experimental/attitude.hpp @@ -28,7 +28,7 @@ class AttitudeSetpointType : public SetpointBase void update( const Eigen::Quaternionf & attidude_setpoint, - const Eigen::Vector3f & thrust_setpoint_ned, float yaw_sp_move_rate_rad_s = 0.F); + const Eigen::Vector3f & thrust_setpoint_frd, float yaw_sp_move_rate_rad_s = 0.F); private: rclcpp::Node & _node; diff --git a/px4_ros2_cpp/include/px4_ros2/control/setpoint_types/experimental/rates.hpp b/px4_ros2_cpp/include/px4_ros2/control/setpoint_types/experimental/rates.hpp index 061b9bb..6b22c7b 100644 --- a/px4_ros2_cpp/include/px4_ros2/control/setpoint_types/experimental/rates.hpp +++ b/px4_ros2_cpp/include/px4_ros2/control/setpoint_types/experimental/rates.hpp @@ -28,7 +28,7 @@ class RatesSetpointType : public SetpointBase void update( const Eigen::Vector3f & rate_setpoints_ned_rad, - const Eigen::Vector3f & thrust_setpoint_ned); + const Eigen::Vector3f & thrust_setpoint_frd); private: rclcpp::Node & _node; diff --git a/px4_ros2_cpp/src/control/setpoint_types/experimental/attitude.cpp b/px4_ros2_cpp/src/control/setpoint_types/experimental/attitude.cpp index 0612e85..5b25654 100644 --- a/px4_ros2_cpp/src/control/setpoint_types/experimental/attitude.cpp +++ b/px4_ros2_cpp/src/control/setpoint_types/experimental/attitude.cpp @@ -19,7 +19,7 @@ AttitudeSetpointType::AttitudeSetpointType(Context & context) void AttitudeSetpointType::update( const Eigen::Quaternionf & attidude_setpoint, - const Eigen::Vector3f & thrust_setpoint_ned, + const Eigen::Vector3f & thrust_setpoint_frd, float yaw_sp_move_rate_rad_s) { onUpdate(); @@ -29,9 +29,9 @@ void AttitudeSetpointType::update( sp.q_d[1] = attidude_setpoint.x(); sp.q_d[2] = attidude_setpoint.y(); sp.q_d[3] = attidude_setpoint.z(); - sp.thrust_body[0] = thrust_setpoint_ned(0); - sp.thrust_body[1] = thrust_setpoint_ned(1); - sp.thrust_body[2] = thrust_setpoint_ned(2); + sp.thrust_body[0] = thrust_setpoint_frd(0); + sp.thrust_body[1] = thrust_setpoint_frd(1); + sp.thrust_body[2] = thrust_setpoint_frd(2); sp.yaw_sp_move_rate = yaw_sp_move_rate_rad_s; sp.timestamp = _node.get_clock()->now().nanoseconds() / 1000; _vehicle_attitude_setpoint_pub->publish(sp); diff --git a/px4_ros2_cpp/src/control/setpoint_types/experimental/rates.cpp b/px4_ros2_cpp/src/control/setpoint_types/experimental/rates.cpp index 29bbd57..312b368 100644 --- a/px4_ros2_cpp/src/control/setpoint_types/experimental/rates.cpp +++ b/px4_ros2_cpp/src/control/setpoint_types/experimental/rates.cpp @@ -19,7 +19,7 @@ RatesSetpointType::RatesSetpointType(Context & context) void RatesSetpointType::update( const Eigen::Vector3f & rate_setpoints_ned_rad, - const Eigen::Vector3f & thrust_setpoint_ned) + const Eigen::Vector3f & thrust_setpoint_frd) { onUpdate(); @@ -27,9 +27,9 @@ void RatesSetpointType::update( sp.roll = rate_setpoints_ned_rad(0); sp.pitch = rate_setpoints_ned_rad(1); sp.yaw = rate_setpoints_ned_rad(2); - sp.thrust_body[0] = thrust_setpoint_ned(0); - sp.thrust_body[1] = thrust_setpoint_ned(1); - sp.thrust_body[2] = thrust_setpoint_ned(2); + sp.thrust_body[0] = thrust_setpoint_frd(0); + sp.thrust_body[1] = thrust_setpoint_frd(1); + sp.thrust_body[2] = thrust_setpoint_frd(2); sp.timestamp = _node.get_clock()->now().nanoseconds() / 1000; _vehicle_rates_setpoint_pub->publish(sp); }