mawei/openpilot-meb
1
0
Fork 0
Code Issues Pull Requests Actions 5 Packages Projects Releases Wiki Activity

Body cleanup + 100Hz locationd (#24168)

Browse Source 
* use PID

* 100hz on the branch

* Better defaults

* fix int clip

* More cleanup

* Fix pid comments

* only notcar gets 100hz

* cleanup

* fix tests

* ignore

* update refs

Co-authored-by: Adeeb Shihadeh <adeebshihadeh@gmail.com>
old-commit-hash: f54e724b5dd8b95e03dd4f46676c5e854bafaea6
This commit is contained in:
HaraldSchafer
2022-04-09 15:22:29 -07:00
committed by GitHub
parent 83f32537de
commit b0444b39d2
7 changed files with 47 additions and 56 deletions
Download Patch File Download Diff File Expand all files Collapse all files

51
selfdrive/car/body/carcontroller.py
View File

@@ -4,12 +4,14 @@ from common.realtime import DT_CTRL
from selfdrive.car.body import bodycan
from opendbc.can.packer import CANPacker
from selfdrive.car.body.values import SPEED_FROM_RPM
from selfdrive.controls.lib.pid import PIDController
MAX_TORQUE = 500
MAX_TORQUE_RATE = 50
MAX_ANGLE_ERROR = 7
MAX_POS_INTEGRATOR = 0.2 # meters
MAX_TURN_INTEGRATOR = 0.1 # meters
MAX_ANGLE_ERROR = np.radians(7)
MAX_POS_INTEGRATOR = 0.2 # meters
MAX_TURN_INTEGRATOR = 0.1 # meters
class CarController():
@@ -17,11 +19,10 @@ class CarController():
self.frame = 0
self.packer = CANPacker(dbc_name)
self.i_speed = 0
self.i_speed_diff = 0
self.i_balance = 0
self.d_balance = 0
# Speed, balance and turn PIDs
self.speed_pid = PIDController(0.115, k_i=0.23, rate=1/DT_CTRL)
self.balance_pid = PIDController(1300, k_i=0, k_d=280, rate=1/DT_CTRL)
self.turn_pid = PIDController(110, k_i=11.5, rate=1/DT_CTRL)
self.torque_r_filtered = 0.
self.torque_l_filtered = 0.
@@ -46,37 +47,23 @@ class CarController():
speed_desired = CC.actuators.accel / 5.
speed_diff_desired = -CC.actuators.steer
# Setpoint speed PID
kp_speed = 0.001 / SPEED_FROM_RPM
ki_speed = 0.002 / SPEED_FROM_RPM
speed_measured = SPEED_FROM_RPM * (CS.out.wheelSpeeds.fl + CS.out.wheelSpeeds.fr) / 2.
speed_error = speed_desired - speed_measured
self.i_speed += speed_error * DT_CTRL
self.i_speed = np.clip(self.i_speed, -MAX_POS_INTEGRATOR, MAX_POS_INTEGRATOR)
set_point = kp_speed * speed_error + ki_speed * self.i_speed
# Balancing PID
kp_balance = 1300
ki_balance = 0
kd_balance = 280
freeze_integrator = ((speed_error < 0 and self.speed_pid.error_integral <= -MAX_POS_INTEGRATOR) or
(speed_error > 0 and self.speed_pid.error_integral >= MAX_POS_INTEGRATOR))
angle_setpoint = self.speed_pid.update(speed_error, freeze_integrator=freeze_integrator)
# Clip angle error, this is enough to get up from stands
p_balance = np.clip((-CC.orientationNED[1]) - set_point, np.radians(-MAX_ANGLE_ERROR), np.radians(MAX_ANGLE_ERROR))
self.i_balance += CS.out.wheelSpeeds.fl + CS.out.wheelSpeeds.fr
self.d_balance = np.clip(-CC.angularVelocity[1], -1., 1.)
torque = int(np.clip((p_balance*kp_balance + self.i_balance*ki_balance + self.d_balance*kd_balance), -1000, 1000))
# yaw recovery PID
kp_turn = 0.95 / SPEED_FROM_RPM
ki_turn = 0.1 / SPEED_FROM_RPM
angle_error = np.clip((-CC.orientationNED[1]) - angle_setpoint, -MAX_ANGLE_ERROR, MAX_ANGLE_ERROR)
angle_error_rate = np.clip(-CC.angularVelocity[1], -1., 1.)
torque = self.balance_pid.update(angle_error, error_rate=angle_error_rate)
speed_diff_measured = SPEED_FROM_RPM * (CS.out.wheelSpeeds.fl - CS.out.wheelSpeeds.fr)
speed_diff_error = speed_diff_measured - speed_diff_desired
self.i_speed_diff += speed_diff_error * DT_CTRL
self.i_speed_diff = np.clip(self.i_speed_diff, -MAX_TURN_INTEGRATOR, MAX_TURN_INTEGRATOR)
torque_diff = int(np.clip(kp_turn * speed_diff_error + ki_turn * self.i_speed_diff, -100, 100))
turn_error = speed_diff_measured - speed_diff_desired
freeze_integrator = ((turn_error < 0 and self.turn_pid.error_integral <= -MAX_POS_INTEGRATOR) or
(turn_error > 0 and self.turn_pid.error_integral >= MAX_POS_INTEGRATOR))
torque_diff = self.turn_pid.update(turn_error, freeze_integrator=freeze_integrator)
# Combine 2 PIDs outputs
torque_r = torque + torque_diff

13
selfdrive/controls/lib/pid.py
View File

@@ -5,10 +5,10 @@ from common.numpy_fast import clip, interp
class PIDController():
def __init__(self, k_p, k_i, k_f=0., k_d=0., pos_limit=None, neg_limit=None, rate=100):
self._k_p = k_p # proportional gain
self._k_i = k_i # integral gain
self._k_d = k_d # feedforward gain
def __init__(self, k_p, k_i, k_f=0., k_d=0., pos_limit=1e308, neg_limit=-1e308, rate=100):
self._k_p = k_p
self._k_i = k_i
self._k_d = k_d
self.k_f = k_f # feedforward gain
if isinstance(self._k_p, Number):
self._k_p = [[0], [self._k_p]]
@@ -22,6 +22,7 @@ class PIDController():
self.i_unwind_rate = 0.3 / rate
self.i_rate = 1.0 / rate
self.speed = 0.0
self.reset()
@@ -37,6 +38,10 @@ class PIDController():
def k_d(self):
return interp(self.speed, self._k_d[0], self._k_d[1])
@property
def error_integral(self):
return self.i/self.k_i
def reset(self):
self.p = 0.0
self.i = 0.0

7
selfdrive/locationd/liblocationd.cc
View File

@@ -7,11 +7,10 @@ extern "C" {
return new Localizer();
}
void localizer_get_message_bytes(Localizer *localizer, uint64_t logMonoTime,
bool inputsOK, bool sensorsOK, bool gpsOK, bool msgValid, char *buff, size_t buff_size)
{
void localizer_get_message_bytes(Localizer *localizer, bool inputsOK, bool sensorsOK, bool gpsOK, bool msgValid,
char *buff, size_t buff_size) {
MessageBuilder msg_builder;
kj::ArrayPtr<char> arr = localizer->get_message_bytes(msg_builder, logMonoTime, inputsOK, sensorsOK, gpsOK, msgValid).asChars();
kj::ArrayPtr<char> arr = localizer->get_message_bytes(msg_builder, inputsOK, sensorsOK, gpsOK, msgValid).asChars();
assert(buff_size >= arr.size());
memcpy(buff, arr.begin(), arr.size());
}

24
selfdrive/locationd/locationd.cc
View File

@@ -456,11 +456,9 @@ void Localizer::handle_msg(const cereal::Event::Reader& log) {
this->update_reset_tracker();
}
kj::ArrayPtr<capnp::byte> Localizer::get_message_bytes(MessageBuilder& msg_builder, uint64_t logMonoTime,
bool inputsOK, bool sensorsOK, bool gpsOK, bool msgValid)
{
kj::ArrayPtr<capnp::byte> Localizer::get_message_bytes(MessageBuilder& msg_builder, bool inputsOK,
bool sensorsOK, bool gpsOK, bool msgValid) {
cereal::Event::Builder evt = msg_builder.initEvent();
evt.setLogMonoTime(logMonoTime);
evt.setValid(msgValid);
cereal::LiveLocationKalman::Builder liveLoc = evt.initLiveLocationKalman();
this->build_live_location(liveLoc);
@@ -492,10 +490,11 @@ void Localizer::determine_gps_mode(double current_time) {
}
int Localizer::locationd_thread() {
const std::initializer_list<const char *> service_list =
{ "gpsLocationExternal", "sensorEvents", "cameraOdometry", "liveCalibration", "carState" };
PubMaster pm({ "liveLocationKalman" });
SubMaster sm(service_list, nullptr, { "gpsLocationExternal" });
const std::initializer_list<const char *> service_list = {"gpsLocationExternal", "sensorEvents", "cameraOdometry", "liveCalibration", "carState", "carParams"};
PubMaster pm({"liveLocationKalman"});
// TODO: remove carParams once we're always sending at 100Hz
SubMaster sm(service_list, nullptr, {"gpsLocationExternal", "carParams"});
uint64_t cnt = 0;
bool filterInitialized = false;
@@ -512,15 +511,16 @@ int Localizer::locationd_thread() {
} else {
filterInitialized = sm.allAliveAndValid();
}
if (sm.updated("cameraOdometry")) {
uint64_t logMonoTime = sm["cameraOdometry"].getLogMonoTime();
// 100Hz publish for notcars, 20Hz for cars
const char* trigger_msg = sm["carParams"].getCarParams().getNotCar() ? "sensorEvents" : "cameraOdometry";
if (sm.updated(trigger_msg)) {
bool inputsOK = sm.allAliveAndValid();
bool sensorsOK = sm.alive("sensorEvents") && sm.valid("sensorEvents");
bool gpsOK = this->isGpsOK();
MessageBuilder msg_builder;
kj::ArrayPtr<capnp::byte> bytes = this->get_message_bytes(msg_builder, logMonoTime, inputsOK, sensorsOK, gpsOK, filterInitialized);
kj::ArrayPtr<capnp::byte> bytes = this->get_message_bytes(msg_builder, inputsOK, sensorsOK, gpsOK, filterInitialized);
pm.send("liveLocationKalman", bytes.begin(), bytes.size());
if (cnt % 1200 == 0 && gpsOK) { // once a minute

2
selfdrive/locationd/locationd.h
View File

@@ -35,7 +35,7 @@ public:
bool isGpsOK();
void determine_gps_mode(double current_time);
kj::ArrayPtr<capnp::byte> get_message_bytes(MessageBuilder& msg_builder, uint64_t logMonoTime,
kj::ArrayPtr<capnp::byte> get_message_bytes(MessageBuilder& msg_builder,
bool inputsOK, bool sensorsOK, bool gpsOK, bool msgValid);
void build_live_location(cereal::LiveLocationKalman::Builder& fix);

4
selfdrive/locationd/test/test_locationd.py
View File

@@ -24,7 +24,7 @@ class TestLocationdLib(unittest.TestCase):
def setUp(self):
header = '''typedef ...* Localizer_t;
Localizer_t localizer_init();
void localizer_get_message_bytes(Localizer_t localizer, uint64_t logMonoTime, 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);'''
self.ffi = FFI()
@@ -41,7 +41,7 @@ void localizer_handle_msg_bytes(Localizer_t localizer, const char *data, size_t
self.lib.localizer_handle_msg_bytes(self.localizer, self.ffi.from_buffer(bytstr), len(bytstr))
def localizer_get_msg(self, t=0, inputsOK=True, sensorsOK=True, gpsOK=True, msgValid=True):
self.lib.localizer_get_message_bytes(self.localizer, t, inputsOK, sensorsOK, gpsOK, msgValid, self.ffi.addressof(self.msg_buff, 0), self.buff_size)
self.lib.localizer_get_message_bytes(self.localizer, inputsOK, sensorsOK, gpsOK, msgValid, self.ffi.addressof(self.msg_buff, 0), self.buff_size)
return log.Event.from_bytes(self.ffi.buffer(self.msg_buff), nesting_limit=self.buff_size // 8)
def test_liblocalizer(self):

2
selfdrive/test/process_replay/ref_commit
View File

@@ -1 +1 @@
c93e3735143a88f0088eb23e6fde56660d4a53e2
de01f1544a1441f89c24c5ea587f3f99632f2b5a