mirror of
https://github.com/sunnypilot/sunnypilot.git
synced 2026-02-19 00:43:54 +08:00
c0ac9bb63c
* live_kf to c++
* first locationd code
* Running in process_replay
* locationd handle cam_odo and live_calib
* log event handlers
* working message receiving
* compiling message sending
* correctly sending some messages
* correct receiving and sending
* update ref_commit with some all_alive_and_valid being false, minor fixes
* fix std abs
* linking on device fix
* fix cpu usage test
* generate kf constants and defines
* fix replay test
* replay without acks, cleanup
* operate on bytearray messages
* cleanup
* send msg fix
* small sleep, less flaky test
* remove python locationd
* review feedback
* bump rednose
old-commit-hash: 3420707ad5
42 lines
843 B
C++
42 lines
843 B
C++
#pragma once
|
|
|
|
#define DEG2RAD(x) ((x) * M_PI / 180.0)
|
|
#define RAD2DEG(x) ((x) * 180.0 / M_PI)
|
|
|
|
struct ECEF {
|
|
double x, y, z;
|
|
Eigen::Vector3d to_vector(){
|
|
return Eigen::Vector3d(x, y, z);
|
|
}
|
|
};
|
|
|
|
struct NED {
|
|
double n, e, d;
|
|
Eigen::Vector3d to_vector(){
|
|
return Eigen::Vector3d(n, e, d);
|
|
}
|
|
};
|
|
|
|
struct Geodetic {
|
|
double lat, lon, alt;
|
|
bool radians=false;
|
|
};
|
|
|
|
ECEF geodetic2ecef(Geodetic g);
|
|
Geodetic ecef2geodetic(ECEF e);
|
|
|
|
class LocalCoord {
|
|
public:
|
|
Eigen::Matrix3d ned2ecef_matrix;
|
|
Eigen::Matrix3d ecef2ned_matrix;
|
|
Eigen::Vector3d init_ecef;
|
|
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);
|
|
};
|