los_specific_force_controller.hpp

/****************************************************************************
 *
 * Copyright 2026 Auterion. All rights reserved.
 *
 ****************************************************************************/
 
#pragma once
 
#include <Eigen/Eigen>
#include <limits>
#include <memory>
 
namespace auterion {
 
class LosSpecificForceControllerImpl;
 
class LosSpecificForceController {
   public:
    struct Parameters {
        double sf_p{0.05};
        double sf_i{5.0};
        double att_lpf_tc{0.1};
        double roll_limit_deg{180.0};
        double pitch_limit_deg{90.0};
        double cam_vfov{30.0};
        double cam_hfov{0.0};
        double cam_pitch{0.0};
        double hover_thrust_normalized{0.5};
        double thrust_min{0.0};
        double thrust_max{1.0};
        double thrust_los_x_max{std::numeric_limits<double>::infinity()};
        double tilt_max_rate_deg_per_s{20.0};
        double roll_max_rate_deg_per_s{90.0};
        double yaw_max_rate_deg_per_s{45.0};
    };
 
    struct State {
        Eigen::Matrix3d los_to_enu{Eigen::Matrix3d::Identity()};
        Eigen::Vector3d sf_sp_enu{Eigen::Vector3d::Zero()};
        Eigen::Vector3d sf_sp_los{Eigen::Vector3d::Zero()};
        Eigen::Vector3d sf_los{Eigen::Vector3d::Zero()};
        Eigen::Vector3d thrust_sp_enu{Eigen::Vector3d::Zero()};
        Eigen::Vector3d thrust_sp_los{Eigen::Vector3d::Zero()};
        double desired_thrust_rate_los_x{0.0};
        Eigen::Quaterniond attitude_sp_enu{Eigen::Quaterniond::Identity()};
        double thrust_command_normalized{0.0};
        double thrust_command_abs{0.0};
        Eigen::Vector3d sf_los_error_prev{Eigen::Vector3d::Zero()};
        Eigen::Vector3d sf_los_prev{Eigen::Vector3d::Zero()};
        bool initialized{false};
    };
 
    explicit LosSpecificForceController(const Parameters& parameters);
 
    void setParameters(const Parameters& parameters);
 
    void setInitialThrustNormalized(double thrust_normalized);
 
    void update(float dt_s, bool hold, double max_thrust_rate_los_x, const Eigen::Vector3d& los,
                const Eigen::Vector3d& acc_sp_enu, const Eigen::Quaterniond& attitude_enu,
                const Eigen::Vector3d& specific_force_flu);
 
    const State& state() const;
 
   private:
    std::shared_ptr<LosSpecificForceControllerImpl> _impl;
};
 
}  // namespace auterion