guidance_control.hpp

#pragma once
 
#include <eigen3/Eigen/Eigen>
#include <functional>
#include <memory>
#include <variant>
 
#include "../../auterion.hpp"
 
namespace auterion {
namespace fixedwing {
 
class GuidanceSetpoint {
   private:
    float _course_setpoint = NAN;
    float _lateral_acceleration_feedforward = NAN;
    float _altitude_setpoint = NAN;
    float _height_rate_setpoint = NAN;
    float _equivalent_airspeed_setpoint = NAN;
 
   public:
    class Config {};
 
    GuidanceSetpoint() = default;
 
    GuidanceSetpoint& withCourse(float course_rad, float lateral_accel_feedforward_m_s2 = NAN) {
        _course_setpoint = course_rad;
        _lateral_acceleration_feedforward = lateral_accel_feedforward_m_s2;
        return *this;
    }
 
    GuidanceSetpoint& withAltitude(float altitude_msl) {
        _altitude_setpoint = altitude_msl;
        return *this;
    }
 
    GuidanceSetpoint& withHeightRate(float height_rate_m_s) {
        _height_rate_setpoint = height_rate_m_s;
        return *this;
    }
 
    GuidanceSetpoint& withEquivalentAirspeed(float equivalent_airspeed_sp) {
        _equivalent_airspeed_setpoint = equivalent_airspeed_sp;
        return *this;
    }
 
    inline float getCourseSetpoint() const { return _course_setpoint; }
    inline float getLateralAccelerationFeedforward() const {
        return _lateral_acceleration_feedforward;
    }
    inline float getAltitudeSetpoint() const { return _altitude_setpoint; }
    inline float getHeightRateSetpoint() const { return _height_rate_setpoint; }
    inline float getEquivalentAirspeedSetpoint() const { return _equivalent_airspeed_setpoint; }
};
 
}  // namespace fixedwing
}  // namespace auterion