Skip to content

Commit

Permalink
control: rename thrust ned to frd
Browse files Browse the repository at this point in the history
  • Loading branch information
GuillaumeLaine authored and bkueng committed Jan 30, 2024
1 parent dd1e285 commit 70c7ed1
Show file tree
Hide file tree
Showing 4 changed files with 10 additions and 10 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -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();
Expand All @@ -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);
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -19,17 +19,17 @@ 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();

px4_msgs::msg::VehicleRatesSetpoint sp{};
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);
}
Expand Down

0 comments on commit 70c7ed1

Please sign in to comment.