Skip to content

Commit

Permalink
Revert "Merge Gernby's Resonant FF steering (#62)"
Browse files Browse the repository at this point in the history 
This reverts commit 234e57d.
  • Loading branch information
@NeonGalaxy75
NeonGalaxy75 committed Dec 16, 2018
1 parent b781ced commit f38e828
Show file tree
Hide file tree
Showing 5 changed files with 30 additions and 172 deletions.
44 changes: 7 additions & 37 deletions selfdrive/can/parser.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -166,9 +166,6 @@ 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 @@ -247,17 +244,8 @@ 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 @@ -266,38 +254,27 @@ 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 @@ -340,8 +317,6 @@ class CANParser {
UpdateCans(sec, cans);
}

if (can_forward_period_ns > 0) ForwardCANData(sec);

UpdateValid(sec);

zmq_msg_close(&msg);
Expand Down Expand Up @@ -376,11 +351,6 @@ 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: 1 addition & 6 deletions selfdrive/car/honda/carstate.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -36,7 +36,6 @@ 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 @@ -243,11 +242,7 @@ 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']
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 = 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
2 changes: 1 addition & 1 deletion selfdrive/car/honda/interface.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -373,7 +373,7 @@ def get_params(candidate, fingerprint):
ret.startAccel = 0.5

ret.steerActuatorDelay = 0.1
ret.steerRateCost = 0.35
ret.steerRateCost = 0.5

return ret

Expand Down
2 changes: 1 addition & 1 deletion selfdrive/controls/controlsd.py
View file
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, CS.steeringRate,
actuators.steer, actuators.steerAngle = LaC.update(active, CS.vEgo, CS.steeringAngle,
CS.steeringPressed, plan.dPoly, angle_offset, CP, VM, PL)

# Send a "steering required alert" if saturation count has reached the limit
Expand Down
147 changes: 20 additions & 127 deletions selfdrive/controls/lib/latcontrol.py
View file
Original file line number Diff line number Diff line change
@@ -1,8 +1,5 @@
import zmq
import math
import numpy as np
import time
import json
from selfdrive.controls.lib.pid import PIController
from selfdrive.controls.lib.drive_helpers import MPC_COST_LAT
from selfdrive.controls.lib.lateral_mpc import libmpc_py
Expand All @@ -24,14 +21,6 @@ def calc_states_after_delay(states, v_ego, steer_angle, curvature_factor, steer_
def get_steer_max(CP, v_ego):
return interp(v_ego, CP.steerMaxBP, CP.steerMaxV)

def apply_deadzone(angle, deadzone):
if angle > deadzone:
angle -= deadzone
elif angle < -deadzone:
angle += deadzone
else:
angle = 0.
return angle

class LatControl(object):
def __init__(self, CP):
Expand All @@ -40,25 +29,6 @@ def __init__(self, CP):
k_f=CP.steerKf, pos_limit=1.0)
self.last_cloudlog_t = 0.0
self.setup_mpc(CP.steerRateCost)
self.smooth_factor = 2.0 * CP.steerActuatorDelay / _DT # Multiplier for inductive component (feed forward)
self.projection_factor = 5.0 * _DT # Mutiplier for reactive component (PI)
self.accel_limit = 5.0 # Desired acceleration limit to prevent "whip steer" (resistive component)
self.ff_angle_factor = 0.5 # Kf multiplier for angle-based feed forward
self.ff_rate_factor = 5.0 # Kf multiplier for rate-based feed forward
self.ratioDelayExp = 2.0 # Exponential coefficient for variable steering rate (delay)
self.ratioDelayScale = 0.0 # Multiplier for variable steering rate (delay)
self.prev_angle_rate = 0
self.feed_forward = 0.0
self.angle_rate_desired = 0.0

# variables for dashboarding
self.context = zmq.Context()
self.steerpub = self.context.socket(zmq.PUB)
self.steerpub.bind("tcp://*:8594")
self.steerdata = ""
self.frames = 0
self.curvature_factor = 0.0
self.slip_factor = 0.0

def setup_mpc(self, steer_rate_cost):
self.libmpc = libmpc_py.libmpc
Expand All @@ -82,58 +52,38 @@ def setup_mpc(self, steer_rate_cost):
def reset(self):
self.pid.reset()

def update(self, active, v_ego, angle_steers, angle_rate, steer_override, d_poly, angle_offset, CP, VM, PL):
cur_time = sec_since_boot()
def update(self, active, v_ego, angle_steers, steer_override, d_poly, angle_offset, CP, VM, PL):
cur_time = sec_since_boot()
self.mpc_updated = False
# TODO: this creates issues in replay when rewinding time: mpc won't run
if self.last_mpc_ts < PL.last_md_ts:
self.last_mpc_ts = PL.last_md_ts
self.angle_steers_des_prev = self.angle_steers_des_mpc

self.curvature_factor = VM.curvature_factor(v_ego)
curvature_factor = VM.curvature_factor(v_ego)

# This is currently disabled, but it is used to compensate for variable steering rate
ratioDelayFactor = 1. + self.ratioDelayScale * abs(angle_steers / 100.) ** self.ratioDelayExp
l_poly = libmpc_py.ffi.new("double[4]", list(PL.PP.l_poly))
r_poly = libmpc_py.ffi.new("double[4]", list(PL.PP.r_poly))
p_poly = libmpc_py.ffi.new("double[4]", list(PL.PP.p_poly))

# Determine a proper delay time that includes the model's processing time, which is variable
plan_age = _DT_MPC + cur_time - float(PL.last_md_ts / 1000000000.0)
total_delay = ratioDelayFactor * CP.steerActuatorDelay + plan_age

# Use steering rate from the last 2 samples to estimate acceleration for a more realistic future steering rate
accelerated_angle_rate = 2.0 * angle_rate - self.prev_angle_rate

# Project the future steering angle for the actuator delay only (not model delay)
projected_angle_steers = ratioDelayFactor * CP.steerActuatorDelay * accelerated_angle_rate + angle_steers

self.l_poly = libmpc_py.ffi.new("double[4]", list(PL.PP.l_poly))
self.r_poly = libmpc_py.ffi.new("double[4]", list(PL.PP.r_poly))
self.p_poly = libmpc_py.ffi.new("double[4]", list(PL.PP.p_poly))

# account for actuation delay and the age of the plan
self.cur_state = calc_states_after_delay(self.cur_state, v_ego, projected_angle_steers, self.curvature_factor, CP.steerRatio, total_delay)
# account for actuation delay
self.cur_state = calc_states_after_delay(self.cur_state, v_ego, angle_steers, curvature_factor, CP.steerRatio, CP.steerActuatorDelay)

v_ego_mpc = max(v_ego, 5.0) # avoid mpc roughness due to low speed
self.libmpc.run_mpc(self.cur_state, self.mpc_solution,
self.l_poly, self.r_poly, self.p_poly,
PL.PP.l_prob, PL.PP.r_prob, PL.PP.p_prob, self.curvature_factor, v_ego_mpc, PL.PP.lane_width)
l_poly, r_poly, p_poly,
PL.PP.l_prob, PL.PP.r_prob, PL.PP.p_prob, curvature_factor, v_ego_mpc, PL.PP.lane_width)

# reset to current steer angle if not active or overriding
if active:
self.isActive = 1
delta_desired = self.mpc_solution[0].delta[1]
else:
self.isActive = 0
delta_desired = math.radians(angle_steers - angle_offset) / CP.steerRatio

self.cur_state[0].delta = delta_desired

self.angle_steers_des_mpc = float(math.degrees(delta_desired * CP.steerRatio) + angle_offset)

# Use the model's solve time instead of cur_time
self.angle_steers_des_time = float(PL.last_md_ts / 1000000000.0)

# Use last 2 desired angles to determine the model's desired steer rate
self.angle_rate_desired = (self.angle_steers_des_mpc - self.angle_steers_des_prev) / _DT_MPC
self.angle_steers_des_time = cur_time
self.mpc_updated = True

# Check for infeasable MPC solution
Expand All @@ -147,81 +97,24 @@ def update(self, active, v_ego, angle_steers, angle_rate, steer_override, d_poly
self.last_cloudlog_t = t
cloudlog.warning("Lateral mpc - nan: True")

elif self.steerdata != "" and self.frames > 10:
self.steerpub.send(self.steerdata)
self.frames = 0
self.steerdata = ""

if v_ego < 0.3 or not active:
output_steer = 0.0
self.feed_forward = 0.0
self.pid.reset()
else:
# Interpolate desired angle between MPC updates
self.angle_steers_des = self.angle_steers_des_prev + self.angle_rate_desired * (cur_time - self.angle_steers_des_time)

# Determine the target steer rate for desired angle, but prevent the acceleration limit from being exceeded
# Restricting the steer rate creates the resistive component needed for resonance
restricted_steer_rate = np.clip(self.angle_steers_des - float(angle_steers) , float(angle_rate) - self.accel_limit, float(angle_rate) + self.accel_limit)

# Determine projected desired angle that is within the acceleration limit (prevent the steering wheel from jerking)
projected_angle_steers_des = self.angle_steers_des + self.projection_factor * restricted_steer_rate

# Determine project angle steers using current steer rate
projected_angle_steers = float(angle_steers) + self.projection_factor * float(angle_rate)

# TODO: ideally we should interp, but for tuning reasons we keep the mpc solution
# constant for 0.05s.
#dt = min(cur_time - self.angle_steers_des_time, _DT_MPC + _DT) + _DT # no greater than dt mpc + dt, to prevent too high extraps
#self.angle_steers_des = self.angle_steers_des_prev + (dt / _DT_MPC) * (self.angle_steers_des_mpc - self.angle_steers_des_prev)
self.angle_steers_des = self.angle_steers_des_mpc
steers_max = get_steer_max(CP, v_ego)
self.pid.pos_limit = steers_max
self.pid.neg_limit = -steers_max

steer_feedforward = self.angle_steers_des # feedforward desired angle
if CP.steerControlType == car.CarParams.SteerControlType.torque:
# Decide which feed forward mode should be used (angle or rate). Use more dominant mode, and only if conditions are met
# Spread feed forward out over a period of time to make it more inductive (for resonance)
if abs(self.ff_rate_factor * float(restricted_steer_rate)) > abs(self.ff_angle_factor * float(self.angle_steers_des) - float(angle_offset)) - 0.5 \
and (abs(float(restricted_steer_rate)) > abs(angle_rate) or (float(restricted_steer_rate) < 0) != (angle_rate < 0)) \
and (float(restricted_steer_rate) < 0) == (float(self.angle_steers_des) - float(angle_offset) - 0.5 < 0):
ff_type = "r"
self.feed_forward = (((self.smooth_factor - 1.) * self.feed_forward) + self.ff_rate_factor * v_ego**2 * float(restricted_steer_rate)) / self.smooth_factor
elif abs(self.angle_steers_des - float(angle_offset)) > 0.5:
ff_type = "a"
self.feed_forward = (((self.smooth_factor - 1.) * self.feed_forward) + self.ff_angle_factor * v_ego**2 * float(apply_deadzone(float(self.angle_steers_des) - float(angle_offset), 0.5))) / self.smooth_factor
else:
ff_type = "r"
self.feed_forward = (((self.smooth_factor - 1.) * self.feed_forward) + 0.0) / self.smooth_factor
else:
self.feed_forward = self.angle_steers_des_mpc # feedforward desired angle
steer_feedforward *= v_ego**2 # proportional to realigning tire momentum (~ lateral accel)
deadzone = 0.0

# Use projected desired and actual angles instead of "current" values, in order to make PI more reactive (for resonance)
output_steer = self.pid.update(projected_angle_steers_des, projected_angle_steers, check_saturation=(v_ego > 10), override=steer_override,
feedforward=self.feed_forward, speed=v_ego, deadzone=deadzone)


# All but the last 3 lines after here are for real-time dashboarding
self.pCost = 0.0
self.lCost = 0.0
self.rCost = 0.0
self.hCost = 0.0
self.srCost = 0.0
self.last_ff_a = 0.0
self.last_ff_r = 0.0
self.center_angle = 0.0
self.steer_zero_crossing = 0.0
self.steer_rate_cost = 0.0
self.avg_angle_rate = 0.0
self.angle_rate_count = 0.0
driver_torque = 0.0
steer_motor = 0.0
self.frames += 1

self.steerdata += ("%d,%s,%d,%d,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%d|" % (self.isActive, \
ff_type, 1 if ff_type == "a" else 0, 1 if ff_type == "r" else 0, self.cur_state[0].x, self.cur_state[0].y, self.cur_state[0].psi, self.cur_state[0].delta, self.cur_state[0].t, self.curvature_factor, self.slip_factor ,self.smooth_factor, self.accel_limit, float(restricted_steer_rate) ,self.ff_angle_factor, self.ff_rate_factor, self.pCost, self.lCost, self.rCost, self.hCost, self.srCost, steer_motor, float(driver_torque), \
self.angle_rate_count, self.angle_rate_desired, self.avg_angle_rate, projected_angle_steers, float(angle_rate), self.steer_zero_crossing, self.center_angle, angle_steers, self.angle_steers_des, angle_offset, \
self.angle_steers_des_mpc, CP.steerRatio, CP.steerKf, CP.steerKpV[0], CP.steerKiV[0], CP.steerRateCost, PL.PP.l_prob, \
PL.PP.r_prob, PL.PP.c_prob, PL.PP.p_prob, self.l_poly[0], self.l_poly[1], self.l_poly[2], self.l_poly[3], self.r_poly[0], self.r_poly[1], self.r_poly[2], self.r_poly[3], \
self.p_poly[0], self.p_poly[1], self.p_poly[2], self.p_poly[3], PL.PP.c_poly[0], PL.PP.c_poly[1], PL.PP.c_poly[2], PL.PP.c_poly[3], PL.PP.d_poly[0], PL.PP.d_poly[1], \
PL.PP.d_poly[2], PL.PP.lane_width, PL.PP.lane_width_estimate, PL.PP.lane_width_certainty, v_ego, self.pid.p, self.pid.i, self.pid.f, int(time.time() * 100) * 10000000))
output_steer = self.pid.update(self.angle_steers_des, angle_steers, check_saturation=(v_ego > 10), override=steer_override,
feedforward=steer_feedforward, speed=v_ego, deadzone=deadzone)

self.sat_flag = self.pid.saturated
self.prev_angle_rate = angle_rate
return output_steer, float(self.angle_steers_des)

0 comments on commit f38e828

Please sign in to comment.