diff --git a/Assets/AWSIM/Scripts/Vehicles/FirstOrderLaggedFloat.cs b/Assets/AWSIM/Scripts/Vehicles/FirstOrderLaggedFloat.cs
new file mode 100644
index 000000000..76de4b9b2
--- /dev/null
+++ b/Assets/AWSIM/Scripts/Vehicles/FirstOrderLaggedFloat.cs
@@ -0,0 +1,59 @@
+using System;
+using UnityEngine;
+
+namespace AWSIM
+{
+ public class FirstOrderLaggedFloat
+ {
+ private float timeConstant;
+
+ private float desiredValue;
+ private float currentValue;
+ private float lastTime;
+
+ public float Value
+ {
+ get
+ {
+ Update();
+ return currentValue;
+ }
+ }
+ public float DesiredValue
+ {
+ set
+ {
+ Update();
+ desiredValue = value;
+ }
+ get
+ {
+ return desiredValue;
+ }
+ }
+
+ public FirstOrderLaggedFloat(float timeConstant, float initialValue)
+ {
+ this.timeConstant = timeConstant;
+
+ desiredValue = currentValue = initialValue;
+ lastTime = Time.time;
+ }
+
+ private void Update()
+ {
+ float dt = Time.time - lastTime;
+
+ if (timeConstant == 0.0f)
+ {
+ currentValue = desiredValue;
+ }
+ else
+ {
+ currentValue += (dt / timeConstant) * (desiredValue - currentValue);
+ }
+
+ lastTime = Time.time;
+ }
+ }
+}
\ No newline at end of file
diff --git a/Assets/AWSIM/Scripts/Vehicles/FirstOrderLaggedFloat.cs.meta b/Assets/AWSIM/Scripts/Vehicles/FirstOrderLaggedFloat.cs.meta
new file mode 100644
index 000000000..ec3daa55e
--- /dev/null
+++ b/Assets/AWSIM/Scripts/Vehicles/FirstOrderLaggedFloat.cs.meta
@@ -0,0 +1,11 @@
+fileFormatVersion: 2
+guid: 963ca01e17396605cbc15c9a3f20e472
+MonoImporter:
+ externalObjects: {}
+ serializedVersion: 2
+ defaultReferences: []
+ executionOrder: 0
+ icon: {instanceID: 0}
+ userData:
+ assetBundleName:
+ assetBundleVariant:
diff --git a/Assets/AWSIM/Scripts/Vehicles/Vehicle.cs b/Assets/AWSIM/Scripts/Vehicles/Vehicle.cs
index d39655933..8ff3418e9 100644
--- a/Assets/AWSIM/Scripts/Vehicles/Vehicle.cs
+++ b/Assets/AWSIM/Scripts/Vehicles/Vehicle.cs
@@ -171,11 +171,15 @@ public float SidewaySlipMultipler
// -MaxSteerAngleInput <= SteerAngleInput <= MaxSteerAngleInput.
[Range(0.01f, 80)]
public float MaxSteerAngleInput = 25f;
+ [SerializeField, Min(0.0f), Tooltip("Set 0 to disable the lag")]
+ private float steerAngleTimeConstant = 0.0f;
// Set value to clamp AccelerationInput (m/s^2).
// -MaxAccelerationInput <= AccelerationInput <= MaxAccelerationInput.
[Range(0.01f, 50)]
public float MaxAccelerationInput = 10;
+ [SerializeField, Min(0.0f), Tooltip("Set 0 to disable the lag")]
+ private float accelerationTimeConstant = 0.0f;
[Header("Inputs")]
@@ -189,15 +193,23 @@ public float SidewaySlipMultipler
/// In the plane, output the force that will result in this acceleration.
/// On a slope, it is affected by the slope resistance, so it does not match the input.
///
- // TODO: Compute first order lag
- public float AccelerationInput;
+ public float AccelerationInput
+ {
+ get => acceleration.DesiredValue;
+ set => acceleration.DesiredValue = Mathf.Clamp(value, -MaxAccelerationInput, MaxAccelerationInput);
+ }
+ private FirstOrderLaggedFloat acceleration;
///
/// Vehicle steering input. Tire angle (degree)
/// Negative is left, positive is right turn tire angle.
///
- // TODO: Compute first order lag
- public float SteerAngleInput;
+ public float SteerAngleInput
+ {
+ get => steerAngle.DesiredValue;
+ set => steerAngle.DesiredValue = Mathf.Clamp(value, -MaxSteerAngleInput, MaxSteerAngleInput);
+ }
+ private FirstOrderLaggedFloat steerAngle;
///
/// Vehicle turn signal input. NONE, LEFT, RIGHT, HAZARD.
@@ -209,6 +221,11 @@ public float SidewaySlipMultipler
///
public Vector3 LocalAcceleration { get; private set; }
+ ///
+ /// Vehicle acceleration (m/s^2)
+ ///
+ public float Acceleration => acceleration.Value;
+
///
/// Vehicle speed (m/s)
///
@@ -217,7 +234,7 @@ public float SidewaySlipMultipler
///
/// Vehicle steering angle (degree)
///
- public float SteerAngle => SteerAngleInput;
+ public float SteerAngle => steerAngle.Value;
public float SteerAngleNormalized => SteerAngle / MaxSteerAngleInput;
@@ -296,6 +313,12 @@ void Awake()
// Initialize with non-slip value
ForwardSlipMultipler = 1f;
SidewaySlipMultipler = 1f;
+
+ // Initialize acceleration
+ acceleration = new FirstOrderLaggedFloat(accelerationTimeConstant, 0.0f);
+
+ // Initialize steer angle
+ steerAngle = new FirstOrderLaggedFloat(steerAngleTimeConstant, 0.0f);
}
// GroundSlipMultiplier changes the slip rate.
@@ -322,10 +345,6 @@ private void OnTriggerExit(Collider other)
void FixedUpdate()
{
- // Clamp input values.
- AccelerationInput = Mathf.Clamp(AccelerationInput, -MaxAccelerationInput, MaxAccelerationInput);
- SteerAngleInput = Mathf.Clamp(SteerAngleInput, -MaxSteerAngleInput, MaxSteerAngleInput);
-
// Compute vehicle infomation.
ComputeVehicleState();
@@ -339,8 +358,7 @@ void FixedUpdate()
if (sleep == false)
{
// Update wheel force.
- var acceleration = AccelerationInput;
- UpdateWheelsForce(acceleration);
+ UpdateWheelsForce(Acceleration);
}
// cache value for next frame.