Skip to content
This repository has been archived by the owner on May 1, 2024. It is now read-only.

Commit

Permalink
restored INDI and LQR controller (disabled by default)
Browse files Browse the repository at this point in the history
use dp_lat_controller params to force enable them
  • Loading branch information
eFiniLan committed Dec 21, 2023
1 parent 931cc12 commit 7ac5758
Show file tree
Hide file tree
Showing 7 changed files with 248 additions and 2 deletions.
5 changes: 3 additions & 2 deletions README.md
Original file line number Diff line number Diff line change
Expand Up @@ -30,7 +30,7 @@ I have decided to make this side project open source for users who wish to:

By making this project open source, I hope to alleviate some of the frustration and complaints about not being able to access the dragonpilot source code.

I encourage users to consider purchasing a [comma 3](https://shop.comma.ai) for the best and up-to-date openpilot experience.
I encourage users to consider purchasing a [comma 3x](https://shop.comma.ai) for the best and up-to-date openpilot experience.


## Limitations
Expand All @@ -43,13 +43,14 @@ I encourage users to consider purchasing a [comma 3](https://shop.comma.ai) for
* Services are not optimized for resource usage, and using all services may result in overheating issues.
* Language files can only be generated in a PC due to missing Qt5 tools.
* webjoystick is disabled as it requires additional python modules. (aiohttp and others)
* Starting from August 7th, 2023, comma has removed ESP/GPS support from Pandas. You can find more details about this change in this [link](https://github.com/commaai/panda/commit/c66b98b2a67441faa4cfcd36c3c9d9f90474cd08).
* Starting from August 7th, 2023, comma has removed ESP/GPS support from Panda. You can find more details about this change in this [link](https://github.com/commaai/panda/commit/c66b98b2a67441faa4cfcd36c3c9d9f90474cd08).
* Going forward, I will focus solely on maintaining the safety aspects of the code, ensuring that vehicle support and safety declarations remain up to date.
* For safety concern, End-to-End / vision only longitudinal control only available in 0.8.16 driving model.


## Configuration

* For research purposes, the INDI and LQR lateral controllers have been restored. Please use the `dp_lat_controller` parameter to override the default controller (0 = DEFAULT, 1 = INDI, 2 = LQR).
* If you are not a Comma Two device, you can use the `dp_no_fan_ctrl` parameter to disable fan-related detection and control.

=======================
Expand Down
1 change: 1 addition & 0 deletions common/params.cc
Original file line number Diff line number Diff line change
Expand Up @@ -212,6 +212,7 @@ std::unordered_map<std::string, uint32_t> keys = {
{"dp_no_gps_ctrl", PERSISTENT},
{"dp_no_fan_ctrl", PERSISTENT},
{"dp_logging", PERSISTENT},
{"dp_lat_controller", PERSISTENT},
};

} // namespace
Expand Down
32 changes: 32 additions & 0 deletions selfdrive/car/interfaces.py
Original file line number Diff line number Diff line change
Expand Up @@ -15,6 +15,7 @@
from openpilot.selfdrive.controls.lib.drive_helpers import V_CRUISE_MAX, get_friction
from openpilot.selfdrive.controls.lib.events import Events
from openpilot.selfdrive.controls.lib.vehicle_model import VehicleModel
from openpilot.common.params import Params

ButtonType = car.CarState.ButtonEvent.Type
GearShifter = car.CarState.GearShifter
Expand Down Expand Up @@ -101,6 +102,13 @@ def get_params(cls, candidate: str, fingerprint: Dict[int, Dict[int, int]], car_
ret = CarInterfaceBase.get_std_params(candidate)
ret = cls._get_params(ret, candidate, fingerprint, car_fw, experimental_long, docs)

# rick - override lat controller
dp_lat_controller = int(Params().get("dp_lat_controller"))
if dp_lat_controller == 1: # indi
ret = cls.configure_indi_tune(ret.lateralTuning)
elif dp_lat_controller == 2: # lqr
ret = cls.configure_lqr_tune(ret.lateralTuning)

# Vehicle mass is published curb weight plus assumed payload such as a human driver; notCars have no assumed payload
if not ret.notCar:
ret.mass = ret.mass + STD_CARGO_KG
Expand Down Expand Up @@ -191,6 +199,30 @@ def configure_torque_tune(candidate, tune, steering_angle_deadzone_deg=0.0, use_
tune.torque.latAccelOffset = 0.0
tune.torque.steeringAngleDeadzoneDeg = steering_angle_deadzone_deg

@staticmethod
def configure_lqr_tune(tune):
tune.init('lqr')
tune.lqr.scale = 1500.0
tune.lqr.ki = 0.05
tune.lqr.a = [0., 1., -0.22619643, 1.21822268]
tune.lqr.b = [-1.92006585e-04, 3.95603032e-05]
tune.lqr.c = [1., 0.]
tune.lqr.k = [-110.73572306, 451.22718255]
tune.lqr.l = [0.3233671, 0.3185757]
tune.lqr.dcGain = 0.002237852961363602

@staticmethod
def configure_indi_tune(tune):
tune.init('indi')
tune.indi.innerLoopGainBP = [0.]
tune.indi.innerLoopGainV = [4.0]
tune.indi.outerLoopGainBP = [0.]
tune.indi.outerLoopGainV = [3.0]
tune.indi.timeConstantBP = [0.]
tune.indi.timeConstantV = [1.0]
tune.indi.actuatorEffectivenessBP = [0.]
tune.indi.actuatorEffectivenessV = [1.0]

@abstractmethod
def _update(self, c: car.CarControl) -> car.CarState:
pass
Expand Down
6 changes: 6 additions & 0 deletions selfdrive/controls/controlsd.py
Original file line number Diff line number Diff line change
Expand Up @@ -22,6 +22,8 @@
from openpilot.selfdrive.controls.lib.latcontrol import LatControl, MIN_LATERAL_CONTROL_SPEED
from openpilot.selfdrive.controls.lib.longcontrol import LongControl
from openpilot.selfdrive.controls.lib.latcontrol_pid import LatControlPID
from openpilot.selfdrive.controls.lib.latcontrol_indi import LatControlINDI
from openpilot.selfdrive.controls.lib.latcontrol_lqr import LatControlLQR
from openpilot.selfdrive.controls.lib.latcontrol_angle import LatControlAngle, STEER_ANGLE_SATURATION_THRESHOLD
from openpilot.selfdrive.controls.lib.latcontrol_torque import LatControlTorque
from openpilot.selfdrive.controls.lib.events import Events, ET
Expand Down Expand Up @@ -157,6 +159,10 @@ def __init__(self, sm=None, pm=None, can_sock=None, CI=None):
self.LaC = LatControlAngle(self.CP, self.CI)
elif self.CP.lateralTuning.which() == 'pid':
self.LaC = LatControlPID(self.CP, self.CI)
elif self.CP.lateralTuning.which() == 'indi':
self.LaC = LatControlINDI(self.CP, self.CI)
elif self.CP.lateralTuning.which() == 'lqr':
self.LaC = LatControlLQR(self.CP, self.CI)
elif self.CP.lateralTuning.which() == 'torque':
self.LaC = LatControlTorque(self.CP, self.CI)

Expand Down
120 changes: 120 additions & 0 deletions selfdrive/controls/lib/latcontrol_indi.py
Original file line number Diff line number Diff line change
@@ -0,0 +1,120 @@
import math
import numpy as np

from cereal import log
from common.filter_simple import FirstOrderFilter
from common.numpy_fast import clip, interp
from common.realtime import DT_CTRL
from selfdrive.controls.lib.latcontrol import LatControl


class LatControlINDI(LatControl):
def __init__(self, CP, CI):
super().__init__(CP, CI)
self.angle_steers_des = 0.

A = np.array([[1.0, DT_CTRL, 0.0],
[0.0, 1.0, DT_CTRL],
[0.0, 0.0, 1.0]])
C = np.array([[1.0, 0.0, 0.0],
[0.0, 1.0, 0.0]])

# Q = np.matrix([[1e-2, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, 10.0]])
# R = np.matrix([[1e-2, 0.0], [0.0, 1e3]])

# (x, l, K) = control.dare(np.transpose(A), np.transpose(C), Q, R)
# K = np.transpose(K)
K = np.array([[7.30262179e-01, 2.07003658e-04],
[7.29394177e+00, 1.39159419e-02],
[1.71022442e+01, 3.38495381e-02]])

self.speed = 0.

self.K = K
self.A_K = A - np.dot(K, C)
self.x = np.array([[0.], [0.], [0.]])

self._RC = (CP.lateralTuning.indi.timeConstantBP, CP.lateralTuning.indi.timeConstantV)
self._G = (CP.lateralTuning.indi.actuatorEffectivenessBP, CP.lateralTuning.indi.actuatorEffectivenessV)
self._outer_loop_gain = (CP.lateralTuning.indi.outerLoopGainBP, CP.lateralTuning.indi.outerLoopGainV)
self._inner_loop_gain = (CP.lateralTuning.indi.innerLoopGainBP, CP.lateralTuning.indi.innerLoopGainV)

self.steer_filter = FirstOrderFilter(0., self.RC, DT_CTRL)
self.reset()

@property
def RC(self):
return interp(self.speed, self._RC[0], self._RC[1])

@property
def G(self):
return interp(self.speed, self._G[0], self._G[1])

@property
def outer_loop_gain(self):
return interp(self.speed, self._outer_loop_gain[0], self._outer_loop_gain[1])

@property
def inner_loop_gain(self):
return interp(self.speed, self._inner_loop_gain[0], self._inner_loop_gain[1])

def reset(self):
super().reset()
self.steer_filter.x = 0.
self.speed = 0.

def update(self, active, CS, VM, params, last_actuators, steer_limited, desired_curvature, desired_curvature_rate, llk):
self.speed = CS.vEgo
# Update Kalman filter
y = np.array([[math.radians(CS.steeringAngleDeg)], [math.radians(CS.steeringRateDeg)]])
self.x = np.dot(self.A_K, self.x) + np.dot(self.K, y)

indi_log = log.ControlsState.LateralINDIState.new_message()
indi_log.steeringAngleDeg = math.degrees(self.x[0])
indi_log.steeringRateDeg = math.degrees(self.x[1])
indi_log.steeringAccelDeg = math.degrees(self.x[2])

steers_des = VM.get_steer_from_curvature(-desired_curvature, CS.vEgo, params.roll)
steers_des += math.radians(params.angleOffsetDeg)
indi_log.steeringAngleDesiredDeg = math.degrees(steers_des)

# desired rate is the desired rate of change in the setpoint, not the absolute desired curvature
rate_des = VM.get_steer_from_curvature(-desired_curvature_rate, CS.vEgo, 0)
indi_log.steeringRateDesiredDeg = math.degrees(rate_des)

if not active:
indi_log.active = False
self.steer_filter.x = 0.0
output_steer = 0
else:
# Expected actuator value
self.steer_filter.update_alpha(self.RC)
self.steer_filter.update(last_actuators.steer)

# Compute acceleration error
rate_sp = self.outer_loop_gain * (steers_des - self.x[0]) + rate_des
accel_sp = self.inner_loop_gain * (rate_sp - self.x[1])
accel_error = accel_sp - self.x[2]

# Compute change in actuator
g_inv = 1. / self.G
delta_u = g_inv * accel_error

# If steering pressed, only allow wind down
if CS.steeringPressed and (delta_u * last_actuators.steer > 0):
delta_u = 0

output_steer = self.steer_filter.x + delta_u

output_steer = clip(output_steer, -self.steer_max, self.steer_max)

indi_log.active = True
indi_log.rateSetPoint = float(rate_sp)
indi_log.accelSetPoint = float(accel_sp)
indi_log.accelError = float(accel_error)
indi_log.delayedOutput = float(self.steer_filter.x)
indi_log.delta = float(delta_u)
indi_log.output = float(output_steer)
indi_log.saturated = self._check_saturation(self.steer_max - abs(output_steer) < 1e-3, CS, steer_limited)

return float(output_steer), float(steers_des), indi_log
85 changes: 85 additions & 0 deletions selfdrive/controls/lib/latcontrol_lqr.py
Original file line number Diff line number Diff line change
@@ -0,0 +1,85 @@
import math
import numpy as np

from common.numpy_fast import clip
from common.realtime import DT_CTRL
from cereal import log
from selfdrive.controls.lib.latcontrol import LatControl


class LatControlLQR(LatControl):
def __init__(self, CP, CI):
super().__init__(CP, CI)
self.scale = CP.lateralTuning.lqr.scale
self.ki = CP.lateralTuning.lqr.ki

self.A = np.array(CP.lateralTuning.lqr.a).reshape((2, 2))
self.B = np.array(CP.lateralTuning.lqr.b).reshape((2, 1))
self.C = np.array(CP.lateralTuning.lqr.c).reshape((1, 2))
self.K = np.array(CP.lateralTuning.lqr.k).reshape((1, 2))
self.L = np.array(CP.lateralTuning.lqr.l).reshape((2, 1))
self.dc_gain = CP.lateralTuning.lqr.dcGain

self.x_hat = np.array([[0], [0]])
self.i_unwind_rate = 0.3 * DT_CTRL
self.i_rate = 1.0 * DT_CTRL

self.reset()

def reset(self):
super().reset()
self.i_lqr = 0.0

def update(self, active, CS, VM, params, last_actuators, steer_limited, desired_curvature, desired_curvature_rate, llk):
# def update(self, active, CS, CP, VM, params, last_actuators, desired_curvature, desired_curvature_rate):
lqr_log = log.ControlsState.LateralLQRState.new_message()

torque_scale = (0.45 + CS.vEgo / 60.0)**2 # Scale actuator model with speed

# Subtract offset. Zero angle should correspond to zero torque
steering_angle_no_offset = CS.steeringAngleDeg - params.angleOffsetAverageDeg

desired_angle = math.degrees(VM.get_steer_from_curvature(-desired_curvature, CS.vEgo, params.roll))

instant_offset = params.angleOffsetDeg - params.angleOffsetAverageDeg
desired_angle += instant_offset # Only add offset that originates from vehicle model errors
lqr_log.steeringAngleDesiredDeg = desired_angle

# Update Kalman filter
angle_steers_k = float(self.C.dot(self.x_hat))
e = steering_angle_no_offset - angle_steers_k
self.x_hat = self.A.dot(self.x_hat) + self.B.dot(CS.steeringTorqueEps / torque_scale) + self.L.dot(e)

if not active:
lqr_log.active = False
lqr_output = 0.
output_steer = 0.
self.reset()
else:
lqr_log.active = True

# LQR
u_lqr = float(desired_angle / self.dc_gain - self.K.dot(self.x_hat))
lqr_output = torque_scale * u_lqr / self.scale

# Integrator
if CS.steeringPressed:
self.i_lqr -= self.i_unwind_rate * float(np.sign(self.i_lqr))
else:
error = desired_angle - angle_steers_k
i = self.i_lqr + self.ki * self.i_rate * error
control = lqr_output + i

if (error >= 0 and (control <= self.steer_max or i < 0.0)) or \
(error <= 0 and (control >= -self.steer_max or i > 0.0)):
self.i_lqr = i

output_steer = lqr_output + self.i_lqr
output_steer = clip(output_steer, -self.steer_max, self.steer_max)

lqr_log.steeringAngleDeg = angle_steers_k
lqr_log.i = self.i_lqr
lqr_log.output = output_steer
lqr_log.lqrOutput = lqr_output
lqr_log.saturated = self._check_saturation(self.steer_max - abs(output_steer) < 1e-3, CS, steer_limited)
return output_steer, desired_angle, lqr_log
1 change: 1 addition & 0 deletions selfdrive/manager/manager.py
Original file line number Diff line number Diff line change
Expand Up @@ -51,6 +51,7 @@ def manager_init() -> None:
("dp_no_fan_ctrl", "0"),
("dp_logging", "1"),
("dp_0813", "1"),
("dp_lat_controller", "0"),
]
if not PC:
default_params.append(("LastUpdateTime", datetime.datetime.utcnow().isoformat().encode('utf8')))
Expand Down

0 comments on commit 7ac5758

Please sign in to comment.