using UnityEngine; using System.Collections; public class QuadCopterController : MonoBehaviour { public float m_YawSpeed = 1.0f; // Yaw speed modifier. public float m_PitchSpeed = 40000.0f; // Pitch speed modifier. public float m_RollSpeed = 1.0f; // Roll speed modifier. public Transform m_AltPos; // Position where we take altitude reading (meters). public Transform m_Thruster1Pos; // Position of Thruster1 (meters). public Transform m_Thruster2Pos; // Position of Thruster2 (meters). public Transform m_Thruster3Pos; // Position of Thruster3 (meters). public Transform m_Thruster4Pos; // Position of Thruster4 (meters). public Logger.LOG_LEVEL m_InputsLogLevel = Logger.LOG_LEVEL.NOTICE; public Logger.LOG_LEVEL m_StateLogLevel = Logger.LOG_LEVEL.NOTICE; public Logger.LOG_LEVEL m_U1AltLogLevel = Logger.LOG_LEVEL.NOTICE; public Logger.LOG_LEVEL m_U4YawLogLevel = Logger.LOG_LEVEL.NOTICE; public Logger.LOG_LEVEL m_U3PitchLogLevel = Logger.LOG_LEVEL.NOTICE; public Logger.LOG_LEVEL m_U2RollLogLevel = Logger.LOG_LEVEL.NOTICE; public Logger.LOG_LEVEL m_ForcesLogLevel = Logger.LOG_LEVEL.NOTICE; struct ControlInputs { public bool m_PowerOn; public float m_AltIncDec; // Increase or decrease altitude. public float m_YawIncDec; // Increase or decrease yaw. public float m_PitchIncDec; // Increase or decrease pitch. public float m_RollIncDec; // Increase or decrease roll. }; struct State { public bool m_PoweredOn; // Whether or not vehicle is powered on. public float m_HAT; // Actual Height Above Terrain (meters). public float m_ActualAlt; // Height above sea-level (MSL) (meters). public float m_ActualYaw; // Actual yaw (degrees) public float m_ActualPitch; // Actual pitch (degrees). public float m_ActualRoll; // Actual roll (degrees). public float m_DesiredAlt; // Desired altitude (meters). public float m_DesiredYaw; // Desired yaw (degrees). public float m_DesiredPitch; // Desired pitch (degrees). public float m_DesiredRoll; // Desired roll (degrees). public float m_U1Alt; // Command for Thrust for Altitude control (Newtons). public float m_U4Yaw; // Command for yaw control (Newtons). public float m_U3Pitch; // Command for pitch control (Newtons). public float m_U2Roll; // Command for roll control (Newtons). public float m_Rotor1SpeedSquared; // (rad s^-1)^2 public float m_Rotor2SpeedSquared; // (rad s^-1)^2 public float m_Rotor3SpeedSquared; // (rad s^-1)^2 public float m_Rotor4SpeedSquared; // (rad s^-1)^2 public Vector3 m_Thruster1Force; // Thruster1 force vector (Newtons). public Vector3 m_Thruster2Force; // Thruster2 force vector (Newtons). public Vector3 m_Thruster3Force; // Thruster3 force vector (Newtons). public Vector3 m_Thruster4Force; // Thruster4 force vector (Newtons). }; private ControlInputs m_CurrentInput; private ControlInputs m_OldInput; private State m_CurrentState; private State m_OldState; private Rigidbody m_Rigidbody; private Logger m_InputsLog; private Logger m_StateLog; private Logger m_ForcesLog; PID m_U1AltPid; PID m_U4YawPid; PID m_U3PitchPid; PID m_U2RollPid; // Constants private float m_d = 1.1e-6f; // N m s^2 = (Drag Factor) private float m_l = 0.24f; // m = (Center-Of-Mass to center of propellor) private float m_ThrustFactor = 54.2e-6f; // N s^2 = (Thrust Factor) private float m_Izz = 8.1e-3f; // N m s^2 = (Moment of inertia about the forward axis = Roll ) private float m_Ixx = 8.1e-3f; // N m s^2 = (Moment of inertia about the right axis = Pitch) private float m_Iyy = 14.2e-3f; // N m s^2 = (Moment of inertial about the up axis = Yaw ) void Start( ) { m_InputsLog = new Logger( "QuadCopterController:Inputs" ); m_StateLog = new Logger( "QuadCopterController:State" ); m_ForcesLog = new Logger( "QuadCopterController:Forces" ); m_Rigidbody = GetComponent(); InitState( m_CurrentState ); m_U1AltPid = new PID( "U1Alt", 20.0f, 0.5f, 10.0f, 20.0f ); m_U4YawPid = new PID( "U4Yaw", 20.0f, 0.5f, 10.0f, 20.0f ); m_U3PitchPid = new PID( "U3Pitch", 1000.0f, 600.0f, 100.0f, 600.0f ); m_U2RollPid = new PID( "U2Roll", 20.0f, 0.5f, 10.0f, 20.0f ); GetInputs( ); } void InitState( State state_ ) { state_.m_PoweredOn = false; state_.m_HAT = 0.0f; state_.m_ActualAlt = 0.0f; state_.m_DesiredAlt = 0.0f; state_.m_DesiredYaw = 0.0f; state_.m_DesiredPitch = 0.0f; state_.m_DesiredRoll = 0.0f; state_.m_U1Alt = 0.0f; state_.m_U4Yaw = 0.0f; state_.m_U3Pitch = 0.0f; state_.m_U2Roll = 0.0f; state_.m_Rotor1SpeedSquared = 0.0f; state_.m_Rotor2SpeedSquared = 0.0f; state_.m_Rotor3SpeedSquared = 0.0f; state_.m_Rotor4SpeedSquared = 0.0f; state_.m_Thruster1Force = Vector3.zero; state_.m_Thruster2Force = Vector3.zero; state_.m_Thruster3Force = Vector3.zero; state_.m_Thruster4Force = Vector3.zero; } // Update is called once per frame void Update( ) { m_InputsLog.SetLogLevel( m_InputsLogLevel ); m_StateLog.SetLogLevel( m_StateLogLevel ); m_U1AltPid.SetLogLevel( m_U1AltLogLevel ); m_U4YawPid.SetLogLevel( m_U4YawLogLevel ); m_U3PitchPid.SetLogLevel( m_U3PitchLogLevel ); m_U2RollPid.SetLogLevel( m_U2RollLogLevel ); m_ForcesLog.SetLogLevel( m_ForcesLogLevel ); GetInputs( ); ProcessInputs( ); } void FixedUpdate( ) { if ( m_Rigidbody.IsSleeping( ) ) { m_Rigidbody.WakeUp( ); } CalculateNewState( ); CalculateForces( ); } void GetInputs( ) { m_OldInput = m_CurrentInput; // Save old input. if ( Input.GetButtonDown( "Power" ) ) { m_CurrentInput.m_PowerOn = !m_CurrentInput.m_PowerOn; } m_CurrentInput.m_AltIncDec = Input.GetAxis( "Throttle" ); m_CurrentInput.m_YawIncDec = Input.GetAxis( "Yaw" ); m_CurrentInput.m_PitchIncDec = Input.GetAxis( "Pitch" ); m_CurrentInput.m_RollIncDec = Input.GetAxis( "Roll" ); m_InputsLog.Log( Logger.LOG_LEVEL.DEBUG2, () => { return "Power: " + m_CurrentInput.m_PowerOn + ", AltIncDec: " + m_CurrentInput.m_AltIncDec + ", YawIncDec: " + m_CurrentInput.m_YawIncDec + ", PitchIncDec: " + m_CurrentInput.m_PitchIncDec + ", RollIncDec: " + m_CurrentInput.m_RollIncDec; } ); } void CalculateNewState( ) { m_OldState = m_CurrentState; // Get Actual values. m_CurrentState.m_PoweredOn = m_CurrentInput.m_PowerOn; m_CurrentState.m_ActualAlt = transform.position.y; m_CurrentState.m_ActualYaw = ConvertAngleToPlusMinus180( transform.rotation.eulerAngles.y ); m_CurrentState.m_ActualPitch = ConvertAngleToPlusMinus180( transform.rotation.eulerAngles.x ); m_CurrentState.m_ActualRoll = ConvertAngleToPlusMinus180( transform.rotation.eulerAngles.z ); // If we have power then process commands. if ( m_CurrentState.m_PoweredOn ) { // Altitude m_CurrentState.m_DesiredAlt += m_CurrentInput.m_AltIncDec * Time.deltaTime; if ( m_CurrentState.m_DesiredAlt <= 0 ) { m_CurrentState.m_DesiredAlt = 0.0f; } // Yaw m_CurrentState.m_DesiredYaw += m_CurrentInput.m_YawIncDec * Time.deltaTime * m_YawSpeed; // Pitch m_CurrentState.m_DesiredPitch += m_CurrentInput.m_PitchIncDec * Time.deltaTime * m_PitchSpeed; m_CurrentState.m_DesiredPitch = Mathf.Clamp( m_CurrentState.m_DesiredPitch, -45.0f, +45.0f ); // Roll m_CurrentState.m_DesiredRoll += m_CurrentInput.m_RollIncDec * Time.deltaTime * m_RollSpeed; m_CurrentState.m_DesiredRoll = Mathf.Clamp( m_CurrentState.m_DesiredRoll, -5.0f, +5.0f ); } else { m_CurrentState.m_DesiredAlt = m_CurrentState.m_ActualAlt; m_CurrentState.m_DesiredYaw = 0.0f; m_CurrentState.m_DesiredPitch = 0.0f; m_CurrentState.m_DesiredRoll = 0.0f; } if ( m_CurrentState.m_PoweredOn ) { // Altitude stabilization. m_CurrentState.m_U1Alt = (m_U1AltPid.Update( m_CurrentState.m_DesiredAlt, m_CurrentState.m_ActualAlt, Time.deltaTime ) + Physics.gravity.magnitude) * m_Rigidbody.mass / (Mathf.Cos( m_CurrentState.m_ActualRoll * Mathf.Deg2Rad ) * Mathf.Cos( m_CurrentState.m_ActualPitch * Mathf.Deg2Rad ) ); m_CurrentState.m_U1Alt = Mathf.Clamp( m_CurrentState.m_U1Alt, 0.0f, Mathf.Infinity ); // Yaw Stabilization m_CurrentState.m_U4Yaw = m_U4YawPid.Update( m_CurrentState.m_DesiredYaw * Mathf.Deg2Rad, m_CurrentState.m_ActualYaw * Mathf.Deg2Rad, Time.deltaTime ) * m_Iyy; // Pitch Stabilization m_CurrentState.m_U3Pitch = m_U3PitchPid.Update( m_CurrentState.m_DesiredPitch * Mathf.Deg2Rad, m_CurrentState.m_ActualPitch * Mathf.Deg2Rad, Time.deltaTime ) * m_Ixx; // Roll Stabilization m_CurrentState.m_U2Roll = m_U2RollPid.Update( m_CurrentState.m_DesiredRoll * Mathf.Deg2Rad, m_CurrentState.m_ActualRoll * Mathf.Deg2Rad, Time.deltaTime ) * m_Izz; } else { // Altitude m_CurrentState.m_U1Alt = 0.0f; m_U1AltPid.Reset( ); // Yaw m_CurrentState.m_U4Yaw = 0.0f; m_U4YawPid.Reset( ); // Pitch m_CurrentState.m_U3Pitch = 0.0f; m_U3PitchPid.Reset( ); // Roll m_CurrentState.m_U2Roll = 0.0f; m_U2RollPid.Reset( ); } // Common terms float oneFourth_b = 1.0f / (4.0f * m_ThrustFactor); float oneHalf_bl = 1.0f / (2.0f * m_ThrustFactor * m_l); float oneFourth_d = 1.0f / (4.0f * m_d); // Inverted movments matrix. // This is what will control the rotor speed and give altitude, yaw, pitch, roll control. m_CurrentState.m_Rotor1SpeedSquared = oneFourth_b * m_CurrentState.m_U1Alt - oneHalf_bl * m_CurrentState.m_U3Pitch - oneFourth_d * m_CurrentState.m_U4Yaw; m_CurrentState.m_Rotor2SpeedSquared = oneFourth_b * m_CurrentState.m_U1Alt - oneHalf_bl * m_CurrentState.m_U2Roll + oneFourth_d * m_CurrentState.m_U4Yaw; m_CurrentState.m_Rotor3SpeedSquared = oneFourth_b * m_CurrentState.m_U1Alt + oneHalf_bl * m_CurrentState.m_U3Pitch - oneFourth_d * m_CurrentState.m_U4Yaw; m_CurrentState.m_Rotor4SpeedSquared = oneFourth_b * m_CurrentState.m_U1Alt + oneHalf_bl * m_CurrentState.m_U2Roll + oneFourth_d * m_CurrentState.m_U4Yaw; m_CurrentState .m_Rotor1SpeedSquared = Mathf.Clamp( m_CurrentState.m_Rotor1SpeedSquared, 0, Mathf.Infinity ); m_CurrentState .m_Rotor2SpeedSquared = Mathf.Clamp( m_CurrentState.m_Rotor2SpeedSquared, 0, Mathf.Infinity ); m_CurrentState .m_Rotor3SpeedSquared = Mathf.Clamp( m_CurrentState.m_Rotor3SpeedSquared, 0, Mathf.Infinity ); m_CurrentState .m_Rotor4SpeedSquared = Mathf.Clamp( m_CurrentState.m_Rotor4SpeedSquared, 0, Mathf.Infinity ); m_StateLog.Log( Logger.LOG_LEVEL.DEBUG2, () => { return "PoweredOn: " + m_CurrentState.m_PoweredOn + ", DesAlt: " + m_CurrentState.m_DesiredAlt + ", ActAlt: " + m_CurrentState.m_ActualAlt + ", DesYaw: " + m_CurrentState.m_DesiredYaw + ", ActYaw: " + m_CurrentState.m_ActualYaw + ", DesPitch: " + m_CurrentState.m_DesiredPitch + ", ActPitch: " + m_CurrentState.m_ActualPitch + ", DesRoll: " + m_CurrentState.m_DesiredRoll + ", ActRoll: " + m_CurrentState.m_ActualRoll; } ); } void ProcessInputs( ) { if ( !m_CurrentInput.m_PowerOn ) { m_CurrentInput.m_AltIncDec = 0f; m_CurrentInput.m_YawIncDec = 0f; m_CurrentInput.m_PitchIncDec = 0f; m_CurrentInput.m_RollIncDec = 0f; } } void CalculateForces( ) { m_CurrentState.m_Thruster1Force = Vector3.zero; m_CurrentState.m_Thruster2Force = Vector3.zero; m_CurrentState.m_Thruster3Force = Vector3.zero; m_CurrentState.m_Thruster4Force = Vector3.zero; m_CurrentState.m_Thruster1Force += m_ThrustFactor * m_CurrentState.m_Rotor1SpeedSquared * transform.up; m_CurrentState.m_Thruster2Force += m_ThrustFactor * m_CurrentState.m_Rotor2SpeedSquared * transform.up; m_CurrentState.m_Thruster3Force += m_ThrustFactor * m_CurrentState.m_Rotor3SpeedSquared * transform.up; m_CurrentState.m_Thruster4Force += m_ThrustFactor * m_CurrentState.m_Rotor4SpeedSquared * transform.up; m_ForcesLog.Log( Logger.LOG_LEVEL.DEBUG2, () => { return "U1Alt: " + m_CurrentState.m_U1Alt + ", U4Yaw: " + m_CurrentState.m_U4Yaw + ", U3Pitch: " + m_CurrentState.m_U3Pitch + ", U2Roll: " + m_CurrentState.m_U2Roll + ", R1: " + m_CurrentState.m_Rotor1SpeedSquared + ", R2: " + m_CurrentState.m_Rotor2SpeedSquared + ", R3: " + m_CurrentState.m_Rotor3SpeedSquared + ", R4: " + m_CurrentState.m_Rotor4SpeedSquared + ", T1: " + m_CurrentState.m_Thruster1Force + ", T2: " + m_CurrentState.m_Thruster2Force + ", T3: " + m_CurrentState.m_Thruster3Force + ", T4: " + m_CurrentState.m_Thruster4Force; } ); Debug.DrawLine( m_Thruster1Pos.transform.position, m_Thruster1Pos.transform.position - m_CurrentState.m_U1Alt * transform.up, Color.green ); m_Rigidbody.AddForceAtPosition( m_CurrentState.m_Thruster1Force, m_Thruster1Pos.transform.position ); Debug.DrawLine( m_Thruster2Pos.transform.position, m_Thruster2Pos.transform.position - m_CurrentState.m_U1Alt * transform.up, Color.green ); m_Rigidbody.AddForceAtPosition( m_CurrentState.m_Thruster2Force, m_Thruster2Pos.transform.position ); Debug.DrawLine( m_Thruster3Pos.transform.position, m_Thruster3Pos.transform.position - m_CurrentState.m_U1Alt * transform.up, Color.green ); m_Rigidbody.AddForceAtPosition( m_CurrentState.m_Thruster3Force, m_Thruster3Pos.transform.position ); Debug.DrawLine( m_Thruster4Pos.transform.position, m_Thruster4Pos.transform.position - m_CurrentState.m_U1Alt * transform.up, Color.green ); m_Rigidbody.AddForceAtPosition( m_CurrentState.m_Thruster4Force, m_Thruster4Pos.transform.position ); /* Vector3 torque = Vector3.zero; torque.x += m_l * m_d * ( m_CurrentState.m_Rotor1SpeedSquared - m_CurrentState.m_Rotor3SpeedSquared ); torque.y += m_l * m_d * ( m_CurrentState.m_Rotor2SpeedSquared - m_CurrentState.m_Rotor4SpeedSquared ); torque.z += m_d * ( m_CurrentState.m_Rotor1SpeedSquared - m_CurrentState.m_Rotor2SpeedSquared + m_CurrentState.m_Rotor3SpeedSquared - m_CurrentState.m_Rotor4SpeedSquared ); m_Rigidbody.AddTorque( torque ); m_StateLog.Log( Logger.LOG_LEVEL.DEBUG2, () => { return "Torque: " + torque; } ); */ } // Get Height Above Terrain float GetHAT( ) { RaycastHit hit; float hat = 0; if ( Physics.Raycast( m_AltPos.transform.position, -transform.up, out hit, Mathf.Infinity ) ) { hat = hit.distance; } return hat; } float ConvertAngleToPlusMinus180( float angle_ ) { if ( angle_ >= 180f ) { angle_ = 0 - (360f - angle_); } if ( angle_ <= -180f ) { angle_ = 0 + (360f + angle_); } return angle_; } }