private void KeepAttitude()
{
pidControllerAttitudeX.Control(globalRotation.X, ref attitude_dx);
pidControllerAttitudeZ.Control(globalRotation.Z, ref attitude_dz);
PhysicsEntity.ApplyTorque(new Vector3(attitude_dx, 0.0f, attitude_dz), false);
}
private void SimulateWind()
{
// Calculate wind intensity fluctuation by the settings
float windIntensityFluctuation = settingsWindIntensityFluctuation / 1000.0f;
float windDirectionFluctuation = settingsWindDirectionFluctuation / 1000.0f;
// Constant wind
float constantWind = sngConstantWind.GenerateNext_Between0And1(windIntensityFluctuation);
constantWind /= 2.0f;
constantWind *= (settingsWindIntensity / 2.0f);
// Wind gust
float windGust = sngWindGust.GenerateNext_OnlyPositive(windIntensityFluctuation * 3.0f);
windGust *= settingsWindIntensity;
totalWindIntensity = constantWind + windGust;
float windDirectionChange = 5.0f * sngWindDirectionFluctuation.GenerateNext(windDirectionFluctuation);
currentWindDirectionAngle = settingsWindDirection + windDirectionChange;
// Create a wind force vector with length 1 and rotate so the wind affects the multicopter from a certain direction
windForceVector = MathHelper.RotateAroundAxis_Y(
new Vector3(1.0f, 0.0f, 0.0f),
new Vector3(),
xna.MathHelper.ToRadians(currentWindDirectionAngle)
);
windForceVector = MathHelper.RotateAroundAxis_X(
windForceVector,
new Vector3(),
xna.MathHelper.ToRadians(windDirectionChange)
);
// Generate oscillation of the multicopter arms
torqueVector.X = sngAttitudeDisturbances.GenerateNext(Globals.RandomGenerator.Next(100, 300) / 10000.0f) / 250.0f;
torqueVector.Y = sngAttitudeDisturbances.GenerateNext(Globals.RandomGenerator.Next(100, 300) / 10000.0f) / 300.0f;
torqueVector.Z = sngAttitudeDisturbances.GenerateNext(Globals.RandomGenerator.Next(100, 300) / 10000.0f) / 250.0f;
// Scale the direction vector with the intensity
windForceVector = Vector3.Scale(windForceVector, totalWindIntensity);
torqueVector = Vector3.Scale(torqueVector, totalWindIntensity + 0.5f);
// Let the global wind force and torque affect the multicopter
PhysicsEntity.ApplyForce(windForceVector, false);
PhysicsEntity.ApplyTorque(torqueVector, false);
// Sum up for debug display
totalForceVector += windForceVector;
}
/// <summary>
/// The frameUpdate method gets called every frame iteration
/// </summary>
/// <param name="frameUpdate">Additional data provided with the current frame</param>
public override void Update(FrameUpdate frameUpdate)
{
// Update state for us and all the shapes that make up the rigid body
PhysicsEntity.UpdateState(true);
// Calculate the raw values by the controller input
raw_thrust += 0.025f * MAX_THRUST * raw_leftY;
raw_yaw = -raw_leftX;
raw_pitch = raw_rightY;
raw_roll = raw_rightX;
// Limit too high thrust values
if (raw_thrust < 0.0f)
{
raw_thrust = 0.0f;
}
else if (raw_thrust > MAX_THRUST)
{
raw_thrust = MAX_THRUST;
}
// Calculate the thrust ratio (used for motor sound and rotation behaviour)
thrust_ratio = raw_thrust / MAX_THRUST;
// Clear total force vector for the plot
totalForceVector.X = 0.0f;
totalForceVector.Y = 0.0f;
totalForceVector.Z = 0.0f;
// Rotation torque
torqueVector.X = raw_roll / 20;
torqueVector.Y = raw_yaw / 10;
torqueVector.Z = raw_pitch / 20;
PhysicsEntity.ApplyTorque(torqueVector, true);
// Calculate the total rotation (for sound effects an propeller)
float rotationFactor = Math.Abs(torqueVector.X) + Math.Abs(torqueVector.Y) + Math.Abs(torqueVector.Z);
// Let the linear force affect the multicopter
foreach (PropellerEntity propellerEntity in propellerEntities)
{
// Calculate the force vector for the current propeller
forceVector = Vector3.Scale(propellerEntity.forceDirectionVector, raw_thrust * THRUST_TO_FORCE_FACTOR);
// Let the linear force act on every single propeller
PhysicsEntity.ApplyForceAtPosition(
forceVector,
propellerEntity.localPosition,
true,
true
);
// Calculate the force to the current force vector
totalForceVector += forceVector;
// Let the propellers rotate depending on the linearForce
propellerEntity.pitchRate = (raw_thrust + (10 * rotationFactor)) * 100;
// Update the propeller entity
propellerEntity.Update(frameUpdate);
}
#region Wind Force
if (windSimulationActivated)
{
SimulateWind();
}
#endregion
#region Auto stabilization
globalRotation = MathHelper.RotateAroundAxis_Y(Rotation, new xna.Vector3(), xna.MathHelper.ToRadians(-Rotation.Y));
// Use PID algorithm to keep the attitude
if (keepAttitude && raw_rightX == 0 && raw_rightY == 0)
{
KeepAttitude();
}
// Use PID algorithm to keep altitude
if (keepAltitude && raw_leftY == 0.0f)
{
KeepAltitude();
}
// Keep position
if (keepPosition && raw_rightX == 0 && raw_rightY == 0)
{
KeepPosition();
}
else
{
pidControllerAttitudeX.referenceValue = 0.0f;
pidControllerAttitudeZ.referenceValue = 0.0f;
}
#endregion
#region Update sound
// Update sound
float newVolume = thrust_ratio + rotationFactor;
soundEntity.Volume = (newVolume > 1.0f) ? 1.0f : newVolume; // TODO In die SoundEntity machen
soundEntity.Pitch = thrust_ratio + rotationFactor * 2;
soundEntity.AudioEmitterPosition = Position;
soundEntity.AudioListenerPosition = frameUpdate.ActiveCamera.Eye;
//soundEntity.AudioListenerLookAt = frameUpdate.ActiveCamera.LookAt; // TODO Schauen was hier falsch läuft
soundEntity.Update3DSound();
#endregion
#region Debug result
// Debug result
updateDebugData.applicationTime = frameUpdate.ApplicationTime;
updateDebugData.position = Position;
updateDebugData.velocity = State.Velocity;
updateDebugData.linearForce = totalForceVector;
//updateDebugData.rotation = Rotation;
updateDebugData.rotation = globalRotation;
updateDebugData.angularVelocity = State.AngularVelocity;
updateDebugData.rotationTorque = torqueVector;
updateDebugData.thrust = raw_thrust;
updateDebugData.pidControllerAttitudeXValue = pidControllerAttitudeX.DebugOutputValue;
updateDebugData.pidControllerAttitudeZValue = pidControllerAttitudeZ.DebugOutputValue;
updateDebugData.pidControllerAltitudeValue = pidControllerAltitude.DebugOutputValue;
updateDebugData.pidControllerPositionXValue = pidControllerPositionX.DebugOutputValue;
updateDebugData.pidControllerPositionZValue = pidControllerPositionZ.DebugOutputValue;
updateDebugData.windDirection = currentWindDirectionAngle;
updateDebugData.windIntensity = totalWindIntensity;
windowStatisticsEventsPort.Post(updateDebugData);
#endregion
#region Stats result
// Stats result
updateStatsData.thrust = raw_thrust;
updateStatsData.thrust_ratio = thrust_ratio;
updateStatsData.altitude = Position.Y;
updateStatsData.speedVector = State.Velocity;
windowControlEventsPort.Post(updateStatsData);
#endregion
// Simulation engine will frameUpdate children
base.Update(frameUpdate);
}
