#include <geo_utils.hpp>

Collaboration diagram for cl_px4_mr::MapProjection:

Public Member Functions
	MapProjection ()=default

	MapProjection (double lat0, double lon0, uint64_t timestamp=0)

void	initReference (double lat0, double lon0, uint64_t timestamp=0)

bool	isInitialized () const

uint64_t	referenceTimestamp () const

double	referenceLat () const

double	referenceLon () const

void	project (double lat, double lon, float &x, float &y) const

void	reproject (float x, float y, double &lat, double &lon) const

Private Attributes
uint64_t	refTimestamp_ = 0

double	refLat_ = 0.0

double	refLon_ = 0.0

double	refSinLat_ = 0.0

double	refCosLat_ = 1.0

bool	initialized_ = false

Detailed Description

Definition at line 69 of file geo_utils.hpp.

Constructor & Destructor Documentation

◆ MapProjection() [1/2]

cl_px4_mr::MapProjection::MapProjection ( )

default

◆ MapProjection() [2/2]

cl_px4_mr::MapProjection::MapProjection	(	double	lat0,
		double	lon0,
		uint64_t	timestamp = 0 )

inline

Definition at line 74 of file geo_utils.hpp.

  {
    initReference(lat0, lon0, timestamp);
  }

References initReference().

Here is the call graph for this function:

Member Function Documentation

◆ initReference()

void cl_px4_mr::MapProjection::initReference	(	double	lat0,
		double	lon0,
		uint64_t	timestamp = 0 )

inline

Definition at line 79 of file geo_utils.hpp.

  {
    refTimestamp_ = timestamp;
    refLat_ = geo_detail::radians(lat0);
    refLon_ = geo_detail::radians(lon0);
    refSinLat_ = std::sin(refLat_);
    refCosLat_ = std::cos(refLat_);
    initialized_ = true;
  }

References initialized_, cl_px4_mr::geo_detail::radians(), refCosLat_, refLat_, refLon_, refSinLat_, and refTimestamp_.

Referenced by MapProjection(), and cl_px4_mr::CpVehicleLocalPosition::onPositionMessage().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ isInitialized()

bool cl_px4_mr::MapProjection::isInitialized ( ) const

inline

Definition at line 89 of file geo_utils.hpp.

89{ return initialized_; }

References initialized_.

Referenced by cl_px4_mr::CpVehicleLocalPosition::globalRefValid(), cl_px4_mr::CpVehicleLocalPosition::onPositionMessage(), cl_px4_mr::CpVehicleLocalPosition::projectToNed(), and cl_px4_mr::CpVehicleLocalPosition::reprojectFromNed().

Here is the caller graph for this function:

◆ project()

void cl_px4_mr::MapProjection::project	(	double	lat,
		double	lon,
		float &	x,
		float &	y ) const

inline

Definition at line 95 of file geo_utils.hpp.

  {
    const double latRad = geo_detail::radians(lat);
    const double lonRad = geo_detail::radians(lon);
 
    const double sinLat = std::sin(latRad);
    const double cosLat = std::cos(latRad);
    const double cosDLon = std::cos(lonRad - refLon_);
 
    const double arg =
      geo_detail::constrain(refSinLat_ * sinLat + refCosLat_ * cosLat * cosDLon, -1.0, 1.0);
    const double c = std::acos(arg);
 
    double k = 1.0;
    if (std::fabs(c) > 0.0)
    {
      k = c / std::sin(c);
    }
 
    x =
      static_cast<float>(k * (refCosLat_ * sinLat - refSinLat_ * cosLat * cosDLon) * kEarthRadiusM);
    y = static_cast<float>(k * cosLat * std::sin(lonRad - refLon_) * kEarthRadiusM);
  }

References cl_px4_mr::geo_detail::constrain(), cl_px4_mr::kEarthRadiusM, cl_px4_mr::geo_detail::radians(), refCosLat_, refLon_, and refSinLat_.

Referenced by cl_px4_mr::CpVehicleLocalPosition::projectToNed().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ referenceLat()

double cl_px4_mr::MapProjection::referenceLat ( ) const

inline

Definition at line 91 of file geo_utils.hpp.

91{ return geo_detail::degrees(refLat_); }

cl_px4_mr::geo_detail::degrees

double degrees(double rad)

Definition geo_utils.hpp:64

References cl_px4_mr::geo_detail::degrees(), and refLat_.

Here is the call graph for this function:

◆ referenceLon()

double cl_px4_mr::MapProjection::referenceLon ( ) const

inline

Definition at line 92 of file geo_utils.hpp.

92{ return geo_detail::degrees(refLon_); }

References cl_px4_mr::geo_detail::degrees(), and refLon_.

Here is the call graph for this function:

◆ referenceTimestamp()

uint64_t cl_px4_mr::MapProjection::referenceTimestamp ( ) const

inline

Definition at line 90 of file geo_utils.hpp.

90{ return refTimestamp_; }

References refTimestamp_.

◆ reproject()

void cl_px4_mr::MapProjection::reproject	(	float	x,
		float	y,
		double &	lat,
		double &	lon ) const

inline

Definition at line 120 of file geo_utils.hpp.

  {
    const double xRad = static_cast<double>(x) / kEarthRadiusM;
    const double yRad = static_cast<double>(y) / kEarthRadiusM;
    const double c = std::sqrt(xRad * xRad + yRad * yRad);
 
    if (std::fabs(c) > 0.0)
    {
      const double sinC = std::sin(c);
      const double cosC = std::cos(c);
 
      const double latRad = std::asin(cosC * refSinLat_ + (xRad * sinC * refCosLat_) / c);
      const double lonRad =
        refLon_ + std::atan2(yRad * sinC, c * refCosLat_ * cosC - xRad * refSinLat_ * sinC);
 
      lat = geo_detail::degrees(latRad);
      lon = geo_detail::degrees(lonRad);
    }
    else
    {
      lat = geo_detail::degrees(refLat_);
      lon = geo_detail::degrees(refLon_);
    }
  }