8#include <px4_msgs/msg/fixed_wing_lateral_setpoint.hpp>
9#include <px4_msgs/msg/fixed_wing_longitudinal_setpoint.hpp>
10#include <px4_msgs/msg/lateral_control_configuration.hpp>
11#include <px4_msgs/msg/longitudinal_control_configuration.hpp>
15#include <px4_ros2/common/setpoint_base.hpp>
23struct FwLateralLongitudinalSetpoint;
24struct FwControlConfiguration;
82 float altitude_amsl_sp,
float course_sp,
83 std::optional<float> equivalent_airspeed_sp = {},
84 std::optional<float> lateral_acceleration_sp = {});
94 float height_rate_sp,
float course_sp,
95 std::optional<float> equivalent_airspeed_sp = {},
96 std::optional<float> lateral_acceleration_sp = {});
98 float desiredUpdateRateHz()
override {
return 30.f;}
101 rclcpp::Node & _node;
102 rclcpp::Publisher<px4_msgs::msg::FixedWingLateralSetpoint>::SharedPtr
104 rclcpp::Publisher<px4_msgs::msg::FixedWingLongitudinalSetpoint>::SharedPtr
105 _fw_longitudinal_sp_pub;
107 rclcpp::Publisher<px4_msgs::msg::LateralControlConfiguration>::SharedPtr
108 _lateral_control_configuration_pub;
109 rclcpp::Publisher<px4_msgs::msg::LongitudinalControlConfiguration>::SharedPtr
110 _longitudinal_control_configuration_pub;
115 std::optional<float> course;
116 std::optional<float> airspeed_direction;
117 std::optional<float> lateral_acceleration;
118 std::optional<float> altitude_msl;
119 std::optional<float> height_rate;
120 std::optional<float> equivalent_airspeed;
130 airspeed_direction = airspeed_direction_sp;
136 lateral_acceleration = lateral_acceleration_sp;
142 altitude_msl = altitude_sp;
148 height_rate = height_rate_sp;
154 equivalent_airspeed = equivalent_airspeed_sp;
161 std::optional<float> min_pitch;
162 std::optional<float> max_pitch;
163 std::optional<float> min_throttle;
164 std::optional<float> max_throttle;
165 std::optional<float> max_lateral_acceleration;
166 std::optional<float> target_climb_rate;
167 std::optional<float> target_sink_rate;
172 min_pitch = min_pitch_sp;
173 max_pitch = max_pitch_sp;
181 min_throttle = min_throttle_sp;
182 max_throttle = max_throttle_sp;
190 max_lateral_acceleration = max_lateral_acceleration_sp;
197 target_sink_rate = target_sink_rate_sp;
204 target_climb_rate = target_climb_rate_sp;
Definition context.hpp:20
Setpoint type for fixedwing control.
Definition fixedwing_lateral_longitudinal.hpp:30
void updateWithAltitude(float altitude_amsl_sp, float course_sp, std::optional< float > equivalent_airspeed_sp={}, std::optional< float > lateral_acceleration_sp={})
Update the setpoint with simplest set of inputs: Altitude and Course. Lateral acceleration setpoints ...
void update(const FwLateralLongitudinalSetpoint &setpoint)
Update the setpoint with full flexibility by passing a FwLateralLongitudinalSetpoint.
void update(const FwLateralLongitudinalSetpoint &setpoint, const FwControlConfiguration &config)
Update the setpoint with full flexibility by passing a FwLateralLongitudinalSetpoint and FwControlCon...
void updateWithHeightRate(float height_rate_sp, float course_sp, std::optional< float > equivalent_airspeed_sp={}, std::optional< float > lateral_acceleration_sp={})
Update the setpoint with simplest set of inputs: Height rate and Course. Lateral acceleration setpoin...
Definition setpoint_base.hpp:20
Definition fixedwing_lateral_longitudinal.hpp:160
Definition fixedwing_lateral_longitudinal.hpp:114
Definition setpoint_base.hpp:25