Merge pull request #313 from mateusz-lichota/improve-wheel-rpm-calcul…

…ation

Improve wheel rpm calculation

UE4Project/Plugins/AirSim/Source/Vehicles/Car/CarPawnSimApi.cpp

@@ -76,7 +76,7 @@ void CarPawnSimApi::createVehicleApi(ACarPawn* pawn, const msr::airlib::GeoPoint
 //these are called on render ticks
 void CarPawnSimApi::updateRenderedState(float dt)
 {
- updateWheelStates(dt);
+ updateWheelStates();
  updateKinematics(dt);
  vehicle_api_->getStatusMessages(vehicle_api_messages_);
 
@@ -596,29 +596,34 @@ float computeAngleChange(float prev_rotation_angle_rad, float new_rotation_angle
  return new_rotation_angle_rad + (new_rotation_angle_rad > prev_rotation_angle_rad ? 0.0f : 2.0f * M_PI) - prev_rotation_angle_rad;
 }
 
-void CarPawnSimApi::updateWheelStates(float dt)
+void CarPawnSimApi::updateWheelStates()
 {
  wheel_states_->time_stamp = msr::airlib::ClockFactory::get()->nowNanos();
 
+ physx::PxVehicleWheelsDynData& mWheelsDynData = movement_->PVehicle->mWheelsDynData;
+
  const float unreal_rotation_angle_to_radians = (2 * M_PI) / -1800.0;
  const float unreal_steering_angle_to_radians = (M_PI) / 180.0;
  const float radians_per_second_to_rpm = 60.0 / (2 * M_PI);
 
- //update kinematics from pawn's movement instead of physics engine
- float fl_new_rotation_angle_rad = movement_->Wheels[0]->GetRotationAngle() * unreal_rotation_angle_to_radians;
- float fr_new_rotation_angle_rad = movement_->Wheels[1]->GetRotationAngle() * unreal_rotation_angle_to_radians;
- float rl_new_rotation_angle_rad = movement_->Wheels[2]->GetRotationAngle() * unreal_rotation_angle_to_radians;
- float rr_new_rotation_angle_rad = movement_->Wheels[3]->GetRotationAngle() * unreal_rotation_angle_to_radians;
-
- wheel_states_->fl.rpm = 0.3 * wheel_states_->fl.rpm + 0.7 * computeAngleChange(wheel_states_->fl.rotation_angle, fl_new_rotation_angle_rad) / dt * radians_per_second_to_rpm;
- wheel_states_->fr.rpm = 0.3 * wheel_states_->fr.rpm + 0.7 * computeAngleChange(wheel_states_->fr.rotation_angle, fr_new_rotation_angle_rad) / dt * radians_per_second_to_rpm;
- wheel_states_->rl.rpm = 0.3 * wheel_states_->rl.rpm + 0.7 * computeAngleChange(wheel_states_->rl.rotation_angle, rl_new_rotation_angle_rad) / dt * radians_per_second_to_rpm;
- wheel_states_->rr.rpm = 0.3 * wheel_states_->rr.rpm + 0.7 * computeAngleChange(wheel_states_->rr.rotation_angle, rr_new_rotation_angle_rad) / dt * radians_per_second_to_rpm;
-
- wheel_states_->fl.rotation_angle = fl_new_rotation_angle_rad;
- wheel_states_->fr.rotation_angle = fr_new_rotation_angle_rad;
- wheel_states_->rl.rotation_angle = rl_new_rotation_angle_rad;
- wheel_states_->rr.rotation_angle = rr_new_rotation_angle_rad;
+ // at very low velocities the angular velocity becomes very unstable,
+ // so a higher smoothing factor is needed to obtain a usable estimate
+ float higher_smoothing_factor_rpm_threshold = 80; 
+ float avg_rear_wheel_rpm = (wheel_states_->rr.rpm + wheel_states_->rl.rpm) / 2;
+ float rpm_smoothing_coeff = 0.7;
+ if (avg_rear_wheel_rpm < higher_smoothing_factor_rpm_threshold) {
+ rpm_smoothing_coeff = 0.9;
+ }
+
+ wheel_states_->fl.rpm = wheel_states_->fl.rpm * (rpm_smoothing_coeff) + (1 - rpm_smoothing_coeff) * OmegaToRPM(mWheelsDynData.getWheelRotationSpeed(0));
+ wheel_states_->fr.rpm = wheel_states_->fl.rpm * (rpm_smoothing_coeff) + (1 - rpm_smoothing_coeff) * OmegaToRPM(mWheelsDynData.getWheelRotationSpeed(1));
+ wheel_states_->rl.rpm = wheel_states_->fl.rpm * (rpm_smoothing_coeff) + (1 - rpm_smoothing_coeff) * OmegaToRPM(mWheelsDynData.getWheelRotationSpeed(2));
+ wheel_states_->rr.rpm = wheel_states_->fl.rpm * (rpm_smoothing_coeff) + (1 - rpm_smoothing_coeff) * OmegaToRPM(mWheelsDynData.getWheelRotationSpeed(3));
+
+ wheel_states_->fl.rotation_angle = mWheelsDynData.getWheelRotationAngle(0);
+ wheel_states_->fr.rotation_angle = mWheelsDynData.getWheelRotationAngle(1);
+ wheel_states_->rl.rotation_angle = mWheelsDynData.getWheelRotationAngle(2);
+ wheel_states_->rr.rotation_angle = mWheelsDynData.getWheelRotationAngle(3);
 
  wheel_states_->fl.steering_angle = movement_->Wheels[0]->GetSteerAngle() * unreal_steering_angle_to_radians;
  wheel_states_->fr.steering_angle = movement_->Wheels[1]->GetSteerAngle() * unreal_steering_angle_to_radians;

UE4Project/Plugins/AirSim/Source/Vehicles/Car/CarPawnSimApi.h

@@ -159,7 +159,7 @@ class CarPawnSimApi : public msr::airlib::VehicleSimApiBase
  void plot(std::istream& s, FColor color, const Vector3r& offset);
  CarPawnSimApi::Pose toPose(const FVector& u_position, const FQuat& u_quat) const;
  void updateKinematics(float dt);
- void updateWheelStates(float dt);
+ void updateWheelStates();
  void setStartPosition(const FVector& position, const FRotator& rotator);
 private:
  Params params_;