From 1eb593d1e2971cb97eba4e4bf0bb59c8e1062bfc Mon Sep 17 00:00:00 2001 From: Mateusz Lichota Date: Sun, 28 Aug 2022 15:08:40 +0200 Subject: [PATCH] Improve wheel rpm calculation --- .../Source/Vehicles/Car/CarPawnSimApi.cpp | 39 +++++++++++-------- .../Source/Vehicles/Car/CarPawnSimApi.h | 2 +- 2 files changed, 23 insertions(+), 18 deletions(-) diff --git a/UE4Project/Plugins/AirSim/Source/Vehicles/Car/CarPawnSimApi.cpp b/UE4Project/Plugins/AirSim/Source/Vehicles/Car/CarPawnSimApi.cpp index 7570cf69..425d04c5 100644 --- a/UE4Project/Plugins/AirSim/Source/Vehicles/Car/CarPawnSimApi.cpp +++ b/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; diff --git a/UE4Project/Plugins/AirSim/Source/Vehicles/Car/CarPawnSimApi.h b/UE4Project/Plugins/AirSim/Source/Vehicles/Car/CarPawnSimApi.h index 53da60fd..3f5a690a 100644 --- a/UE4Project/Plugins/AirSim/Source/Vehicles/Car/CarPawnSimApi.h +++ b/UE4Project/Plugins/AirSim/Source/Vehicles/Car/CarPawnSimApi.h @@ -160,7 +160,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_;