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

locationd: initial value for gps_std_factor (#28533)

Browse Source 
* setup_gps method

* Add LocalizerGnssSource

* slight refactor

* make it more readable

* Move gnss_source initialization to configure_gnss_souce

* Add gps_variance_factor, gps_vertical_variance_factor and gps_time_offset

* add header changes
old-commit-hash: 242e8d2ca879665a0dbbf49ea291b86d4f62461d
This commit is contained in:
Kacper Rączy
2023-06-14 01:12:53 +02:00
committed by GitHub
parent 40bc230fef
commit c11ead32d6
3 changed files with 39 additions and 23 deletions
Download Patch File Download Diff File Expand all files Collapse all files
selfdrive/locationd/locationd.cc
+27 -19
View File
@@ -70,7 +70,7 @@ static VectorXd rotate_std(const MatrixXdr& rot_matrix, const VectorXd& std_in)
return rotate_cov(rot_matrix, std_in.array().square().matrix().asDiagonal()).diagonal().array().sqrt();
}
Localizer::Localizer() {
Localizer::Localizer(LocalizerGnssSource gnss_source) {
this->kf = std::make_unique<LiveKalman>();
this->reset_kalman();
@@ -84,10 +84,9 @@ Localizer::Localizer() {
VectorXd ecef_pos = this->kf->get_x().segment<STATE_ECEF_POS_LEN>(STATE_ECEF_POS_START);
this->converter = std::make_unique<LocalCoord>((ECEF) { .x = ecef_pos[0], .y = ecef_pos[1], .z = ecef_pos[2] });
this->configure_gnss_source(gnss_source);
}
Localizer::Localizer(bool has_ublox) : Localizer() { ublox_available = has_ublox; }
void Localizer::build_live_location(cereal::LiveLocationKalman::Builder& fix) {
VectorXd predicted_state = this->kf->get_x();
MatrixXdr predicted_cov = this->kf->get_P();
@@ -325,12 +324,7 @@ void Localizer::handle_gps(double current_time, const cereal::GpsLocationData::R
VectorXd ecef_pos = this->converter->ned2ecef({ 0.0, 0.0, 0.0 }).to_vector();
VectorXd ecef_vel = this->converter->ned2ecef({ log.getVNED()[0], log.getVNED()[1], log.getVNED()[2] }).to_vector() - ecef_pos;
float ecef_pos_std;
if (ublox_available) {
ecef_pos_std = std::sqrt(std::pow(log.getAccuracy(), 2) + std::pow(log.getVerticalAccuracy(), 2));
} else {
ecef_pos_std = std::sqrt(3 * std::pow(log.getVerticalAccuracy(), 2));
}
float ecef_pos_std = std::sqrt(this->gps_variance_factor * std::pow(log.getAccuracy(), 2) + this->gps_vertical_variance_factor * std::pow(log.getVerticalAccuracy(), 2));
MatrixXdr ecef_pos_R = Vector3d::Constant(std::pow(this->gps_std_factor * ecef_pos_std, 2)).asDiagonal();
MatrixXdr ecef_vel_R = Vector3d::Constant(std::pow(this->gps_std_factor * log.getSpeedAccuracy(), 2)).asDiagonal();
@@ -372,10 +366,7 @@ void Localizer::handle_gnss(double current_time, const cereal::GnssMeasurements:
}
double sensor_time = log.getMeasTime() * 1e-9;
if (ublox_available)
sensor_time -= GPS_UBLOX_SENSOR_TIME_OFFSET;
else
sensor_time -= GPS_QUECTEL_SENSOR_TIME_OFFSET;
sensor_time -= this->gps_time_offset;
auto ecef_pos_v = log.getPositionECEF().getValue();
VectorXd ecef_pos = Vector3d(ecef_pos_v[0], ecef_pos_v[1], ecef_pos_v[2]);
@@ -664,16 +655,33 @@ void Localizer::determine_gps_mode(double current_time) {
}
}
int Localizer::locationd_thread() {
ublox_available = Params().getBool("UbloxAvailable", true);
const char* gps_location_socket;
if (ublox_available) {
gps_location_socket = "gpsLocationExternal";
void Localizer::configure_gnss_source(LocalizerGnssSource source) {
this->gnss_source = source;
if (source == LocalizerGnssSource::UBLOX) {
this->gps_std_factor = 10.0;
this->gps_variance_factor = 1.0;
this->gps_vertical_variance_factor = 1.0;
this->gps_time_offset = GPS_UBLOX_SENSOR_TIME_OFFSET;
} else {
gps_location_socket = "gpsLocation";
this->gps_std_factor = 2.0;
this->gps_variance_factor = 0.0;
this->gps_vertical_variance_factor = 3.0;
this->gps_time_offset = GPS_QUECTEL_SENSOR_TIME_OFFSET;
}
}
int Localizer::locationd_thread() {
LocalizerGnssSource source;
const char* gps_location_socket;
if (Params().getBool("UbloxAvailable", true)) {
source = LocalizerGnssSource::UBLOX;
gps_location_socket = "gpsLocationExternal";
} else {
source = LocalizerGnssSource::QCOM;
gps_location_socket = "gpsLocation";
}
this->configure_gnss_source(source);
const std::initializer_list<const char *> service_list = {gps_location_socket, "cameraOdometry", "liveCalibration",
"carState", "carParams", "accelerometer", "gyroscope"};