From 19dd5f3e3246c94fcb70a2a8e09b5213e1c4ba15 Mon Sep 17 00:00:00 2001 From: Vehicle Researcher Date: Thu, 10 Aug 2017 02:32:38 -0700 Subject: [PATCH] openpilot v0.3.6 tweaks --- panda | 2 +- selfdrive/can/parser.cc | 7 +++++-- selfdrive/can/parser.py | 9 +++++++-- selfdrive/can/parser_common.h | 1 + selfdrive/car/honda/carstate.py | 15 ++++++++++++--- selfdrive/controls/controlsd.py | 3 ++- selfdrive/controls/lib/drive_helpers.py | 8 ++++---- selfdrive/controls/lib/pathplanner.py | 4 +++- selfdrive/test/plant/plant.py | 6 +++--- 9 files changed, 38 insertions(+), 17 deletions(-) diff --git a/panda b/panda index ff8a4bd4e8..9bb9a7f20f 160000 --- a/panda +++ b/panda @@ -1 +1 @@ -Subproject commit ff8a4bd4e844c2b0a0dd7efb321b31781d8034c8 +Subproject commit 9bb9a7f20f4592e04fc49c65a43f84582457c937 diff --git a/selfdrive/can/parser.cc b/selfdrive/can/parser.cc index 8507a5eebf..7e7913bf43 100644 --- a/selfdrive/can/parser.cc +++ b/selfdrive/can/parser.cc @@ -66,13 +66,14 @@ struct MessageState { std::vector parse_sigs; std::vector vals; + uint16_t ts; uint64_t seen; uint64_t check_threshold; uint8_t counter; uint8_t counter_fail; - bool parse(uint64_t sec, uint64_t dat) { + bool parse(uint64_t sec, uint16_t ts_, uint64_t dat) { for (int i=0; i < parse_sigs.size(); i++) { auto& sig = parse_sigs[i]; @@ -96,6 +97,7 @@ struct MessageState { vals[i] = tmp * sig.factor + sig.offset; } + ts = ts_; seen = sec; return true; @@ -222,7 +224,7 @@ class CANParser { DEBUG(" proc %X: %lx\n", cmsg.getAddress(), p); - state_it->second.parse(sec, p); + state_it->second.parse(sec, cmsg.getBusTime(), p); } } @@ -285,6 +287,7 @@ class CANParser { const Signal &sig = state.parse_sigs[i]; ret.push_back((SignalValue){ .address = state.address, + .ts = state.ts, .name = sig.name, .value = state.vals[i], }); diff --git a/selfdrive/can/parser.py b/selfdrive/can/parser.py index 4a9490c772..6e2c21cb26 100644 --- a/selfdrive/can/parser.py +++ b/selfdrive/can/parser.py @@ -25,6 +25,7 @@ typedef struct MessageParseOptions { typedef struct SignalValue { uint32_t address; + uint16_t ts; const char* name; double value; } SignalValue; @@ -45,6 +46,7 @@ class CANParser(object): def __init__(self, dbc_name, signals, checks=[], bus=0): self.can_valid = True self.vl = defaultdict(dict) + self.ts = defaultdict(dict) sig_names = dict((name, ffi.new("char[]", name)) for name, _, _ in signals) @@ -87,7 +89,9 @@ class CANParser(object): cv = self.can_values[i] address = cv.address # print hex(cv.address), ffi.string(cv.name) - self.vl[address][ffi.string(cv.name)] = cv.value + name = ffi.string(cv.name) + self.vl[address][name] = cv.value + self.ts[address][name] = cv.ts ret.add(address) return ret @@ -161,6 +165,7 @@ if __name__ == "__main__": while True: cp.update(int(sec_since_boot()*1e9), True) - print cp.vl + # print cp.vl + print cp.ts print cp.can_valid time.sleep(0.01) diff --git a/selfdrive/can/parser_common.h b/selfdrive/can/parser_common.h index 34676a690c..e3b944f83e 100644 --- a/selfdrive/can/parser_common.h +++ b/selfdrive/can/parser_common.h @@ -21,6 +21,7 @@ struct MessageParseOptions { struct SignalValue { uint32_t address; + uint16_t ts; const char* name; double value; }; diff --git a/selfdrive/car/honda/carstate.py b/selfdrive/car/honda/carstate.py index b63220b70d..d2c0f974ae 100644 --- a/selfdrive/car/honda/carstate.py +++ b/selfdrive/car/honda/carstate.py @@ -259,6 +259,8 @@ class CarState(object): self.cp = get_can_parser(CP) self.user_gas, self.user_gas_pressed = 0., 0 + self.brake_switch_prev = 0 + self.brake_switch_ts = 0 self.cruise_buttons = 0 self.cruise_setting = 0 @@ -311,8 +313,6 @@ class CarState(object): else: self.steer_error = cp.vl[0x18F]['STEER_STATUS'] not in [0,2,4,6] self.steer_not_allowed = cp.vl[0x18F]['STEER_STATUS'] != 0 - if cp.vl[0x18F]['STEER_STATUS'] != 0: - print cp.vl[0x18F]['STEER_STATUS'] self.brake_error = cp.vl[0x1B0]['BRAKE_ERROR_1'] or cp.vl[0x1B0]['BRAKE_ERROR_2'] self.esp_disabled = cp.vl[0x1A4]['ESP_DISABLED'] # calc best v_ego estimate, by averaging two opposite corners @@ -371,6 +371,7 @@ class CarState(object): self.blinker_on = cp.vl[0x294]['LEFT_BLINKER'] or cp.vl[0x294]['RIGHT_BLINKER'] self.left_blinker_on = cp.vl[0x294]['LEFT_BLINKER'] self.right_blinker_on = cp.vl[0x294]['RIGHT_BLINKER'] + if self.accord: # on the accord, this doesn't seem to include cruise control self.car_gas = cp.vl[0x17C]['PEDAL_GAS'] @@ -382,7 +383,15 @@ class CarState(object): else: self.car_gas = cp.vl[0x130]['CAR_GAS'] self.steer_override = abs(cp.vl[0x18F]['STEER_TORQUE_SENSOR']) > 1200 - self.brake_pressed = cp.vl[0x17C]['BRAKE_PRESSED'] or cp.vl[0x17C]['BRAKE_SWITCH'] + + # brake switch has shown some single time step noise, so only considered when + # switch is on for at least 2 consecutive CAN samples + self.brake_pressed = cp.vl[0x17C]['BRAKE_PRESSED'] or \ + (cp.vl[0x17C]['BRAKE_SWITCH'] and self.brake_switch_prev and \ + cp.ts[0x17C]['BRAKE_SWITCH'] != self.brake_switch_ts) + self.brake_switch_prev = cp.vl[0x17C]['BRAKE_SWITCH'] + self.brake_switch_ts = cp.ts[0x17C]['BRAKE_SWITCH'] + self.user_brake = cp.vl[0x1A4]['USER_BRAKE'] self.standstill = not cp.vl[0x1B0]['WHEELS_MOVING'] self.v_cruise_pcm = cp.vl[0x324]['CRUISE_SPEED_PCM'] diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index abd7afa228..b747341de4 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -274,7 +274,8 @@ class Controls(object): if self.rk.frame % 5 == 2 and self.plan.lateralValid: # *** run this at 20hz again *** self.angle_offset = learn_angle_offset(self.enabled, self.CS.vEgo, self.angle_offset, - self.plan.dPoly, self.LaC.y_des, self.CS.steeringPressed) + self.PL.PP.c_poly, self.PL.PP.c_prob, self.LaC.y_des, + self.CS.steeringPressed) # *** gas/brake PID loop *** final_gas, final_brake = self.LoC.update(self.enabled, self.CS.vEgo, self.v_cruise_kph, diff --git a/selfdrive/controls/lib/drive_helpers.py b/selfdrive/controls/lib/drive_helpers.py index b8a9ddc1f7..2d01b40ca0 100644 --- a/selfdrive/controls/lib/drive_helpers.py +++ b/selfdrive/controls/lib/drive_helpers.py @@ -3,20 +3,20 @@ from common.numpy_fast import clip def rate_limit(new_value, last_value, dw_step, up_step): return clip(new_value, last_value + dw_step, last_value + up_step) -def learn_angle_offset(lateral_control, v_ego, angle_offset, d_poly, y_des, steer_override): +def learn_angle_offset(lateral_control, v_ego, angle_offset, c_poly, c_prob, y_des, steer_override): # simple integral controller that learns how much steering offset to put to have the car going straight + # while being in the middle of the lane min_offset = -5. # deg max_offset = 5. # deg alpha = 1./36000. # correct by 1 deg in 2 mins, at 30m/s, with 50cm of error, at 20Hz min_learn_speed = 1. # learn less at low speed or when turning - alpha_v = alpha*(max(v_ego - min_learn_speed, 0.))/(1. + 0.5*abs(y_des)) + alpha_v = alpha * c_prob * (max(v_ego - min_learn_speed, 0.)) / (1. + 0.5*abs(y_des)) # only learn if lateral control is active and if driver is not overriding: if lateral_control and not steer_override: - angle_offset += d_poly[3] * alpha_v + angle_offset += c_poly[3] * alpha_v angle_offset = clip(angle_offset, min_offset, max_offset) return angle_offset - diff --git a/selfdrive/controls/lib/pathplanner.py b/selfdrive/controls/lib/pathplanner.py index d7b0e6a135..d65367b995 100644 --- a/selfdrive/controls/lib/pathplanner.py +++ b/selfdrive/controls/lib/pathplanner.py @@ -62,6 +62,8 @@ class PathPlanner(object): def __init__(self): self.dead = True self.d_poly = [0., 0., 0., 0.] + self.c_poly = [0., 0., 0., 0.] + self.c_prob = 0. self.last_model = 0. self.lead_dist, self.lead_prob, self.lead_var = 0, 0, 1 self._path_pinv = compute_path_pinv() @@ -81,7 +83,7 @@ class PathPlanner(object): self.lead_var = md.model.lead.std**2 # compute target path - self.d_poly, _, _ = calc_desired_path(l_poly, r_poly, p_poly, l_prob, r_prob, p_prob, v_ego) + self.d_poly, self.c_poly, self.c_prob = calc_desired_path(l_poly, r_poly, p_poly, l_prob, r_prob, p_prob, v_ego) self.last_model = cur_time self.dead = False diff --git a/selfdrive/test/plant/plant.py b/selfdrive/test/plant/plant.py index 51a0e9409c..ec9e5b590f 100755 --- a/selfdrive/test/plant/plant.py +++ b/selfdrive/test/plant/plant.py @@ -204,12 +204,12 @@ class Plant(object): print "%6.2f m %6.2f m/s %6.2f m/s2 %.2f ang gas: %.2f brake: %.2f steer: %5.2f lead_rel: %6.2f m %6.2f m/s" % (distance, speed, acceleration, self.angle_steer, gas, brake, steer_torque, d_rel, v_rel) # ******** publish the car ******** - vls = [self.speed_sensor(speed), self.speed_sensor(speed), self.speed_sensor(speed), self.speed_sensor(speed), + vls = [self.speed_sensor(speed), self.speed_sensor(speed), self.speed_sensor(speed), self.speed_sensor(speed), self.speed_sensor(speed), self.angle_steer, 0, self.gear_choice, speed!=0, 0, 0, 0, 0, - self.v_cruise, not self.seatbelt, self.seatbelt, self.brake_pressed, + self.v_cruise, not self.seatbelt, self.seatbelt, self.brake_pressed, 0., self.user_gas, cruise_buttons, self.esp_disabled, 0, - self.user_brake, self.steer_error, self.speed_sensor(speed), self.brake_error, + self.user_brake, self.steer_error, self.brake_error, self.brake_error, self.gear_shifter, self.main_on, self.acc_status, self.pedal_gas, self.cruise_setting, # left_blinker, right_blinker, counter