public void ControlUpdate(motor[] motors, Transmitter tx, Sensors sensors)
{
if (armed)
{
if (Modes.BARO_MODE)
{//35 degrees max inclination
roll.Control(sensors.GetRotation().x, sensors.GetAngularVelocity().x, -tx.GetRoll() * 35);
pitch.Control(sensors.GetRotation().z, sensors.GetAngularVelocity().z, -tx.GetPitch() * 35);
yaw.Control(sensors.GetRotation().y, sensors.GetAngularVelocity().y, tx.GetYaw() * 50);
}
//Calculates and applies the force to the motors
for (int i = 0; i < 4; i++)
{
//General force (throttle)
motors[i].force = tx.GetThrottle() * 2.0F; //:) This should take into account the air density, air temp, altitude, batteries charge, etc.
//Pitch force
motors[i].force -= motors[i].position.x * (pitch.p + pitch.i + pitch.d);
//Roll force
motors[i].force += motors[i].position.z * (roll.p + roll.i + roll.d);// + (-0.1F + Random.value*0.2F);
}
if (tx.GetYaw() < -0.9 && tx.GetThrottle() < 0.1)
{
contCyclesBeforeArmUnarm++;
}
else
{
contCyclesBeforeArmUnarm = 0;
}
if (contCyclesBeforeArmUnarm >= 100)
{
armed = false;
Debug.Log("UNARMED");
}
}
else
{
for (int i = 0; i < 4; i++)
{
motors[i].force = 0f;
}
if (tx.GetYaw() > 0.9 && tx.GetThrottle() < 0.1)
{
contCyclesBeforeArmUnarm++;
}
else
{
contCyclesBeforeArmUnarm = 0;
}
if (contCyclesBeforeArmUnarm >= 100)
{
armed = true;
Debug.Log("ARMED");
}
}
}