示例#1
0
 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");
         }
     }
 }