Skip to content

Commit

Permalink
Gernby's Dampen MPC Interpolation Auto-Tune
Browse files Browse the repository at this point in the history 
Squashed commit of the following:

commit e0505d8
Merge: 7eefd27 7ce6376
Author: Greg Esmond <gernby2002@yahoo.com>
Date:   Thu Apr 11 12:17:28 2019 -0500

    Merge branch 'dampen-mpc-interp-auto-tune' of https://github.com/Gernby/openpilot into dampen-mpc-interp-auto-tune

commit 7eefd27
Author: Greg Esmond <gernby2002@yahoo.com>
Date:   Thu Apr 11 12:14:55 2019 -0500

    fixed kegmon.json initialization

commit 7ce6376
Author: Gernby <gernby2002@yahoo.com>
Date:   Wed Apr 10 18:19:18 2019 -0500

    Fixed merge error

commit 9a5e449
Merge: da7a3a8 e312b2e
Author: Greg Esmond <gernby2002@yahoo.com>
Date:   Wed Apr 10 15:43:38 2019 -0500

    Merge branch 'dampen-mpc-interp-auto-tune' of https://github.com/Gernby/openpilot into dampen-mpc-interp-auto-tune

commit da7a3a8
Merge: a5f29a8 7978afa
Author: Greg Esmond <gernby2002@yahoo.com>
Date:   Wed Apr 10 15:43:15 2019 -0500

    merged

commit a5f29a8
Author: Greg Esmond <gernby2002@yahoo.com>
Date:   Wed Apr 10 15:17:51 2019 -0500

    fixed kegman_conf

commit e312b2e
Author: Gernby <gernby2002@yahoo.com>
Date:   Wed Apr 10 12:50:19 2019 -0500

    add rate compensation for steering angle on ALL Toyota's

commit 6af7793
Author: Greg Esmond <gernby2002@yahoo.com>
Date:   Tue Apr 9 15:37:01 2019 -0500

    updated dashfile and modified realtime_priority

commit 4c9a1d6
Merge: 22bd796 166a0b0
Author: Greg Esmond <gernby2002@yahoo.com>
Date:   Tue Apr 9 12:11:25 2019 -0500

    refined auto-tune and auto-resume for Bosch

commit 166a0b0
Merge: 2607a20 dc90f37d
Author: Greg Esmond <gernby2002@yahoo.com>
Date:   Mon Apr 8 21:36:40 2019 -0500

    merge fix

commit 2607a20
Merge: 9c51023 e89fd12
Author: Greg Esmond <gernby2002@yahoo.com>
Date:   Mon Apr 8 21:09:12 2019 -0500

    Merge branch 'devel' of https://github.com/commaai/openpilot into rate-based-feedforward-PR

commit 9c51023
Author: Greg Esmond <gernby2002@yahoo.com>
Date:   Mon Apr 8 21:09:05 2019 -0500

    added auto-tuning for feedforward gain

commit 22bd796
Author: Greg Esmond <gernby2002@yahoo.com>
Date:   Mon Apr 8 17:37:07 2019 -0500

    added rough lead speed as resume trigger

commit d5e42de
Author: Greg Esmond <gernby2002@yahoo.com>
Date:   Mon Apr 8 16:51:08 2019 -0500

    mild refactoring and disabling of lane filter

commit c775ef6
Author: Greg Esmond <gernby2002@yahoo.com>
Date:   Mon Apr 8 16:13:48 2019 -0500

    more auto-tune improvements

commit dc90f37
Author: Gernby <gernby2002@yahoo.com>
Date:   Mon Apr 8 08:02:38 2019 -0500

    Reduce default gain values

    While testing logic to auto-tune the angle and rate gain values, I found that the rate gain value can be **much smaller** without losing its effectiveness.  My Honda Accord performed well with a rate gain below 0.01, so it seems that 0.2 is not as conservative as I first thought.

    I've also reduced the default angle gain, since 1.0 should perform nearly the same with the proposed logic as it does with standard logic.

commit 390e1d0
Author: Greg Esmond <gernby2002@yahoo.com>
Date:   Sun Apr 7 21:17:19 2019 -0500

    much improved auto-tune

commit b88e1f3
Author: Greg Esmond <gernby2002@yahoo.com>
Date:   Sun Apr 7 17:52:46 2019 -0500

    added auto-tune POC

commit 13344ae
Author: Greg Esmond <gernby2002@yahoo.com>
Date:   Sun Apr 7 13:32:08 2019 -0500

    Added dampened desired rate

commit 9926cd4
Author: Greg Esmond <gernby2002@yahoo.com>
Date:   Sat Apr 6 19:29:05 2019 -0500

    mostly disable lane filter for now

commit 78fd669
Author: Greg Esmond <gernby2002@yahoo.com>
Date:   Sat Apr 6 18:16:40 2019 -0500

    cleaned up live tuning

commit 89f6de9
Author: Greg Esmond <gernby2002@yahoo.com>
Date:   Sat Apr 6 17:12:41 2019 -0500

    updated tune.py

commit 0cceb50
Author: Greg Esmond <gernby2002@yahoo.com>
Date:   Sat Apr 6 17:08:53 2019 -0500

    added lane filter and rate-based feed-forward

commit 10ce6cc
Author: Greg Esmond <gernby2002@yahoo.com>
Date:   Fri Apr 5 13:56:01 2019 -0500

    Added rate-based feedforward for latcontrol

commit 5bfcb82
Author: Greg Esmond <gernby2002@yahoo.com>
Date:   Fri Apr 5 13:36:39 2019 -0500

    Added rate-based feedforward for lateral control

commit c177a58
Author: Greg Esmond <gernby2002@yahoo.com>
Date:   Fri Apr 5 10:08:17 2019 -0500

    added new params to dashfile

commit 9377c29
Author: Greg Esmond <gernby2002@yahoo.com>
Date:   Fri Apr 5 00:43:40 2019 -0500

    'dis is good'dis is good

commit 509bee5
Author: Greg Esmond <gernby2002@yahoo.com>
Date:   Thu Apr 4 17:25:32 2019 -0500

    added rate-based feed forward

commit fb775fb
Author: Greg Esmond <gernby2002@yahoo.com>
Date:   Wed Apr 3 22:07:38 2019 -0500

    tweaked the lane filter AND disabled rate-based FF

commit df2f4f2
Author: Greg Esmond <gernby2002@yahoo.com>
Date:   Wed Apr 3 16:25:00 2019 -0500

    improved exit filter AND added rate-based feedforward

commit 4c445d7
Author: Greg Esmond <gernby2002@yahoo.com>
Date:   Tue Apr 2 22:06:09 2019 -0500

    Working really nicely

commit 4ef7f48
Author: Greg Esmond <gernby2002@yahoo.com>
Date:   Tue Apr 2 20:14:53 2019 -0500

    mostly tested, and awesome

commit 9520ca9
Author: Greg Esmond <gernby2002@yahoo.com>
Date:   Mon Apr 1 10:26:52 2019 -0500

    fixed steer_status frame count check

commit 9d832a8
Author: Greg Esmond <gernby2002@yahoo.com>
Date:   Mon Apr 1 10:17:50 2019 -0500

    fixed realtime priorities and LiveParameters

commit 21a6e73
Author: Greg Esmond <gernby2002@yahoo.com>
Date:   Sat Mar 30 19:38:08 2019 -0500

    improved lane filtering

commit 875261d
Author: Greg Esmond <gernby2002@yahoo.com>
Date:   Sat Mar 30 17:38:54 2019 -0500

    lane filtering in model_parser is not tested

commit 0845916
Author: Greg Esmond <gernby2002@yahoo.com>
Date:   Wed Mar 27 18:50:58 2019 -0500

    updated default parameters for dampening

commit ecb67bd
Author: Greg Esmond <gernby2002@yahoo.com>
Date:   Wed Mar 27 18:44:07 2019 -0500

    working awesome!

commit 78890d6
Author: Greg Esmond <gernby2002@yahoo.com>
Date:   Wed Mar 27 16:42:49 2019 -0500

    Works great!

commit eaf9a8d
Merge: 5a9d89e 29b2d3c
Author: Greg Esmond <gernby2002@yahoo.com>
Date:   Wed Mar 27 16:18:32 2019 -0500

    testing-sauce-0.5.10

commit 29b2d3c
Author: Greg Esmond <gernby2002@yahoo.com>
Date:   Wed Mar 27 08:37:43 2019 -0500

    fixed default values

commit a191bdd
Author: Greg Esmond <gernby2002@yahoo.com>
Date:   Wed Mar 27 08:27:06 2019 -0500

    fix for boardd sync

commit 9170a94
Merge: eb69a0f d744113
Author: Gernby <gernby2002@yahoo.com>
Date:   Tue Mar 26 16:22:56 2019 -0500

    Merge pull request commaai#5 from Gernby/temp-test

    Added kegman's live-tuning

commit d744113
Author: Greg Esmond <gernby2002@yahoo.com>
Date:   Tue Mar 26 15:33:11 2019 -0500

    added kegman's live-tuning

commit 84ca9a7
Author: Greg Esmond <gernby2002@yahoo.com>
Date:   Mon Mar 25 21:29:29 2019 -0500

    adding live-tuning

commit eb69a0f
Author: Greg Esmond <gernby2002@yahoo.com>
Date:   Mon Mar 25 15:17:49 2019 -0500

    revised damp and react parameters

commit 722224c
Author: Greg Esmond <gernby2002@yahoo.com>
Date:   Mon Mar 25 08:34:19 2019 -0500

    fixed by clean reset

commit 388f8cd
Author: Greg Esmond <gernby2002@yahoo.com>
Date:   Mon Mar 25 07:40:04 2019 -0500

    Broken!

commit d555019
Merge: 7f08767 c97076a
Author: Greg Esmond <gernby2002@yahoo.com>
Date:   Sun Mar 24 13:07:02 2019 -0500

    Merge branch 'dampen-mpc-interp' of https://github.com/Gernby/openpilot into dampen-mpc-interp

commit 7f08767
Author: Greg Esmond <gernby2002@yahoo.com>
Date:   Sun Mar 24 12:54:24 2019 -0500

    fixed for angle-steer

commit c97076a
Merge: 463c80b 27e0adc
Author: Gernby <gernby2002@yahoo.com>
Date:   Sun Mar 24 12:32:19 2019 -0500

    Merge pull request commaai#4 from commaai/devel

    merging commaai/devel

commit 16624bd
Merge: 463c80b 27e0adc
Author: Greg Esmond <gernby2002@yahoo.com>
Date:   Sun Mar 24 12:13:04 2019 -0500

    Merge branch 'devel' of https://github.com/commaai/openpilot into dampen-mpc-interp

commit 463c80b
Author: Greg Esmond <gernby2002@yahoo.com>
Date:   Sun Mar 24 12:11:34 2019 -0500

    fixed latcontrol reset damp values

commit a07ac15
Author: Greg Esmond <gernby2002@yahoo.com>
Date:   Sun Mar 24 12:03:08 2019 -0500

    Updated steer and mpc dampening

commit 4017b1d
Merge: 52ae8fe 3c96b40
Author: Greg Esmond <gernby2002@yahoo.com>
Date:   Wed Mar 20 19:40:15 2019 -0500

    merge fixes

commit 52ae8fe
Author: Greg Esmond <gernby2002@yahoo.com>
Date:   Wed Mar 20 19:33:28 2019 -0500

    fixed sync issue with extended drives

commit 3c96b40
Author: Gernby <gernby2002@yahoo.com>
Date:   Tue Mar 5 15:42:23 2019 -0600

    Fixed issue with thermald between drives

commit 104c4a8
Author: Greg Esmond <gernby2002@yahoo.com>
Date:   Mon Mar 4 14:25:37 2019 -0600

    fixed some cleanup mistakes.

commit 36de288
Author: Greg Esmond <gernby2002@yahoo.com>
Date:   Mon Mar 4 14:11:03 2019 -0600

    another cleanup fixanother cleanup fix

commit 1412a60
Author: Greg Esmond <gernby2002@yahoo.com>
Date:   Mon Mar 4 13:50:14 2019 -0600

    fixed a couple cleanup mistakes

commit ffd8c3a
Author: Greg Esmond <gernby2002@yahoo.com>
Date:   Sun Mar 3 21:33:22 2019 -0600

    more refinements for PR

commit dbe8a75
Author: Greg Esmond <gernby2002@yahoo.com>
Date:   Sun Mar 3 21:10:43 2019 -0600

    added syncID for Accord

commit da84d41
Author: Greg Esmond <gernby2002@yahoo.com>
Date:   Sun Mar 3 20:38:49 2019 -0600

    preparing for PRpreparing for PR

commit db34d8c
Author: Greg Esmond <gernby2002@yahoo.com>
Date:   Fri Mar 1 11:33:35 2019 -0600

    ignore visiond

commit 64cd45f
Author: Greg Esmond <gernby2002@yahoo.com>
Date:   Fri Feb 22 13:31:04 2019 -0600

    gitignore

commit 031996f
Author: Greg Esmond <gernby2002@yahoo.com>
Date:   Fri Feb 22 11:59:29 2019 -0600

    git cleanup

commit e7c52d5
Merge: 2629080 eef45dc
Author: Greg Esmond <gernby2002@yahoo.com>
Date:   Fri Feb 22 11:54:53 2019 -0600

    Merge branch 'devel' of https://github.com/gernby/openpilot into devel

commit 2629080
Author: Greg Esmond <gernby2002@yahoo.com>
Date:   Fri Feb 22 11:54:12 2019 -0600

    git config

commit eef45dc
Merge: 9e28ef9 2ea0602
Author: Greg Esmond <gernby2002@yahoo.com>
Date:   Fri Feb 22 11:43:20 2019 -0600

    Merge branch 'devel' of https://github.com/Gernby/openpilot into devel

commit 2ea0602
Merge: a1819ea 4101ddc
Author: Greg Esmond <gernby2002@yahoo.com>
Date:   Fri Feb 22 11:40:19 2019 -0600

    Merge branch 'devel' of https://github.com/gernby/openpilot into devel

commit a1819ea
Author: Greg Esmond <gernby2002@yahoo.com>
Date:   Fri Feb 22 11:34:35 2019 -0600

    git config

commit 4101ddc
Author: Gernby <gernby2002@yahoo.com>
Date:   Fri Feb 22 11:30:44 2019 -0600

    Update README.md

commit 9e28ef9
Author: Greg Esmond <gernby2002@yahoo.com>
Date:   Fri Feb 22 11:29:18 2019 -0600

    git config

commit 5a0aae3
Author: Greg Esmond <gernby2002@yahoo.com>
Date:   Fri Feb 22 11:03:04 2019 -0600

    git config

commit 4712458
Merge: e57a9ac 73c8877
Author: Greg Esmond <gernby2002@yahoo.com>
Date:   Fri Feb 22 10:55:26 2019 -0600

    Merge branch 'devel' of https://github.com/Gernby/openpilot into deve

commit e57a9ac
Author: Greg Esmond <gernby2002@yahoo.com>
Date:   Fri Feb 22 10:54:33 2019 -0600

    configuring git

commit 73c8877
Merge: 3a49dac 5c8aa72
Author: Greg Esmond <gernby2002@yahoo.com>
Date:   Thu Feb 21 23:05:47 2019 -0600

    Merge branch 'devel' of https://github.com/commaai/openpilot into devel

commit 3a49dac
Author: Greg Esmond <gernby2002@yahoo.com>
Date:   Thu Feb 21 23:05:18 2019 -0600

    added sftp

commit 4f8dd77
Author: Gernby <gernby2002@yahoo.com>
Date:   Sat Feb 16 13:21:35 2019 -0600

    Update README.md

commit 17a5c7b
Author: Gernby <gernby2002@yahoo.com>
Date:   Sat Feb 16 12:56:00 2019 -0600

    Update README.md

commit 4ed0725
Author: Greg Esmond <gernby2002@yahoo.com>
Date:   Mon Feb 4 19:48:49 2019 -0600

    another merge fix

commit 167ef82
Author: Greg Esmond <gernby2002@yahoo.com>
Date:   Mon Feb 4 19:46:04 2019 -0600

    Merge fix

commit 6d737ca
Merge: baa7b73e 2c9d5af
Author: Greg Esmond <gernby2002@yahoo.com>
Date:   Mon Feb 4 19:32:53 2019 -0600

    Merge branch 'devel' of https://github.com/commaa/openpilot into

commit baa7b73
Merge: 845508b 504f750
Author: Greg Esmond <gernby2002@yahoo.com>
Date:   Wed Jan 16 09:32:20 2019 -0600

    Merge branch 'devel' of https://github.com/Gernby/openpilot into devel

commit 504f750
Author: Gernby <gernby2002@yahoo.com>
Date:   Thu Jan 10 17:11:57 2019 -0600

    0.5.6 visiond

commit 5443792
Author: Gernby <gernby2002@yahoo.com>
Date:   Thu Jan 10 17:11:15 2019 -0600

    Delete visiond

commit 845508b
Author: Greg Esmond <gernby2002@yahoo.com>
Date:   Wed Jan 9 20:43:57 2019 -0600

    fixed max torque, for more actual torque
  • Loading branch information
kegman committed Apr 11, 2019
1 parent 6b8e3b5 commit f937265
Show file tree
Hide file tree
Showing 29 changed files with 4,546 additions and 386 deletions.
3 changes: 2 additions & 1 deletion cereal/car.capnp
View file
Expand Up @@ -351,8 +351,9 @@ struct CarParams {
longitudinalKpV @37 :List(Float32);
longitudinalKiBP @38 :List(Float32);
longitudinalKiV @39 :List(Float32);

steerLimitAlert @29 :Bool;
steerMPCReactTime @51 :Float32;
steerMPCDampTime @52 :Float32;

vEgoStopping @30 :Float32; # Speed at which the car goes into stopping state
directAccelControl @31 :Bool; # Does the car have direct accel control or just gas/brake
Expand Down
10 changes: 10 additions & 0 deletions cereal/log.capnp
View file
Expand Up @@ -391,12 +391,18 @@ struct Live100Data {
upSteer @8 :Float32;
uiSteer @9 :Float32;
ufSteer @34 :Float32;
angleFFRatio @52 :Float32;
rateFFGain @53 :Float32;
angleFFGain @54 :Float32;
aTargetMinDEPRECATED @10 :Float32;
aTargetMaxDEPRECATED @11 :Float32;
aTarget @35 :Float32;
jerkFactor @12 :Float32;
angleSteers @13 :Float32; # Steering angle in degrees.
angleSteersDes @29 :Float32;
dampAngleSteersDes @55 :Float32;
dampRateSteersDes @56 :Float32;
angleSteersNoise @57 :Float32;
curvature @37 :Float32; # path curvature from vehicle model
hudLeadDEPRECATED @14 :Int32;
cumLagMs @15 :Float32;
Expand Down Expand Up @@ -613,6 +619,10 @@ struct PathPlan {

angleSteers @8 :Float32; # deg
rateSteers @13 :Float32; # deg/s
mpcAngles @14 :List(Float32);
mpcRates @15 :List(Float32);
mpcTimes @16 :List(Float32);
laneProb @17 :Float32;
valid @9 :Bool;
paramsValid @10 :Bool;
modelValid @12 :Bool;
Expand Down
535 changes: 535 additions & 0 deletions dashboard.py
View file

Large diffs are not rendered by default.

99 changes: 99 additions & 0 deletions dashfile.py
View file
@@ -0,0 +1,99 @@
#!/usr/bin/env python
import csv
import zmq
import time
import numpy as np
import selfdrive.messaging as messaging
from selfdrive.services import service_list
from common.realtime import set_realtime_priority, Ratekeeper
import os, os.path

# Polling rate should be twice the data rate to prevent aliasing
def main(rate=100):
set_realtime_priority(5)
context = zmq.Context()
poller = zmq.Poller()

live100 = messaging.sub_sock(context, service_list['live100'].port, conflate=False, poller=poller)
carState = messaging.sub_sock(context, service_list['carState'].port, conflate=True, poller=poller)
can = None #messaging.sub_sock(context, service_list['can'].port, conflate=True, poller=poller)

vEgo = 0.0
_live100 = None
_can = None

frame_count = 0
skipped_count = 0

rk = Ratekeeper(rate, print_delay_threshold=np.inf)

# simple version for working with CWD
#print len([name for name in os.listdir('.') if os.path.isfile(name)])

# path joining version for other paths
DIR = '/sdcard/tuning'
filenumber = len([name for name in os.listdir(DIR) if os.path.isfile(os.path.join(DIR, name))])

kegman_counter = 0
monoTimeOffset = 0
receiveTime = 0
angle_rate = 0.0

print("start")
with open(DIR + '/dashboard_file_%d.csv' % filenumber, mode='w') as dash_file:
print("opened")
dash_writer = csv.writer(dash_file, delimiter=',', quotechar='', quoting=csv.QUOTE_NONE)
print("initialized")
dash_writer.writerow(['ff_rate','ff_angle', 'angleGain','rateGain','actualNoise','angle_steers_des','angle_steers','dampened_angle_steers_des','v_ego','steer_override','p','i','f','time'])
print("first row")

while 1:
for socket, event in poller.poll(0):
if socket is can:
_can = messaging.recv_one(socket)
print(_can)

if socket is carState:
_carState = messaging.drain_sock(socket)
for cs in _carState:
angle_rate = cs.carState.steeringRate

if socket is live100:
_live100 = messaging.drain_sock(socket)
for l100 in _live100:
vEgo = l100.live100.vEgo
if vEgo > 0: # and l100.live100.active:
receiveTime = int(monoTimeOffset + l100.logMonoTime)
if (abs(receiveTime - int(time.time() * 1000000000)) > 10000000000):
monoTimeOffset = (time.time() * 1000000000) - l100.logMonoTime
receiveTime = int(monoTimeOffset + l100.logMonoTime)
frame_count += 1
dash_writer.writerow([str(round(1.0 - l100.live100.angleFFRatio, 2)),
str(round(l100.live100.angleFFRatio, 2)),
str(round(l100.live100.angleFFGain, 2)),
str(round(l100.live100.rateFFGain, 5)),
str(round(l100.live100.angleSteersNoise, 2)),
str(round(l100.live100.angleSteersDes, 2)),
str(round(l100.live100.angleSteers, 2)),
str(round(l100.live100.dampAngleSteersDes, 2)),
str(round(l100.live100.vEgo, 1)),
1 if l100.live100.steerOverride else 0,
str(round(l100.live100.upSteer, 4)),
str(round(l100.live100.uiSteer, 4)),
str(round(l100.live100.ufSteer, 4)),
str(receiveTime)])
else:
skipped_count += 1
else:
skipped_count += 1
if frame_count % 200 == 0:
print("captured = %d" % frame_count)
frame_count += 1
if skipped_count % 200 == 0:
print("skipped = %d" % skipped_count)
skipped_count += 1

rk.keep_time()

if __name__ == "__main__":
main()
7 changes: 4 additions & 3 deletions launch_chffrplus.sh
View file
Expand Up @@ -13,8 +13,9 @@ function launch {
git clean -xdf &&
exec "${BASH_SOURCE[0]}"
fi
# apply update
fi

# no cpu rationing for now
echo 0-3 > /dev/cpuset/background/cpus
echo 0-3 > /dev/cpuset/system-background/cpus
Expand All @@ -26,8 +27,8 @@ function launch {

# start manager
cd selfdrive
./manager.py
#./manager.py > log.txt
#./manager.py
./manager.py > log.txt

# if broken, keep on screen error
while true; do sleep 1; done
Expand Down
126 changes: 94 additions & 32 deletions selfdrive/boardd/boardd.cc
View file
Expand Up @@ -65,6 +65,9 @@ pthread_t safety_setter_thread_handle = -1;
pthread_t pigeon_thread_handle = -1;
bool pigeon_needs_init;

int big_recv;
uint32_t big_data[RECV_SIZE*2];

void pigeon_init();
void *pigeon_thread(void *crap);

Expand Down Expand Up @@ -216,11 +219,14 @@ void handle_usb_issue(int err, const char func[]) {
// TODO: check other errors, is simply retrying okay?
}

void can_recv(void *s) {
bool can_recv(void *s, bool force_send) {
int err;
uint32_t data[RECV_SIZE/4];
int recv;
uint32_t f1, f2;
int recv, big_index;
uint32_t f1, f2, address;
bool frame_sent;
uint64_t cur_time;
frame_sent = false;

// do recv
pthread_mutex_lock(&usb_lock);
Expand All @@ -236,38 +242,52 @@ void can_recv(void *s) {

pthread_mutex_unlock(&usb_lock);

// return if length is 0
if (recv <= 0) {
return;
// return if both buffers are empty
if ((big_recv <= 0) && (recv <= 0)) {
return true;
}

// create message
capnp::MallocMessageBuilder msg;
cereal::Event::Builder event = msg.initRoot<cereal::Event>();
event.setLogMonoTime(nanos_since_boot());
big_index = big_recv/0x10;
for (int i = 0; i<(recv/0x10); i++) {
big_data[(big_index + i)*4] = data[i*4];
big_data[(big_index + i)*4+1] = data[i*4+1];
big_data[(big_index + i)*4+2] = data[i*4+2];
big_data[(big_index + i)*4+3] = data[i*4+3];
big_recv += 0x10;
}
if (force_send) {
frame_sent = true;

auto canData = event.initCan(recv/0x10);
capnp::MallocMessageBuilder msg;
cereal::Event::Builder event = msg.initRoot<cereal::Event>();
event.setLogMonoTime(nanos_since_boot());

// populate message
for (int i = 0; i<(recv/0x10); i++) {
if (data[i*4] & 4) {
// extended
canData[i].setAddress(data[i*4] >> 3);
//printf("got extended: %x\n", data[i*4] >> 3);
} else {
// normal
canData[i].setAddress(data[i*4] >> 21);
auto can_data = event.initCan(big_recv/0x10);

// populate message
for (int i = 0; i<(big_recv/0x10); i++) {
if (big_data[i*4] & 4) {
// extended
can_data[i].setAddress(big_data[i*4] >> 3);
//printf("got extended: %x\n", big_data[i*4] >> 3);
} else {
// normal
can_data[i].setAddress(big_data[i*4] >> 21);
}
can_data[i].setBusTime(big_data[i*4+1] >> 16);
int len = big_data[i*4+1]&0xF;
can_data[i].setDat(kj::arrayPtr((uint8_t*)&big_data[i*4+2], len));
can_data[i].setSrc((big_data[i*4+1] >> 4) & 0xff);
}
canData[i].setBusTime(data[i*4+1] >> 16);
int len = data[i*4+1]&0xF;
canData[i].setDat(kj::arrayPtr((uint8_t*)&data[i*4+2], len));
canData[i].setSrc((data[i*4+1] >> 4) & 0xff);

// send to can
auto words = capnp::messageToFlatArray(msg);
auto bytes = words.asBytes();
zmq_send(s, bytes.begin(), bytes.size(), 0);
big_recv = 0;
}

// send to can
auto words = capnp::messageToFlatArray(msg);
auto bytes = words.asBytes();
zmq_send(s, bytes.begin(), bytes.size(), 0);
return frame_sent;
}

void can_health(void *s) {
Expand Down Expand Up @@ -456,11 +476,53 @@ void *can_recv_thread(void *crap) {
void *publisher = zmq_socket(context, ZMQ_PUB);
zmq_bind(publisher, "tcp://*:8006");

// run at ~200hz
bool frame_sent, skip_once, force_send;
uint64_t wake_time, cur_time, last_long_sleep;
int recv_state = 0;
force_send = true;
last_long_sleep = 1e-3 * nanos_since_boot();
wake_time = last_long_sleep;

while (!do_exit) {
can_recv(publisher);
// 5ms
usleep(5*1000);

frame_sent = can_recv(publisher, force_send);

// drain the Panda twice at 4.5ms intervals, then once at 1.0ms interval (twice max if sync_id is set)
if (recv_state++ < 2) {
last_long_sleep = 1e-3 * nanos_since_boot();
wake_time += 4500;
force_send = false;
if (last_long_sleep < wake_time) {
usleep(wake_time - last_long_sleep);
}
else {
if ((last_long_sleep - wake_time) > 5e5) {
// probably a new drive
wake_time = last_long_sleep;
}
else {
if (recv_state < 2) {
wake_time += 4500;
recv_state++;
if (last_long_sleep < wake_time) {
usleep(wake_time - last_long_sleep);
}
else {
printf(" lagging!\n");
}
}
}
}
}
else {
force_send = true;
recv_state = 0;
wake_time += 1000;
cur_time = 1e-3 * nanos_since_boot();
if (wake_time > cur_time) {
usleep(wake_time - cur_time);
}
}
}
return NULL;
}
Expand Down
14 changes: 9 additions & 5 deletions selfdrive/can/parser.cc
View file
Expand Up @@ -334,15 +334,19 @@ class CANParser {

// multiple recv is fine
bool first = wait;
while (1) {
while (first || drain) {
if (first) {
err = zmq_msg_recv(&msg, subscriber, 0);
first = false;
} else {
// Drain the queue at startup
usleep(500);
err = zmq_msg_recv(&msg, subscriber, ZMQ_DONTWAIT);
}
if (err < 0) break;

if (err < 0) {
drain = false;
break;
}
// format for board, make copy due to alignment issues, will be freed on out of scope
auto amsg = kj::heapArray<capnp::word>((zmq_msg_size(&msg) / sizeof(capnp::word)) + 1);
memcpy(amsg.begin(), zmq_msg_data(&msg), zmq_msg_size(&msg));
Expand All @@ -354,10 +358,9 @@ class CANParser {
auto cans = event.getCan();

UpdateCans(sec, cans);
UpdateValid(sec);
}

UpdateValid(sec);

zmq_msg_close(&msg);
}

Expand Down Expand Up @@ -386,6 +389,7 @@ class CANParser {

private:
const int bus;
bool drain = true;
// zmq vars
void *context = NULL;
void *subscriber = NULL;
Expand Down
5 changes: 4 additions & 1 deletion selfdrive/car/chrysler/interface.py
View file
Expand Up @@ -78,6 +78,8 @@ def get_params(candidate, fingerprint):
ret.steerKf = 0.00006 # full torque for 10 deg at 80mph means 0.00007818594
ret.steerActuatorDelay = 0.1
ret.steerRateCost = 0.7
ret.steerMPCReactTime = 0.025 # increase total MPC projected time by 25 ms
ret.steerMPCDampTime = 0.25 # dampen desired angle over 250ms (5 mpc cycles)

if candidate in (CAR.JEEP_CHEROKEE, CAR.JEEP_CHEROKEE_2019):
ret.wheelbase = 2.91 # in meters
Expand Down Expand Up @@ -140,7 +142,8 @@ def get_params(candidate, fingerprint):
def update(self, c):
# ******************* do can recv *******************
canMonoTimes = []
self.cp.update(int(sec_since_boot() * 1e9), False)

self.cp.update(int(sec_since_boot() * 1e9), True)
self.cp_cam.update(int(sec_since_boot() * 1e9), False)
self.CS.update(self.cp, self.cp_cam)

Expand Down

0 comments on commit f937265

Please sign in to comment.