diff --git a/cereal b/cereal index e2604ab1c31fd6..77983194eace55 160000 --- a/cereal +++ b/cereal @@ -1 +1 @@ -Subproject commit e2604ab1c31fd6faa09002049b76e8872ee72cc2 +Subproject commit 77983194eace552f8598a85a76355681b7d49b2c diff --git a/selfdrive/car/gm/carcontroller.py b/selfdrive/car/gm/carcontroller.py index 4db0bb05de477c..02dcc4a8f9aaf4 100644 --- a/selfdrive/car/gm/carcontroller.py +++ b/selfdrive/car/gm/carcontroller.py @@ -11,6 +11,9 @@ VisualAlert = car.CarControl.HUDControl.VisualAlert +# only use pitch-compensated acceleration at 10m/s+ +ACCEL_PITCH_FACTOR_BP = [5., 10.] # [m/s] +ACCEL_PITCH_FACTOR_V = [0., 1.] # [unitless in [0-1]] class CarController(): def __init__(self, dbc_name, CP, VM): @@ -58,20 +61,15 @@ def update(self, enabled, CS, frame, actuators, can_sends.append(gmcan.create_steering_control(self.packer_pt, CanBus.POWERTRAIN, apply_steer, idx, lkas_enabled)) # Gas/regen prep - # apply pitch "shifted deadzone" that gives smooth output in addition to applying a deadzone - if abs(CS.pitch_accel) > CS.pitch_accel_deadzone: - pitch = CS.pitch_accel + (CS.pitch_accel_deadzone if CS.pitch_accel < 0. else -CS.pitch_accel_deadzone) - else: - pitch = 0. - gravity_x = -9.8 * sin(pitch) * CS.pitch_accel_factor if not enabled or CS.pause_long_on_gas_press: # Stock ECU sends max regen when not enabled. apply_gas = P.MAX_ACC_REGEN apply_brake = 0 else: - apply_gas = interp(actuators.accel - gravity_x, P.GAS_LOOKUP_BP, P.GAS_LOOKUP_V) - pitch_brake_lowspeed_factor = interp(CS.vEgo, CS.pitch_accel_brake_lowspeed_lockout_bp, CS.pitch_accel_brake_lowspeed_lockout_v) - apply_brake = interp(actuators.accel - gravity_x * pitch_brake_lowspeed_factor, P.BRAKE_LOOKUP_BP, P.BRAKE_LOOKUP_V) + k = interp(CS.out.vEgo, ACCEL_PITCH_FACTOR_BP, ACCEL_PITCH_FACTOR_V) + brake_accel = k * actuators.accelPitchCompensated + (1. - k) * actuators.accel + apply_gas = interp(actuators.accelPitchCompensated, P.GAS_LOOKUP_BP, P.GAS_LOOKUP_V) + apply_brake = interp(brake_accel, P.BRAKE_LOOKUP_BP, P.BRAKE_LOOKUP_V) t = sec_since_boot() v_rel = CS.coasting_lead_v - CS.vEgo diff --git a/selfdrive/car/gm/carstate.py b/selfdrive/car/gm/carstate.py index 9dd02051646a78..222267b63d49a4 100644 --- a/selfdrive/car/gm/carstate.py +++ b/selfdrive/car/gm/carstate.py @@ -125,13 +125,6 @@ def __init__(self, CP): self.pitch_raw = 0. # radians self.pitch_ema = 1/100 self.pitch_future_time = 0.5 # seconds - self.pitch_accel_factor = 0.85 - self.pitch_accel_deadzone = 0.01 # radians ~ ±1% grade - self.pitch_accel = 0. - self.pitch_accel_raw = 0. - self.pitch_accel_future_time = 0.8 - self.pitch_accel_brake_lowspeed_lockout_bp = [i * CV.MPH_TO_MS for i in [5., 15.]] - self.pitch_accel_brake_lowspeed_lockout_v = [0., 1.] # similar to over-speed coast braking, lockout coast/one-pedal logic first for engine/regen braking, and then for actual brakes. @@ -361,7 +354,6 @@ def update(self, pt_cp, loopback_cp): ret.onePedalBrakeMode = self.one_pedal_brake_mode self.pitch = self.pitch_ema * self.pitch_raw + (1 - self.pitch_ema) * self.pitch - self.pitch_accel = self.pitch_ema * self.pitch_accel_raw + (1 - self.pitch_ema) * self.pitch_accel ret.pitch = self.pitch ret.autoHoldActivated = self.autoHoldActivated diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index d5d464637649fb..34bead163e4c4f 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -16,7 +16,9 @@ from selfdrive.controls.lib.lane_planner import CAMERA_OFFSET from selfdrive.controls.lib.drive_helpers import update_v_cruise, initialize_v_cruise, V_CRUISE_MIN from selfdrive.controls.lib.drive_helpers import get_lag_adjusted_curvature +from selfdrive.controls.lib.drive_helpers import apply_deadzone from selfdrive.controls.lib.longcontrol import LongControl, STARTING_TARGET_SPEED +from selfdrive.controls.lib.lane_planner import TRAJECTORY_SIZE from selfdrive.controls.lib.latcontrol_pid import LatControlPID from selfdrive.controls.lib.latcontrol_indi import LatControlINDI from selfdrive.controls.lib.latcontrol_lqr import LatControlLQR @@ -24,6 +26,7 @@ from selfdrive.controls.lib.latcontrol_torque import LatControlTorque from selfdrive.controls.lib.events import Events, ET from selfdrive.controls.lib.alertmanager import AlertManager +from selfdrive.controls.lib.vehicle_model import ACCELERATION_DUE_TO_GRAVITY from selfdrive.controls.lib.vehicle_model import VehicleModel from selfdrive.locationd.calibrationd import Calibration from selfdrive.modeld.constants import T_IDXS @@ -171,6 +174,8 @@ def __init__(self, sm=None, pm=None, can_sock=None): self.logged_comm_issue = False self.v_target = 0.0 self.a_target = 0.0 + self.pitch = 0.0 + self.pitch_accel_deadzone = 0.01 # [radians] ≈ 1% grade # TODO: no longer necessary, aside from process replay self.sm['liveParameters'].valid = True @@ -539,12 +544,9 @@ def state_control(self, CS): lat_plan = self.sm['lateralPlan'] long_plan = self.sm['longitudinalPlan'] - if self.sm.valid.get('liveLocationKalman', False) and self.sm.valid.get('modelV2', False) and len(self.sm['modelV2'].orientation.y) >= 12 and len(self.sm['liveLocationKalman'].calibratedOrientationNED.value) > 1 and sec_since_boot() > 120.: - current_pitch = clip(self.sm['liveLocationKalman'].calibratedOrientationNED.value[1], -MAX_ABS_PITCH, MAX_ABS_PITCH) + if self.sm.updated['liveParameters'] and len(self.sm['modelV2'].orientation.y) == TRAJECTORY_SIZE: future_pitch_diff = clip(interp(self.CI.CS.pitch_future_time, T_IDXS, self.sm['modelV2'].orientation.y), -MAX_ABS_PRED_PITCH_DELTA, MAX_ABS_PRED_PITCH_DELTA) - self.CI.CS.pitch_raw = current_pitch + future_pitch_diff - future_pitch_diff = clip(interp(self.CI.CS.pitch_accel_future_time, T_IDXS, self.sm['modelV2'].orientation.y), -MAX_ABS_PRED_PITCH_DELTA, MAX_ABS_PRED_PITCH_DELTA) - self.CI.CS.pitch_accel_raw = current_pitch + clip(future_pitch_diff, -MAX_ABS_PRED_PITCH_DELTA, MAX_ABS_PRED_PITCH_DELTA) + self.CI.CS.pitch_raw = self.sm['liveParameters'].pitch + future_pitch_diff actuators = car.CarControl.Actuators.new_message() @@ -570,6 +572,12 @@ def state_control(self, CS): t_since_plan = (self.sm.frame - self.sm.rcv_frame['longitudinalPlan']) * DT_CTRL actuators.accel = self.LoC.update(self.active, CS, self.CP, long_plan, pid_accel_limits, t_since_plan) + # compute pitch-compensated accel + if self.sm.updated['liveParameters']: + self.pitch = apply_deadzone(self.sm['liveParameters'].pitchFutureLong, self.pitch_accel_deadzone) + actuators.accelPitchCompensated = actuators.accel + ACCELERATION_DUE_TO_GRAVITY * math.sin(self.pitch) + + self.CI.CS.coasting_long_plan = long_plan.longitudinalPlanSource self.CI.CS.coasting_lead_d = long_plan.leadDist self.CI.CS.coasting_lead_v = long_plan.leadV diff --git a/selfdrive/controls/lib/drive_helpers.py b/selfdrive/controls/lib/drive_helpers.py index 1c16b581676ed3..3f98d64a96b66f 100644 --- a/selfdrive/controls/lib/drive_helpers.py +++ b/selfdrive/controls/lib/drive_helpers.py @@ -43,6 +43,14 @@ class MPC_COST_LONG: ACCELERATION = 10.0 JERK = 20.0 +def apply_deadzone(error, deadzone): + if error > deadzone: + error -= deadzone + elif error < - deadzone: + error += deadzone + else: + error = 0. + return error def rate_limit(new_value, last_value, dw_step, up_step): return clip(new_value, last_value + dw_step, last_value + up_step) diff --git a/selfdrive/locationd/models/car_kf.py b/selfdrive/locationd/models/car_kf.py index 75534efa5a701b..0c40d48fd7fda9 100755 --- a/selfdrive/locationd/models/car_kf.py +++ b/selfdrive/locationd/models/car_kf.py @@ -39,6 +39,7 @@ class States(): YAW_RATE = _slice(1) # [rad/s] STEER_ANGLE = _slice(1) # [rad] ROAD_ROLL = _slice(1) # [rad] + ROAD_PITCH = _slice(1) # [rad] class CarKalman(KalmanFilter): @@ -53,6 +54,7 @@ class CarKalman(KalmanFilter): 10.0, 0.0, 0.0, 0.0, + 0.0, 0.0 ]) @@ -67,6 +69,7 @@ class CarKalman(KalmanFilter): math.radians(0.1)**2, math.radians(0.1)**2, math.radians(1)**2, + math.radians(1)**2, ]) P_initial = Q.copy() @@ -74,6 +77,7 @@ class CarKalman(KalmanFilter): ObservationKind.STEER_ANGLE: np.atleast_2d(math.radians(0.05)**2), ObservationKind.ANGLE_OFFSET_FAST: np.atleast_2d(math.radians(10.0)**2), ObservationKind.ROAD_ROLL: np.atleast_2d(math.radians(1.0)**2), + ObservationKind.ROAD_PITCH: np.atleast_2d(math.radians(1.0)**2), ObservationKind.STEER_RATIO: np.atleast_2d(5.0**2), ObservationKind.STIFFNESS: np.atleast_2d(0.5**2), ObservationKind.ROAD_FRAME_X_SPEED: np.atleast_2d(0.1**2), @@ -112,6 +116,7 @@ def generate_code(generated_dir): angle_offset = state[States.ANGLE_OFFSET, :][0, 0] angle_offset_fast = state[States.ANGLE_OFFSET_FAST, :][0, 0] theta = state[States.ROAD_ROLL, :][0, 0] + phi = state[States.ROAD_PITCH, :][0, 0] sa = state[States.STEER_ANGLE, :][0, 0] sR = state[States.STEER_RATIO, :][0, 0] @@ -156,6 +161,7 @@ def generate_code(generated_dir): [sp.Matrix([sR]), ObservationKind.STEER_RATIO, None], [sp.Matrix([sf]), ObservationKind.STIFFNESS, None], [sp.Matrix([theta]), ObservationKind.ROAD_ROLL, None], + [sp.Matrix([phi]), ObservationKind.ROAD_PITCH, None], ] gen_code(generated_dir, name, f_sym, dt, state_sym, obs_eqs, dim_state, dim_state, global_vars=global_vars) diff --git a/selfdrive/locationd/models/constants.py b/selfdrive/locationd/models/constants.py index f22f503ee0cd74..a77af1c5205a3c 100644 --- a/selfdrive/locationd/models/constants.py +++ b/selfdrive/locationd/models/constants.py @@ -39,6 +39,7 @@ class ObservationKind: STEER_RATIO = 29 # [-] ROAD_FRAME_X_SPEED = 30 # (x) [m/s] ROAD_ROLL = 31 # [rad] + ROAD_PITCH = 34 # [rad] names = [ 'Unknown', @@ -72,6 +73,7 @@ class ObservationKind: 'Steer Ratio', 'Road Frame x speed', 'Road Roll', + 'Road Pitch', ] @classmethod diff --git a/selfdrive/locationd/paramsd.py b/selfdrive/locationd/paramsd.py index 22f3b4f3bce509..87e421d3532971 100755 --- a/selfdrive/locationd/paramsd.py +++ b/selfdrive/locationd/paramsd.py @@ -9,15 +9,19 @@ from cereal import car from common.params import Params, put_nonblocking from common.realtime import set_realtime_priority, DT_MDL -from common.numpy_fast import clip +from common.numpy_fast import clip, interp +from selfdrive.controls.lib.lane_planner import TRAJECTORY_SIZE from selfdrive.locationd.models.car_kf import CarKalman, ObservationKind, States from selfdrive.locationd.models.constants import GENERATED_DIR +from selfdrive.modeld.constants import T_IDXS from selfdrive.swaglog import cloudlog MAX_ANGLE_OFFSET_DELTA = 20 * DT_MDL # Max 20 deg/s -ROLL_MAX_DELTA = np.radians(20.0) * DT_MDL # 20deg in 1 second is well within curvature limits +ROLL_MAX_DELTA = math.radians(20.0) * DT_MDL # 20deg in 1 second is well within curvature limits ROLL_MIN, ROLL_MAX = math.radians(-10), math.radians(10) +PITCH_MAX_DELTA = math.radians(10.0) * DT_MDL # 10°/s +PITCH_MIN, PITCH_MAX = math.radians(-19), math.radians(19) # steepest roads in US are ~18° class ParamsLearner: def __init__(self, CP, steer_ratio, stiffness_factor, angle_offset, P_initial=None): @@ -34,8 +38,13 @@ def __init__(self, CP, steer_ratio, stiffness_factor, angle_offset, P_initial=No self.speed = 0.0 self.roll = 0.0 + self.pitch = 0.0 self.steering_pressed = False self.steering_angle = 0.0 + + self.future_pitch_delay = 0.4 + (CP.longitudinalActuatorDelayUpperBound + CP.longitudinalActuatorDelayLowerBound) / 2 + self.future_pitch_ema_k = 1/10 + self.future_pitch_ema = 0.0 self.valid = True @@ -56,6 +65,19 @@ def handle_log(self, t, which, msg): roll = 0.0 roll_std = np.radians(10.0) self.roll = clip(roll, self.roll - ROLL_MAX_DELTA, self.roll + ROLL_MAX_DELTA) + + localizer_pitch = msg.orientationNED.value[1] + localizer_pitch_std = np.radians(1) if np.isnan(msg.orientationNED.std[1]) else msg.orientationNED.std[1] + pitch_valid = msg.orientationNED.valid and PITCH_MIN < localizer_pitch < PITCH_MAX + if pitch_valid: + pitch = localizer_pitch + # Experimentally found multiplier of 2 to be best trade-off between stability and accuracy or similar? + pitch_std = 2 * localizer_pitch_std + else: + # This is done to bound the road pitch estimate when localizer values are invalid + pitch = 0.0 + pitch_std = np.radians(10.0) + self.pitch = clip(pitch, self.pitch - PITCH_MAX_DELTA, self.pitch + PITCH_MAX_DELTA) yaw_rate_valid = msg.angularVelocityCalibrated.valid yaw_rate_valid = yaw_rate_valid and 0 < yaw_rate_std < 10 # rad/s @@ -74,6 +96,12 @@ def handle_log(self, t, which, msg): ObservationKind.ROAD_ROLL, np.array([[self.roll]]), np.array([np.atleast_2d(roll_std**2)])) + + self.kf.predict_and_observe(t, + ObservationKind.ROAD_PITCH, + np.array([[self.pitch]]), + np.array([np.atleast_2d(pitch_std**2)])) + self.kf.predict_and_observe(t, ObservationKind.ANGLE_OFFSET_FAST, np.array([[0]])) # We observe the current stiffness and steer ratio (with a high observation noise) to bound @@ -95,6 +123,12 @@ def handle_log(self, t, which, msg): if self.active: self.kf.predict_and_observe(t, ObservationKind.STEER_ANGLE, np.array([[math.radians(msg.steeringAngleDeg)]])) self.kf.predict_and_observe(t, ObservationKind.ROAD_FRAME_X_SPEED, np.array([[self.speed]])) + elif which == 'modelV2' and self.active and len(msg.orientation.y) == TRAJECTORY_SIZE: + future_pitch_diff = interp(self.future_pitch_delay, T_IDXS, msg.orientation.y) + future_pitch = float(self.kf.x[States.ROAD_PITCH]) + future_pitch_diff + future_pitch = clip(future_pitch, self.future_pitch_ema - PITCH_MAX_DELTA, self.future_pitch_ema + PITCH_MAX_DELTA) + self.future_pitch_ema = self.future_pitch_ema_k * future_pitch \ + + (1 - self.future_pitch_ema_k) * self.future_pitch_ema if not self.active: # Reset time when stopped so uncertainty doesn't grow @@ -107,7 +141,7 @@ def main(sm=None, pm=None): set_realtime_priority(5) if sm is None: - sm = messaging.SubMaster(['liveLocationKalman', 'carState'], poll=['liveLocationKalman']) + sm = messaging.SubMaster(['liveLocationKalman', 'carState', 'modelV2'], poll=['liveLocationKalman']) if pm is None: pm = messaging.PubMaster(['liveParameters']) @@ -184,6 +218,8 @@ def main(sm=None, pm=None): msg.liveParameters.steerRatio = float(x[States.STEER_RATIO]) msg.liveParameters.stiffnessFactor = float(x[States.STIFFNESS]) msg.liveParameters.roll = float(x[States.ROAD_ROLL]) + msg.liveParameters.pitch = float(x[States.ROAD_PITCH]) + msg.liveParameters.pitchFutureLong = float(learner.future_pitch_ema) msg.liveParameters.angleOffsetAverageDeg = angle_offset_average msg.liveParameters.angleOffsetDeg = angle_offset msg.liveParameters.valid = all((