Quadcopter simulation in Unity

[varunvp]

Hey guys,
This is my first post in the forum, and I didn't know where it would fit, so I posted here.

I built a quadcopter with my friends some time back, in a workshop, but was disappointed that we could not learn to program it. Hence I don't know anything about how quadcopters are stabilized. I have researched some stuff, and have come to know that PID is what I need, but I don't understand how it translates to motor speed, and hence force applied.

So I'm trying to simulate a quadcopter in Unity, a game engine, which can simulate physics too. I have 4 points attached at each end of a disc. Force is exerted from these 4 points on the disc, and hence flight is achieved. I haven't modelled the propellers yet because I want to get the basic flight physics and control sorted out.

Control is achieved by changing the force applied at each of these points, instead of the motor RPM. Want to do it this way because I want to get a feel of the physics involved. Gravity is also simulated here.

Next thing, I'm trying to achieve flight stabilization, hovering and control, in my simulated quadcopter.
First I wanted to tackle basic hovering at a height. I then learnt this is not so basic as I thought it would be.
Here are my crude attemt at a program ( in C#, syntax is very similar to C.)

public class ApplyForce : MonoBehaviour {

public Transform FrontNozzle;
public Transform BackNozzle;
public Transform LeftNozzle;
public Transform RightNozzle;
private Rigidbody Disk;
private Transform _transform;
public float controlForce; //used for directional control (not important now)
public float force; //This variable is used to exert the main thrusting force
public float maxForce;
public float minForce;
public float height;
private float prevHeight;
public PID pidController;
public bool stabilize;
public Transform ground;
private float mass;
// Use this for initialization
void Start () 
{
Disk = GetComponent<Rigidbody>();
_transform = transform;
height = SenseHeight();
prevHeight = height;
}

float capturedHeight = 0;
float acceleration;
float velocity;
float lastVelocity = 0;
void FixedUpdate () 
{
var fb = -CrossPlatformInput.GetAxis("Vertical");
var lr = -CrossPlatformInput.GetAxis("Horizontal");

height = SenseHeight();

if(stabilize)
{
//captures the height at which our quad has to be stabilized
if(capturedHeight == 0)
capturedHeight = SenseHeight(); 

height = SenseHeight();

if(height > capturedHeight)
{
force = 0;

velocity = Disk.velocity.magnitude;
acceleration = (velocity - lastVelocity) / Time.fixedDeltaTime;
//capturing the acceleration during the downward fall
lastVelocity = velocity;

}
else
{
//use the last captured acceleration to counteract the falling quadcopter
force = pidController.Update(acceleration, force, Time.fixedDeltaTime);
   //setpoint   , actual, dT
if(force < 0)
force = 0;
}
}

else
capturedHeight = 0;

//Exerts force at the 4 points
Disk.AddForceAtPosition(_transform.forward * (force + (controlForce * ((fb>0)?fb:0))), FrontNozzle.position);
Disk.AddForceAtPosition(_transform.forward * (force + (controlForce * ((fb<0)?-fb:0))), BackNozzle.position);
Disk.AddForceAtPosition(_transform.forward * (force + (controlForce * ((lr>0)?lr:0))), LeftNozzle.position);
Disk.AddForceAtPosition(_transform.forward * (force + (controlForce * ((lr<0)?-lr:0))), RightNozzle.position);
}

Is the code anywhere close to what is used in a typical hovering algorithm?

I did refer to this thread.
https://forum.arduino.cc/index.php?topic=134819.msg1015267#msg1015267

But could not translate it to anything in terms of force.

I would be pleased if anyone suggested improvements in my code or an alternate algorithm atleast, so I can translate it to code.

Thanks!