PX4 ROS 2 Interface Library
Library to interface with PX4 from a companion computer using ROS 2
Loading...
Searching...
No Matches
fixedwing_lateral_longitudinal.hpp
1/****************************************************************************
2 * Copyright (c) 2025 PX4 Development Team.
3 * SPDX-License-Identifier: BSD-3-Clause
4 ****************************************************************************/
5
6#pragma once
7
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>
12#include <Eigen/Eigen>
13#include <optional>
14
15#include <px4_ros2/common/setpoint_base.hpp>
16
17namespace px4_ros2
18{
23struct FwLateralLongitudinalSetpoint;
24struct FwControlConfiguration;
25
30{
31public:
33
34 ~FwLateralLongitudinalSetpointType() override = default;
35
36 Configuration getConfiguration() override;
37
55 void update(
56 const FwLateralLongitudinalSetpoint & setpoint,
57 const FwControlConfiguration & config);
58
72 void update(const FwLateralLongitudinalSetpoint & setpoint);
73
82 float altitude_amsl_sp, float course_sp,
83 std::optional<float> equivalent_airspeed_sp = {},
84 std::optional<float> lateral_acceleration_sp = {});
85
94 float height_rate_sp, float course_sp,
95 std::optional<float> equivalent_airspeed_sp = {},
96 std::optional<float> lateral_acceleration_sp = {});
97
98 float desiredUpdateRateHz() override {return 30.f;}
99
100private:
101 rclcpp::Node & _node;
102 rclcpp::Publisher<px4_msgs::msg::FixedWingLateralSetpoint>::SharedPtr
103 _fw_lateral_sp_pub;
104 rclcpp::Publisher<px4_msgs::msg::FixedWingLongitudinalSetpoint>::SharedPtr
105 _fw_longitudinal_sp_pub;
106
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;
111};
112
114{
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;
121
122 FwLateralLongitudinalSetpoint & withCourse(float course_sp)
123 {
124 course = course_sp;
125 return *this;
126 }
127
128 FwLateralLongitudinalSetpoint & withAirspeedDirection(float airspeed_direction_sp)
129 {
130 airspeed_direction = airspeed_direction_sp;
131 return *this;
132 }
133
134 FwLateralLongitudinalSetpoint & withLateralAcceleration(float lateral_acceleration_sp)
135 {
136 lateral_acceleration = lateral_acceleration_sp;
137 return *this;
138 }
139
140 FwLateralLongitudinalSetpoint & withAltitude(float altitude_sp)
141 {
142 altitude_msl = altitude_sp;
143 return *this;
144 }
145
146 FwLateralLongitudinalSetpoint & withHeightRate(float height_rate_sp)
147 {
148 height_rate = height_rate_sp;
149 return *this;
150 }
151
152 FwLateralLongitudinalSetpoint & withEquivalentAirspeed(float equivalent_airspeed_sp)
153 {
154 equivalent_airspeed = equivalent_airspeed_sp;
155 return *this;
156 }
157};
158
160{
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;
168
169 FwControlConfiguration & withPitchLimits(float min_pitch_sp, float max_pitch_sp)
170 {
171
172 min_pitch = min_pitch_sp;
173 max_pitch = max_pitch_sp;
174
175 return *this;
176 }
177
178 FwControlConfiguration & withThrottleLimits(float min_throttle_sp, float max_throttle_sp)
179 {
180
181 min_throttle = min_throttle_sp;
182 max_throttle = max_throttle_sp;
183
184 return *this;
185 }
186
187 FwControlConfiguration & withMaxAcceleration(float max_lateral_acceleration_sp)
188 {
189
190 max_lateral_acceleration = max_lateral_acceleration_sp;
191 return *this;
192 }
193
194 FwControlConfiguration & withTargetSinkRate(float target_sink_rate_sp)
195 {
196
197 target_sink_rate = target_sink_rate_sp;
198 return *this;
199 }
200
201 FwControlConfiguration & withTargetClimbRate(float target_climb_rate_sp)
202 {
203
204 target_climb_rate = target_climb_rate_sp;
205 return *this;
206 }
207};
208
210} /* namespace px4_ros2 */
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