Guided5Dof.hpp

Go to the documentation of this file.

// Vulcan Pseudo-5DOF Dynamics
// Pure stateless dynamics functions for guided vehicle simulation
//
// Pseudo-5DOF: 3-DOF translation + 2-DOF attitude (pitch/yaw) with roll
// commanded. Attitude responds to commands via second-order transfer function
// dynamics. Supports bank-to-turn and skid-to-turn steering laws.
#pragma once
 
#include <vulcan/core/VulcanTypes.hpp>
 
#include <janus/janus.hpp>
 
namespace vulcan::dynamics {
 
// =============================================================================
// Position Kinematics
// =============================================================================

template <typename Scalar>
Vec3<Scalar> position_dot(const Vec3<Scalar> &velocity) {
    return velocity;
}
 
// =============================================================================
// Attitude Dynamics via Transfer Function
// =============================================================================

template <typename Scalar>
Scalar attitude_response_accel(const Scalar &angle, const Scalar &angle_dot,
                               const Scalar &angle_cmd, const Scalar &omega_n,
                               const Scalar &zeta) {
    Scalar omega_sq = omega_n * omega_n;
    return omega_sq * (angle_cmd - angle) -
           Scalar(2) * zeta * omega_n * angle_dot;
}
 
// =============================================================================
// Force Direction for Different Steering Laws
// =============================================================================

template <typename Scalar>
Vec3<Scalar> thrust_direction_btt(const Scalar &gamma, const Scalar &chi) {
    Scalar cos_gamma = janus::cos(gamma);
    Scalar sin_gamma = janus::sin(gamma);
    Scalar cos_chi = janus::cos(chi);
    Scalar sin_chi = janus::sin(chi);
 
    // Thrust aligned with velocity direction
    return Vec3<Scalar>{
        cos_gamma * cos_chi, // North
        cos_gamma * sin_chi, // East
        -sin_gamma           // Down (negative for nose up)
    };
}

template <typename Scalar>
Vec3<Scalar> lift_direction_btt(const Scalar &gamma, const Scalar &chi,
                                const Scalar &phi) {
    Scalar cos_gamma = janus::cos(gamma);
    Scalar sin_gamma = janus::sin(gamma);
    Scalar cos_chi = janus::cos(chi);
    Scalar sin_chi = janus::sin(chi);
    Scalar cos_phi = janus::cos(phi);
    Scalar sin_phi = janus::sin(phi);
 
    // "Up" direction in velocity frame (perpendicular to velocity, in vertical
    // plane)
    Vec3<Scalar> up_v{sin_gamma * cos_chi, sin_gamma * sin_chi, cos_gamma};
 
    // "Right" direction (perpendicular to velocity, horizontal)
    // When chi=0 (North), right = East = (0, 1, 0)
    Vec3<Scalar> right_v{-sin_chi, cos_chi, Scalar(0)};
 
    // Lift direction rotated by bank angle
    return cos_phi * up_v + sin_phi * right_v;
}

template <typename Scalar>
Vec3<Scalar> thrust_direction_stt(const Scalar &theta, const Scalar &psi) {
    Scalar cos_theta = janus::cos(theta);
    Scalar sin_theta = janus::sin(theta);
    Scalar cos_psi = janus::cos(psi);
    Scalar sin_psi = janus::sin(psi);
 
    return Vec3<Scalar>{
        cos_theta * cos_psi, // North
        cos_theta * sin_psi, // East
        -sin_theta           // Down
    };
}

template <typename Scalar>
Vec3<Scalar> side_force_direction_stt(const Scalar &theta, const Scalar &psi) {
    Scalar cos_psi = janus::cos(psi);
    Scalar sin_psi = janus::sin(psi);
 
    // Body Y is perpendicular to body X, in horizontal plane
    return Vec3<Scalar>{-sin_psi, cos_psi, Scalar(0)};
}

template <typename Scalar>
Vec3<Scalar> normal_force_direction_stt(const Scalar &theta,
                                        const Scalar &psi) {
    Scalar cos_theta = janus::cos(theta);
    Scalar sin_theta = janus::sin(theta);
    Scalar cos_psi = janus::cos(psi);
    Scalar sin_psi = janus::sin(psi);
 
    // Body -Z (up in body frame)
    return Vec3<Scalar>{sin_theta * cos_psi, sin_theta * sin_psi, cos_theta};
}
 
// =============================================================================
// Velocity Derivatives
// =============================================================================

template <typename Scalar>
Vec3<Scalar> velocity_dot_btt(const Scalar &thrust, const Scalar &drag,
                              const Scalar &lift, const Scalar &mass,
                              const Scalar &gravity, const Scalar &gamma,
                              const Scalar &chi, const Scalar &phi) {
    Vec3<Scalar> T_dir = thrust_direction_btt(gamma, chi);
    Vec3<Scalar> L_dir = lift_direction_btt(gamma, chi, phi);
    Vec3<Scalar> D_dir = -T_dir; // Drag opposes thrust direction
 
    Vec3<Scalar> grav{Scalar(0), Scalar(0), gravity}; // Z-down
 
    return (thrust / mass) * T_dir + (drag / mass) * D_dir +
           (lift / mass) * L_dir + grav;
}

template <typename Scalar>
Vec3<Scalar> velocity_dot_stt(const Scalar &thrust, const Scalar &drag,
                              const Scalar &normal_force,
                              const Scalar &side_force, const Scalar &mass,
                              const Scalar &gravity, const Scalar &theta,
                              const Scalar &psi) {
    Vec3<Scalar> T_dir = thrust_direction_stt(theta, psi);
    Vec3<Scalar> N_dir = normal_force_direction_stt(theta, psi);
    Vec3<Scalar> S_dir = side_force_direction_stt(theta, psi);
    Vec3<Scalar> D_dir = -T_dir; // Drag opposes thrust
 
    Vec3<Scalar> grav{Scalar(0), Scalar(0), gravity}; // Z-down
 
    return (thrust / mass) * T_dir + (drag / mass) * D_dir +
           (normal_force / mass) * N_dir + (side_force / mass) * S_dir + grav;
}
 
// =============================================================================
// Flight Path Angle Derivatives
// =============================================================================

template <typename Scalar>
Scalar gamma_dot(const Scalar &lift, const Scalar &weight, const Scalar &mass,
                 const Scalar &velocity, const Scalar &gamma,
                 const Scalar &phi) {
    Scalar cos_phi = janus::cos(phi);
    Scalar cos_gamma = janus::cos(gamma);
    return (lift * cos_phi - weight * cos_gamma) /
           (mass * velocity + Scalar(1e-12));
}

template <typename Scalar>
Scalar chi_dot_btt(const Scalar &lift, const Scalar &mass,
                   const Scalar &velocity, const Scalar &gamma,
                   const Scalar &phi) {
    Scalar sin_phi = janus::sin(phi);
    Scalar cos_gamma = janus::cos(gamma);
    return (lift * sin_phi) /
           (mass * velocity * (cos_gamma + Scalar(1e-12)) + Scalar(1e-12));
}

template <typename Scalar>
Scalar load_factor_from_lift(const Scalar &lift, const Scalar &weight) {
    return lift / (weight + Scalar(1e-12));
}

template <typename Scalar>
Scalar bank_for_turn_rate(const Scalar &velocity, const Scalar &chi_dot,
                          const Scalar &gravity) {
    return janus::atan(velocity * chi_dot / gravity);
}
 
} // namespace vulcan::dynamics