body_frame_specific_force_controller.hpp

/****************************************************************************
 *
 * Copyright 2026 Auterion AG. All rights reserved.
 *
 ****************************************************************************/
 
#pragma once
 
#include <Eigen/Eigen>
#include <memory>
#include <optional>
 
namespace auterion {
 
class BodyFrameSpecificForceControllerImpl;
 
class BodyFrameSpecificForceController {
   public:
    struct Parameters {
        double roll_gain{0.0};
        double max_roll_angle{45.0};
        double sf_roll_i_gain{0.0};
 
        double sf_z_gain_p{0.0};
        double sf_z_gain_i{0.0};
 
        double sf_y_gain_p{0.0};
        double sf_y_gain_i{0.0};
 
        double max_side_acc_trim{0.0};
        double trim_airspeed{20.0};
 
        double approach_airspeed{20.0};
        double airspeed_gain_p{0.0};
        double airspeed_gain_i{0.0};
        double airspeed_ramp_slope{2.0};
 
        double pitch_lim_camera_vertical_fov{0.0};
        double camera_tilt{0.0};
    };
 
    struct State {
        Eigen::Vector3d sf_sp_enu{Eigen::Vector3d::Zero()};
        Eigen::Vector3d sf_sp_body{Eigen::Vector3d::Zero()};
        double validated_airspeed{0.0};
        double sf_sp_y_bank{0.0};
        double sf_sp_y_skid{0.0};
        double max_side_acceleration{0.0};
        double roll_error{0.0};
        double sp_p_unlim{0.0};
        double sf_y_error{0.0};
        double sf_y_error_integral{0.0};
        double sf_z_error_integral{0.0};
        double sf_roll_error_integral{0.0};
        double airspeed_error_integral{0.0};
        Eigen::Vector3d body_rates_sp{Eigen::Vector3d::Zero()};
        double thrust_sp{0.5};
    };
 
    explicit BodyFrameSpecificForceController(const Parameters& parameters);
 
    void setParameters(const Parameters& parameters);
 
    void update(double dt, const Eigen::Vector3d& acc_sp_enu,
                const Eigen::Quaterniond& attitude_enu, const Eigen::Vector3d& specific_force_flu,
                std::optional<double> true_airspeed,
                std::optional<Eigen::Vector3d> los = std::nullopt);
 
    const State& state() const;
 
   private:
    std::shared_ptr<BodyFrameSpecificForceControllerImpl> _impl;
};
 
}  // namespace auterion