diff --git a/release/files_common b/release/files_common index d1ddced759..d297b550db 100644 --- a/release/files_common +++ b/release/files_common @@ -280,12 +280,6 @@ selfdrive/locationd/models/constants.py selfdrive/locationd/calibrationd.py selfdrive/locationd/calibration_helpers.py -selfdrive/locationd/locationd_yawrate.cc -selfdrive/locationd/locationd_yawrate.h -selfdrive/locationd/params_learner.cc -selfdrive/locationd/params_learner.h -selfdrive/locationd/paramsd.cc - selfdrive/logcatd/SConscript selfdrive/logcatd/logcatd.cc diff --git a/selfdrive/locationd/SConscript b/selfdrive/locationd/SConscript index fbecaa1bae..59516574d8 100644 --- a/selfdrive/locationd/SConscript +++ b/selfdrive/locationd/SConscript @@ -1,12 +1,6 @@ Import('env', 'common', 'cereal', 'messaging') -loc_objs = [ - "locationd_yawrate.cc", - "params_learner.cc", - "paramsd.cc"] -loc_libs = [cereal, messaging, 'zmq', common, 'capnp', 'kj', 'json11', 'pthread'] -env.Program("paramsd", loc_objs, LIBS=loc_libs) -env.SharedLibrary("locationd", loc_objs, LIBS=loc_libs) +loc_libs = [cereal, messaging, 'zmq', common, 'capnp', 'kj', 'pthread'] env.Program("ubloxd", [ "ubloxd.cc", diff --git a/selfdrive/locationd/liblocationd_py.py b/selfdrive/locationd/liblocationd_py.py deleted file mode 100644 index 09d2bb756c..0000000000 --- a/selfdrive/locationd/liblocationd_py.py +++ /dev/null @@ -1,28 +0,0 @@ -import os - -from cffi import FFI - -locationd_dir = os.path.dirname(os.path.abspath(__file__)) -liblocationd_fn = os.path.join(locationd_dir, "liblocationd.so") - - -ffi = FFI() -ffi.cdef(""" -void *localizer_init(void); -void localizer_handle_log(void * localizer, const unsigned char * data, size_t len); -double localizer_get_yaw(void * localizer); -double localizer_get_bias(void * localizer); -double localizer_get_t(void * localizer); -void *params_learner_init(size_t len, char * params, double angle_offset, double stiffness_factor, double steer_ratio, double learning_rate); -bool params_learner_update(void * params_learner, double psi, double u, double sa); -double params_learner_get_ao(void * params_learner); -double params_learner_get_slow_ao(void * params_learner); -double params_learner_get_x(void * params_learner); -double params_learner_get_sR(void * params_learner); -double * localizer_get_P(void * localizer); -void localizer_set_P(void * localizer, double * P); -double * localizer_get_state(void * localizer); -void localizer_set_state(void * localizer, double * state); -""") - -liblocationd = ffi.dlopen(liblocationd_fn) diff --git a/selfdrive/locationd/locationd_yawrate.cc b/selfdrive/locationd/locationd_yawrate.cc deleted file mode 100644 index e682ec3909..0000000000 --- a/selfdrive/locationd/locationd_yawrate.cc +++ /dev/null @@ -1,150 +0,0 @@ -#include -#include - -#include -#include -#include - -#include "cereal/gen/cpp/log.capnp.h" - -#include "locationd_yawrate.h" - - -void Localizer::update_state(const Eigen::Matrix &C, const double R, double current_time, double meas) { - double dt = current_time - prev_update_time; - - if (dt < 0) { - dt = 0; - } else { - prev_update_time = current_time; - } - - x = A * x; - P = A * P * A.transpose() + dt * Q; - - double y = meas - C * x; - double S = R + C * P * C.transpose(); - Eigen::Vector2d K = P * C.transpose() * (1.0 / S); - x = x + K * y; - P = (I - K * C) * P; -} - -void Localizer::handle_sensor_events(capnp::List::Reader sensor_events, double current_time) { - for (cereal::SensorEventData::Reader sensor_event : sensor_events){ - if (sensor_event.getSensor() == 5 && sensor_event.getType() == 16) { - double meas = -sensor_event.getGyroUncalibrated().getV()[0]; - update_state(C_gyro, R_gyro, current_time, meas); - } - } -} - -void Localizer::handle_camera_odometry(cereal::CameraOdometry::Reader camera_odometry, double current_time) { - double R = pow(5 * camera_odometry.getRotStd()[2], 2); - double meas = camera_odometry.getRot()[2]; - update_state(C_posenet, R, current_time, meas); -} - -void Localizer::handle_controls_state(cereal::ControlsState::Reader controls_state, double current_time) { - steering_angle = controls_state.getAngleSteers() * DEGREES_TO_RADIANS; - car_speed = controls_state.getVEgo(); -} - - -Localizer::Localizer() { - // States: [yaw rate, gyro bias] - A << - 1, 0, - 0, 1; - - Q << - pow(.1, 2.0), 0, - 0, pow(0.05/ 100.0, 2.0), - P << - pow(10000.0, 2.0), 0, - 0, pow(10000.0, 2.0); - - I << - 1, 0, - 0, 1; - - C_posenet << 1, 0; - C_gyro << 1, 1; - x << 0, 0; - - R_gyro = pow(0.025, 2.0); -} - -void Localizer::handle_log(cereal::Event::Reader event) { - double current_time = event.getLogMonoTime() / 1.0e9; - - // Initialize update_time on first update - if (prev_update_time < 0) { - prev_update_time = current_time; - } - - auto type = event.which(); - switch(type) { - case cereal::Event::CONTROLS_STATE: - handle_controls_state(event.getControlsState(), current_time); - break; - case cereal::Event::CAMERA_ODOMETRY: - handle_camera_odometry(event.getCameraOdometry(), current_time); - break; - case cereal::Event::SENSOR_EVENTS: - handle_sensor_events(event.getSensorEvents(), current_time); - break; - default: - break; - } -} - - -extern "C" { - void *localizer_init(void) { - Localizer * localizer = new Localizer; - return (void*)localizer; - } - - void localizer_handle_log(void * localizer, const unsigned char * data, size_t len) { - const kj::ArrayPtr view((const capnp::word*)data, len); - capnp::FlatArrayMessageReader msg(view); - cereal::Event::Reader event = msg.getRoot(); - - Localizer * loc = (Localizer*) localizer; - loc->handle_log(event); - } - - double localizer_get_yaw(void * localizer) { - Localizer * loc = (Localizer*) localizer; - return loc->x[0]; - } - double localizer_get_bias(void * localizer) { - Localizer * loc = (Localizer*) localizer; - return loc->x[1]; - } - - double * localizer_get_state(void * localizer) { - Localizer * loc = (Localizer*) localizer; - return loc->x.data(); - } - - void localizer_set_state(void * localizer, double * state) { - Localizer * loc = (Localizer*) localizer; - memcpy(loc->x.data(), state, 4 * sizeof(double)); - } - - double localizer_get_t(void * localizer) { - Localizer * loc = (Localizer*) localizer; - return loc->prev_update_time; - } - - double * localizer_get_P(void * localizer) { - Localizer * loc = (Localizer*) localizer; - return loc->P.data(); - } - - void localizer_set_P(void * localizer, double * P) { - Localizer * loc = (Localizer*) localizer; - memcpy(loc->P.data(), P, 16 * sizeof(double)); - } -} diff --git a/selfdrive/locationd/locationd_yawrate.h b/selfdrive/locationd/locationd_yawrate.h deleted file mode 100644 index 58999e5f37..0000000000 --- a/selfdrive/locationd/locationd_yawrate.h +++ /dev/null @@ -1,33 +0,0 @@ -#pragma once - -#include -#include "cereal/gen/cpp/log.capnp.h" - -#define DEGREES_TO_RADIANS 0.017453292519943295 - -class Localizer -{ - Eigen::Matrix2d A; - Eigen::Matrix2d I; - Eigen::Matrix2d Q; - Eigen::Matrix C_posenet; - Eigen::Matrix C_gyro; - - double R_gyro; - - void update_state(const Eigen::Matrix &C, const double R, double current_time, double meas); - void handle_sensor_events(capnp::List::Reader sensor_events, double current_time); - void handle_camera_odometry(cereal::CameraOdometry::Reader camera_odometry, double current_time); - void handle_controls_state(cereal::ControlsState::Reader controls_state, double current_time); - -public: - Eigen::Vector2d x; - Eigen::Matrix2d P; - double steering_angle = 0; - double car_speed = 0; - double prev_update_time = -1; - - Localizer(); - void handle_log(cereal::Event::Reader event); - -}; diff --git a/selfdrive/locationd/params_learner.cc b/selfdrive/locationd/params_learner.cc deleted file mode 100644 index 66f378a1da..0000000000 --- a/selfdrive/locationd/params_learner.cc +++ /dev/null @@ -1,118 +0,0 @@ -#include -#include -#include - -#include -#include -#include "cereal/gen/cpp/log.capnp.h" -#include "cereal/gen/cpp/car.capnp.h" -#include "params_learner.h" - -// #define DEBUG - -template -T clip(const T& n, const T& lower, const T& upper) { - return std::max(lower, std::min(n, upper)); -} - -ParamsLearner::ParamsLearner(cereal::CarParams::Reader car_params, - double angle_offset, - double stiffness_factor, - double steer_ratio, - double learning_rate) : - ao(angle_offset * DEGREES_TO_RADIANS), - slow_ao(angle_offset * DEGREES_TO_RADIANS), - x(stiffness_factor), - sR(steer_ratio) { - - cF0 = car_params.getTireStiffnessFront(); - cR0 = car_params.getTireStiffnessRear(); - - l = car_params.getWheelbase(); - m = car_params.getMass(); - - aF = car_params.getCenterToFront(); - aR = l - aF; - - min_sr = MIN_SR * car_params.getSteerRatio(); - max_sr = MAX_SR * car_params.getSteerRatio(); - min_sr_th = MIN_SR_TH * car_params.getSteerRatio(); - max_sr_th = MAX_SR_TH * car_params.getSteerRatio(); - alpha1 = 0.01 * learning_rate; - alpha2 = 0.0005 * learning_rate; - alpha3 = 0.1 * learning_rate; - alpha4 = 1.0 * learning_rate; -} - -bool ParamsLearner::update(double psi, double u, double sa) { - if (u > 10.0 && fabs(sa) < (DEGREES_TO_RADIANS * 90.)) { - double ao_diff = 2.0*cF0*cR0*l*u*x*(1.0*cF0*cR0*l*u*x*(ao - sa) + psi*sR*(cF0*cR0*pow(l, 2)*x - m*pow(u, 2)*(aF*cF0 - aR*cR0)))/(pow(sR, 2)*pow(cF0*cR0*pow(l, 2)*x - m*pow(u, 2)*(aF*cF0 - aR*cR0), 2)); - double new_ao = ao - alpha1 * ao_diff; - - double slow_ao_diff = 2.0*cF0*cR0*l*u*x*(1.0*cF0*cR0*l*u*x*(slow_ao - sa) + psi*sR*(cF0*cR0*pow(l, 2)*x - m*pow(u, 2)*(aF*cF0 - aR*cR0)))/(pow(sR, 2)*pow(cF0*cR0*pow(l, 2)*x - m*pow(u, 2)*(aF*cF0 - aR*cR0), 2)); - double new_slow_ao = slow_ao - alpha2 * slow_ao_diff; - - double new_x = x - alpha3 * (-2.0*cF0*cR0*l*m*pow(u, 3)*(slow_ao - sa)*(aF*cF0 - aR*cR0)*(1.0*cF0*cR0*l*u*x*(slow_ao - sa) + psi*sR*(cF0*cR0*pow(l, 2)*x - m*pow(u, 2)*(aF*cF0 - aR*cR0)))/(pow(sR, 2)*pow(cF0*cR0*pow(l, 2)*x - m*pow(u, 2)*(aF*cF0 - aR*cR0), 3))); - double new_sR = sR - alpha4 * (-2.0*cF0*cR0*l*u*x*(slow_ao - sa)*(1.0*cF0*cR0*l*u*x*(slow_ao - sa) + psi*sR*(cF0*cR0*pow(l, 2)*x - m*pow(u, 2)*(aF*cF0 - aR*cR0)))/(pow(sR, 3)*pow(cF0*cR0*pow(l, 2)*x - m*pow(u, 2)*(aF*cF0 - aR*cR0), 2))); - - ao = new_ao; - slow_ao = new_slow_ao; - x = new_x; - sR = new_sR; - } - -#ifdef DEBUG - std::cout << "Instant AO: " << (RADIANS_TO_DEGREES * ao) << "\tAverage AO: " << (RADIANS_TO_DEGREES * slow_ao); - std::cout << "\tStiffness: " << x << "\t sR: " << sR << std::endl; -#endif - - ao = clip(ao, -MAX_ANGLE_OFFSET, MAX_ANGLE_OFFSET); - slow_ao = clip(slow_ao, -MAX_ANGLE_OFFSET, MAX_ANGLE_OFFSET); - x = clip(x, MIN_STIFFNESS, MAX_STIFFNESS); - sR = clip(sR, min_sr, max_sr); - - bool valid = fabs(slow_ao) < MAX_ANGLE_OFFSET_TH; - valid = valid && sR > min_sr_th; - valid = valid && sR < max_sr_th; - return valid; -} - - -extern "C" { - void *params_learner_init(size_t len, char * params, double angle_offset, double stiffness_factor, double steer_ratio, double learning_rate) { - - auto amsg = kj::heapArray((len / sizeof(capnp::word)) + 1); - memcpy(amsg.begin(), params, len); - - capnp::FlatArrayMessageReader cmsg(amsg); - cereal::CarParams::Reader car_params = cmsg.getRoot(); - - ParamsLearner * p = new ParamsLearner(car_params, angle_offset, stiffness_factor, steer_ratio, learning_rate); - return (void*)p; - } - - bool params_learner_update(void * params_learner, double psi, double u, double sa) { - ParamsLearner * p = (ParamsLearner*) params_learner; - return p->update(psi, u, sa); - } - - double params_learner_get_ao(void * params_learner){ - ParamsLearner * p = (ParamsLearner*) params_learner; - return p->ao; - } - - double params_learner_get_x(void * params_learner){ - ParamsLearner * p = (ParamsLearner*) params_learner; - return p->x; - } - - double params_learner_get_slow_ao(void * params_learner){ - ParamsLearner * p = (ParamsLearner*) params_learner; - return p->slow_ao; - } - - double params_learner_get_sR(void * params_learner){ - ParamsLearner * p = (ParamsLearner*) params_learner; - return p->sR; - } -} diff --git a/selfdrive/locationd/params_learner.h b/selfdrive/locationd/params_learner.h deleted file mode 100644 index 4d97551b3b..0000000000 --- a/selfdrive/locationd/params_learner.h +++ /dev/null @@ -1,35 +0,0 @@ -#pragma once - -#define DEGREES_TO_RADIANS 0.017453292519943295 -#define RADIANS_TO_DEGREES (1.0 / DEGREES_TO_RADIANS) - -#define MAX_ANGLE_OFFSET (10.0 * DEGREES_TO_RADIANS) -#define MAX_ANGLE_OFFSET_TH (9.0 * DEGREES_TO_RADIANS) -#define MIN_STIFFNESS 0.5 -#define MAX_STIFFNESS 2.0 -#define MIN_SR 0.5 -#define MAX_SR 2.0 -#define MIN_SR_TH 0.55 -#define MAX_SR_TH 1.9 - -class ParamsLearner { - double cF0, cR0; - double aR, aF; - double l, m; - - double min_sr, max_sr, min_sr_th, max_sr_th; - double alpha1, alpha2, alpha3, alpha4; - -public: - double ao; - double slow_ao; - double x, sR; - - ParamsLearner(cereal::CarParams::Reader car_params, - double angle_offset, - double stiffness_factor, - double steer_ratio, - double learning_rate); - - bool update(double psi, double u, double sa); -}; diff --git a/selfdrive/locationd/paramsd.cc b/selfdrive/locationd/paramsd.cc deleted file mode 100644 index aee957bc81..0000000000 --- a/selfdrive/locationd/paramsd.cc +++ /dev/null @@ -1,135 +0,0 @@ -#include -#include -#include -#include -#include - -#include -#include "json11.hpp" - -#include "common/swaglog.h" -#include "common/params.h" -#include "common/timing.h" - -#include "messaging.hpp" -#include "locationd_yawrate.h" -#include "params_learner.h" - -#include "common/util.h" - -void sigpipe_handler(int sig) { - LOGE("SIGPIPE received"); -} - - -int main(int argc, char *argv[]) { - signal(SIGPIPE, (sighandler_t)sigpipe_handler); - - SubMaster sm({"controlsState", "sensorEvents", "cameraOdometry"}); - PubMaster pm({"liveParameters"}); - - Localizer localizer; - - // Read car params - std::vector params; - LOGW("waiting for params to set vehicle model"); - while (true) { - params = read_db_bytes("CarParams"); - if (params.size() > 0) break; - usleep(100*1000); - } - LOGW("got %d bytes CarParams", params.size()); - - // make copy due to alignment issues - auto amsg = kj::heapArray((params.size() / sizeof(capnp::word)) + 1); - memcpy(amsg.begin(), params.data(), params.size()); - - capnp::FlatArrayMessageReader cmsg(amsg); - cereal::CarParams::Reader car_params = cmsg.getRoot(); - - // Read params from previous run - std::string fingerprint = car_params.getCarFingerprint(); - std::string vin = car_params.getCarVin(); - double sR = car_params.getSteerRatio(); - double x = 1.0; - double ao = 0.0; - std::vector live_params = read_db_bytes("LiveParameters"); - if (live_params.size() > 0){ - std::string err; - std::string str(live_params.begin(), live_params.end()); - auto json = json11::Json::parse(str, err); - if (json.is_null() || !err.empty()) { - std::string log = "Error parsing json: " + err; - LOGW(log.c_str()); - } else { - std::string new_fingerprint = json["carFingerprint"].string_value(); - std::string new_vin = json["carVin"].string_value(); - - if (fingerprint == new_fingerprint && vin == new_vin) { - std::string log = "Parameter starting with: " + str; - LOGW(log.c_str()); - - sR = json["steerRatio"].number_value(); - x = json["stiffnessFactor"].number_value(); - ao = json["angleOffsetAverage"].number_value(); - } - } - } - - ParamsLearner learner(car_params, ao, x, sR, 1.0); - - // Main loop - int save_counter = 0; - while (true){ - if (sm.update(100) == 0) continue; - - if (sm.updated("controlsState")){ - localizer.handle_log(sm["controlsState"]); - save_counter++; - - double yaw_rate = -localizer.x[0]; - bool valid = learner.update(yaw_rate, localizer.car_speed, localizer.steering_angle); - - double angle_offset_degrees = RADIANS_TO_DEGREES * learner.ao; - double angle_offset_average_degrees = RADIANS_TO_DEGREES * learner.slow_ao; - - capnp::MallocMessageBuilder msg; - cereal::Event::Builder event = msg.initRoot(); - event.setLogMonoTime(nanos_since_boot()); - auto live_params = event.initLiveParameters(); - live_params.setValid(valid); - live_params.setYawRate(localizer.x[0]); - live_params.setGyroBias(localizer.x[1]); - live_params.setAngleOffset(angle_offset_degrees); - live_params.setAngleOffsetAverage(angle_offset_average_degrees); - live_params.setStiffnessFactor(learner.x); - live_params.setSteerRatio(learner.sR); - - pm.send("liveParameters", msg); - - // Save parameters every minute - if (save_counter % 6000 == 0) { - json11::Json json = json11::Json::object { - {"carVin", vin}, - {"carFingerprint", fingerprint}, - {"steerRatio", learner.sR}, - {"stiffnessFactor", learner.x}, - {"angleOffsetAverage", angle_offset_average_degrees}, - }; - - std::string out = json.dump(); - std::async(std::launch::async, - [out]{ - write_db_value("LiveParameters", out.c_str(), out.length()); - }); - } - } - if (sm.updated("sensorEvents")){ - localizer.handle_log(sm["sensorEvents"]); - } - if (sm.updated("cameraOdometry")){ - localizer.handle_log(sm["cameraOdometry"]); - } - } - return 0; -} diff --git a/selfdrive/locationd/test/test_params_learner.py b/selfdrive/locationd/test/test_params_learner.py deleted file mode 100755 index df0e851cd7..0000000000 --- a/selfdrive/locationd/test/test_params_learner.py +++ /dev/null @@ -1,50 +0,0 @@ -#!/usr/bin/env python3 - -import numpy as np -import unittest - -from selfdrive.car.honda.interface import CarInterface -from selfdrive.car.honda.values import CAR -from selfdrive.controls.lib.vehicle_model import VehicleModel -from selfdrive.locationd.liblocationd_py import liblocationd # pylint: disable=no-name-in-module, import-error - - -class TestParamsLearner(unittest.TestCase): - def setUp(self): - - self.CP = CarInterface.get_params(CAR.CIVIC) - bts = self.CP.to_bytes() - - self.params_learner = liblocationd.params_learner_init(len(bts), bts, 0.0, 1.0, self.CP.steerRatio, 1.0) - - def test_convergence(self): - # Setup vehicle model with wrong parameters - VM_sim = VehicleModel(self.CP) - x_target = 0.75 - sr_target = self.CP.steerRatio - 0.5 - ao_target = -1.0 - VM_sim.update_params(x_target, sr_target) - - # Run simulation - times = np.arange(0, 15*3600, 0.01) - angle_offset = np.radians(ao_target) - steering_angles = np.radians(10 * np.sin(2 * np.pi * times / 100.)) + angle_offset - speeds = 10 * np.sin(2 * np.pi * times / 1000.) + 25 - - for i, _ in enumerate(times): - u = speeds[i] - sa = steering_angles[i] - psi = VM_sim.yaw_rate(sa - angle_offset, u) - liblocationd.params_learner_update(self.params_learner, psi, u, sa) - - # Verify learned parameters - sr = liblocationd.params_learner_get_sR(self.params_learner) - ao_slow = np.degrees(liblocationd.params_learner_get_slow_ao(self.params_learner)) - x = liblocationd.params_learner_get_x(self.params_learner) - self.assertAlmostEqual(x_target, x, places=1) - self.assertAlmostEqual(ao_target, ao_slow, places=1) - self.assertAlmostEqual(sr_target, sr, places=1) - - -if __name__ == "__main__": - unittest.main()