lateral_longitudinal.hpp

/****************************************************************************
 * Copyright (c) 2025 PX4 Development Team.
 * SPDX-License-Identifier: BSD-3-Clause
 ****************************************************************************/
 
#pragma once
 
#include <Eigen/Eigen>
#include <optional>
#include <px4_msgs/msg/fixed_wing_lateral_setpoint.hpp>
#include <px4_msgs/msg/fixed_wing_longitudinal_setpoint.hpp>
#include <px4_msgs/msg/lateral_control_configuration.hpp>
#include <px4_msgs/msg/longitudinal_control_configuration.hpp>
#include <px4_ros2/common/exception.hpp>
#include <px4_ros2/common/setpoint_base.hpp>
 
namespace px4_ros2 {
struct FwLateralLongitudinalSetpoint;
struct FwControlConfiguration;
 
class FwLateralLongitudinalSetpointType : public SetpointBase {
 public:
  explicit FwLateralLongitudinalSetpointType(Context& context, bool is_position_optional = false);
 
  ~FwLateralLongitudinalSetpointType() override = default;
 
  SetpointType getSetpointType() override
  {
    return px4_msgs::msg::SetpointConfig::TYPE_FIXEDWING_LATERAL_LONGITUDINAL;
  }
 
  void clearOptionalRequirements(
      px4_msgs::msg::SetpointConfigReply& setpoint_config_reply) override;
 
  void update(const FwLateralLongitudinalSetpoint& setpoint, const FwControlConfiguration& config);
 
  void update(const FwLateralLongitudinalSetpoint& setpoint);
 
  void updateWithAltitude(float altitude_amsl_sp, float course_sp,
                          std::optional<float> equivalent_airspeed_sp = {},
                          std::optional<float> lateral_acceleration_sp = {});
 
  void updateWithHeightRate(float height_rate_sp, float course_sp,
                            std::optional<float> equivalent_airspeed_sp = {},
                            std::optional<float> lateral_acceleration_sp = {});
 
  float desiredUpdateRateHz() override { return 30.f; }
 
 private:
  void publishConfigurationIfNeeded();
 
  rclcpp::Node& _node;
  bool _local_position_is_optional;
  rclcpp::Publisher<px4_msgs::msg::FixedWingLateralSetpoint>::SharedPtr _fw_lateral_sp_pub;
  rclcpp::Publisher<px4_msgs::msg::FixedWingLongitudinalSetpoint>::SharedPtr
      _fw_longitudinal_sp_pub;
 
  rclcpp::Publisher<px4_msgs::msg::LateralControlConfiguration>::SharedPtr
      _lateral_control_configuration_pub;
  rclcpp::Publisher<px4_msgs::msg::LongitudinalControlConfiguration>::SharedPtr
      _longitudinal_control_configuration_pub;
 
  rclcpp::Time _last_config_published_time{};
  std::optional<px4_msgs::msg::LateralControlConfiguration> _current_lateral_configuration;
  std::optional<px4_msgs::msg::LongitudinalControlConfiguration>
      _current_longitudinal_configuration;
};
 
struct FwLateralLongitudinalSetpoint {
  std::optional<float> course;
  std::optional<float> airspeed_direction;
  std::optional<float> lateral_acceleration;
  std::optional<float> altitude_msl;
  std::optional<float> height_rate;
  std::optional<float> equivalent_airspeed;
  std::optional<float> throttle_direct;
 
  FwLateralLongitudinalSetpoint& withCourse(float course_sp)
  {
    course = course_sp;
    return *this;
  }
 
  FwLateralLongitudinalSetpoint& withAirspeedDirection(float airspeed_direction_sp)
  {
    airspeed_direction = airspeed_direction_sp;
    return *this;
  }
 
  FwLateralLongitudinalSetpoint& withLateralAcceleration(float lateral_acceleration_sp)
  {
    lateral_acceleration = lateral_acceleration_sp;
    return *this;
  }
 
  FwLateralLongitudinalSetpoint& withAltitude(float altitude_sp)
  {
    altitude_msl = altitude_sp;
    return *this;
  }
 
  FwLateralLongitudinalSetpoint& withHeightRate(float height_rate_sp)
  {
    height_rate = height_rate_sp;
    return *this;
  }
 
  FwLateralLongitudinalSetpoint& withEquivalentAirspeed(float equivalent_airspeed_sp)
  {
    equivalent_airspeed = equivalent_airspeed_sp;
    return *this;
  }
 
  FwLateralLongitudinalSetpoint& withThrottleDirect(float throttle_direct_sp)
  {
    throttle_direct = throttle_direct_sp;
    return *this;
  }
};
 
struct FwControlConfiguration {
  std::optional<float> min_pitch;
  std::optional<float> max_pitch;
  std::optional<float> min_throttle;
  std::optional<float> max_throttle;
  std::optional<float> max_lateral_acceleration;
  std::optional<float> target_climb_rate;
  std::optional<float> target_sink_rate;
  std::optional<float> speed_weight;
 
  FwControlConfiguration& withPitchLimits(float min_pitch_sp, float max_pitch_sp)
  {
    min_pitch = min_pitch_sp;
    max_pitch = max_pitch_sp;
 
    return *this;
  }
 
  FwControlConfiguration& withThrottleLimits(float min_throttle_sp, float max_throttle_sp)
  {
    min_throttle = min_throttle_sp;
    max_throttle = max_throttle_sp;
 
    return *this;
  }
 
  FwControlConfiguration& withMaxAcceleration(float max_lateral_acceleration_sp)
  {
    max_lateral_acceleration = max_lateral_acceleration_sp;
    return *this;
  }
 
  FwControlConfiguration& withTargetSinkRate(float target_sink_rate_sp)
  {
    target_sink_rate = target_sink_rate_sp;
    return *this;
  }
 
  FwControlConfiguration& withTargetClimbRate(float target_climb_rate_sp)
  {
    target_climb_rate = target_climb_rate_sp;
    return *this;
  }
 
  FwControlConfiguration& withSpeedWeight(float speed_weight_sp)
  {
    if (speed_weight_sp < 0.0 || speed_weight_sp > 2.0) {
      throw Exception("Speed weight must be between 0 and 2.");
    }
    speed_weight = speed_weight_sp;
    return *this;
  }
};
 
} /* namespace px4_ros2 */