sadmen
/
openpilot_comma
mirror of https://github.com/commaai/openpilot.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

init

Browse Source
pull/26850/head
Kurt Nistelberger 3 years ago
parent 2ff7c1d8a9
commit dc2f23056c
5 changed files with 37 additions and 41 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 4
      selfdrive/locationd/liblocationd.cc
  2. 33
      selfdrive/locationd/locationd.cc
  3. 1
      selfdrive/locationd/locationd.h
  4. 4
      selfdrive/locationd/test/_test_locationd_lib.py
  5. 36
      selfdrive/locationd/test/test_locationd.py

4
selfdrive/locationd/liblocationd.cc
Unescape View File

@ -3,8 +3,8 @@
extern "C" { extern "C" {
typedef Localizer* Localizer_t; typedef Localizer* Localizer_t;
Localizer *localizer_init() { Localizer *localizer_init(bool has_ublox) {
return new Localizer(); return new Localizer(has_ublox);
} }
void localizer_get_message_bytes(Localizer *localizer, bool inputsOK, bool sensorsOK, bool gpsOK, bool msgValid, void localizer_get_message_bytes(Localizer *localizer, bool inputsOK, bool sensorsOK, bool gpsOK, bool msgValid,

33
selfdrive/locationd/locationd.cc
Unescape View File

@ -84,6 +84,8 @@ Localizer::Localizer() {
this->converter = std::make_unique<LocalCoord>((ECEF) { .x = ecef_pos[0], .y = ecef_pos[1], .z = ecef_pos[2] }); this->converter = std::make_unique<LocalCoord>((ECEF) { .x = ecef_pos[0], .y = ecef_pos[1], .z = ecef_pos[2] });
} }
Localizer::Localizer(bool has_ublox) : Localizer() { ublox_available = has_ublox; }
void Localizer::build_live_location(cereal::LiveLocationKalman::Builder& fix) { void Localizer::build_live_location(cereal::LiveLocationKalman::Builder& fix) {
VectorXd predicted_state = this->kf->get_x(); VectorXd predicted_state = this->kf->get_x();
MatrixXdr predicted_cov = this->kf->get_P(); MatrixXdr predicted_cov = this->kf->get_P();
@ -227,7 +229,7 @@ void Localizer::handle_sensor(double current_time, const cereal::SensorEventData
} }
double sensor_time = 1e-9 * log.getTimestamp(); double sensor_time = 1e-9 * log.getTimestamp();
// sensor time and log time should be close // sensor time and log time should be close
if (std::abs(current_time - sensor_time) > 0.1) { if (std::abs(current_time - sensor_time) > 0.1) {
LOGE("Sensor reading ignored, sensor timestamp more than 100ms off from log time"); LOGE("Sensor reading ignored, sensor timestamp more than 100ms off from log time");
@ -437,7 +439,7 @@ void Localizer::handle_car_state(double current_time, const cereal::CarState::Re
void Localizer::handle_cam_odo(double current_time, const cereal::CameraOdometry::Reader& log) { void Localizer::handle_cam_odo(double current_time, const cereal::CameraOdometry::Reader& log) {
VectorXd rot_device = this->device_from_calib * floatlist2vector(log.getRot()); VectorXd rot_device = this->device_from_calib * floatlist2vector(log.getRot());
VectorXd trans_device = this->device_from_calib * floatlist2vector(log.getTrans()); VectorXd trans_device = this->device_from_calib * floatlist2vector(log.getTrans());
if (!this->is_timestamp_valid(current_time)) { if (!this->is_timestamp_valid(current_time)) {
this->observation_timings_invalid = true; this->observation_timings_invalid = true;
return; return;
@ -574,12 +576,12 @@ void Localizer::handle_msg(const cereal::Event::Reader& log) {
this->handle_sensor(t, log.getAccelerometer()); this->handle_sensor(t, log.getAccelerometer());
} else if (log.isGyroscope()) { } else if (log.isGyroscope()) {
this->handle_sensor(t, log.getGyroscope()); this->handle_sensor(t, log.getGyroscope());
} else if (log.isGpsLocation()) { //} else if (log.isGpsLocation()) {
this->handle_gps(t, log.getGpsLocation(), GPS_QUECTEL_SENSOR_TIME_OFFSET); // this->handle_gps(t, log.getGpsLocation(), GPS_QUECTEL_SENSOR_TIME_OFFSET);
} else if (log.isGpsLocationExternal()) { //} else if (log.isGpsLocationExternal()) {
this->handle_gps(t, log.getGpsLocationExternal(), GPS_UBLOX_SENSOR_TIME_OFFSET); // this->handle_gps(t, log.getGpsLocationExternal(), GPS_UBLOX_SENSOR_TIME_OFFSET);
//} else if (log.isGnssMeasurements()) { } else if (log.isGnssMeasurements()) {
// this->handle_gnss(t, log.getGnssMeasurements()); this->handle_gnss(t, log.getGnssMeasurements());
} else if (log.isCarState()) { } else if (log.isCarState()) {
this->handle_car_state(t, log.getCarState()); this->handle_car_state(t, log.getCarState());
} else if (log.isCameraOdometry()) { } else if (log.isCameraOdometry()) {
@ -616,7 +618,7 @@ bool Localizer::critical_services_valid(std::map<std::string, double> critical_s
return true; return true;
} }
bool Localizer::is_timestamp_valid(double current_time) { bool Localizer::is_timestamp_valid(double current_time) {
double filter_time = this->kf->get_filter_time(); double filter_time = this->kf->get_filter_time();
if (!std::isnan(filter_time) && ((filter_time - current_time) > MAX_FILTER_REWIND_TIME)) { if (!std::isnan(filter_time) && ((filter_time - current_time) > MAX_FILTER_REWIND_TIME)) {
LOGE("Observation timestamp is older than the max rewind threshold of the filter"); LOGE("Observation timestamp is older than the max rewind threshold of the filter");
@ -643,19 +645,12 @@ void Localizer::determine_gps_mode(double current_time) {
int Localizer::locationd_thread() { int Localizer::locationd_thread() {
ublox_available = Params().getBool("UbloxAvailable", true); ublox_available = Params().getBool("UbloxAvailable", true);
const std::initializer_list<const char *> service_list = {"gnssMeasurements", "cameraOdometry", "liveCalibration",
const char* gps_location_socket;
if (ublox_available) {
gps_location_socket = "gpsLocationExternal";
} else {
gps_location_socket = "gpsLocation";
}
const std::initializer_list<const char *> service_list = {gps_location_socket, "cameraOdometry", "liveCalibration",
"carState", "carParams", "accelerometer", "gyroscope"}; "carState", "carParams", "accelerometer", "gyroscope"};
PubMaster pm({"liveLocationKalman"});
// TODO: remove carParams once we're always sending at 100Hz // TODO: remove carParams once we're always sending at 100Hz
SubMaster sm(service_list, {}, nullptr, {gps_location_socket, "carParams"}); SubMaster sm(service_list, {}, nullptr, {"carParams"});
PubMaster pm({"liveLocationKalman"});
uint64_t cnt = 0; uint64_t cnt = 0;
bool filterInitialized = false; bool filterInitialized = false;

1
selfdrive/locationd/locationd.h
Unescape View File

@ -24,6 +24,7 @@
class Localizer { class Localizer {
public: public:
Localizer(); Localizer();
Localizer(bool has_ublox);
int locationd_thread(); int locationd_thread();

4
selfdrive/locationd/test/_test_locationd_lib.py
Unescape View File

@ -19,7 +19,7 @@ LIBLOCATIOND_PATH = os.path.abspath(os.path.join(os.path.dirname(__file__), '../
class TestLocationdLib(unittest.TestCase): class TestLocationdLib(unittest.TestCase):
def setUp(self): def setUp(self):
header = '''typedef ...* Localizer_t; header = '''typedef ...* Localizer_t;
Localizer_t localizer_init(); Localizer_t localizer_init(bool has_ublox);
void localizer_get_message_bytes(Localizer_t localizer, bool inputsOK, bool sensorsOK, bool gpsOK, bool msgValid, char *buff, size_t buff_size); void localizer_get_message_bytes(Localizer_t localizer, bool inputsOK, bool sensorsOK, bool gpsOK, bool msgValid, char *buff, size_t buff_size);
void localizer_handle_msg_bytes(Localizer_t localizer, const char *data, size_t size);''' void localizer_handle_msg_bytes(Localizer_t localizer, const char *data, size_t size);'''
@ -27,7 +27,7 @@ void localizer_handle_msg_bytes(Localizer_t localizer, const char *data, size_t
self.ffi.cdef(header) self.ffi.cdef(header)
self.lib = self.ffi.dlopen(LIBLOCATIOND_PATH) self.lib = self.ffi.dlopen(LIBLOCATIOND_PATH)
self.localizer = self.lib.localizer_init() self.localizer = self.lib.localizer_init(True) # default to ublox
self.buff_size = 2048 self.buff_size = 2048
self.msg_buff = self.ffi.new(f'char[{self.buff_size}]') self.msg_buff = self.ffi.new(f'char[{self.buff_size}]')

36
selfdrive/locationd/test/test_locationd.py
Unescape View File

@ -8,13 +8,14 @@ import capnp
import cereal.messaging as messaging import cereal.messaging as messaging
from cereal.services import service_list from cereal.services import service_list
from common.params import Params from common.params import Params
from common.transformations.coordinates import ecef2geodetic
from selfdrive.manager.process_config import managed_processes from selfdrive.manager.process_config import managed_processes
class TestLocationdProc(unittest.TestCase): class TestLocationdProc(unittest.TestCase):
MAX_WAITS = 1000 MAX_WAITS = 1000
LLD_MSGS = ['gpsLocationExternal', 'cameraOdometry', 'carState', 'liveCalibration', LLD_MSGS = ['gnssMeasurements', 'cameraOdometry', 'carState', 'liveCalibration',
'accelerometer', 'gyroscope', 'magnetometer'] 'accelerometer', 'gyroscope', 'magnetometer']
def setUp(self): def setUp(self):
@ -45,16 +46,14 @@ class TestLocationdProc(unittest.TestCase):
except capnp.lib.capnp.KjException: except capnp.lib.capnp.KjException:
msg = messaging.new_message(name, 0) msg = messaging.new_message(name, 0)
if name == "gpsLocationExternal": if name == "gnssMeasurements":
msg.gpsLocationExternal.flags = 1 msg.gnssMeasurements.measTime = t
msg.gpsLocationExternal.verticalAccuracy = 1.0 msg.gnssMeasurements.positionECEF.value = [self.x , self.y, self.z]
msg.gpsLocationExternal.speedAccuracy = 1.0 msg.gnssMeasurements.positionECEF.std = [0,0,0]
msg.gpsLocationExternal.bearingAccuracyDeg = 1.0 msg.gnssMeasurements.positionECEF.valid = True
msg.gpsLocationExternal.vNED = [0.0, 0.0, 0.0] msg.gnssMeasurements.velocityECEF.value = []
msg.gpsLocationExternal.latitude = self.lat msg.gnssMeasurements.velocityECEF.std = [0,0,0]
msg.gpsLocationExternal.longitude = self.lon msg.gnssMeasurements.velocityECEF.valid = True
msg.gpsLocationExternal.unixTimestampMillis = t * 1e6
msg.gpsLocationExternal.altitude = self.alt
elif name == 'cameraOdometry': elif name == 'cameraOdometry':
msg.cameraOdometry.rot = [0.0, 0.0, 0.0] msg.cameraOdometry.rot = [0.0, 0.0, 0.0]
msg.cameraOdometry.rotStd = [0.0, 0.0, 0.0] msg.cameraOdometry.rotStd = [0.0, 0.0, 0.0]
@ -67,9 +66,11 @@ class TestLocationdProc(unittest.TestCase):
# first reset params # first reset params
Params().remove('LastGPSPosition') Params().remove('LastGPSPosition')
self.lat = 30 + (random.random() * 10.0) self.x = -2710700 + (random.random() * 1e5)
self.lon = -70 + (random.random() * 10.0) self.y = -4280600 + (random.random() * 1e5)
self.alt = 5 + (random.random() * 10.0) self.z = 3850300 + (random.random() * 1e5)
self.lat, self.lon, self.alt = ecef2geodetic([self.x, self.y, self.z])
self.fake_duration = 90 # secs self.fake_duration = 90 # secs
# get fake messages at the correct frequency, listed in services.py # get fake messages at the correct frequency, listed in services.py
fake_msgs = [] fake_msgs = []
@ -83,10 +84,9 @@ class TestLocationdProc(unittest.TestCase):
time.sleep(1) # wait for async params write time.sleep(1) # wait for async params write
lastGPS = json.loads(Params().get('LastGPSPosition')) lastGPS = json.loads(Params().get('LastGPSPosition'))
self.assertAlmostEqual(lastGPS['latitude'], self.lat, places=4)
self.assertAlmostEqual(lastGPS['latitude'], self.lat, places=3) self.assertAlmostEqual(lastGPS['longitude'], self.lon, places=4)
self.assertAlmostEqual(lastGPS['longitude'], self.lon, places=3) self.assertAlmostEqual(lastGPS['altitude'], self.alt, places=4)
self.assertAlmostEqual(lastGPS['altitude'], self.alt, places=3)
if __name__ == "__main__": if __name__ == "__main__":

Write Preview
Loading…
Cancel
Save