diff --git a/.github/PULL_REQUEST_TEMPLATE/car_port.md b/.github/PULL_REQUEST_TEMPLATE/car_port.md index c7aa2b96c2..c1581d2055 100644 --- a/.github/PULL_REQUEST_TEMPLATE/car_port.md +++ b/.github/PULL_REQUEST_TEMPLATE/car_port.md @@ -8,7 +8,7 @@ assignees: '' **Checklist** -- [ ] added entry to CAR in selfdrive/car/*/values.py and ran `selfdrive/car/docs.py` to generate new docs +- [ ] added entry to CAR in selfdrive/car/*/values.py and ran `selfdrive/opcar/docs.py` to generate new docs - [ ] test route added to [routes.py](https://github.com/commaai/openpilot/blob/master/selfdrive/car/tests/routes.py) - [ ] route with openpilot: - [ ] route with stock system: diff --git a/.github/pull_request_template.md b/.github/pull_request_template.md index 2b4a5ed48f..9e0c54218e 100644 --- a/.github/pull_request_template.md +++ b/.github/pull_request_template.md @@ -44,7 +44,7 @@ Explain how you tested this bug fix. **Checklist** -- [ ] added entry to CAR in selfdrive/car/*/values.py and ran `selfdrive/car/docs.py` to generate new docs +- [ ] added entry to CAR in selfdrive/car/*/values.py and ran `selfdrive/opcar/docs.py` to generate new docs - [ ] test route added to [routes.py](https://github.com/commaai/openpilot/blob/master/selfdrive/car/tests/routes.py) - [ ] route with openpilot: - [ ] route with stock system: diff --git a/.importlinter b/.importlinter index 6e68b69c20..5e96db4469 100644 --- a/.importlinter +++ b/.importlinter @@ -29,10 +29,6 @@ forbidden_modules = openpilot.tinygrad ignore_imports = # remove these - openpilot.selfdrive.car.body.carcontroller -> openpilot.selfdrive.controls.lib.pid - openpilot.selfdrive.car.tests.test_docs -> openpilot.common.basedir - openpilot.selfdrive.car.docs -> openpilot.common.basedir - openpilot.selfdrive.car.gm.interface -> openpilot.common.basedir openpilot.selfdrive.car.interfaces -> openpilot.common.basedir @@ -53,8 +49,6 @@ ignore_imports = openpilot.selfdrive.car.tests.test_models -> openpilot.common.params openpilot.selfdrive.car.tests.test_models -> openpilot.common.basedir openpilot.selfdrive.car.card -> openpilot.selfdrive.pandad - openpilot.selfdrive.car.tests.test_docs -> openpilot.selfdrive.debug.dump_car_docs - openpilot.selfdrive.car.tests.test_docs -> openpilot.selfdrive.debug.print_docs_diff openpilot.selfdrive.car.tests.test_car_interfaces -> openpilot.selfdrive.pandad openpilot.selfdrive.car.tests.test_models -> openpilot.selfdrive.pandad openpilot.selfdrive.car.tests.test_car_interfaces -> openpilot.selfdrive.test.fuzzy_generation diff --git a/pyproject.toml b/pyproject.toml index 0606317089..884c305ab3 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -145,6 +145,7 @@ testpaths = [ "common", "selfdrive/pandad", "selfdrive/car", + "selfdrive/opcar", "selfdrive/controls", "selfdrive/locationd", "selfdrive/monitoring", diff --git a/selfdrive/car/body/carcontroller.py b/selfdrive/car/body/carcontroller.py index abc2b88f53..24d50c18be 100644 --- a/selfdrive/car/body/carcontroller.py +++ b/selfdrive/car/body/carcontroller.py @@ -1,12 +1,69 @@ import copy import numpy as np +from numbers import Number from opendbc.can.packer import CANPacker from openpilot.selfdrive.car import DT_CTRL +from openpilot.selfdrive.car.common.numpy_fast import clip, interp from openpilot.selfdrive.car.body import bodycan from openpilot.selfdrive.car.body.values import SPEED_FROM_RPM from openpilot.selfdrive.car.interfaces import CarControllerBase -from openpilot.selfdrive.controls.lib.pid import PIDController + + +class PIController: + def __init__(self, k_p, k_i, pos_limit=1e308, neg_limit=-1e308, rate=100): + self._k_p = k_p + self._k_i = k_i + if isinstance(self._k_p, Number): + self._k_p = [[0], [self._k_p]] + if isinstance(self._k_i, Number): + self._k_i = [[0], [self._k_i]] + + self.pos_limit = pos_limit + self.neg_limit = neg_limit + + self.i_unwind_rate = 0.3 / rate + self.i_rate = 1.0 / rate + self.speed = 0.0 + + self.reset() + + @property + def k_p(self): + return interp(self.speed, self._k_p[0], self._k_p[1]) + + @property + def k_i(self): + return interp(self.speed, self._k_i[0], self._k_i[1]) + + @property + def error_integral(self): + return self.i/self.k_i + + def reset(self): + self.p = 0.0 + self.i = 0.0 + self.control = 0 + + def update(self, error, speed=0.0, freeze_integrator=False): + self.speed = speed + + self.p = float(error) * self.k_p + + i = self.i + error * self.k_i * self.i_rate + control = self.p + i + + # Update when changing i will move the control away from the limits + # or when i will move towards the sign of the error + if ((error >= 0 and (control <= self.pos_limit or i < 0.0)) or + (error <= 0 and (control >= self.neg_limit or i > 0.0))) and \ + not freeze_integrator: + self.i = i + + control = self.p + self.i + + self.control = clip(control, self.neg_limit, self.pos_limit) + return self.control MAX_TORQUE = 500 @@ -22,8 +79,8 @@ class CarController(CarControllerBase): self.packer = CANPacker(dbc_name) # PIDs - self.turn_pid = PIDController(110, k_i=11.5, rate=1/DT_CTRL) - self.wheeled_speed_pid = PIDController(110, k_i=11.5, rate=1/DT_CTRL) + self.turn_pid = PIController(110, k_i=11.5, rate=1/DT_CTRL) + self.wheeled_speed_pid = PIController(110, k_i=11.5, rate=1/DT_CTRL) self.torque_r_filtered = 0. self.torque_l_filtered = 0. diff --git a/selfdrive/car/docs.py b/selfdrive/car/docs.py old mode 100755 new mode 100644 index 78b320d9c4..a9e9c7ee83 --- a/selfdrive/car/docs.py +++ b/selfdrive/car/docs.py @@ -1,12 +1,8 @@ -#!/usr/bin/env python3 -import argparse from collections import defaultdict import jinja2 -import os from enum import Enum from natsort import natsorted -from openpilot.common.basedir import BASEDIR from openpilot.selfdrive.car import gen_empty_fingerprint from openpilot.selfdrive.car.structs import CarParams from openpilot.selfdrive.car.docs_definitions import CarDocs, Column, CommonFootnote, PartType @@ -21,10 +17,6 @@ def get_all_footnotes() -> dict[Enum, int]: return {fn: idx + 1 for idx, fn in enumerate(all_footnotes)} -CARS_MD_OUT = os.path.join(BASEDIR, "docs", "CARS.md") -CARS_MD_TEMPLATE = os.path.join(BASEDIR, "selfdrive", "car", "CARS_template.md") - - def get_all_car_docs() -> list[CarDocs]: all_car_docs: list[CarDocs] = [] footnotes = get_all_footnotes() @@ -65,16 +57,3 @@ def generate_cars_md(all_car_docs: list[CarDocs], template_fn: str) -> str: group_by_make=group_by_make, footnotes=footnotes, Column=Column) return cars_md - - -if __name__ == "__main__": - parser = argparse.ArgumentParser(description="Auto generates supported cars documentation", - formatter_class=argparse.ArgumentDefaultsHelpFormatter) - - parser.add_argument("--template", default=CARS_MD_TEMPLATE, help="Override default template filename") - parser.add_argument("--out", default=CARS_MD_OUT, help="Override default generated filename") - args = parser.parse_args() - - with open(args.out, 'w') as f: - f.write(generate_cars_md(get_all_car_docs(), args.template)) - print(f"Generated and written to {args.out}") diff --git a/selfdrive/car/tests/test_docs.py b/selfdrive/car/tests/test_docs.py index 40ad07b283..52d8976517 100644 --- a/selfdrive/car/tests/test_docs.py +++ b/selfdrive/car/tests/test_docs.py @@ -1,16 +1,12 @@ from collections import defaultdict -import os import pytest import re -from openpilot.common.basedir import BASEDIR from openpilot.selfdrive.car.car_helpers import interfaces -from openpilot.selfdrive.car.docs import CARS_MD_OUT, CARS_MD_TEMPLATE, generate_cars_md, get_all_car_docs +from openpilot.selfdrive.car.docs import get_all_car_docs from openpilot.selfdrive.car.docs_definitions import Cable, Column, PartType, Star from openpilot.selfdrive.car.honda.values import CAR as HONDA from openpilot.selfdrive.car.values import PLATFORMS -from openpilot.selfdrive.debug.dump_car_docs import dump_car_docs -from openpilot.selfdrive.debug.print_docs_diff import print_car_docs_diff class TestCarDocs: @@ -18,19 +14,6 @@ class TestCarDocs: def setup_class(cls): cls.all_cars = get_all_car_docs() - def test_generator(self): - generated_cars_md = generate_cars_md(self.all_cars, CARS_MD_TEMPLATE) - with open(CARS_MD_OUT) as f: - current_cars_md = f.read() - - assert generated_cars_md == current_cars_md, "Run selfdrive/car/docs.py to update the compatibility documentation" - - def test_docs_diff(self): - dump_path = os.path.join(BASEDIR, "selfdrive", "car", "tests", "cars_dump") - dump_car_docs(dump_path) - print_car_docs_diff(dump_path) - os.remove(dump_path) - def test_duplicate_years(self, subtests): make_model_years = defaultdict(list) for car in self.all_cars: diff --git a/selfdrive/opcar/__init__.py b/selfdrive/opcar/__init__.py new file mode 100644 index 0000000000..ddc217f21b --- /dev/null +++ b/selfdrive/opcar/__init__.py @@ -0,0 +1 @@ +# This folder will be renamed back to car after the opendbc car split diff --git a/selfdrive/opcar/docs.py b/selfdrive/opcar/docs.py new file mode 100755 index 0000000000..99bf0b055b --- /dev/null +++ b/selfdrive/opcar/docs.py @@ -0,0 +1,21 @@ +#!/usr/bin/env python3 +import argparse +import os + +from openpilot.common.basedir import BASEDIR +from openpilot.selfdrive.car.docs import get_all_car_docs, generate_cars_md + +CARS_MD_OUT = os.path.join(BASEDIR, "docs", "CARS.md") +CARS_MD_TEMPLATE = os.path.join(BASEDIR, "selfdrive", "car", "CARS_template.md") + +if __name__ == "__main__": + parser = argparse.ArgumentParser(description="Auto generates supported cars documentation", + formatter_class=argparse.ArgumentDefaultsHelpFormatter) + + parser.add_argument("--template", default=CARS_MD_TEMPLATE, help="Override default template filename") + parser.add_argument("--out", default=CARS_MD_OUT, help="Override default generated filename") + args = parser.parse_args() + + with open(args.out, 'w') as f: + f.write(generate_cars_md(get_all_car_docs(), args.template)) + print(f"Generated and written to {args.out}") diff --git a/selfdrive/opcar/tests/__init__.py b/selfdrive/opcar/tests/__init__.py new file mode 100644 index 0000000000..e69de29bb2 diff --git a/selfdrive/opcar/tests/test_docs.py b/selfdrive/opcar/tests/test_docs.py new file mode 100644 index 0000000000..f200bc292d --- /dev/null +++ b/selfdrive/opcar/tests/test_docs.py @@ -0,0 +1,26 @@ +import os + +from openpilot.common.basedir import BASEDIR +from openpilot.selfdrive.car.docs import generate_cars_md, get_all_car_docs +from openpilot.selfdrive.debug.dump_car_docs import dump_car_docs +from openpilot.selfdrive.debug.print_docs_diff import print_car_docs_diff +from openpilot.selfdrive.opcar.docs import CARS_MD_OUT, CARS_MD_TEMPLATE + + +class TestCarDocs: + @classmethod + def setup_class(cls): + cls.all_cars = get_all_car_docs() + + def test_generator(self): + generated_cars_md = generate_cars_md(self.all_cars, CARS_MD_TEMPLATE) + with open(CARS_MD_OUT) as f: + current_cars_md = f.read() + + assert generated_cars_md == current_cars_md, "Run selfdrive/opcar/docs.py to update the compatibility documentation" + + def test_docs_diff(self): + dump_path = os.path.join(BASEDIR, "selfdrive", "car", "tests", "cars_dump") + dump_car_docs(dump_path) + print_car_docs_diff(dump_path) + os.remove(dump_path)