stl_formula_45Deg.tbt

// SPDX-FileCopyrightText: 2023 German Aerospace Center (DLR)
// SPDX-License-Identifier: Apache-2.0

Deg45_HeightAboveShip = 20.0;
Deg45_DistanceToShip = 30.0;
Deg45_AngleToShip = 135.0;
Deg45_AboveTouchdown = 20.0;

Lateral_HeightAboveShip = 20.0;
Lateral_DistanceToShip = 20.0;
Lateral_AngleToShip = 90.0;
Lateral_AboveTouchdown = 20.0;

Oblique_HeightAboveShip = 20.0;
Oblique_DistanceToShip = 30.0;
Oblique_AngleToShip = 135.0;
Oblique_AngleOblique = 45.0;
Oblique_AboveTouchdown = 20.0;

Straight_HeightAboveShip = 20.0;
Straight_DistanceToShip = 20.0;
Straight_AngleToShip = 180.0;
Straight_AboveTouchdown = 20.0;

In_Position_Pos_Slack = 2.5;
In_Position_HeadingAligned_Slack = 1.0;
In_Position_HeadingOblique_Slack = 1.0;
In_Position_Velocity_Slack = 2.0;
Move_to_Touchdown_Slack = 2.5;
Descend_Slack = 1.0;




<>  
(
    in_pos(uas_x, uas_y, uas_z, ship_x, ship_y, ship_z, ship_heading)
    [
        In_Position_Pos_Slack - 
        !sqrt(
            (ship_x + Deg45_DistanceToShip * !cos(!degToRad(Deg45_AngleToShip) + ship_heading) - uas_x)^2 +
            (ship_y + Deg45_DistanceToShip * !sin(!degToRad(Deg45_AngleToShip) + ship_heading) - uas_y)^2 + 
            (ship_z + Deg45_HeightAboveShip  - uas_z)^2
        )
    ]
    &
    O (
        [0..5]  combined_inpos_ha_va(uas_x, uas_y, uas_z, uas_u, uas_v, uas_w, uas_heading, ship_x, ship_y, ship_z, ship_u, ship_v, ship_w, ship_heading)
                [
                    !min(
                        In_Position_Pos_Slack - 
                        !sqrt(
                            (ship_x + Deg45_DistanceToShip * !cos(!degToRad(Deg45_AngleToShip) + ship_heading) - uas_x)^2 +
                            (ship_y + Deg45_DistanceToShip * !sin(!degToRad(Deg45_AngleToShip) + ship_heading) - uas_y)^2 + 
                            (ship_z + Deg45_HeightAboveShip  - uas_z)^2
                        )
                        , 
                        !min(
                            In_Position_HeadingAligned_Slack - !abs(!radToDeg(uas_heading - ship_heading))
                            , 
                            In_Position_Velocity_Slack - !abs(!sqrt( uas_u^2 + uas_v^2 + uas_w^2) - !sqrt( ship_u^2 + ship_v^2 + ship_w^2))
                        )
                    )
                ]
        & 
        OOOOO (
             <>  combined_movetp_ha(uas_x, uas_y, uas_z, uas_heading, ship_x, ship_y, ship_z, ship_heading)
                [
                    !min(
                        Move_to_Touchdown_Slack - 
                        !sqrt(
                            (uas_x - ship_x)^2 +
                            (uas_y - ship_y)^2 +
                            (uas_z - (ship_z + Deg45_AboveTouchdown))^2
                        )
                    ,
                        In_Position_HeadingAligned_Slack - !abs(!radToDeg(uas_heading - ship_heading))
                    )
                ]
                &
                O (
                    <> descend(uas_x, uas_y, uas_z, ship_x, ship_y, ship_z)
        [
            Descend_Slack - 
            !sqrt(
                (uas_x - ship_x)^2 +
                (uas_y - ship_y)^2 +
                (uas_z - ship_z )^2
            )
        ]
                )
        )
    )
)