mawei/StarPilot
1
0
Fork 0
mirror of https://github.com/firestar5683/StarPilot.git synced 2026-07-02 12:02:09 +08:00
Code Issues Packages Projects Releases Wiki Activity

locationd: add filterState (#28340)

Browse Source 
* Add filterState to published messages

* Hide it behind DEBUG flag

* Update cereal

* Update cereal
old-commit-hash: 9abe95c949747e87f059ea0810c03b01305110c5
This commit is contained in:
Kacper Rączy
2023-06-01 00:01:12 +02:00
committed by GitHub
parent 3fc8d79044
commit 37fb563225
2 changed files with 6 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files
cereal
+1 -1
Submodule cereal updated: 3f950792d8...172b9b7dc0
selfdrive/locationd/locationd.cc
+5
View File
@@ -35,6 +35,8 @@ const float GPS_VEL_STD_RESET_THRESHOLD = 0.5;
const float GPS_ORIENTATION_ERROR_RESET_THRESHOLD = 1.0;
const int GPS_ORIENTATION_ERROR_RESET_CNT = 3;
const bool DEBUG = getenv("DEBUG") != nullptr && std::string(getenv("DEBUG")) != "0";
static VectorXd floatlist2vector(const capnp::List<float, capnp::Kind::PRIMITIVE>::Reader& floatlist) {
VectorXd res(floatlist.size());
for (int i = 0; i < floatlist.size(); i++) {
@@ -161,6 +163,9 @@ void Localizer::build_live_location(cereal::LiveLocationKalman::Builder& fix) {
init_measurement(fix.initVelocityCalibrated(), vel_calib, vel_calib_std, this->calibrated);
init_measurement(fix.initAngularVelocityCalibrated(), ang_vel_calib, ang_vel_calib_std, this->calibrated);
init_measurement(fix.initAccelerationCalibrated(), acc_calib, acc_calib_std, this->calibrated);
if (DEBUG) {
init_measurement(fix.initFilterState(), predicted_state, predicted_std, true);
}
double old_mean = 0.0, new_mean = 0.0;
int i = 0;