Quadcopter simulation in Unity

Hey guys,
This is my first post in the forum.

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. 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.)

Code:

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!

 

UPDATE: Seems this hovering part is working. Here is the code:

Code:

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;
public float force;
public float maxForce;
public float minForce;
public float height;
private float prevHeight;
public PID pidController;
public bool stabilize;
public float interval;
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;
float tempForce;
float timer;
void FixedUpdate ()
{
var fb = -CrossPlatformInput.GetAxis("Vertical");
var lr = -CrossPlatformInput.GetAxis("Horizontal");

if(stabilize)
{
timer += Time.deltaTime;
if(timer > interval)
{
velocity = Disk.velocity.y;
acceleration = (velocity) / interval;
tempForce = -(acceleration + 9.81f) * 2;

lastVelocity = velocity;
timer= 0;

}
force = pidController.Update(tempForce , force, interval);

if(force < 0)
force = 0;

}


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);

}
}

I set the interval to 0.05 s and Kp for PID to 2, others to 0. If I directly assign tempForce to force, the disc gradually keeps coming down.

But I keep seeing frequent spikes in the force variable, some times going to 20 N. I guess it's the formula adjusting frequently. Have to find a way so that the force slowly normalizes to the weight of the disc. I have derived the formula from v = u + at, the proof of derivation is attached.


Next I have to do the forward/backward and left/right movements.
Thanks!