mirror of
https://github.com/sunnypilot/sunnypilot.git
synced 2026-02-19 11:13:53 +08:00
c18e7da3c2
* locationd at 20hz * update ref * bump cereal * dont modify global state * add scons files * ecef2geodetic and geodetic2ecef * Finish local coords class * Add header file * Add orientation.cc * cleanup * Add functions to header file * Add cython wrapper * y u no work? * This passes the tests * test rot2quat and quat2rot * Teste euler2rot and rot2euler * rot_matrix * test ecef_euler_from_ned and ned_euler_from_ecef * add benchmark * Add test * Consistent newlines * no more radians supported in geodetic * test localcoord single * test localcoord single * all tests pass * Unused import * Add alternate namings * Add source for formulas * no explicit tests needed * remove benchmark * Add release files * Typo * Remove print statement * no access to raw transform matrix * temporarily add tolerance * handcode quat2euler * update ref
37 lines
699 B
C++
37 lines
699 B
C++
#pragma once
|
|
|
|
struct ECEF {
|
|
double x, y, z;
|
|
Eigen::Vector3d to_vector(){
|
|
return Eigen::Vector3d(x, y, z);
|
|
}
|
|
};
|
|
|
|
struct NED {
|
|
double n, e, d;
|
|
};
|
|
|
|
struct Geodetic {
|
|
double lat, lon, alt;
|
|
bool radians=false;
|
|
};
|
|
|
|
ECEF geodetic2ecef(Geodetic g);
|
|
Geodetic ecef2geodetic(ECEF e);
|
|
|
|
class LocalCoord {
|
|
private:
|
|
Eigen::Matrix3d ned2ecef_matrix;
|
|
Eigen::Matrix3d ecef2ned_matrix;
|
|
Eigen::Vector3d init_ecef;
|
|
public:
|
|
LocalCoord(Geodetic g, ECEF e);
|
|
LocalCoord(Geodetic g) : LocalCoord(g, ::geodetic2ecef(g)) {}
|
|
LocalCoord(ECEF e) : LocalCoord(::ecef2geodetic(e), e) {}
|
|
|
|
NED ecef2ned(ECEF e);
|
|
ECEF ned2ecef(NED n);
|
|
NED geodetic2ned(Geodetic g);
|
|
Geodetic ned2geodetic(NED n);
|
|
};
|