Skip to content

Commit

Permalink
Merge with kegman-gernbyFFsteer-latest (#69)
Browse files Browse the repository at this point in the history
* Honda Pilot PID Tuning

* Adjustable Follow Distance w/HUD (#12)

* TR

* TR

* TR

* TR

* TR

* TR

* TR

* TR

* remove readdistancelines - causes boot error

* copying cereal

* arne's cereal

* HUD distance lines display

* capnpp add

* revert

* revert

* Set ACC HUD to KPH

* Test - set HUD_ACC distance line to 2

* revert cruise_kph

LKAS error

* TR

* TR

* TR

* TR

* TR

* TR

* TR

* TR

* remove readdistancelines - causes boot error

* copying cereal

* HUD distance lines display

* capnpp add

* revert

* revert

* Set ACC HUD to KPH

* Test - set HUD_ACC distance line to 2

* revert cruise_kph

LKAS error

* Remove custom btn var line

could be crashing the code when button pressed

* HUD distance lines

* HUD distance lines

* passing CS.CP.readdistancelines

* Hardcode hud object dist_lines var

* Initializes CS.readdistancelines before passing to hud obj

* use integer read_distance_lines instead

* Add HUD_DISTANCE_3 for horizontal HUD bars on dash

* Default to 2 bars follow distance (1.8s)

* Code Cleanup for Distance Adjustment

Cleanup

Cleanup

Cleanup

Cleanup

Cleanup

Cleanup

Cleanup

Cleanup

Cleanup

* Auto-distance at surface street speeds

Override short distance setting when slow

Override distance when slow

Override distance if slow

Override distance when slow

Override distance when slow

Increase follow distance when below 60 kph

Lead car and rel. velocity to distance override

Override distance bars

revert

Override and restore dist based on speed and accel

Step up distance when < 40 km/h and restore

Revert

When using 1 bar distance:  Slow the car down earlier and more aggressively  (#18)

* Change one bar braking profile to 3 bars

* Update planner.py

* Update planner.py

* Update planner.py

* Update planner.py

* Update planner.py

* Update planner.py

* Update planner.py

* Update planner.py

* fake lead car vars

* fake lead vars

* Auto Distance Bar Working - tuning values

* Logging to log.txt

* Override hysteresis for one bar follow

* Only set override when decelerating

* syntax error fix for override flag

* Switch to 1.8s for softer braking

* Revert "Switch to 1.8s for softer braking"

This reverts commit d53623062ef814509db925fe4e624c3ae8aff1f1.

* Disable OTA update for safety

* Physical LKAS button disables OP steer and shows dashed lines on HUD (#19)

* Change one bar braking profile to 3 bars

* Update planner.py

* Update planner.py

* Update planner.py

* Update planner.py

* Update planner.py

* Update planner.py

* Update planner.py

* Update planner.py

* fake lead car vars

* fake lead vars

* Auto Distance Bar Working - tuning values

* Logging to log.txt

* Override hysteresis for one bar follow

* Only set override when decelerating

* syntax error fix for override flag

* Switch to 1.8s for softer braking

* Disable LKAS

* Apply Steer = 0 when user presses LKAS button

* Add Lkas button

* Add LK mode to HUDD data object

* Add Dashed Lanes to HUD

* Adjust HDD object

* return LKAS var in CarState

* Return LK var to CS

* Syntax error fix

* Update carcontroller.py

* testing hud object hardcoded hud_lanes and lkMode

* Restore accidentally deleted steer command

* Update carcontroller.py

* Update carcontroller.py

* Don't show solid lanes if OP steering disabled

* Dashed lines fix during engagement

eliminate steering alert when lkas disabled

lkas button logic cleanup

Reverse the dashed-lane indicator

No steering alert if LKAS disabled

revert Alert - LKAS errors during steer disable

Disable audible alert when steering disabled

Delete CS.aEgo < -0.5 override condition

* Remove chime disable when steering overriden

One bar profile change to 1.8s if < 65 kph

Simplified

chime instead of beep disable for LK override

* Adjust BRAKE_STOPPING_TARGET to 0.25

Keeps the brake pump from going crazy at a stop

* Add relative velocity to auto distance for 1 bar setting (#26)

* CAMERA_OFFSET = 0.08

* Revert Cost_long to comma value of 0.1

* Do not override to 1.8s when accel from stop

* Update logic for override distance bars

* Override distance when < 65kph and v_rel not > 5

* Use rel acceleration to reset to 1 bar

* Use vRel

* Update planner.py

* v_lead - vEgo > 1

* vLead - vEgo = -1

* vLead-vEgo < -1.5

* Cleanup Arne's code for distance

* vLead - vEgo < 1.25

* Back to vLead - vEgo < -1

* Disable OP steering and audio alarm when blinkers on (#28)

* LKAS disable when blinkers are on

* Update carcontroller.py

* Update carcontroller.py

* fix syntax error

* Critical alarm if cruise disengages while braking (#32)

* Update pathplanner.py

* Loud alert in case cruise disables when braking

* re-add if statement on speed for loud alert

* Update interface.py

* increase stopping distance to car by 1.5m

* Increase stopping distance to 2m

* Revert CAMERA_OFFSET to 0.06 (comma default)

* Lane width reduction to 2.85m

* Reduce tailgating for one bar distance at surface street speeds & code cleanup (#43)

* Gernby's Lane width filter fix (#41)

* Fix lane width filtering

* Fixed lane width filtering

* CAMERA_OFFSET = 0.08

* Minimum distance for one bar distance setting

* Don't tailgate at slow speeds with 1 bar distance

* x_lead < 5 only

* x_lead < 5 syntax fix

* self.v_rel < -1 or (x_lead < 10 and self.v_rel >= -1 and self.v_rel < 1)

* Tail gating auto distance change

* x_lead < 7.5 and self.v_rel >= -1 and self.v_rel < 0.2

* self.lastTR > 0 re-adding reinitialization

* Revert "Gernby's Lane width filter fix (#41)"

This reverts commit d081cfd8a05d2eb32e0148d477c806e65662dea7.

* Increase min distance to 10m for auto distance

* Auto distance code cleanup

* Add lead car fast approaching autodistance

* Auto distance Code cleanup

* Don't re-initialize MPC if already initialized

* self.lastTR != instead of >

* Initialize TR = 1.8

* include if statements vs just assigning boolean

* switch to 1 and 0 for TRUE AND FALSE

* Revert "switch to 1 and 0 for TRUE AND FALSE"

This reverts commit 58dcf718cdc3fca636229bf29942669670cb4487.

* Revert "include if statements vs just assigning boolean"

This reverts commit da7b5fa5088d62e78ba57d9e4c01e89160e77b9e.

* Revert "Initialize TR = 1.8"

This reverts commit f9c2571faeb0acaab818bcff146a2a397c0f07ff.

* Initialize TR=1.8 inside override loop

* Cleanup TR assignment for override loop

* Code Cleanup

* Syntax error

* Syntax error

* if statements for autodistance vars

* tr-last assignment indent

* CS.readdistancelines assignment to self.tr_last

* remove else MPC init

* Flatten out elif ladder for autodistance

* Removed shrinking_fast stopping for now

Braking is too sudden with 3 bar distance when radar detects car.  Shrinking gap takes care of this with 2 bar distance.

* Code Cleanup

* Revert CAMERA_OFFSET to Comma default of 0.06

* Merge Gernby's Resonant Steering (#51)

* Learn_angle_steer min max increased to +/-7 degs

* Increase min one bar distance to 12m for autodist

* Merge Gernby's Feed-Forward-Accel-Rate Branch (#49)

* Fixed feed-forward to consider steer_offset

* fixed a small oops on types

* Testing strategies for zero-crossing

* Moved angle stat collection to latcontrol.py

* First version that runs

* added outbound interface for feed-forward table

* First working

* Added more metrics

* Adjusted parameter timing

* performance tweaks

* Cleanup

* untested tweaks

* minor tweaks

* many untested changes

* going the other way with some things...

* Best yet

* cleaned up personalized "stuff"

* more cleanup for general use

* untested: minor adjustment to further reduce noise

* Fixed defect in desired angle interpolation

* some cleanup

* reverted change to Ki

* Reverted changes to manager.py

* Added steering rate consideration to latcontrol

* cleaned up for PR

* Fixed merge

* Testing approach when desired angle close to actual

* trying rate-based feed-forward

* added self-tuning parms for rate and angle FF

* fixed trim logic, and persisted to flash

* working amazingly well

* decreased time lapse for angle-based FF adjust

* many tweaks, self-tuning is a bitch

* simulteneous dual mode feedforward working very well

* added angular accelleration limit

* added super awesome angular accel limit

* non-specific save

* switching directions again

* oops

* ugly code with amazing results

* awesome, but untested after some cleanup

* more cleanup

* cleanup of the cleanup?  Need to test

* works amazingly well ... big O face

* cleanup

* I wish git was batter for cleanup

* removed Steer Offset because Pilot no likey

* Increase tailgating 1 bar resume to 0.5 m/s

* Gernby's resonant steering v2

* fixed an oops for non-bosch hondas

* CAMERA_OFFSET = 0.08

* More conservative tailgating distance and resume

* More conservative auto-distance for 1 bar setting

* Revert CAMERA_OFFSET to 0.06

* Revert auto-distance for 1 bar distance setting

* Revert "Merge Gernby's Resonant Steering" (#53)

* Revert "Revert auto-distance for 1 bar distance setting"

This reverts commit b7e5fb5.

* Revert "Revert CAMERA_OFFSET to 0.06"

This reverts commit 05cf21d.

* Revert "Merge Gernby's Resonant Steering (#51)"

This reverts commit d35aa19.

* better crowned road centering: learn_angle +/-7

* Implement csouers 67-70% charging to preserve batt

* slightly harder braking when tailgating

* x_lead < 5 or (x_lead < 17.5 and self.v_rel < 0.5)

* Proper alerts on Eon and disable flashing HUD alert when blinkers on or LKAS off (#63)

* Disable steer alert when LKAS off

* Display steering unavailable on Eon when LKAS off

* Revert chime disable in favor of new method

* x_lead < 6 or (x_lead < 17.5 and self.v_rel < 0.5)

* logic correction on HUD alert

* blinkers on disables HUD alarm

* Add Manual Steering alert for LKAS Off mode

* Call manual steering alert when LKAS off

* Call blinkers alert when blinkers on

* Add blinkers alert

* Adjust Blinkers alert logic to include LKAS mode

* Fix blinker logic

* Add new steering required alerts to capnp

* Trace LKAS / LDW error

* Disable hud alert when LKAS off or blinkers on

* Disable HUD alerts if LKAS off or blinkers on

* Disable other ACC alert if LKAS off or blinkers on

* Revert testing of steering required alert override

* Disable steer saturate alert if blinkers on or LKAS off

* Disable steer sat alert if LKAS off or blinkers on

* Revert steerSaturated logic

* disable steer limit alert when LKAS off or blinkers on

* Fix Syntax error

* Revert  steerLimit alert disable

* Disable steerSat alert when blinkers on or LKAS off

* Revert stopping distance to default for pedal lurch

* Re-intro 2m stopping distance

* x_lead = 6

* Merge Gernby's latest Feed forward resonant (#68)

* Fixed feed-forward to consider steer_offset

* fixed a small oops on types

* Testing strategies for zero-crossing

* Moved angle stat collection to latcontrol.py

* First version that runs

* added outbound interface for feed-forward table

* First working

* Added more metrics

* Adjusted parameter timing

* performance tweaks

* Cleanup

* untested tweaks

* minor tweaks

* many untested changes

* going the other way with some things...

* Best yet

* cleaned up personalized "stuff"

* more cleanup for general use

* untested: minor adjustment to further reduce noise

* Fixed defect in desired angle interpolation

* some cleanup

* reverted change to Ki

* Reverted changes to manager.py

* Added steering rate consideration to latcontrol

* cleaned up for PR

* Fixed merge

* Testing approach when desired angle close to actual

* trying rate-based feed-forward

* added self-tuning parms for rate and angle FF

* fixed trim logic, and persisted to flash

* working amazingly well

* decreased time lapse for angle-based FF adjust

* many tweaks, self-tuning is a bitch

* simulteneous dual mode feedforward working very well

* added angular accelleration limit

* added super awesome angular accel limit

* non-specific save

* switching directions again

* oops

* ugly code with amazing results

* awesome, but untested after some cleanup

* more cleanup

* cleanup of the cleanup?  Need to test

* works amazingly well ... big O face

* cleanup

* I wish git was batter for cleanup

* hopefully proper merge of merged fixes of merge

* fixed an oops for non-bosch hondas

* added steer rate to future state calculation

* added actual acceleration to future projected state

* fixed projected angle error for PIF

* untested ...

* added comments

* completely UNtested

* untested merge

* battling shitty git

* git is sucking giant monkey balls

* Trying to figure out what git did...

* still trying to un-git myself

* Finally got it fixed, works awesome!

* Hopefully not corrupted by git

* hopefully fixed white space corruption

* working again, hopefully resolved git issue

* Revert "Implement csouers 67-70% charging to preserve batt"

This reverts commit e3249be.
  • Loading branch information
NeonGalaxy75 authored Dec 16, 2018
1 parent f38e828 commit 99031cf
Show file tree
Hide file tree
Showing 7 changed files with 233 additions and 43 deletions.
44 changes: 37 additions & 7 deletions selfdrive/can/parser.cc
Original file line number Diff line number Diff line change
Expand Up @@ -166,6 +166,9 @@ class CANParser {
subscriber = zmq_socket(context, ZMQ_SUB);
zmq_setsockopt(subscriber, ZMQ_SUBSCRIBE, "", 0);

forwarder = zmq_socket(context, ZMQ_PUB);
zmq_bind(forwarder, "tcp://*:8592");

std::string tcp_addr_str;

if (sendcan) {
Expand Down Expand Up @@ -244,8 +247,17 @@ class CANParser {
// parse the messages
for (int i = 0; i < msg_count; i++) {
auto cmsg = cans[i];

if (cmsg.getDat().size() > 8) continue; //shouldnt ever happen
uint8_t dat[8] = {0};
memcpy(dat, cmsg.getDat().begin(), cmsg.getDat().size());

if (can_forward_period_ns > 0) raw_can_values[cmsg.getSrc()][cmsg.getAddress()] = read_u64_be(dat);

//if (((cmsg.getAddress() == 0xe4 or cmsg.getAddress() == 0x33d) and cmsg.getSrc() == bus) or \
// (cmsg.getSrc() != bus and cmsg.getAddress() != 0x33d and cmsg.getAddress() != 0xe4 and \
// (cmsg.getAddress() < 0x240 or cmsg.getAddress() > 0x24A))) {
if (cmsg.getSrc() != bus) {
// DEBUG("skip %d: wrong bus\n", cmsg.getAddress());
continue;
}
auto state_it = message_states.find(cmsg.getAddress());
Expand All @@ -254,27 +266,38 @@ class CANParser {
continue;
}

if (cmsg.getDat().size() > 8) continue; //shouldnt ever happen
uint8_t dat[8] = {0};
memcpy(dat, cmsg.getDat().begin(), cmsg.getDat().size());

// Assumes all signals in the message are of the same type (little or big endian)
// TODO: allow signals within the same message to have different endianess
auto& sig = message_states[cmsg.getAddress()].parse_sigs[0];
if (sig.is_little_endian) {
p = read_u64_le(dat);
} else {
p = read_u64_be(dat);
}
}

DEBUG(" proc %X: %llx\n", cmsg.getAddress(), p);

state_it->second.parse(sec, cmsg.getBusTime(), p);
}
}

void ForwardCANData(uint64_t sec) {
if (sec > next_can_forward_ns) {
next_can_forward_ns += can_forward_period_ns;
// next_can_forward_ns starts at 0, so it needs to be reset. Also handle delays.
if (sec > next_can_forward_ns) next_can_forward_ns = sec + can_forward_period_ns;
std::string canOut = "";
for (auto src : raw_can_values) {
for (auto pid : src.second) {
canOut = canOut + std::to_string(src.first) + " " + std::to_string(pid.first) + " " + std::to_string(pid.second) + "|";
}
}
zmq_send(forwarder, canOut.data(), canOut.size(), 0);
}
}

void UpdateValid(uint64_t sec) {
can_valid = true;
can_valid = true;
for (const auto& kv : message_states) {
const auto& state = kv.second;
if (state.check_threshold > 0 && (sec - state.seen) > state.check_threshold) {
Expand Down Expand Up @@ -317,6 +340,8 @@ class CANParser {
UpdateCans(sec, cans);
}

if (can_forward_period_ns > 0) ForwardCANData(sec);

UpdateValid(sec);

zmq_msg_close(&msg);
Expand Down Expand Up @@ -351,6 +376,11 @@ class CANParser {
void *context = NULL;
void *subscriber = NULL;

void *forwarder = NULL;
uint64_t can_forward_period_ns = 100000000;
uint64_t next_can_forward_ns = 0;
std::unordered_map<uint32_t, std::unordered_map<uint32_t, uint64_t>> raw_can_values;

const DBC *dbc = NULL;
std::unordered_map<uint32_t, MessageState> message_states;
};
Expand Down
7 changes: 6 additions & 1 deletion selfdrive/car/honda/carstate.py
Original file line number Diff line number Diff line change
Expand Up @@ -36,6 +36,7 @@ def get_can_signals(CP):
("WHEEL_SPEED_RR", "WHEEL_SPEEDS", 0),
("STEER_ANGLE", "STEERING_SENSORS", 0),
("STEER_ANGLE_RATE", "STEERING_SENSORS", 0),
("STEER_ANGLE_OFFSET", "STEERING_SENSORS", 0),
("STEER_TORQUE_SENSOR", "STEER_STATUS", 0),
("LEFT_BLINKER", "SCM_FEEDBACK", 0),
("RIGHT_BLINKER", "SCM_FEEDBACK", 0),
Expand Down Expand Up @@ -242,7 +243,11 @@ def update(self, cp, cp_cam):
self.user_gas_pressed = self.user_gas > 0 # this works because interceptor read < 0 when pedal position is 0. Once calibrated, this will change

self.gear = 0 if self.CP.carFingerprint == CAR.CIVIC else cp.vl["GEARBOX"]['GEAR']
self.angle_steers = cp.vl["STEERING_SENSORS"]['STEER_ANGLE']
if self.CP.carFingerprint in HONDA_BOSCH:
self.angle_steers = cp.vl["STEERING_SENSORS"]['STEER_ANGLE'] + cp.vl["STEERING_SENSORS"]['STEER_ANGLE_OFFSET']
else:
self.angle_steers = cp.vl["STEERING_SENSORS"]['STEER_ANGLE']

self.angle_steers_rate = cp.vl["STEERING_SENSORS"]['STEER_ANGLE_RATE']

#self.cruise_setting = cp.vl["SCM_BUTTONS"]['CRUISE_SETTING']
Expand Down
8 changes: 4 additions & 4 deletions selfdrive/car/honda/interface.py
Original file line number Diff line number Diff line change
Expand Up @@ -196,7 +196,7 @@ def get_params(candidate, fingerprint):
tire_stiffness_factor = 1.
# Civic at comma has modified steering FW, so different tuning for the Neo in that car
is_fw_modified = os.getenv("DONGLE_ID") in ['99c94dc769b5d96e']
ret.steerKpV, ret.steerKiV = [[0.33], [0.10]] if is_fw_modified else [[0.8], [0.24]]
ret.steerKpV, ret.steerKiV = [[0.33], [0.10]] if is_fw_modified else [[0.6], [0.18]]
if is_fw_modified:
ret.steerKf = 0.00003
ret.longitudinalKpBP = [0., 5., 35.]
Expand All @@ -211,7 +211,7 @@ def get_params(candidate, fingerprint):
ret.centerToFront = centerToFront_civic
ret.steerRatio = 14.63 # 10.93 is spec end-to-end
tire_stiffness_factor = 1.
ret.steerKpV, ret.steerKiV = [[0.8], [0.24]]
ret.steerKpV, ret.steerKiV = [[0.6], [0.18]]
ret.longitudinalKpBP = [0., 5., 35.]
ret.longitudinalKpV = [1.2, 0.8, 0.5]
ret.longitudinalKiBP = [0., 35.]
Expand Down Expand Up @@ -256,7 +256,7 @@ def get_params(candidate, fingerprint):
ret.centerToFront = ret.wheelbase * 0.41
ret.steerRatio = 15.3 # as spec
tire_stiffness_factor = 0.444 # not optimized yet
ret.steerKpV, ret.steerKiV = [[0.8], [0.24]]
ret.steerKpV, ret.steerKiV = [[0.6], [0.18]]
ret.longitudinalKpBP = [0., 5., 35.]
ret.longitudinalKpV = [1.2, 0.8, 0.5]
ret.longitudinalKiBP = [0., 35.]
Expand All @@ -283,7 +283,7 @@ def get_params(candidate, fingerprint):
ret.centerToFront = ret.wheelbase * 0.38
ret.steerRatio = 15.0 # as spec
tire_stiffness_factor = 0.444 # not optimized yet
ret.steerKpV, ret.steerKiV = [[0.8], [0.24]]
ret.steerKpV, ret.steerKiV = [[0.6], [0.18]]
ret.longitudinalKpBP = [0., 5., 35.]
ret.longitudinalKpV = [1.2, 0.8, 0.5]
ret.longitudinalKiBP = [0., 35.]
Expand Down
2 changes: 1 addition & 1 deletion selfdrive/controls/controlsd.py
Original file line number Diff line number Diff line change
Expand Up @@ -274,7 +274,7 @@ def state_control(plan, CS, CP, state, events, v_cruise_kph, v_cruise_kph_last,
v_cruise_kph, plan.vTarget, plan.vTargetFuture, plan.aTarget,
CP, PL.lead_1)
# Steering PID loop and lateral MPC
actuators.steer, actuators.steerAngle = LaC.update(active, CS.vEgo, CS.steeringAngle,
actuators.steer, actuators.steerAngle = LaC.update(active, CS.vEgo, CS.steeringAngle, CS.steeringRate,
CS.steeringPressed, plan.dPoly, angle_offset, CP, VM, PL)

# Send a "steering required alert" if saturation count has reached the limit
Expand Down
Loading

0 comments on commit 99031cf

Please sign in to comment.