algorithms.h

Go to the documentation of this file.

// === algorithms.h ====================================================================================================
//                                               Sen Infrastructure
//                   Released under the Apache License v2.0 (SPDX-License-Identifier Apache-2.0).
//                                    See the LICENSE.txt file for more information.
//                   © Airbus SAS, Airbus Helicopters, and Airbus Defence and Space SAU/GmbH/SAS.
// =====================================================================================================================
 
#ifndef SEN_UTIL_DR_ALGORITHMS_H
#define SEN_UTIL_DR_ALGORITHMS_H
 
// sen
#include "sen/core/base/duration.h"
#include "sen/core/base/numbers.h"
#include "sen/core/base/quantity.h"
#include "sen/core/base/timestamp.h"
 
// std
#include <array>
 
namespace sen::util
{


SEN_NON_RANGED_QUANTITY(LengthMeters, f64)
 

SEN_NON_RANGED_QUANTITY(VelocityMetersPerSecond, f32)
 

SEN_NON_RANGED_QUANTITY(AccelerationMetersPerSecondSquared, f32)
 

SEN_NON_RANGED_QUANTITY(AngleRadians, f32)
 

SEN_NON_RANGED_QUANTITY(AngularVelocityRadiansPerSecond, f32)
 

SEN_NON_RANGED_QUANTITY(AngularAccelerationRadiansPerSecondSquared, f32)
 

SEN_RANGED_QUANTITY(LatitudeDegrees, f64, -90.0, 90.0)
 

SEN_RANGED_QUANTITY(LongitudeDegrees, f64, -180.0, 180.0)

SEN_NON_RANGED_QUANTITY(TimeSeconds, f64)

SEN_NON_RANGED_QUANTITY(DampingCoefficient, f64)

enum class SpatialAlgorithm
{
  drStatic = 0,
  drFPW = 1,
  drRPW = 2,
  drRVW = 3,
  drFVW = 4,
  drFPB = 5,
  drRPB = 6,
  drRVB = 7,
  drFVB = 8,
};
 
constexpr std::array<SpatialAlgorithm, 4U> bodyAlgorithms {SpatialAlgorithm::drFPB,
                                                           SpatialAlgorithm::drRPB,
                                                           SpatialAlgorithm::drRVB,
                                                           SpatialAlgorithm::drFVB};
 
// The structs below are also tabulated in docs/users_guide/util_library.md, with their
// units and frames spelled out for readers who do not read C++. Update both together.
// --8<-- [start:dr_config]
struct DrConfig
{
  bool smoothing = true;

  LengthMeters maxDistance = 100000.0;

  sen::Duration maxDeltaTime {std::chrono::seconds(1)};

  sen::Duration smoothingInterval {std::chrono::milliseconds(20)};

  sen::Duration positionConvergenceTime {std::chrono::milliseconds(500)};

  DampingCoefficient positionDamping = 1.0;

  sen::Duration orientationConvergenceTime {std::chrono::milliseconds(50)};

  DampingCoefficient orientationDamping = 20.0;

  bool useCommitTimeAsOrigin = true;

  SpatialAlgorithm algorithm = SpatialAlgorithm::drRVW;
};
// --8<-- [end:dr_config]

struct Location
{
  LengthMeters x;
  LengthMeters y;
  LengthMeters z;
};

struct GeodeticWorldLocation
{
  LatitudeDegrees latitude;
  LongitudeDegrees longitude;
  LengthMeters altitude;
};

struct Orientation
{
  AngleRadians psi;
  AngleRadians theta;
  AngleRadians phi;
};

struct Velocity
{
  VelocityMetersPerSecond x;
  VelocityMetersPerSecond y;
  VelocityMetersPerSecond z;
};

struct Acceleration
{
  AccelerationMetersPerSecondSquared x;
  AccelerationMetersPerSecondSquared y;
  AccelerationMetersPerSecondSquared z;
};

struct AngularVelocity
{
  AngularVelocityRadiansPerSecond x;
  AngularVelocityRadiansPerSecond y;
  AngularVelocityRadiansPerSecond z;
};

struct AngularAcceleration
{
  AngularAccelerationRadiansPerSecondSquared x;
  AngularAccelerationRadiansPerSecondSquared y;
  AngularAccelerationRadiansPerSecondSquared z;
};
 
// --8<-- [start:situation]
struct Situation
{
  bool isFrozen = false;

  sen::TimeStamp timeStamp;

  Location worldLocation {};

  Orientation orientation {};

  Velocity velocityVector {};

  AngularVelocity angularVelocity {};

  Acceleration accelerationVector {};

  AngularAcceleration angularAcceleration {};
};
// --8<-- [end:situation]
 
// --8<-- [start:geodetic_situation]
struct GeodeticSituation
{
  bool isFrozen = false;

  sen::TimeStamp timeStamp;

  GeodeticWorldLocation worldLocation {};

  Orientation orientation {};

  Velocity velocityVector {};

  AngularVelocity angularVelocity {};

  Acceleration accelerationVector {};

  AngularAcceleration angularAcceleration {};
};
// --8<-- [end:geodetic_situation]

[[nodiscard]] Situation drFpw(const Situation& value, sen::TimeStamp time) noexcept;

[[nodiscard]] Situation drFpb(const Situation& value, sen::TimeStamp time) noexcept;

[[nodiscard]] Situation drRpw(const Situation& value, sen::TimeStamp time) noexcept;

[[nodiscard]] Situation drRpb(const Situation& value, sen::TimeStamp time) noexcept;

[[nodiscard]] Situation drRvw(const Situation& value, sen::TimeStamp time) noexcept;

[[nodiscard]] Situation drRvb(const Situation& value, sen::TimeStamp time) noexcept;

[[nodiscard]] Situation drFvw(const Situation& value, sen::TimeStamp time) noexcept;

[[nodiscard]] Situation drFvb(const Situation& value, sen::TimeStamp time) noexcept;

 
}  // namespace sen::util
 
#endif  // SEN_UTIL_DR_ALGORITHMS_H