Ashin Walpola
0e9525162d
obu-firmware builds against vanetza-idf from microbu-esp32c5/external, but that tree was gitignored, so a clone of this repository could not build the firmware it ships. It is now committed here as ordinary files in its own folder, microbu-esp32c5/: the colleague's commit cf4b99f plus the V2X2MAP bridge's signature verification (--trust) used on the bench. Nothing is fetched from or pushed to the colleague's repository; this repository and its remotes carry everything. The folder's own .gitignore keeps build output, downloaded components and private key material out, as it did there; the committed file set is identical to that repository's tracked files. The ESP32-C5 is still flashed from obu-firmware/, which only takes vanetza-idf from microbu-esp32c5/, so the two stay separate folders. FLASHING.md says how to take a newer version of the colleague's tree (copy it over the folder, rebuild, test, commit).
50 lines
1.4 KiB
C++
50 lines
1.4 KiB
C++
#include <vanetza/geodesy/haversine.hpp>
|
|
#include <boost/units/cmath.hpp>
|
|
#include <cmath>
|
|
#include <limits>
|
|
|
|
namespace vanetza
|
|
{
|
|
namespace geodesy
|
|
{
|
|
namespace haversine
|
|
{
|
|
|
|
// arithmetic mean radius of WGS84 ellipsoid
|
|
static const units::Length earth_radius = 6371008.8 * units::si::meter;
|
|
|
|
units::Length distance(const GeodeticPosition& a, const GeodeticPosition& b)
|
|
{
|
|
using boost::units::sin;
|
|
using boost::units::cos;
|
|
|
|
const units::Angle delta_phi { b.latitude - a.latitude };
|
|
const units::Angle delta_lambda { b.longitude - a.longitude };
|
|
|
|
const auto sin_dphi = sin(delta_phi / 2.0);
|
|
const auto sin_dlambda = sin(delta_lambda / 2.0);
|
|
const auto h = sin_dphi * sin_dphi + cos(a.latitude) * cos(b.latitude) * sin_dlambda * sin_dlambda;
|
|
const auto c = 2.0 * std::atan2(std::sqrt(h), std::sqrt(1.0 - h));
|
|
|
|
return earth_radius * c;
|
|
}
|
|
|
|
CartesianPosition local_cartesian(
|
|
const GeodeticPosition& origin,
|
|
const GeodeticPosition& position)
|
|
{
|
|
using boost::units::cos;
|
|
|
|
const double dlat = units::Angle {position.latitude - origin.latitude }.value();
|
|
const double dlon = units::Angle {position.longitude - origin.longitude }.value();
|
|
|
|
const units::Length x = dlon * cos(origin.latitude) * earth_radius;
|
|
const units::Length y = dlat * earth_radius;
|
|
|
|
return CartesianPosition(x, y);
|
|
}
|
|
|
|
} // namespace haversine
|
|
} // namespace geodesy
|
|
} // namespace vanetza
|