ImuComponent.h

// Copyright Mars Rover 2025
 
#pragma once
 
#include "CoreMinimal.h"
#include "GameFramework/Actor.h"
#include "ImuComponent.generated.h"
 
class URoveCommUDPWrapper;
class URoveCommManifestWrapper;
 
/** 
 * @brief IMU DESC
 */
UCLASS(ClassGroup = (Custom), meta = (BlueprintSpawnableComponent))
class ROVESOSIMULATOR_API UImuComponent : public UActorComponent
{
  GENERATED_BODY()
 
public:
  UImuComponent();
 
  UFUNCTION(BlueprintCallable, Category = "RoveComm")
  void InitializeRoveComm(URoveCommUDPWrapper* InUDPWrapper, URoveCommManifestWrapper* InManifestWrapper);
 
  // Expose IMU data to blueprints for debugging/monitoring
  UPROPERTY(BlueprintReadOnly, Category = "IMU")
  FVector LinearAcceleration;
 
  UPROPERTY(BlueprintReadOnly, Category = "IMU")
  FVector AngularVelocity;
 
  UPROPERTY(BlueprintReadOnly, Category = "IMU")
  FQuat Orientation;
 
  // Option to include gravity in acceleration (real IMUs measure gravity)
  UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "IMU")
  bool bIncludeGravity = true;
 
  // Gravity magnitude in m/s^2 (default Earth gravity)
  UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "IMU")
  float GravityMagnitude = 9.81f;
 
  UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "RoveComm")
  URoveCommUDPWrapper* UDPWrapper;
 
  UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "RoveComm")
  URoveCommManifestWrapper* ManifestWrapper;
 
  // The IP address and Port to send the IMU data to.
  UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "RoveComm")
  FString TargetIPAddress = "127.0.0.1";
 
  UPROPERTY(EditAnywhere, BlueprintReadWrite, Category = "RoveComm")
  int32 TargetPort = 11000;
 
 
protected:
  virtual void BeginPlay() override;
 
public:
  virtual void TickComponent(float DeltaTime, ELevelTick TickType, FActorComponentTickFunction* ThisTickFunction) override;
 
private:
  // State variables to store data from the previous frame
  FVector LastVelocity;
  FTransform LastTransform;
  bool bIsFirstFrame;
 
  // Private function to perform the calculations
  void UpdateImuData(float DeltaTime);
 
  // Function to handle coordinate system transformation
  void TransformToZedImuFrame(
    const FVector& InLinearAcceleration,
    const FVector& InAngularVelocity,
    const FQuat& InOrientation,
    FVector& OutLinearAcceleration,
    FVector& OutAngularVelocity,
    FQuat& OutOrientation
  );
 
  // Helper function to calculate angular velocity from quaternion delta
  FVector CalculateAngularVelocity(const FQuat& CurrentQuat, const FQuat& PreviousQuat, float DeltaTime);
 
  bool bIsRoveCommInitialized;
  int32 AccelDataId;
  int32 ImuDataId;
  int32 GyroDataId;
  int32 QuatDataId;
};