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

Locationd input sanity checks (#20845)

Browse Source 
* Add first sanity checks on inputs

* more gps checks

* make sure test values pass sanity checks

* this localizer supports cannonball runs

Co-authored-by: Harald Schafer <harald.the.engineer@gmail.com>
old-commit-hash: c62bb106407f50b785570f6b01736839e2b42514
This commit is contained in:
Willem Melching
2021-05-06 22:39:50 +02:00
committed by GitHub
parent 9cfda07c9e
commit 4707416cdd
2 changed files with 54 additions and 10 deletions
Download Patch File Download Diff File Expand all files Collapse all files
selfdrive/locationd/locationd.cc
+46 -4
View File
@@ -7,6 +7,12 @@ using namespace EKFS;
using namespace Eigen;
ExitHandler do_exit;
const double ACCEL_SANITY_CHECK = 100.0; // m/s^2
const double ROTATION_SANITY_CHECK = 10.0; // rad/s
const double TRANS_SANITY_CHECK = 200.0; // m/s
const double CALIB_RPY_SANITY_CHECK = 0.5; // rad (+- 30 deg)
const double ALTITUDE_SANITY_CHECK = 10000; // m
const double MIN_STD_SANITY_CHECK = 1e-5; // m or rad
static VectorXd floatlist2vector(const capnp::List<float, capnp::Kind::PRIMITIVE>::Reader& floatlist) {
VectorXd res(floatlist.size());
@@ -183,7 +189,10 @@ void Localizer::handle_sensors(double current_time, const capnp::List<cereal::Se
// Gyro Uncalibrated
if (sensor_reading.getSensor() == SENSOR_GYRO_UNCALIBRATED && sensor_reading.getType() == SENSOR_TYPE_GYROSCOPE_UNCALIBRATED) {
auto v = sensor_reading.getGyroUncalibrated().getV();
this->kf->predict_and_observe(sensor_time, OBSERVATION_PHONE_GYRO, { Vector3d(-v[2], -v[1], -v[0]) });
auto meas = Vector3d(-v[2], -v[1], -v[0]);
if (meas.norm() < ROTATION_SANITY_CHECK) {
this->kf->predict_and_observe(sensor_time, OBSERVATION_PHONE_GYRO, { meas });
}
}
// Accelerometer
@@ -194,7 +203,10 @@ void Localizer::handle_sensors(double current_time, const capnp::List<cereal::Se
// 40m/s**2 is a good filter for falling detection, no false positives in 20k minutes of driving
this->device_fell |= (floatlist2vector(v) - Vector3d(10.0, 0.0, 0.0)).norm() > 40.0;
this->kf->predict_and_observe(sensor_time, OBSERVATION_PHONE_ACCEL, { Vector3d(-v[2], -v[1], -v[0]) });
auto meas = Vector3d(-v[2], -v[1], -v[0]);
if (meas.norm() < ACCEL_SANITY_CHECK) {
this->kf->predict_and_observe(sensor_time, OBSERVATION_PHONE_ACCEL, { meas });
}
}
}
}
@@ -205,8 +217,21 @@ void Localizer::handle_gps(double current_time, const cereal::GpsLocationData::R
return;
}
this->last_gps_fix = current_time;
// Sanity checks
if ((log.getVerticalAccuracy() <= 0) || (log.getSpeedAccuracy() <= 0) || (log.getBearingAccuracyDeg() <= 0)) {
return;
}
if ((std::abs(log.getLatitude()) > 90) || (std::abs(log.getLongitude()) > 180) || (std::abs(log.getAltitude()) > ALTITUDE_SANITY_CHECK)) {
return;
}
if (floatlist2vector(log.getVNED()).norm() > TRANS_SANITY_CHECK){
return;
}
// Process message
this->last_gps_fix = current_time;
Geodetic geodetic = { log.getLatitude(), log.getLongitude(), log.getAltitude() };
this->converter = std::make_unique<LocalCoord>(geodetic);
@@ -255,9 +280,21 @@ void Localizer::handle_cam_odo(double current_time, const cereal::CameraOdometry
VectorXd rot_device = this->device_from_calib * floatlist2vector(log.getRot());
VectorXd trans_device = this->device_from_calib * floatlist2vector(log.getTrans());
if ((rot_device.norm() > ROTATION_SANITY_CHECK) || (trans_device.norm() > TRANS_SANITY_CHECK)) {
return;
}
VectorXd rot_calib_std = floatlist2vector(log.getRotStd());
VectorXd trans_calib_std = floatlist2vector(log.getTransStd());
if ((rot_calib_std.minCoeff() <= MIN_STD_SANITY_CHECK) || (trans_calib_std.minCoeff() <= MIN_STD_SANITY_CHECK)){
return;
}
if ((rot_calib_std.norm() > 10 * ROTATION_SANITY_CHECK) || (trans_calib_std.norm() > 10 * TRANS_SANITY_CHECK)) {
return;
}
this->posenet_stds.pop_front();
this->posenet_stds.push_back(trans_calib_std[0]);
@@ -275,7 +312,12 @@ void Localizer::handle_cam_odo(double current_time, const cereal::CameraOdometry
void Localizer::handle_live_calib(double current_time, const cereal::LiveCalibrationData::Reader& log) {
if (log.getRpyCalib().size() > 0) {
this->calib = floatlist2vector(log.getRpyCalib());
auto calib = floatlist2vector(log.getRpyCalib());
if ((calib.minCoeff() < -CALIB_RPY_SANITY_CHECK) || (calib.maxCoeff() > CALIB_RPY_SANITY_CHECK)){
return;
}
this->calib = calib;
this->device_from_calib = euler2rot(this->calib);
this->calib_from_device = this->device_from_calib.transpose();
this->calibrated = log.getCalStatus() == 1;
selfdrive/locationd/test/test_locationd.py
+8 -6
View File
@@ -45,7 +45,7 @@ void localizer_handle_msg_bytes(Localizer_t localizer, const char *data, size_t
def test_liblocalizer(self):
msg = messaging.new_message('liveCalibration')
msg.liveCalibration.validBlocks = random.randint(1, 10)
msg.liveCalibration.rpyCalib = [random.random() for _ in range(3)]
msg.liveCalibration.rpyCalib = [random.random() / 10 for _ in range(3)]
self.localizer_handle_msg(msg)
liveloc = self.localizer_get_msg()
@@ -84,17 +84,17 @@ void localizer_handle_msg_bytes(Localizer_t localizer, const char *data, size_t
for _ in range(20 * VISION_DECIMATION): # size of hist_old
msg = messaging.new_message('cameraOdometry')
msg.cameraOdometry.rot = [0.0, 0.0, 0.0]
msg.cameraOdometry.rotStd = [0.0, 0.0, 0.0]
msg.cameraOdometry.rotStd = [0.1, 0.1, 0.1]
msg.cameraOdometry.trans = [0.0, 0.0, 0.0]
msg.cameraOdometry.transStd = [2.0, 0.0, 0.0]
msg.cameraOdometry.transStd = [2.0, 0.1, 0.1]
self.localizer_handle_msg(msg)
for _ in range(20 * VISION_DECIMATION): # size of hist_new
msg = messaging.new_message('cameraOdometry')
msg.cameraOdometry.rot = [0.0, 0.0, 0.0]
msg.cameraOdometry.rotStd = [0.0, 0.0, 0.0]
msg.cameraOdometry.rotStd = [1.0, 1.0, 1.0]
msg.cameraOdometry.trans = [0.0, 0.0, 0.0]
msg.cameraOdometry.transStd = [8.1, 0.0, 0.0] # more than 4 times larger
msg.cameraOdometry.transStd = [10.1, 0.1, 0.1] # more than 4 times larger
self.localizer_handle_msg(msg)
ret = self.localizer_get_msg()
@@ -136,7 +136,9 @@ class TestLocationdProc(unittest.TestCase):
msg = messaging.new_message('gpsLocationExternal')
msg.logMonoTime = 0
msg.gpsLocationExternal.flags = 1
msg.gpsLocationExternal.verticalAccuracy = 0.0
msg.gpsLocationExternal.verticalAccuracy = 1.0
msg.gpsLocationExternal.speedAccuracy = 1.0
msg.gpsLocationExternal.bearingAccuracyDeg = 1.0
msg.gpsLocationExternal.vNED = [0.0, 0.0, 0.0]
msg.gpsLocationExternal.latitude = lat
msg.gpsLocationExternal.longitude = lon