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

openpilot v0.7.8 release

Browse Source
This commit is contained in:
Vehicle Researcher
2020-08-13 00:37:05 +00:00
parent f370bf5ba6
commit 0aa4867be4
147 changed files with 3679 additions and 2713 deletions
Download Patch File Download Diff File Expand all files Collapse all files
selfdrive/locationd/SConscript
+1 -7
View File
@@ -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",
selfdrive/locationd/calibrationd.py
+8 -2
View File
@@ -126,13 +126,21 @@ class Calibrator():
def send_data(self, pm):
calib = get_calib_from_vp(self.vp)
if self.valid_blocks > 0:
max_vp_calib = np.array(get_calib_from_vp(np.max(self.vps[:self.valid_blocks], axis=0)))
min_vp_calib = np.array(get_calib_from_vp(np.min(self.vps[:self.valid_blocks], axis=0)))
calib_spread = np.abs(max_vp_calib - min_vp_calib)
else:
calib_spread = np.zeros(3)
extrinsic_matrix = get_view_frame_from_road_frame(0, calib[1], calib[2], model_height)
cal_send = messaging.new_message('liveCalibration')
cal_send.liveCalibration.validBlocks = self.valid_blocks
cal_send.liveCalibration.calStatus = self.cal_status
cal_send.liveCalibration.calPerc = min(100 * (self.valid_blocks * BLOCK_SIZE + self.idx) // (INPUTS_NEEDED * BLOCK_SIZE), 100)
cal_send.liveCalibration.extrinsicMatrix = [float(x) for x in extrinsic_matrix.flatten()]
cal_send.liveCalibration.rpyCalib = [float(x) for x in calib]
cal_send.liveCalibration.rpyCalibSpread = [float(x) for x in calib_spread]
pm.send('liveCalibration', cal_send)
@@ -170,8 +178,6 @@ def calibrationd_thread(sm=None, pm=None):
if DEBUG and new_vp is not None:
print('got new vp', new_vp)
# decimate outputs for efficiency
def main(sm=None, pm=None):
calibrationd_thread(sm, pm)
selfdrive/locationd/locationd.py
+42 -54
View File
@@ -1,7 +1,6 @@
#!/usr/bin/env python3
import numpy as np
import sympy as sp
import cereal.messaging as messaging
import common.transformations.coordinates as coord
from common.transformations.orientation import ecef_euler_from_ned, \
@@ -23,6 +22,7 @@ from rednose.helpers.sympy_helpers import euler_rotate
VISION_DECIMATION = 2
SENSOR_DECIMATION = 10
POSENET_STD_HIST = 40
def to_float(arr):
@@ -52,7 +52,7 @@ class Localizer():
self.kf = LiveKalman(GENERATED_DIR)
self.reset_kalman()
self.max_age = .2 # seconds
self.max_age = .1 # seconds
self.disabled_logs = disabled_logs
self.calib = np.zeros(3)
self.device_from_calib = np.eye(3)
@@ -63,11 +63,13 @@ class Localizer():
self.posenet_invalid_count = 0
self.posenet_speed = 0
self.car_speed = 0
self.posenet_stds = 10*np.ones((POSENET_STD_HIST))
self.converter = coord.LocalCoord.from_ecef(self.kf.x[States.ECEF_POS])
self.unix_timestamp_millis = 0
self.last_gps_fix = 0
self.device_fell = False
@staticmethod
def msg_from_state(converter, calib_from_device, H, predicted_state, predicted_cov):
@@ -112,57 +114,42 @@ class Localizer():
#ned_vel_std = self.converter.ecef2ned(fix_ecef + vel_ecef + vel_ecef_std) - self.converter.ecef2ned(fix_ecef + vel_ecef)
fix = messaging.log.LiveLocationKalman.new_message()
fix.positionGeodetic.value = to_float(fix_pos_geo)
#fix.positionGeodetic.std = to_float(fix_pos_geo_std)
#fix.positionGeodetic.valid = True
fix.positionECEF.value = to_float(fix_ecef)
fix.positionECEF.std = to_float(fix_ecef_std)
fix.positionECEF.valid = True
fix.velocityECEF.value = to_float(vel_ecef)
fix.velocityECEF.std = to_float(vel_ecef_std)
fix.velocityECEF.valid = True
fix.velocityNED.value = to_float(ned_vel)
#fix.velocityNED.std = to_float(ned_vel_std)
#fix.velocityNED.valid = True
fix.velocityDevice.value = to_float(vel_device)
fix.velocityDevice.std = to_float(vel_device_std)
fix.velocityDevice.valid = True
fix.accelerationDevice.value = to_float(predicted_state[States.ACCELERATION])
fix.accelerationDevice.std = to_float(predicted_std[States.ACCELERATION_ERR])
fix.accelerationDevice.valid = True
fix.orientationECEF.value = to_float(orientation_ecef)
fix.orientationECEF.std = to_float(orientation_ecef_std)
fix.orientationECEF.valid = True
fix.calibratedOrientationECEF.value = to_float(calibrated_orientation_ecef)
#fix.calibratedOrientationECEF.std = to_float(calibrated_orientation_ecef_std)
#fix.calibratedOrientationECEF.valid = True
fix.orientationNED.value = to_float(orientation_ned)
#fix.orientationNED.std = to_float(orientation_ned_std)
#fix.orientationNED.valid = True
fix.angularVelocityDevice.value = to_float(predicted_state[States.ANGULAR_VELOCITY])
fix.angularVelocityDevice.std = to_float(predicted_std[States.ANGULAR_VELOCITY_ERR])
fix.angularVelocityDevice.valid = True
# write measurements to msg
measurements = [
# measurement field, value, std, valid
(fix.positionGeodetic, fix_pos_geo, np.nan*np.zeros(3), True),
(fix.positionECEF, fix_ecef, fix_ecef_std, True),
(fix.velocityECEF, vel_ecef, vel_ecef_std, True),
(fix.velocityNED, ned_vel, np.nan*np.zeros(3), True),
(fix.velocityDevice, vel_device, vel_device_std, True),
(fix.accelerationDevice, predicted_state[States.ACCELERATION], predicted_std[States.ACCELERATION_ERR], True),
(fix.orientationECEF, orientation_ecef, orientation_ecef_std, True),
(fix.calibratedOrientationECEF, calibrated_orientation_ecef, np.nan*np.zeros(3), True),
(fix.orientationNED, orientation_ned, np.nan*np.zeros(3), True),
(fix.angularVelocityDevice, predicted_state[States.ANGULAR_VELOCITY], predicted_std[States.ANGULAR_VELOCITY_ERR], True),
(fix.velocityCalibrated, vel_calib, vel_calib_std, True),
(fix.angularVelocityCalibrated, ang_vel_calib, ang_vel_calib_std, True),
(fix.accelerationCalibrated, acc_calib, acc_calib_std, True),
]
for field, value, std, valid in measurements:
# TODO: can we write the lists faster?
field.value = to_float(value)
field.std = to_float(std)
field.valid = valid
fix.velocityCalibrated.value = to_float(vel_calib)
fix.velocityCalibrated.std = to_float(vel_calib_std)
fix.velocityCalibrated.valid = True
fix.angularVelocityCalibrated.value = to_float(ang_vel_calib)
fix.angularVelocityCalibrated.std = to_float(ang_vel_calib_std)
fix.angularVelocityCalibrated.valid = True
fix.accelerationCalibrated.value = to_float(acc_calib)
fix.accelerationCalibrated.std = to_float(acc_calib_std)
fix.accelerationCalibrated.valid = True
return fix
def liveLocationMsg(self, time):
def liveLocationMsg(self):
fix = self.msg_from_state(self.converter, self.calib_from_device, self.H, self.kf.x, self.kf.P)
# experimentally found these values, no false positives in 20k minutes of driving
old_mean, new_mean = np.mean(self.posenet_stds[:POSENET_STD_HIST//2]), np.mean(self.posenet_stds[POSENET_STD_HIST//2:])
std_spike = new_mean/old_mean > 4 and new_mean > 7
if abs(self.posenet_speed - self.car_speed) > max(0.4 * self.car_speed, 5.0):
self.posenet_invalid_count += 1
else:
self.posenet_invalid_count = 0
fix.posenetOK = self.posenet_invalid_count < 4
fix.posenetOK = not (std_spike and self.car_speed > 5)
fix.deviceStable = not self.device_fell
self.device_fell = False
#fix.gpsWeek = self.time.week
#fix.gpsTimeOfWeek = self.time.tow
@@ -178,15 +165,10 @@ class Localizer():
def update_kalman(self, time, kind, meas, R=None):
try:
self.kf.predict_and_observe(time, kind, meas, R=R)
self.kf.predict_and_observe(time, kind, meas, R)
except KalmanError:
cloudlog.error("Error in predict and observe, kalman reset")
self.reset_kalman()
#idx = bisect_right([x[0] for x in self.observation_buffer], time)
#self.observation_buffer.insert(idx, (time, kind, meas))
#while len(self.observation_buffer) > 0 and self.observation_buffer[-1][0] - self.observation_buffer[0][0] > self.max_age:
# else:
# self.observation_buffer.pop(0)
def handle_gps(self, current_time, log):
# ignore the message if the fix is invalid
@@ -244,6 +226,8 @@ class Localizer():
trans_device = self.device_from_calib.dot(log.trans)
trans_device_std = self.device_from_calib.dot(log.transStd)
self.posenet_speed = np.linalg.norm(trans_device)
self.posenet_stds[:-1] = self.posenet_stds[1:]
self.posenet_stds[-1] = trans_device_std[0]
self.update_kalman(current_time,
ObservationKind.CAMERA_ODO_TRANSLATION,
np.concatenate([trans_device, 10*trans_device_std]))
@@ -260,6 +244,10 @@ class Localizer():
# Accelerometer
if sensor_reading.sensor == 1 and sensor_reading.type == 1:
# check if device fell, estimate 10 for g
# 40m/s**2 is a good filter for falling detection, no false positives in 20k minutes of driving
self.device_fell = self.device_fell or (np.linalg.norm(np.array(sensor_reading.acceleration.v) - np.array([10, 0, 0])) > 40)
self.acc_counter += 1
if self.acc_counter % SENSOR_DECIMATION == 0:
v = sensor_reading.acceleration.v
@@ -322,7 +310,7 @@ def locationd_thread(sm, pm, disabled_logs=None):
msg = messaging.new_message('liveLocationKalman')
msg.logMonoTime = t
msg.liveLocationKalman = localizer.liveLocationMsg(t * 1e-9)
msg.liveLocationKalman = localizer.liveLocationMsg()
msg.liveLocationKalman.inputsOK = sm.all_alive_and_valid()
msg.liveLocationKalman.sensorsOK = sm.alive['sensorEvents'] and sm.valid['sensorEvents']
selfdrive/locationd/locationd_yawrate.cc
-150
View File
@@ -1,150 +0,0 @@
#include <iostream>
#include <cmath>
#include <capnp/message.h>
#include <capnp/serialize-packed.h>
#include <eigen3/Eigen/Dense>
#include "cereal/gen/cpp/log.capnp.h"
#include "locationd_yawrate.h"
void Localizer::update_state(const Eigen::Matrix<double, 1, 2> &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<cereal::SensorEventData>::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<const capnp::word> view((const capnp::word*)data, len);
capnp::FlatArrayMessageReader msg(view);
cereal::Event::Reader event = msg.getRoot<cereal::Event>();
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));
}
}
selfdrive/locationd/locationd_yawrate.h
-33
View File
@@ -1,33 +0,0 @@
#pragma once
#include <eigen3/Eigen/Dense>
#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<double, 1, 2> C_posenet;
Eigen::Matrix<double, 1, 2> C_gyro;
double R_gyro;
void update_state(const Eigen::Matrix<double, 1, 2> &C, const double R, double current_time, double meas);
void handle_sensor_events(capnp::List<cereal::SensorEventData>::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);
};
selfdrive/locationd/models/live_kf.py
+1 -1
View File
@@ -206,7 +206,7 @@ class LiveKalman():
ObservationKind.ECEF_ORIENTATION_FROM_GPS: np.diag([.2**2, .2**2, .2**2, .2**2])}
# init filter
self.filter = EKF_sym(generated_dir, self.name, self.Q, self.initial_x, np.diag(self.initial_P_diag), self.dim_state, self.dim_state_err)
self.filter = EKF_sym(generated_dir, self.name, self.Q, self.initial_x, np.diag(self.initial_P_diag), self.dim_state, self.dim_state_err, max_rewind_age=0.2)
@property
def x(self):
selfdrive/locationd/params_learner.cc
-118
View File
@@ -1,118 +0,0 @@
#include <algorithm>
#include <cmath>
#include <iostream>
#include <capnp/message.h>
#include <capnp/serialize-packed.h>
#include "cereal/gen/cpp/log.capnp.h"
#include "cereal/gen/cpp/car.capnp.h"
#include "params_learner.h"
// #define DEBUG
template <typename T>
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<capnp::word>((len / sizeof(capnp::word)) + 1);
memcpy(amsg.begin(), params, len);
capnp::FlatArrayMessageReader cmsg(amsg);
cereal::CarParams::Reader car_params = cmsg.getRoot<cereal::CarParams>();
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;
}
}
selfdrive/locationd/params_learner.h
-35
View File
@@ -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);
};
selfdrive/locationd/paramsd.cc
-135
View File
@@ -1,135 +0,0 @@
#include <future>
#include <iostream>
#include <cassert>
#include <csignal>
#include <unistd.h>
#include <capnp/serialize-packed.h>
#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<char> 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<capnp::word>((params.size() / sizeof(capnp::word)) + 1);
memcpy(amsg.begin(), params.data(), params.size());
capnp::FlatArrayMessageReader cmsg(amsg);
cereal::CarParams::Reader car_params = cmsg.getRoot<cereal::CarParams>();
// 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<char> 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<cereal::Event>();
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;
}