Guidance attitude

Simplified the guidance setpoint calculation