dead_reckoner_impl.h

Go to the documentation of this file.

// === dead_reckoner_impl.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_DETAIL_DEAD_RECKONER_IMPL_H
#define SEN_UTIL_DR_DETAIL_DEAD_RECKONER_IMPL_H
 
// sen
#include "sen/util/dr/algorithms.h"
 
// std
#include <type_traits>
 
namespace sen::util::impl
{


template <typename T, typename = void>
struct HasCommitTime: std::false_type
{
};
 
template <typename T>
struct HasCommitTime<T, std::void_t<decltype(std::declval<const T&>().asObject().getLastCommitTime())>>: std::true_type
{
};

template <typename T>
[[nodiscard]] T toRpr(const Location& value);

template <typename T>
[[nodiscard]] T toRpr(const Orientation& value);

template <typename T>
[[nodiscard]] T toRpr(const Velocity& value);

template <typename T>
[[nodiscard]] T toRpr(const Acceleration& value);

template <typename T>
[[nodiscard]] T toRpr(const AngularVelocity& value);

template <typename T>
[[nodiscard]] Location fromRprLocation(const T& value);

template <typename T>
[[nodiscard]] Orientation fromRprOrientation(const T& value);

template <typename T>
[[nodiscard]] Velocity fromRprVelocity(const T& value);

template <typename T>
[[nodiscard]] Acceleration fromRprAcceleration(const T& value);

template <typename T>
[[nodiscard]] AngularVelocity fromRprAngularVelocity(const T& value);

[[nodiscard]] GeodeticWorldLocation toLla(const Location& worldLocation) noexcept;

[[nodiscard]] Location toEcef(const GeodeticWorldLocation& latLonAlt) noexcept;

[[nodiscard]] Orientation ecefToNed(const Orientation& value, const GeodeticWorldLocation& latLonAlt) noexcept;

[[nodiscard]] Velocity ecefToNed(const Velocity& value, const GeodeticWorldLocation& geodeticPosition) noexcept;

[[nodiscard]] Acceleration ecefToNed(const Acceleration& value, const GeodeticWorldLocation& geodeticPosition) noexcept;

[[nodiscard]] Orientation nedToEcef(const Orientation& value, const GeodeticWorldLocation& latLonAlt) noexcept;

[[nodiscard]] Velocity nedToEcef(const Velocity& value, const GeodeticWorldLocation& location) noexcept;

[[nodiscard]] Acceleration nedToEcef(const Acceleration& value, const GeodeticWorldLocation& location) noexcept;

[[nodiscard]] GeodeticSituation toGeodeticSituation(const Situation& value) noexcept;

[[nodiscard]] Situation toSituation(const GeodeticSituation& value) noexcept;

[[nodiscard]] Situation toSituation(const GeodeticSituation& value,
                                    const Location& ecefPosition,
                                    const Orientation& ecefOrientation) noexcept;

[[nodiscard]] Velocity bodyToNed(const Velocity& value, const Orientation& orientationNed) noexcept;

[[nodiscard]] AngularVelocity bodyToNed(const AngularVelocity& value, const Orientation& orientationNed) noexcept;

[[nodiscard]] AngularAcceleration bodyToNed(const AngularAcceleration& value,
                                            const Orientation& orientationNed) noexcept;

[[nodiscard]] Acceleration bodyToNed(const Acceleration& value, const Orientation& orientationNed) noexcept;

void smoothImpl(Situation& situation, const Situation& update, const DrConfig& config);

[[nodiscard]] LengthMeters computeDistance(const Location& start, const Location& end);

 
//-------------------------------------------------------------------------------------------------------------------
// Inline implementation
//-------------------------------------------------------------------------------------------------------------------
 
template <typename T>
inline T toRpr(const Location& value)
{
  return {value.x, value.y, value.z};
}
 
template <typename T>
inline T toRpr(const Orientation& value)
{
  return {value.psi, value.theta, value.phi};
}
 
template <typename T>
inline T toRpr(const Velocity& value)
{
  return {value.x, value.y, value.z};
}
 
template <typename T>
inline T toRpr(const Acceleration& value)
{
  return {value.x, value.y, value.z};
}
 
template <typename T>
inline T toRpr(const AngularVelocity& value)
{
  return {value.x, value.y, value.z};
}
 
template <typename T>
inline Location fromRprLocation(const T& value)
{
  return {value.x, value.y, value.z};
}
 
template <typename T>
inline Orientation fromRprOrientation(const T& value)
{
  return {value.psi, value.theta, value.phi};
}

template <typename T>
inline Velocity fromRprVelocity(const T& value)
{
  return {value.xVelocity, value.yVelocity, value.zVelocity};
}

template <typename T>
inline Acceleration fromRprAcceleration(const T& value)
{
  return {value.xAcceleration, value.yAcceleration, value.zAcceleration};
}

template <typename T>
inline AngularVelocity fromRprAngularVelocity(const T& value)
{
  return {value.xAngularVelocity, value.yAngularVelocity, value.zAngularVelocity};
}
 
}  // namespace sen::util::impl
 
#endif  // SEN_UTIL_DR_DETAIL_DEAD_RECKONER_IMPL_H