From 097364849e568d7edae6c87d62679ecfddf2b72a Mon Sep 17 00:00:00 2001 From: Shane Date: Mon, 16 Dec 2019 19:39:51 -0600 Subject: [PATCH 01/30] add my libraries --- README.md | 337 ++++----------------------------------- common/data_collector.py | 98 ++++++++++++ common/op_params.py | 116 ++++++++++++++ common/travis_checker.py | 2 + op_edit.py | 175 ++++++++++++++++++++ 5 files changed, 426 insertions(+), 302 deletions(-) create mode 100644 common/data_collector.py create mode 100644 common/op_params.py create mode 100644 common/travis_checker.py create mode 100644 op_edit.py diff --git a/README.md b/README.md index 1c12443c2b9adc..d307f39377bcb2 100644 --- a/README.md +++ b/README.md @@ -1,302 +1,35 @@ -[![](https://i.imgur.com/UetIFyH.jpg)](#) - -Table of Contents -======================= - -* [What is openpilot?](#what-is-openpilot) -* [Integration with Stock Features](#integration-with-stock-features) -* [Supported Hardware](#supported-hardware) -* [Supported Cars](#supported-cars) -* [Community Maintained Cars and Features](#community-maintained-cars-and-features) -* [Installation Instructions](#installation-instructions) -* [Limitations of openpilot ALC and LDW](#limitations-of-openpilot-alc-and-ldw) -* [Limitations of openpilot ACC and FCW](#limitations-of-openpilot-acc-and-fcw) -* [Limitations of openpilot DM](#limitations-of-openpilot-dm) -* [User Data and comma Account](#user-data-and-comma-account) -* [Safety and Testing](#safety-and-testing) -* [Testing on PC](#testing-on-pc) -* [Community and Contributing](#community-and-contributing) -* [Directory Structure](#directory-structure) -* [Licensing](#licensing) - ---- - -What is openpilot? ------- - -[openpilot](http://github.com/commaai/openpilot) is an open source driver assistance system. Currently, openpilot performs the functions of Adaptive Cruise Control (ACC), Automated Lane Centering (ALC), Forward Collision Warning (FCW) and Lane Departure Warning (LDW) for a growing variety of supported [car makes, models and model years](#supported-cars). In addition, while openpilot is engaged, a camera based Driver Monitoring (DM) feature alerts distracted and asleep drivers. - - - - - - - - - - - - - - -
- -Integration with Stock Features ------- - -In all supported cars: -* Stock Lane Keep Assist (LKA) and stock ALC are replaced by openpilot ALC, which only functions when openpilot is engaged by the user. -* Stock LDW is replaced by openpilot LDW. - -Additionally, on specific supported cars (see ACC column in [supported cars](#supported-cars)): -* Stock ACC is replaced by openpilot ACC. -* openpilot FCW operates in addition to stock FCW. - -openpilot should preserve all other vehicle's stock features, including, but are not limited to: FCW, Automatic Emergency Braking (AEB), auto high-beam, blind spot warning, and side collision warning. - -Supported Hardware ------- - -At the moment, openpilot supports the [EON DevKit](https://comma.ai/shop/products/eon-dashcam-devkit). A [car harness](https://comma.ai/shop/products/car-harness) is recommended to connect the EON to the car. In the future, we'd like to support other platforms as well. - -Supported Cars ------- - -| Make | Model (US Market Reference) | Supported Package | ACC | No ACC accel below | No ALC below | -| ----------| -----------------------------------| ------------------| -----------------| -------------------| -------------| -| Acura | ILX 2016-18 | AcuraWatch Plus | openpilot | 25mph6 | 25mph | -| Acura | RDX 2016-18 | AcuraWatch Plus | openpilot | 25mph6 | 12mph | -| Chrysler | Pacifica 2017-181 | Adaptive Cruise | Stock | 0mph | 9mph | -| Chrysler | Pacifica Hybrid 2017-181| Adaptive Cruise | Stock | 0mph | 9mph | -| Chrysler | Pacifica Hybrid 20191 | Adaptive Cruise | Stock | 0mph | 39mph | -| Honda | Accord 2018-19 | All | Stock | 0mph | 3mph | -| Honda | Accord Hybrid 2018-19 | All | Stock | 0mph | 3mph | -| Honda | Civic Sedan/Coupe 2016-18 | Honda Sensing | openpilot | 0mph | 12mph | -| Honda | Civic Sedan/Coupe 2019 | Honda Sensing | Stock | 0mph | 2mph | -| Honda | Civic Hatchback 2017-19 | Honda Sensing | Stock | 0mph | 12mph | -| Honda | CR-V 2015-16 | Touring | openpilot | 25mph6 | 12mph | -| Honda | CR-V 2017-19 | Honda Sensing | Stock | 0mph | 12mph | -| Honda | CR-V Hybrid 2017-2019 | Honda Sensing | Stock | 0mph | 12mph | -| Honda | Fit 2018-19 | Honda Sensing | openpilot | 25mph6 | 12mph | -| Honda | Odyssey 2018-20 | Honda Sensing | openpilot | 25mph6 | 0mph | -| Honda | Passport 2019 | All | openpilot | 25mph6 | 12mph | -| Honda | Pilot 2016-18 | Honda Sensing | openpilot | 25mph6 | 12mph | -| Honda | Pilot 2019 | All | openpilot | 25mph6 | 12mph | -| Honda | Ridgeline 2017-19 | Honda Sensing | openpilot | 25mph6 | 12mph | -| Hyundai | Santa Fe 20192 | All | Stock | 0mph | 0mph | -| Hyundai | Elantra 2017-192 | SCC + LKAS | Stock | 19mph | 34mph | -| Hyundai | Genesis 20182 | All | Stock | 19mph | 34mph | -| Jeep | Grand Cherokee 2016-181 | Adaptive Cruise | Stock | 0mph | 9mph | -| Jeep | Grand Cherokee 20191 | Adaptive Cruise | Stock | 0mph | 39mph | -| Kia | Optima 20192 | SCC + LKAS | Stock | 0mph | 0mph | -| Kia | Sorento 20182 | All | Stock | 0mph | 0mph | -| Kia | Stinger 20182 | SCC + LKAS | Stock | 0mph | 0mph | -| Lexus | CT Hybrid 2017-18 | All | Stock5| 0mph | 0mph | -| Lexus | ES Hybrid 2019 | All | openpilot | 0mph | 0mph | -| Lexus | RX Hybrid 2016-19 | All | Stock5| 0mph | 0mph | -| Lexus | IS 2017-2019 | All | Stock | 22mph | 0mph | -| Lexus | IS Hybrid 2017 | All | Stock | 0mph | 0mph | -| Subaru | Crosstrek 2018-19 | EyeSight | Stock | 0mph | 0mph | -| Subaru | Impreza 2019-20 | EyeSight | Stock | 0mph | 0mph | -| Toyota | Avalon 2016 | TSS-P | Stock5| 20mph6 | 0mph | -| Toyota | Avalon 2017-18 | All | Stock5| 20mph6 | 0mph | -| Toyota | Camry 2018-19 | All | Stock | 0mph3 | 0mph | -| Toyota | Camry Hybrid 2018-19 | All | Stock | 0mph3 | 0mph | -| Toyota | C-HR 2017-19 | All | Stock | 0mph | 0mph | -| Toyota | C-HR Hybrid 2017-19 | All | Stock | 0mph | 0mph | -| Toyota | Corolla 2017-19 | All | Stock5| 20mph6 | 0mph | -| Toyota | Corolla 2020 | All | openpilot | 0mph | 0mph | -| Toyota | Corolla Hatchback 2019 | All | openpilot | 0mph | 0mph | -| Toyota | Corolla Hybrid 2020 | All | openpilot | 0mph | 0mph | -| Toyota | Highlander 2017-19 | All | Stock5| 0mph | 0mph | -| Toyota | Highlander Hybrid 2017-19 | All | Stock5| 0mph | 0mph | -| Toyota | Prius 2016 | TSS-P | Stock5| 0mph | 0mph | -| Toyota | Prius 2017-19 | All | Stock5| 0mph | 0mph | -| Toyota | Prius Prime 2017-20 | All | Stock5| 0mph | 0mph | -| Toyota | Rav4 2016 | TSS-P | Stock5| 20mph6 | 0mph | -| Toyota | Rav4 2017-18 | All | Stock5| 20mph6 | 0mph | -| Toyota | Rav4 2019 | All | openpilot | 0mph | 0mph | -| Toyota | Rav4 Hybrid 2016 | TSS-P | Stock5| 0mph | 0mph | -| Toyota | Rav4 Hybrid 2017-18 | All | Stock5| 0mph | 0mph | -| Toyota | Sienna 2018 | All | Stock5| 0mph | 0mph | -| Volkswagen| Golf 2016-194 | Driver Assistance | Stock | 0mph | 0mph | - -1Requires a [panda](https://comma.ai/shop/products/panda-obd-ii-dongle) and [FCA giraffe](https://comma.ai/shop/products/giraffe)
-2Requires a [panda](https://comma.ai/shop/products/panda-obd-ii-dongle) and open sourced [Hyundai giraffe](https://github.com/commaai/neo/tree/master/giraffe/hyundai), designed for the 2019 Sante Fe; pinout may differ for other Hyundai and Kia models.
-328mph for Camry 4CYL L, 4CYL LE and 4CYL SE which don't have Full-Speed Range Dynamic Radar Cruise Control.
-4Requires a [custom connector](https://community.comma.ai/wiki/index.php/Volkswagen#Integration_at_R242_Camera) for the [car harness](https://comma.ai/shop/products/car-harness)
- -Community Maintained Cars and Features ------- - -| Make | Model (US Market Reference) | Supported Package | ACC | No ACC accel below | No ALC below | -| ----------| -----------------------------------| ------------------| -----------------| -------------------| -------------| -| Buick | Regal 20187 | Adaptive Cruise | openpilot | 0mph | 7mph | -| Chevrolet | Malibu 20177 | Adaptive Cruise | openpilot | 0mph | 7mph | -| Chevrolet | Volt 2017-187 | Adaptive Cruise | openpilot | 0mph | 7mph | -| Cadillac | ATS 20187 | Adaptive Cruise | openpilot | 0mph | 7mph | -| GMC | Acadia Denali 20187 | Adaptive Cruise | openpilot | 0mph | 7mph | -| Holden | Astra 20177 | Adaptive Cruise | openpilot | 0mph | 7mph | - -5When disconnecting the Driver Support Unit (DSU), openpilot ACC will replace stock ACC. For DSU locations, see [Toyota Wiki page](https://community.comma.ai/wiki/index.php/Toyota). ***NOTE: disconnecting the DSU disables Automatic Emergency Braking (AEB).***
-6[Comma Pedal](https://community.comma.ai/wiki/index.php/Comma_Pedal) is used to provide stop-and-go capability to some of the openpilot-supported cars that don't currently support stop-and-go. Here is how to [build a Comma Pedal](https://medium.com/@jfrux/comma-pedal-building-with-macrofab-6328bea791e8). ***NOTE: The Comma Pedal is not officially supported by [comma](https://comma.ai).***
-7Requires a [panda](https://comma.ai/shop/products/panda-obd-ii-dongle) and [community built giraffe](https://zoneos.com/volt/). ***NOTE: disconnecting the ASCM disables Automatic Emergency Braking (AEB).***
- -Community Maintained Cars and Features are not verified by comma to meet our [safety model](SAFETY.md). Be extra cautious using them. They are only available after enabling the toggle in `Settings->Developer->Enable Community Features`. - -Installation Instructions ------- - -Install openpilot on a EON by entering ``https://openpilot.comma.ai`` during the installer setup. - -Follow this [video instructions](https://youtu.be/3nlkomHathI) to properly mount the EON on the windshield. Note: openpilot features an automatic pose calibration routine and openpilot performance should not be affected by small pitch and yaw misalignments caused by imprecise EON mounting. - -Before placing the device on your windshield, check the state and local laws and ordinances where you drive. Some state laws prohibit or restrict the placement of objects on the windshield of a motor vehicle. - -You will be able to engage openpilot after reviewing the onboarding screens and finishing the calibration procedure. - -Limitations of openpilot ALC and LDW ------- - -openpilot ALC and openpilot LDW do not automatically drive the vehicle or reduce the amount of attention that must be paid to operate your vehicle. The driver must always keep control of the steering wheel and be ready to correct the openpilot ALC action at all times. - -Many factors can impact the performance of openpilot ALC and openpilot LDW, causing them to be unable to function as intended. These include, but are not limited to: - -* Poor visibility (heavy rain, snow, fog, etc.) or weather conditions that may interfere with sensor operation. -* The road facing camera is obstructed, covered or damaged by mud, ice, snow, etc. -* Obstruction caused by applying excessive paint or adhesive products (such as wraps, stickers, rubber coating, etc.) onto the vehicle. -* The EON is mounted incorrectly. -* When in sharp curves, like on-off ramps, intersections etc...; openpilot is designed to be limited in the amount of steering torque it can produce. -* In the presence of restricted lanes or construction zones. -* When driving on highly banked roads or in presence of strong cross-wind. -* Extremely hot or cold temperatures. -* Bright light (due to oncoming headlights, direct sunlight, etc.). -* Driving on hills, narrow, or winding roads. - -The list above does not represent an exhaustive list of situations that may interfere with proper operation of openpilot components. It is the driver's responsibility to be in control of the vehicle at all times. - -Limitations of openpilot ACC and FCW ------- - -openpilot ACC and openpilot FCW are not systems that allow careless or inattentive driving. It is still necessary for the driver to pay close attention to the vehicle’s surroundings and to be ready to re-take control of the gas and the brake at all times. - -Many factors can impact the performance of openpilot ACC and openpilot FCW, causing them to be unable to function as intended. These include, but are not limited to: - -* Poor visibility (heavy rain, snow, fog, etc.) or weather conditions that may interfere with sensor operation. -* The road facing camera or radar are obstructed, covered, or damaged by mud, ice, snow, etc. -* Obstruction caused by applying excessive paint or adhesive products (such as wraps, stickers, rubber coating, etc.) onto the vehicle. -* The EON is mounted incorrectly. -* Approaching a toll booth, a bridge or a large metal plate. -* When driving on roads with pedestrians, cyclists, etc... -* In presence of traffic signs or stop lights, which are not detected by openpilot at this time. -* When the posted speed limit is below the user selected set speed. openpilot does not detect speed limits at this time. -* In presence of vehicles in the same lane that are not moving. -* When abrupt braking maneuvers are required. openpilot is designed to be limited in the amount of deceleration and acceleration that it can produce. -* When surrounding vehicles perform close cut-ins from neighbor lanes. -* Driving on hills, narrow, or winding roads. -* Extremely hot or cold temperatures. -* Bright light (due to oncoming headlights, direct sunlight, etc.). -* Interference from other equipment that generates radar waves. - -The list above does not represent an exhaustive list of situations that may interfere with proper operation of openpilot components. It is the driver's responsibility to be in control of the vehicle at all times. - -Limitations of openpilot DM ------- - -openpilot DM should not be considered an exact measurements of the status of alertness of the driver. - -Many factors can impact the performance of openpilot DM, causing it to be unable to function as intended. These include, but are not limited to: - -* Low light conditions, such as driving at night or in dark tunnels. -* Bright light (due to oncoming headlights, direct sunlight, etc.). -* The driver face is partially or completely outside field of view of the driver facing camera. -* Right hand driving vehicles. -* The driver facing camera is obstructed, covered, or damaged. - -The list above does not represent an exhaustive list of situations that may interfere with proper operation of openpilot components. A driver should not rely on openpilot DM to assess their level of attention. - -User Data and comma Account ------- - -By default, openpilot uploads the driving data to our servers. You can also access your data by pairing with the comma connect app ([iOS](https://apps.apple.com/us/app/comma-connect/id1456551889), [Android](https://play.google.com/store/apps/details?id=ai.comma.connect&hl=en_US)). We use your data to train better models and improve openpilot for everyone. - -openpilot is open source software: the user is free to disable data collection if they wish to do so. - -openpilot logs the road facing camera, CAN, GPS, IMU, magnetometer, thermal sensors, crashes, and operating system logs. -The driver facing camera is only logged if you explicitly opt-in in settings. The microphone is not recorded. - -By using openpilot, you agree to [our Privacy Policy](https://my.comma.ai/privacy). You understand that use of this software or its related services will generate certain types of user data, which may be logged and stored at the sole discretion of comma. By accepting this agreement, you grant an irrevocable, perpetual, worldwide right to comma for the use of this data. - -Safety and Testing ----- - -* openpilot observes ISO26262 guidelines, see [SAFETY.md](SAFETY.md) for more detail. -* openpilot has software in the loop [tests](run_docker_tests.sh) that run on every commit. -* The safety model code lives in panda and is written in C, see [code rigor](https://github.com/commaai/panda#code-rigor) for more details. -* panda has software in the loop [safety tests](https://github.com/commaai/panda/tree/master/tests/safety). -* Internally, we have a hardware in the loop Jenkins test suite that builds and unit tests the various processes. -* panda has additional hardware in the loop [tests](https://github.com/commaai/panda/blob/master/Jenkinsfile). -* We run the latest openpilot in a testing closet containing 10 EONs continuously replaying routes. - -Testing on PC ------- - -Check out [openpilot-tools](https://github.com/commaai/openpilot-tools): lots of tools you can use to replay driving data, test and develop openpilot from your pc. - -Community and Contributing ------- - -openpilot is developed by [comma](https://comma.ai/) and by users like you. We welcome both pull requests and issues on [GitHub](http://github.com/commaai/openpilot). Bug fixes and new car ports are encouraged. - -You can add support for your car by following guides we have written for [Brand](https://medium.com/@comma_ai/how-to-write-a-car-port-for-openpilot-7ce0785eda84) and [Model](https://medium.com/@comma_ai/openpilot-port-guide-for-toyota-models-e5467f4b5fe6) ports. Generally, a car with adaptive cruise control and lane keep assist is a good candidate. [Join our Discord](https://discord.comma.ai) to discuss car ports: most car makes have a dedicated channel. - -Want to get paid to work on openpilot? [comma is hiring](https://comma.ai/jobs/). We also have a [bounty program](https://comma.ai/bounties.html). - -And [follow us on Twitter](https://twitter.com/comma_ai). - -Directory Structure ------- - . - ├── apk # The apk files used for the UI - ├── cereal # The messaging spec and libs used for all logs on EON - ├── common # Library like functionality we've developed here - ├── installer/updater # Manages auto-updates of openpilot - ├── opendbc # Files showing how to interpret data from cars - ├── panda # Code used to communicate on CAN - ├── phonelibs # Libraries used on EON - ├── pyextra # Libraries used on EON - └── selfdrive # Code needed to drive the car - ├── assets # Fonts and images for UI - ├── athena # Allows communication with the app - ├── boardd # Daemon to talk to the board - ├── camerad # Driver to capture images from the camera sensors - ├── car # Car specific code to read states and control actuators - ├── common # Shared C/C++ code for the daemons - ├── controls # Perception, planning and controls - ├── debug # Tools to help you debug and do car ports - ├── locationd # Soon to be home of precise location - ├── logcatd # Android logcat as a service - ├── loggerd # Logger and uploader of car data - ├── modeld # Driving model runner - ├── monitoringd # Driver monitoring model runner - ├── proclogd # Logs information from proc - ├── sensord # IMU / GPS interface code - ├── tests # Unit tests, system tests and a car simulator - └── ui # The UI - -To understand how the services interact, see `cereal/service_list.yaml`. - -Licensing ------- - -openpilot is released under the MIT license. Some parts of the software are released under other licenses as specified. - -Any user of this software shall indemnify and hold harmless comma.ai, Inc. and its directors, officers, employees, agents, stockholders, affiliates, subcontractors and customers from and against all allegations, claims, actions, suits, demands, damages, liabilities, obligations, losses, settlements, judgments, costs and expenses (including without limitation attorneys’ fees and costs) which arise out of, relate to or result from any use of this software by user. - -**THIS IS ALPHA QUALITY SOFTWARE FOR RESEARCH PURPOSES ONLY. THIS IS NOT A PRODUCT. -YOU ARE RESPONSIBLE FOR COMPLYING WITH LOCAL LAWS AND REGULATIONS. -NO WARRANTY EXPRESSED OR IMPLIED.** - ---- - - +Shane's Stock Additions (0.7) +===== + +This branch is simply stock openpilot with some additions to help it drive as smooth as possible on my 2017 Toyota Corolla. + + +Highlight Features +==== + +1. **(NOT YET ADDED) Dynamic gas**: This aims to provide a smoother driving experience in stop and go traffic by modifying the maximum gas that can be applied based on your current velocity and the relative velocity of the lead car. It'll also of course increase the maximum gas when the lead is accelerating to help you get up to speed quicker than stock. And smoother; this eliminates the jerking you get from stock openpilot with comma pedal. Better tuning will be next. +2. **Dynamic follow**: This is my dynamic follow from 0.5, where it changes your TR (following distance) dynamically based on multiple vehicle factors, as well as data from the lead vehicle. [Here's an old write up from a while ago explaining how it works exactly. Some of it might be out of date, but how it functions is the same.](https://github.com/ShaneSmiskol/openpilot/blob/dynamic-follow/README.md) +3. **(NOT YET ADDED) Two PID loops to control gas and brakes independently (new!)**: If you have a Toyota Corolla with a comma pedal, you'll love this addition. Two longitudinal PID loops are set up in `longcontrol.py` so that one is running with comma pedal tuning to control the gas, and the other is running stock non-pedal tuning for better braking control. In the car, this feels miles better than stock openpilot, and nearly as good as your stock Toyota cruise control before you pulled out your DSU! It won't accelerate up to stopped cars and brake at the last moment anymore. +3. **Custom wheel offset to reduce lane hugging**: Stock openpilot doesn't seem to be able to identify your car's true angle offset. With the `LaneHugging` module you can specify a custom angle offset to be added to your desired steering angle. Simply find the angle your wheel is at when you're driving on a straight highway. By default, this is disabled, to enable you can: + - Use the `opEdit` class in the root directory of openpilot. To use it, simply open an `ssh` shell and enter the commands below: + ```python + cd /data/openpilot + python op_edit.py + ``` + You'll be greeted with a list of your parameters you can explore, enter the number corresponding to `lane_hug_direction`. Your options are to enter `'left'` or `'right'` for whichever direction your car has a tendency to hug toward. `None` will disable the feature. + Finally you'll need to enter your absolute angle offset (negative will be converted to positive) with the `opParams` parameter: `lane_hug_angle_offset`. +4. **Custom following distance**: Using the `following_distance` parameter in `opParams`, you can specify a custom TR value to always be used. Afraid of technology and want to give yourself the highest following distance out there? Try out 2.7s! Are you daredevil and don't care about pissing off the car you're tailgating ahead? Try 0.9s! + - Again, you can use `opEdit` to change this: + ```python + cd /data/openpilot + python op_edit.py + ``` + Then enter the number for the `following_distance` parameter and set to a float or integer between `0.9` and `2.7`. `None` will use dynamic follow! +5. **Customize this branch (opEdit Parameter class)**: This is a handy tool to change your `opParams` parameters without diving into any json files or code. You can specify parameters to be used in any fork's operation that supports `opParams`. First, ssh in to your EON and make sure you're in `/data/openpilot`, then start `opEdit`: + ```python + cd /data/openpilot + python op_edit.py + ``` + A list of parameters that you can change are located [here](https://github.com/ShaneSmiskol/openpilot/blob/stock_additions-07-final/common/op_params.py#L29). + + Parameters are stored at `/data/op_params.json` \ No newline at end of file diff --git a/common/data_collector.py b/common/data_collector.py new file mode 100644 index 00000000000000..17410cfe9a9ce1 --- /dev/null +++ b/common/data_collector.py @@ -0,0 +1,98 @@ +from common.travis_checker import travis +from selfdrive.swaglog import cloudlog +import threading +import time +import os +from common.op_params import opParams + +op_params = opParams() + +class DataCollector: + def __init__(self, file_path, keys, write_frequency=60, write_threshold=2): + """ + This class provides an easy way to set up your own custom data collector to gather custom data. + Parameters: + file_path (str): The path you want your custom data to be written to. + keys: (list): A string list containing the names of the values you want to collect. + Your data list needs to be in this order. + write_frequency (int/float): The rate at which to write data in seconds. + write_threshold (int): The length of the data list we need to collect before considering writing. + Example: + data_collector = DataCollector('/data/openpilot/custom_data', ['v_ego', 'a_ego', 'custom_dict'], write_frequency=120) + """ + + self.log_data = op_params.get('log_data', False) + self.file_path = file_path + self.keys = keys + self.write_frequency = write_frequency + self.write_threshold = write_threshold + self.data = [] + self.last_write_time = time.time() + self.thread_running = False + self.is_initialized = False + + def initialize(self): # add keys to top of data file + if not os.path.exists(self.file_path) and not travis: + with open(self.file_path, "w") as f: + f.write('{}\n'.format(self.keys)) + self.is_initialized = True + + def append(self, sample): + """ + Appends your sample to a central self.data variable that gets written to your specified file path every n seconds. + Parameters: + sample: Can be any type of data. List, dictionary, numbers, strings, etc. + Or a combination: dictionaries, booleans, and floats in a list + Continuing from the example above, we assume that the first value is your velocity, and the second + is your acceleration. IMPORTANT: If your values and keys are not in the same order, you will have trouble figuring + what data is what when you want to process it later. + Example: + data_collector.append([17, 0.5, {'a': 1}]) + """ + + if self.log_data: + if len(sample) != len(self.keys): + raise Exception("You need the same amount of data as you specified in your keys") + if not self.is_initialized: + self.initialize() + self.data.append(sample) + self.check_if_can_write() + + def reset(self, reset_type=None): + if reset_type in ['data', 'all']: + self.data = [] + if reset_type in ['time', 'all']: + self.last_write_time = time.time() + + def check_if_can_write(self): + """ + You shouldn't ever need to call this. It checks if we should write, then calls a thread to do so + with a copy of the current gathered data. Then it clears the self.data variable so that new data + can be added and it won't be duplicated in the next write. + If the thread is still writing by the time of the next write, which shouldn't ever happen unless + you set a low write frequency, it will skip creating another write thread. If this occurs, + something is wrong with writing. + """ + + if (time.time() - self.last_write_time) >= self.write_frequency and len(self.data) >= self.write_threshold and not travis: + if not self.thread_running: + write_thread = threading.Thread(target=self.write, args=(self.data,)) + write_thread.daemon = True + write_thread.start() + # self.write(self.data) # non threaded approach + self.reset(reset_type='all') + elif self.write_frequency > 30: + cloudlog.warning('DataCollector write thread is taking a while to write data.') + + def write(self, current_data): + """ + Only write data that has been added so far in background. self.data is still being appended to in + foreground so in the next write event, new data will be written. This eliminates lag causing openpilot + critical processes to pause while a lot of data is being written. + """ + + self.thread_running = True + with open(self.file_path, "a") as f: + f.write('{}\n'.format('\n'.join(map(str, current_data)))) # json takes twice as long to write + self.reset(reset_type='time') + self.thread_running = False diff --git a/common/op_params.py b/common/op_params.py new file mode 100644 index 00000000000000..983dfd3a23e584 --- /dev/null +++ b/common/op_params.py @@ -0,0 +1,116 @@ +import os +import json +import time +import string +import random +from common.travis_checker import travis + + +def write_params(params, params_file): + if not travis: + with open(params_file, "w") as f: + json.dump(params, f, indent=2, sort_keys=True) + os.chmod(params_file, 0o764) + + +def read_params(params_file, default_params): + try: + with open(params_file, "r") as f: + params = json.load(f) + return params, True + except Exception as e: + print(e) + params = default_params + return params, False + + +class opParams: + def __init__(self): + self.default_params = {'camera_offset': {'default': 0.06, 'allowed_types': [float, int], 'description': 'Your camera offset to use in lane_planner.py'}, + 'awareness_factor': {'default': 2.0, 'allowed_types': [float, int], 'description': 'Multiplier for the awareness times'}, + 'lane_hug_direction': {'default': None, 'allowed_types': [type(None), str], 'description': "(NoneType, 'left', 'right'): Direction of your lane hugging, if present. None will disable this modification"}, + 'lane_hug_angle_offset': {'default': 0.0, 'allowed_types': [float, int], 'description': ('This is the angle your wheel reads when driving straight at highway speeds. ' + 'Used to offset desired angle_steers in latcontrol to help fix lane hugging. ' + 'Enter absolute value here, direction is determined by parameter \'lane_hug_direction\'')}, + 'use_car_caching': {'default': True, 'allowed_types': [bool], 'description': 'Whether to use fingerprint caching'}, + 'force_pedal': {'default': False, 'allowed_types': [bool], 'description': "If openpilot isn't recognizing your comma pedal, set this to True"}, + 'following_distance': {'default': None, 'allowed_types': [type(None), float], 'description': 'None has no effect, while setting this to a float will let you change the TR'}, + 'alca_nudge_required': {'default': True, 'allowed_types': [bool], 'description': ('Whether to wait for applied torque to the wheel (nudge) before making lane changes. ' + 'If False, lane change will occur IMMEDIATELY after signaling')}, + 'alca_min_speed': {'default': 35.0, 'allowed_types': [float, int], 'description': 'The minimum speed allowed for an automatic lane change (in MPH)'}} + + self.params = {} + self.params_file = "/data/op_params.json" + self.kegman_file = "/data/kegman.json" + self.last_read_time = time.time() + self.read_timeout = 1.0 # max frequency to read with self.get(...) (sec) + self.force_update = False # replaces values with default params if True, not just add add missing key/value pairs + self.run_init() # restores, reads, and updates params + + def create_id(self): # creates unique identifier to send with sentry errors. please update uniqueID with op_edit.py to your username! + need_id = False + if "uniqueID" not in self.params: + need_id = True + if "uniqueID" in self.params and self.params["uniqueID"] is None: + need_id = True + if need_id: + random_id = ''.join([random.choice(string.ascii_lowercase + string.ascii_uppercase + string.digits) for i in range(15)]) + self.params["uniqueID"] = random_id + + def add_default_params(self): + prev_params = dict(self.params) + if not travis: + self.create_id() + for key in self.default_params: + if self.force_update: + self.params[key] = self.default_params[key]['default'] + elif key not in self.params: + self.params[key] = self.default_params[key]['default'] + return prev_params == self.params + + def format_default_params(self): + return {key: self.default_params[key]['default'] for key in self.default_params} + + def run_init(self): # does first time initializing of default params, and/or restoring from kegman.json + if travis: + self.params = self.format_default_params() + return + self.params = self.format_default_params() # in case any file is corrupted + to_write = False + no_params = False + if os.path.isfile(self.params_file): + self.params, read_status = read_params(self.params_file, self.format_default_params()) + if read_status: + to_write = not self.add_default_params() # if new default data has been added + else: # don't overwrite corrupted params, just print to screen + print("ERROR: Can't read op_params.json file") + elif os.path.isfile(self.kegman_file): + to_write = True # write no matter what + try: + with open(self.kegman_file, "r") as f: # restore params from kegman + self.params = json.load(f) + self.add_default_params() + except: + print("ERROR: Can't read kegman.json file") + else: + no_params = True # user's first time running a fork with kegman_conf or op_params + if to_write or no_params: + write_params(self.params, self.params_file) + + def put(self, key, value): + self.params.update({key: value}) + write_params(self.params, self.params_file) + + def get(self, key=None, default=None): # can specify a default value if key doesn't exist + if (time.time() - self.last_read_time) >= self.read_timeout and not travis: # make sure we aren't reading file too often + self.params, read_status = read_params(self.params_file, self.format_default_params()) + self.last_read_time = time.time() + if key is None: # get all + return self.params + else: + return self.params[key] if key in self.params else default + + def delete(self, key): + if key in self.params: + del self.params[key] + write_params(self.params, self.params_file) diff --git a/common/travis_checker.py b/common/travis_checker.py new file mode 100644 index 00000000000000..74f2d256d07d43 --- /dev/null +++ b/common/travis_checker.py @@ -0,0 +1,2 @@ +from common.basedir import BASEDIR +travis = BASEDIR.strip('/').split('/')[0] != 'data' diff --git a/op_edit.py b/op_edit.py new file mode 100644 index 00000000000000..584a541cd1ce98 --- /dev/null +++ b/op_edit.py @@ -0,0 +1,175 @@ +from common.op_params import opParams +import time +import ast + + +class opEdit: # use by running `python /data/openpilot/op_edit.py` + def __init__(self): + self.op_params = opParams() + self.params = None + print('Welcome to the opParams command line editor!') + print('Here are your parameters:\n') + self.run_loop() + + def run_loop(self): + print('Welcome to the opParams command line editor!') + print('Here are your parameters:\n') + while True: + self.params = self.op_params.get() + values_list = [self.params[i] if len(str(self.params[i])) < 20 else '{} ... {}'.format(str(self.params[i])[:30], str(self.params[i])[-15:]) for i in self.params] + to_print = ['{}. {}: {} (type: {})'.format(idx + 1, i, values_list[idx], str(type(self.params[i])).split("'")[1]) for idx, i in enumerate(self.params)] + to_print.append('{}. Add new parameter!'.format(len(self.params) + 1)) + to_print.append('{}. Delete parameter!'.format(len(self.params) + 2)) + print('\n'.join(to_print)) + print('\nChoose a parameter to explore (by integer index): ') + choice = input('>> ') + parsed, choice = self.parse_choice(choice) + if parsed == 'continue': + continue + elif parsed == 'add': + if self.add_parameter() == 'error': + return + elif parsed == 'change': + if self.change_parameter(choice) == 'error': + return + elif parsed == 'delete': + if self.delete_parameter() == 'error': + return + elif parsed == 'error': + return + + def parse_choice(self, choice): + if choice.isdigit(): + choice = int(choice) + elif choice == '': + print('Exiting...') + return 'error', choice + else: + print('\nNot an integer!\n', flush=True) + time.sleep(1.5) + return 'retry', choice + if choice not in range(1, len(self.params) + 3): # three for add/delete parameter + print('Not in range!\n', flush=True) + time.sleep(1.5) + return 'continue', choice + + if choice == len(self.params) + 1: # add new parameter + return 'add', choice + + if choice == len(self.params) + 2: # delete parameter + return 'delete', choice + + return 'change', choice + + def change_parameter(self, choice): + chosen_key = list(self.params)[choice - 1] + extra_info = False + if chosen_key in self.op_params.default_params: + extra_info = True + param_allowed_types = self.op_params.default_params[chosen_key]['allowed_types'] + param_description = self.op_params.default_params[chosen_key]['description'] + + old_value = self.params[chosen_key] + print('Chosen parameter: {}'.format(chosen_key)) + print('Current value: {} (type: {})'.format(old_value, str(type(old_value)).split("'")[1])) + if extra_info: + print('\nDescription: {}'.format(param_description)) + print('Allowed types: {}\n'.format(', '.join([str(i).split("'")[1] for i in param_allowed_types]))) + print('Enter your new value:') + new_value = input('>> ') + if len(new_value) == 0: + print('Entered value cannot be empty!') + return 'error' + status, new_value = self.parse_input(new_value) + if not status: + print('Cannot parse input, exiting!') + return 'error' + + if extra_info and not any([isinstance(new_value, typ) for typ in param_allowed_types]): + print('The type of data you entered ({}) is not allowed with this parameter!\n'.format(str(type(new_value)).split("'")[1])) + time.sleep(1.5) + return + + print('\nOld value: {} (type: {})'.format(old_value, str(type(old_value)).split("'")[1])) + print('New value: {} (type: {})'.format(new_value, str(type(new_value)).split("'")[1])) + print('Do you want to save this?') + choice = input('[Y/n]: ').lower() + if choice == 'y': + self.op_params.put(chosen_key, new_value) + print('\nSaved!\n') + else: + print('\nNot saved!\n', flush=True) + time.sleep(1.5) + + def parse_input(self, dat): + try: + dat = ast.literal_eval(dat) + except: + try: + dat = ast.literal_eval('"{}"'.format(dat)) + except ValueError: + return False, dat + return True, dat + + def delete_parameter(self): + print('Enter the name of the parameter to delete:') + key = input('>> ') + status, key = self.parse_input(key) + if not status: + print('Cannot parse input, exiting!') + return 'error' + if not isinstance(key, str): + print('Input must be a string!') + return 'error' + if key not in self.params: + print("Parameter doesn't exist!") + return 'error' + + value = self.params.get(key) + print('Parameter name: {}'.format(key)) + print('Parameter value: {} (type: {})'.format(value, str(type(value)).split("'")[1])) + print('Do you want to delete this?') + + choice = input('[Y/n]: ').lower() + if choice == 'y': + self.op_params.delete(key) + print('\nDeleted!\n') + else: + print('\nNot saved!\n', flush=True) + time.sleep(1.5) + + def add_parameter(self): + print('Type the name of your new parameter:') + key = input('>> ') + if len(key) == 0: + print('Entered key cannot be empty!') + return 'error' + status, key = self.parse_input(key) + if not status: + print('Cannot parse input, exiting!') + return 'error' + if not isinstance(key, str): + print('Input must be a string!') + return 'error' + + print("Enter the data you'd like to save with this parameter:") + value = input('>> ') + status, value = self.parse_input(value) + if not status: + print('Cannot parse input, exiting!') + return 'error' + + print('Parameter name: {}'.format(key)) + print('Parameter value: {} (type: {})'.format(value, str(type(value)).split("'")[1])) + print('Do you want to save this?') + + choice = input('[Y/n]: ').lower() + if choice == 'y': + self.op_params.put(key, value) + print('\nSaved!\n') + else: + print('\nNot saved!\n', flush=True) + time.sleep(1.5) + + +opEdit() From c793351fa195cce1bfe6641063cd5015e0436902 Mon Sep 17 00:00:00 2001 From: Shane Date: Mon, 16 Dec 2019 19:41:09 -0600 Subject: [PATCH 02/30] tuning for 17 corolla --- selfdrive/car/toyota/interface.py | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/selfdrive/car/toyota/interface.py b/selfdrive/car/toyota/interface.py index 3c6772228c470c..90a0baf31bc989 100755 --- a/selfdrive/car/toyota/interface.py +++ b/selfdrive/car/toyota/interface.py @@ -108,11 +108,14 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), vin="", has_relay stop_and_go = False ret.safetyParam = 100 ret.wheelbase = 2.70 - ret.steerRatio = 18.27 + ret.steerRatio = 17.8 tire_stiffness_factor = 0.444 # not optimized yet ret.mass = 2860. * CV.LB_TO_KG + STD_CARGO_KG # mean between normal and hybrid ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.2], [0.05]] - ret.lateralTuning.pid.kf = 0.00003 # full torque for 20 deg at 80mph means 0.00007818594 + ret.lateralTuning.pid.kf = 0.00003 * 0.5112 # full torque for 20 deg at 80mph means 0.00007818594 + if ret.enableGasInterceptor: + ret.longitudinalTuning.kpV = [1.0, 0.66, 0.42] + ret.longitudinalTuning.kiV = [0.135, 0.09] elif candidate == CAR.LEXUS_RXH: stop_and_go = True From 5d6ecf0bc1b9ab4233a88ae959550da98a13e583 Mon Sep 17 00:00:00 2001 From: Shane Date: Mon, 16 Dec 2019 19:42:14 -0600 Subject: [PATCH 03/30] Revert "tuning for 17 corolla" This reverts commit c793351fa195cce1bfe6641063cd5015e0436902. --- selfdrive/car/toyota/interface.py | 7 ++----- 1 file changed, 2 insertions(+), 5 deletions(-) diff --git a/selfdrive/car/toyota/interface.py b/selfdrive/car/toyota/interface.py index 90a0baf31bc989..3c6772228c470c 100755 --- a/selfdrive/car/toyota/interface.py +++ b/selfdrive/car/toyota/interface.py @@ -108,14 +108,11 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), vin="", has_relay stop_and_go = False ret.safetyParam = 100 ret.wheelbase = 2.70 - ret.steerRatio = 17.8 + ret.steerRatio = 18.27 tire_stiffness_factor = 0.444 # not optimized yet ret.mass = 2860. * CV.LB_TO_KG + STD_CARGO_KG # mean between normal and hybrid ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.2], [0.05]] - ret.lateralTuning.pid.kf = 0.00003 * 0.5112 # full torque for 20 deg at 80mph means 0.00007818594 - if ret.enableGasInterceptor: - ret.longitudinalTuning.kpV = [1.0, 0.66, 0.42] - ret.longitudinalTuning.kiV = [0.135, 0.09] + ret.lateralTuning.pid.kf = 0.00003 # full torque for 20 deg at 80mph means 0.00007818594 elif candidate == CAR.LEXUS_RXH: stop_and_go = True From f114e0b4d2fce653f54e7b0febca479cb3325916 Mon Sep 17 00:00:00 2001 From: Shane Date: Mon, 16 Dec 2019 19:45:42 -0600 Subject: [PATCH 04/30] move long tuning to top --- selfdrive/car/toyota/interface.py | 24 ++++++++++++------------ 1 file changed, 12 insertions(+), 12 deletions(-) diff --git a/selfdrive/car/toyota/interface.py b/selfdrive/car/toyota/interface.py index 3c6772228c470c..865fc7cb674feb 100755 --- a/selfdrive/car/toyota/interface.py +++ b/selfdrive/car/toyota/interface.py @@ -52,6 +52,18 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), vin="", has_relay ret.steerActuatorDelay = 0.12 # Default delay, Prius has larger delay ret.steerLimitTimer = 0.4 + ret.enableGasInterceptor = 0x201 in fingerprint[0] + + if ret.enableGasInterceptor: + ret.gasMaxBP = [0., 9., 35] + ret.gasMaxV = [0.2, 0.5, 0.7] + ret.longitudinalTuning.kpV = [1.2, 0.8, 0.5] + ret.longitudinalTuning.kiV = [0.18, 0.12] + else: + ret.gasMaxBP = [0.] + ret.gasMaxV = [0.5] + ret.longitudinalTuning.kpV = [3.6, 2.4, 1.5] + ret.longitudinalTuning.kiV = [0.54, 0.36] if candidate not in [CAR.PRIUS, CAR.RAV4, CAR.RAV4H]: # These cars use LQR/INDI ret.lateralTuning.init('pid') @@ -250,7 +262,6 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), vin="", has_relay # In TSS2 cars the camera does long control ret.enableDsu = is_ecu_disconnected(fingerprint[0], FINGERPRINTS, ECU_FINGERPRINT, candidate, ECU.DSU) and candidate not in TSS2_CAR ret.enableApgs = False # is_ecu_disconnected(fingerprint[0], FINGERPRINTS, ECU_FINGERPRINT, candidate, ECU.APGS) - ret.enableGasInterceptor = 0x201 in fingerprint[0] ret.openpilotLongitudinalControl = ret.enableCamera and (ret.enableDsu or candidate in TSS2_CAR) cloudlog.warning("ECU Camera Simulated: %r", ret.enableCamera) cloudlog.warning("ECU DSU Simulated: %r", ret.enableDsu) @@ -271,17 +282,6 @@ def get_params(candidate, fingerprint=gen_empty_fingerprint(), vin="", has_relay ret.stoppingControl = False ret.startAccel = 0.0 - if ret.enableGasInterceptor: - ret.gasMaxBP = [0., 9., 35] - ret.gasMaxV = [0.2, 0.5, 0.7] - ret.longitudinalTuning.kpV = [1.2, 0.8, 0.5] - ret.longitudinalTuning.kiV = [0.18, 0.12] - else: - ret.gasMaxBP = [0.] - ret.gasMaxV = [0.5] - ret.longitudinalTuning.kpV = [3.6, 2.4, 1.5] - ret.longitudinalTuning.kiV = [0.54, 0.36] - return ret # returns a car.CarState From 5a1c25575434396b20fee30239267e220561fca0 Mon Sep 17 00:00:00 2001 From: Shane Date: Mon, 16 Dec 2019 19:47:15 -0600 Subject: [PATCH 05/30] add op_params to awareness_time, and camera_offset --- selfdrive/controls/lane_hugging.py | 19 +++++++++++++++++++ selfdrive/controls/lib/driver_monitor.py | 9 ++++++++- selfdrive/controls/lib/lane_planner.py | 3 ++- 3 files changed, 29 insertions(+), 2 deletions(-) create mode 100644 selfdrive/controls/lane_hugging.py diff --git a/selfdrive/controls/lane_hugging.py b/selfdrive/controls/lane_hugging.py new file mode 100644 index 00000000000000..5860a567205be4 --- /dev/null +++ b/selfdrive/controls/lane_hugging.py @@ -0,0 +1,19 @@ +from common.op_params import opParams + + +class LaneHugging: + def __init__(self): + self.op_params = opParams() + self.direction = self.op_params.get('lane_hug_direction', None) # if lane hugging is present and which side. None, 'left', or 'right' + if isinstance(self.direction, str): + self.direction = self.direction.lower() + self.angle_offset = abs(self.op_params.get('lane_hug_angle_offset', 0.0)) + + def offset_mod(self, angle_steers_des): + # negative angles: right + # positive angles: left + if self.direction == 'left': + angle_steers_des -= self.angle_offset + elif self.direction == 'right': + angle_steers_des += self.angle_offset + return angle_steers_des diff --git a/selfdrive/controls/lib/driver_monitor.py b/selfdrive/controls/lib/driver_monitor.py index 13a8130b7c9d76..cfce9aa8061020 100644 --- a/selfdrive/controls/lib/driver_monitor.py +++ b/selfdrive/controls/lib/driver_monitor.py @@ -3,8 +3,15 @@ from selfdrive.controls.lib.drive_helpers import create_event, EventTypes as ET from common.filter_simple import FirstOrderFilter from common.stat_live import RunningStatFilter +from common.op_params import opParams +from common.travis_checker import travis -_AWARENESS_TIME = 100. # 1.6 minutes limit without user touching steering wheels make the car enter a terminal status +if not travis: + awareness_factor = opParams().get('awareness_factor', default=2.0) +else: + awareness_factor = 1 + +_AWARENESS_TIME = 100. * awareness_factor # 1.6 minutes limit without user touching steering wheels make the car enter a terminal status _AWARENESS_PRE_TIME_TILL_TERMINAL = 25. # a first alert is issued 25s before expiration _AWARENESS_PROMPT_TIME_TILL_TERMINAL = 15. # a second alert is issued 15s before start decelerating the car _DISTRACTED_TIME = 11. diff --git a/selfdrive/controls/lib/lane_planner.py b/selfdrive/controls/lib/lane_planner.py index 0e84ad8b9f7502..ed838d42871f5a 100644 --- a/selfdrive/controls/lib/lane_planner.py +++ b/selfdrive/controls/lib/lane_planner.py @@ -1,8 +1,9 @@ +from common.op_params import opParams from common.numpy_fast import interp import numpy as np from cereal import log -CAMERA_OFFSET = 0.06 # m from center car to camera +CAMERA_OFFSET = opParams().get('camera_offset', 0.06) # m from center car to camera def compute_path_pinv(l=50): deg = 3 From 9b5210b03aee279b65dbb05c284c897807d31150 Mon Sep 17 00:00:00 2001 From: Shane Date: Mon, 16 Dec 2019 19:54:37 -0600 Subject: [PATCH 06/30] add alca parameters to lane changing, min speed and nudge required --- common/op_params.py | 2 +- selfdrive/controls/lib/latcontrol_pid.py | 5 ++++- selfdrive/controls/lib/pathplanner.py | 17 ++++++++++++----- selfdrive/crash.py | 2 +- 4 files changed, 18 insertions(+), 8 deletions(-) diff --git a/common/op_params.py b/common/op_params.py index 983dfd3a23e584..fef9d765de9b03 100644 --- a/common/op_params.py +++ b/common/op_params.py @@ -37,7 +37,7 @@ def __init__(self): 'following_distance': {'default': None, 'allowed_types': [type(None), float], 'description': 'None has no effect, while setting this to a float will let you change the TR'}, 'alca_nudge_required': {'default': True, 'allowed_types': [bool], 'description': ('Whether to wait for applied torque to the wheel (nudge) before making lane changes. ' 'If False, lane change will occur IMMEDIATELY after signaling')}, - 'alca_min_speed': {'default': 35.0, 'allowed_types': [float, int], 'description': 'The minimum speed allowed for an automatic lane change (in MPH)'}} + 'alca_min_speed': {'default': 30.0, 'allowed_types': [float, int], 'description': 'The minimum speed allowed for an automatic lane change (in MPH)'}} self.params = {} self.params_file = "/data/op_params.json" diff --git a/selfdrive/controls/lib/latcontrol_pid.py b/selfdrive/controls/lib/latcontrol_pid.py index 6a59565509ce8d..03b8695e7a5330 100644 --- a/selfdrive/controls/lib/latcontrol_pid.py +++ b/selfdrive/controls/lib/latcontrol_pid.py @@ -2,6 +2,7 @@ from selfdrive.controls.lib.drive_helpers import get_steer_max from cereal import car from cereal import log +from selfdrive.controls.lane_hugging import LaneHugging class LatControlPID(): @@ -10,6 +11,7 @@ def __init__(self, CP): (CP.lateralTuning.pid.kiBP, CP.lateralTuning.pid.kiV), k_f=CP.lateralTuning.pid.kf, pos_limit=1.0, sat_limit=CP.steerLimitTimer) self.angle_steers_des = 0. + self.lane_hugging = LaneHugging() def reset(self): self.pid.reset() @@ -24,7 +26,8 @@ def update(self, active, v_ego, angle_steers, angle_steers_rate, eps_torque, ste pid_log.active = False self.pid.reset() else: - self.angle_steers_des = path_plan.angleSteers # get from MPC/PathPlanner + # self.angle_steers_des = path_plan.angleSteers # get from MPC/PathPlanner + self.angle_steers_des = self.lane_hugging.offset_mod(path_plan.angleSteers) steers_max = get_steer_max(CP, v_ego) self.pid.pos_limit = steers_max diff --git a/selfdrive/controls/lib/pathplanner.py b/selfdrive/controls/lib/pathplanner.py index 87e3c520fca08f..ced7fdf3e479f1 100644 --- a/selfdrive/controls/lib/pathplanner.py +++ b/selfdrive/controls/lib/pathplanner.py @@ -8,6 +8,7 @@ from selfdrive.config import Conversions as CV import cereal.messaging as messaging from cereal import log +from common.op_params import opParams LaneChangeState = log.PathPlan.LaneChangeState LaneChangeDirection = log.PathPlan.LaneChangeDirection @@ -54,6 +55,9 @@ def __init__(self, CP): self.lane_change_state = LaneChangeState.off self.lane_change_timer = 0.0 self.prev_one_blinker = False + self.op_params = opParams() + self.alca_nudge_required = self.op_params.get('alca_nudge_required', default=True) + self.alca_min_speed = self.op_params.get('alca_min_speed', default=30.0) def setup_mpc(self): self.libmpc = libmpc_py.libmpc @@ -97,16 +101,19 @@ def update(self, sm, pm, CP, VM): elif sm['carState'].rightBlinker: lane_change_direction = LaneChangeDirection.right - if lane_change_direction == LaneChangeDirection.left: - torque_applied = sm['carState'].steeringTorque > 0 and sm['carState'].steeringPressed + if self.alca_nudge_required: + if lane_change_direction == LaneChangeDirection.left: + torque_applied = sm['carState'].steeringTorque > 0 and sm['carState'].steeringPressed + else: + torque_applied = sm['carState'].steeringTorque < 0 and sm['carState'].steeringPressed else: - torque_applied = sm['carState'].steeringTorque < 0 and sm['carState'].steeringPressed + torque_applied = True lane_change_prob = self.LP.l_lane_change_prob + self.LP.r_lane_change_prob # State transitions # off - if False: # self.lane_change_state == LaneChangeState.off and one_blinker and not self.prev_one_blinker: + if self.lane_change_state == LaneChangeState.off and one_blinker and not self.prev_one_blinker: self.lane_change_state = LaneChangeState.preLaneChange # pre @@ -124,7 +131,7 @@ def update(self, sm, pm, CP, VM): self.lane_change_state = LaneChangeState.preLaneChange # Don't allow starting lane change below 45 mph - if (v_ego < 45 * CV.MPH_TO_MS) and (self.lane_change_state == LaneChangeState.preLaneChange): + if (v_ego < self.alca_min_speed * CV.MPH_TO_MS) and (self.lane_change_state == LaneChangeState.preLaneChange): self.lane_change_state = LaneChangeState.off if self.lane_change_state in [LaneChangeState.off, LaneChangeState.preLaneChange]: diff --git a/selfdrive/crash.py b/selfdrive/crash.py index 6ceb56b627756c..7bc306fff4869e 100644 --- a/selfdrive/crash.py +++ b/selfdrive/crash.py @@ -19,7 +19,7 @@ def install(): else: from raven import Client from raven.transport.http import HTTPTransport - client = Client('https://1994756b5e6f41cf939a4c65de45f4f2:cefebaf3a8aa40d182609785f7189bd7@app.getsentry.com/77924', + client = Client('https://137e8e621f114f858f4c392c52e18c6d:8aba82f49af040c8aac45e95a8484970@sentry.io/1404547', install_sys_hook=False, transport=HTTPTransport, release=version, tags={'dirty': dirty}) def capture_exception(*args, **kwargs): From 49375c4dfc8119a79b6851eda00cb42c6e29b3c1 Mon Sep 17 00:00:00 2001 From: Shane Date: Mon, 16 Dec 2019 19:57:51 -0600 Subject: [PATCH 07/30] add ability to change TR dynamically --- selfdrive/controls/lib/long_mpc.py | 2 +- .../lib_mpc_export/acado_solver.c | 45 +++++++++---------- .../lib/longitudinal_mpc/libmpc_py.py | 5 ++- .../lib/longitudinal_mpc/longitudinal_mpc.c | 27 +++++++++-- 4 files changed, 49 insertions(+), 30 deletions(-) diff --git a/selfdrive/controls/lib/long_mpc.py b/selfdrive/controls/lib/long_mpc.py index f694af3771599e..332891dca118e0 100644 --- a/selfdrive/controls/lib/long_mpc.py +++ b/selfdrive/controls/lib/long_mpc.py @@ -92,7 +92,7 @@ def update(self, pm, CS, lead, v_cruise_setpoint): # Calculate mpc t = sec_since_boot() - n_its = self.libmpc.run_mpc(self.cur_state, self.mpc_solution, self.a_lead_tau, a_lead) + n_its = self.libmpc.run_mpc(self.cur_state, self.mpc_solution, self.a_lead_tau, a_lead, 1.8) duration = int((sec_since_boot() - t) * 1e9) if LOG_MPC: diff --git a/selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_solver.c b/selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_solver.c index 8cfc06f3b8ef70..2adeb4397e55ca 100644 --- a/selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_solver.c +++ b/selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_solver.c @@ -68,7 +68,7 @@ acadoWorkspace.evGu[lRun1 * 3 + 2] = acadoWorkspace.state[14]; return ret; } -void acado_evaluateLSQ(const real_t* in, real_t* out) +void acado_evaluateLSQ(const real_t* in, real_t* out, double TR) { const real_t* xd = in; const real_t* u = in + 3; @@ -78,29 +78,29 @@ real_t* a = acadoWorkspace.objAuxVar; /* Compute intermediate quantities: */ a[0] = (sqrt((xd[1]+(real_t)(5.0000000000000000e-01)))); -a[1] = (exp(((real_t)(2.9999999999999999e-01)*(((((((xd[1]*(real_t)(1.8000000000000000e+00))-((od[1]-xd[1])*(real_t)(1.8000000000000000e+00)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((od[1]*od[1])/(real_t)(1.9620000000000001e+01)))+(real_t)(4.0000000000000000e+00))-(od[0]-xd[0]))/(a[0]+(real_t)(1.0000000000000001e-01)))))); +a[1] = (exp(((real_t)(2.9999999999999999e-01)*(((((((xd[1]*(real_t)(TR))-((od[1]-xd[1])*(real_t)(TR)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((od[1]*od[1])/(real_t)(1.9620000000000001e+01)))+(real_t)(4.0000000000000000e+00))-(od[0]-xd[0]))/(a[0]+(real_t)(1.0000000000000001e-01)))))); a[2] = ((real_t)(1.0000000000000000e+00)/(a[0]+(real_t)(1.0000000000000001e-01))); -a[3] = (exp(((real_t)(2.9999999999999999e-01)*(((((((xd[1]*(real_t)(1.8000000000000000e+00))-((od[1]-xd[1])*(real_t)(1.8000000000000000e+00)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((od[1]*od[1])/(real_t)(1.9620000000000001e+01)))+(real_t)(4.0000000000000000e+00))-(od[0]-xd[0]))/(a[0]+(real_t)(1.0000000000000001e-01)))))); +a[3] = (exp(((real_t)(2.9999999999999999e-01)*(((((((xd[1]*(real_t)(TR))-((od[1]-xd[1])*(real_t)(TR)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((od[1]*od[1])/(real_t)(1.9620000000000001e+01)))+(real_t)(4.0000000000000000e+00))-(od[0]-xd[0]))/(a[0]+(real_t)(1.0000000000000001e-01)))))); a[4] = (((real_t)(2.9999999999999999e-01)*(((real_t)(0.0000000000000000e+00)-((real_t)(0.0000000000000000e+00)-(real_t)(1.0000000000000000e+00)))*a[2]))*a[3]); a[5] = ((real_t)(1.0000000000000000e+00)/(real_t)(1.9620000000000001e+01)); a[6] = (1.0/sqrt((xd[1]+(real_t)(5.0000000000000000e-01)))); a[7] = (a[6]*(real_t)(5.0000000000000000e-01)); a[8] = (a[2]*a[2]); -a[9] = (((real_t)(2.9999999999999999e-01)*(((((real_t)(1.8000000000000000e+00)-((real_t)(-1.8000000000000000e+00)))+((xd[1]+xd[1])*a[5]))*a[2])-((((((((xd[1]*(real_t)(1.8000000000000000e+00))-((od[1]-xd[1])*(real_t)(1.8000000000000000e+00)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((od[1]*od[1])/(real_t)(1.9620000000000001e+01)))+(real_t)(4.0000000000000000e+00))-(od[0]-xd[0]))*a[7])*a[8])))*a[3]); +a[9] = (((real_t)(2.9999999999999999e-01)*(((((real_t)(TR)-((real_t)(-TR)))+((xd[1]+xd[1])*a[5]))*a[2])-((((((((xd[1]*(real_t)(TR))-((od[1]-xd[1])*(real_t)(TR)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((od[1]*od[1])/(real_t)(1.9620000000000001e+01)))+(real_t)(4.0000000000000000e+00))-(od[0]-xd[0]))*a[7])*a[8])))*a[3]); a[10] = ((real_t)(1.0000000000000000e+00)/(((real_t)(5.0000000000000003e-02)*xd[1])+(real_t)(5.0000000000000000e-01))); a[11] = ((real_t)(1.0000000000000000e+00)/(real_t)(1.9620000000000001e+01)); a[12] = (a[10]*a[10]); /* Compute outputs: */ out[0] = (a[1]-(real_t)(1.0000000000000000e+00)); -out[1] = (((od[0]-xd[0])-((real_t)(4.0000000000000000e+00)+((((xd[1]*(real_t)(1.8000000000000000e+00))-((od[1]-xd[1])*(real_t)(1.8000000000000000e+00)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((od[1]*od[1])/(real_t)(1.9620000000000001e+01)))))/(((real_t)(5.0000000000000003e-02)*xd[1])+(real_t)(5.0000000000000000e-01))); +out[1] = (((od[0]-xd[0])-((real_t)(4.0000000000000000e+00)+((((xd[1]*(real_t)(TR))-((od[1]-xd[1])*(real_t)(TR)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((od[1]*od[1])/(real_t)(1.9620000000000001e+01)))))/(((real_t)(5.0000000000000003e-02)*xd[1])+(real_t)(5.0000000000000000e-01))); out[2] = (xd[2]*(((real_t)(1.0000000000000001e-01)*xd[1])+(real_t)(1.0000000000000000e+00))); out[3] = (u[0]*(((real_t)(1.0000000000000001e-01)*xd[1])+(real_t)(1.0000000000000000e+00))); out[4] = a[4]; out[5] = a[9]; out[6] = (real_t)(0.0000000000000000e+00); out[7] = (((real_t)(0.0000000000000000e+00)-(real_t)(1.0000000000000000e+00))*a[10]); -out[8] = ((((real_t)(0.0000000000000000e+00)-(((real_t)(1.8000000000000000e+00)-((real_t)(-1.8000000000000000e+00)))+((xd[1]+xd[1])*a[11])))*a[10])-((((od[0]-xd[0])-((real_t)(4.0000000000000000e+00)+((((xd[1]*(real_t)(1.8000000000000000e+00))-((od[1]-xd[1])*(real_t)(1.8000000000000000e+00)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((od[1]*od[1])/(real_t)(1.9620000000000001e+01)))))*(real_t)(5.0000000000000003e-02))*a[12])); +out[8] = ((((real_t)(0.0000000000000000e+00)-(((real_t)(TR)-((real_t)(-TR)))+((xd[1]+xd[1])*a[11])))*a[10])-((((od[0]-xd[0])-((real_t)(4.0000000000000000e+00)+((((xd[1]*(real_t)(TR))-((od[1]-xd[1])*(real_t)(TR)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((od[1]*od[1])/(real_t)(1.9620000000000001e+01)))))*(real_t)(5.0000000000000003e-02))*a[12])); out[9] = (real_t)(0.0000000000000000e+00); out[10] = (real_t)(0.0000000000000000e+00); out[11] = (xd[2]*(real_t)(1.0000000000000001e-01)); @@ -114,7 +114,7 @@ out[18] = (real_t)(0.0000000000000000e+00); out[19] = (((real_t)(1.0000000000000001e-01)*xd[1])+(real_t)(1.0000000000000000e+00)); } -void acado_evaluateLSQEndTerm(const real_t* in, real_t* out) +void acado_evaluateLSQEndTerm(const real_t* in, real_t* out, double TR) { const real_t* xd = in; const real_t* od = in + 3; @@ -123,28 +123,28 @@ real_t* a = acadoWorkspace.objAuxVar; /* Compute intermediate quantities: */ a[0] = (sqrt((xd[1]+(real_t)(5.0000000000000000e-01)))); -a[1] = (exp(((real_t)(2.9999999999999999e-01)*(((((((xd[1]*(real_t)(1.8000000000000000e+00))-((od[1]-xd[1])*(real_t)(1.8000000000000000e+00)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((od[1]*od[1])/(real_t)(1.9620000000000001e+01)))+(real_t)(4.0000000000000000e+00))-(od[0]-xd[0]))/(a[0]+(real_t)(1.0000000000000001e-01)))))); +a[1] = (exp(((real_t)(2.9999999999999999e-01)*(((((((xd[1]*(real_t)(TR))-((od[1]-xd[1])*(real_t)(TR)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((od[1]*od[1])/(real_t)(1.9620000000000001e+01)))+(real_t)(4.0000000000000000e+00))-(od[0]-xd[0]))/(a[0]+(real_t)(1.0000000000000001e-01)))))); a[2] = ((real_t)(1.0000000000000000e+00)/(a[0]+(real_t)(1.0000000000000001e-01))); -a[3] = (exp(((real_t)(2.9999999999999999e-01)*(((((((xd[1]*(real_t)(1.8000000000000000e+00))-((od[1]-xd[1])*(real_t)(1.8000000000000000e+00)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((od[1]*od[1])/(real_t)(1.9620000000000001e+01)))+(real_t)(4.0000000000000000e+00))-(od[0]-xd[0]))/(a[0]+(real_t)(1.0000000000000001e-01)))))); +a[3] = (exp(((real_t)(2.9999999999999999e-01)*(((((((xd[1]*(real_t)(TR))-((od[1]-xd[1])*(real_t)(TR)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((od[1]*od[1])/(real_t)(1.9620000000000001e+01)))+(real_t)(4.0000000000000000e+00))-(od[0]-xd[0]))/(a[0]+(real_t)(1.0000000000000001e-01)))))); a[4] = (((real_t)(2.9999999999999999e-01)*(((real_t)(0.0000000000000000e+00)-((real_t)(0.0000000000000000e+00)-(real_t)(1.0000000000000000e+00)))*a[2]))*a[3]); a[5] = ((real_t)(1.0000000000000000e+00)/(real_t)(1.9620000000000001e+01)); a[6] = (1.0/sqrt((xd[1]+(real_t)(5.0000000000000000e-01)))); a[7] = (a[6]*(real_t)(5.0000000000000000e-01)); a[8] = (a[2]*a[2]); -a[9] = (((real_t)(2.9999999999999999e-01)*(((((real_t)(1.8000000000000000e+00)-((real_t)(-1.8000000000000000e+00)))+((xd[1]+xd[1])*a[5]))*a[2])-((((((((xd[1]*(real_t)(1.8000000000000000e+00))-((od[1]-xd[1])*(real_t)(1.8000000000000000e+00)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((od[1]*od[1])/(real_t)(1.9620000000000001e+01)))+(real_t)(4.0000000000000000e+00))-(od[0]-xd[0]))*a[7])*a[8])))*a[3]); +a[9] = (((real_t)(2.9999999999999999e-01)*(((((real_t)(TR)-((real_t)(-TR)))+((xd[1]+xd[1])*a[5]))*a[2])-((((((((xd[1]*(real_t)(TR))-((od[1]-xd[1])*(real_t)(TR)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((od[1]*od[1])/(real_t)(1.9620000000000001e+01)))+(real_t)(4.0000000000000000e+00))-(od[0]-xd[0]))*a[7])*a[8])))*a[3]); a[10] = ((real_t)(1.0000000000000000e+00)/(((real_t)(5.0000000000000003e-02)*xd[1])+(real_t)(5.0000000000000000e-01))); a[11] = ((real_t)(1.0000000000000000e+00)/(real_t)(1.9620000000000001e+01)); a[12] = (a[10]*a[10]); /* Compute outputs: */ out[0] = (a[1]-(real_t)(1.0000000000000000e+00)); -out[1] = (((od[0]-xd[0])-((real_t)(4.0000000000000000e+00)+((((xd[1]*(real_t)(1.8000000000000000e+00))-((od[1]-xd[1])*(real_t)(1.8000000000000000e+00)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((od[1]*od[1])/(real_t)(1.9620000000000001e+01)))))/(((real_t)(5.0000000000000003e-02)*xd[1])+(real_t)(5.0000000000000000e-01))); +out[1] = (((od[0]-xd[0])-((real_t)(4.0000000000000000e+00)+((((xd[1]*(real_t)(TR))-((od[1]-xd[1])*(real_t)(TR)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((od[1]*od[1])/(real_t)(1.9620000000000001e+01)))))/(((real_t)(5.0000000000000003e-02)*xd[1])+(real_t)(5.0000000000000000e-01))); out[2] = (xd[2]*(((real_t)(1.0000000000000001e-01)*xd[1])+(real_t)(1.0000000000000000e+00))); out[3] = a[4]; out[4] = a[9]; out[5] = (real_t)(0.0000000000000000e+00); out[6] = (((real_t)(0.0000000000000000e+00)-(real_t)(1.0000000000000000e+00))*a[10]); -out[7] = ((((real_t)(0.0000000000000000e+00)-(((real_t)(1.8000000000000000e+00)-((real_t)(-1.8000000000000000e+00)))+((xd[1]+xd[1])*a[11])))*a[10])-((((od[0]-xd[0])-((real_t)(4.0000000000000000e+00)+((((xd[1]*(real_t)(1.8000000000000000e+00))-((od[1]-xd[1])*(real_t)(1.8000000000000000e+00)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((od[1]*od[1])/(real_t)(1.9620000000000001e+01)))))*(real_t)(5.0000000000000003e-02))*a[12])); +out[7] = ((((real_t)(0.0000000000000000e+00)-(((real_t)(TR)-((real_t)(-TR)))+((xd[1]+xd[1])*a[11])))*a[10])-((((od[0]-xd[0])-((real_t)(4.0000000000000000e+00)+((((xd[1]*(real_t)(TR))-((od[1]-xd[1])*(real_t)(TR)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((od[1]*od[1])/(real_t)(1.9620000000000001e+01)))))*(real_t)(5.0000000000000003e-02))*a[12])); out[8] = (real_t)(0.0000000000000000e+00); out[9] = (real_t)(0.0000000000000000e+00); out[10] = (xd[2]*(real_t)(1.0000000000000001e-01)); @@ -207,7 +207,7 @@ tmpQN1[7] = + tmpQN2[6]*tmpFx[1] + tmpQN2[7]*tmpFx[4] + tmpQN2[8]*tmpFx[7]; tmpQN1[8] = + tmpQN2[6]*tmpFx[2] + tmpQN2[7]*tmpFx[5] + tmpQN2[8]*tmpFx[8]; } -void acado_evaluateObjective( ) +void acado_evaluateObjective( double TR ) { int runObj; for (runObj = 0; runObj < 20; ++runObj) @@ -219,7 +219,7 @@ acadoWorkspace.objValueIn[3] = acadoVariables.u[runObj]; acadoWorkspace.objValueIn[4] = acadoVariables.od[runObj * 2]; acadoWorkspace.objValueIn[5] = acadoVariables.od[runObj * 2 + 1]; -acado_evaluateLSQ( acadoWorkspace.objValueIn, acadoWorkspace.objValueOut ); +acado_evaluateLSQ( acadoWorkspace.objValueIn, acadoWorkspace.objValueOut, TR ); acadoWorkspace.Dy[runObj * 4] = acadoWorkspace.objValueOut[0]; acadoWorkspace.Dy[runObj * 4 + 1] = acadoWorkspace.objValueOut[1]; acadoWorkspace.Dy[runObj * 4 + 2] = acadoWorkspace.objValueOut[2]; @@ -235,7 +235,7 @@ acadoWorkspace.objValueIn[1] = acadoVariables.x[61]; acadoWorkspace.objValueIn[2] = acadoVariables.x[62]; acadoWorkspace.objValueIn[3] = acadoVariables.od[40]; acadoWorkspace.objValueIn[4] = acadoVariables.od[41]; -acado_evaluateLSQEndTerm( acadoWorkspace.objValueIn, acadoWorkspace.objValueOut ); +acado_evaluateLSQEndTerm( acadoWorkspace.objValueIn, acadoWorkspace.objValueOut, TR ); acadoWorkspace.DyN[0] = acadoWorkspace.objValueOut[0]; acadoWorkspace.DyN[1] = acadoWorkspace.objValueOut[1]; @@ -412,7 +412,7 @@ int lRun3; int lRun4; int lRun5; /** Row vector of size: 20 */ -static const int xBoundIndices[ 20 ] = +static const int xBoundIndices[ 20 ] = { 4, 7, 10, 13, 16, 19, 22, 25, 28, 31, 34, 37, 40, 43, 46, 49, 52, 55, 58, 61 }; acado_moveGuE( acadoWorkspace.evGu, acadoWorkspace.E ); acado_moveGxT( &(acadoWorkspace.evGx[ 9 ]), acadoWorkspace.T ); @@ -4589,12 +4589,12 @@ acado_multEDu( &(acadoWorkspace.E[ 624 ]), &(acadoWorkspace.x[ 21 ]), &(acadoVar acado_multEDu( &(acadoWorkspace.E[ 627 ]), &(acadoWorkspace.x[ 22 ]), &(acadoVariables.x[ 60 ]) ); } -int acado_preparationStep( ) +int acado_preparationStep( double TR ) { int ret; ret = acado_modelSimulation(); -acado_evaluateObjective( ); +acado_evaluateObjective( TR ); acado_condensePrep( ); return ret; } @@ -4660,7 +4660,7 @@ acadoVariables.x[60] = xEnd[0]; acadoVariables.x[61] = xEnd[1]; acadoVariables.x[62] = xEnd[2]; } -else if (strategy == 2) +else if (strategy == 2) { acadoWorkspace.state[0] = acadoVariables.x[60]; acadoWorkspace.state[1] = acadoVariables.x[61]; @@ -4726,7 +4726,7 @@ kkt += fabs(acadoWorkspace.ubA[index] * prd); return kkt; } -real_t acado_getObjective( ) +real_t acado_getObjective( TR ) { real_t objVal; @@ -4746,7 +4746,7 @@ acadoWorkspace.objValueIn[3] = acadoVariables.u[lRun1]; acadoWorkspace.objValueIn[4] = acadoVariables.od[lRun1 * 2]; acadoWorkspace.objValueIn[5] = acadoVariables.od[lRun1 * 2 + 1]; -acado_evaluateLSQ( acadoWorkspace.objValueIn, acadoWorkspace.objValueOut ); +acado_evaluateLSQ( acadoWorkspace.objValueIn, acadoWorkspace.objValueOut, TR ); acadoWorkspace.Dy[lRun1 * 4] = acadoWorkspace.objValueOut[0] - acadoVariables.y[lRun1 * 4]; acadoWorkspace.Dy[lRun1 * 4 + 1] = acadoWorkspace.objValueOut[1] - acadoVariables.y[lRun1 * 4 + 1]; acadoWorkspace.Dy[lRun1 * 4 + 2] = acadoWorkspace.objValueOut[2] - acadoVariables.y[lRun1 * 4 + 2]; @@ -4757,7 +4757,7 @@ acadoWorkspace.objValueIn[1] = acadoVariables.x[61]; acadoWorkspace.objValueIn[2] = acadoVariables.x[62]; acadoWorkspace.objValueIn[3] = acadoVariables.od[40]; acadoWorkspace.objValueIn[4] = acadoVariables.od[41]; -acado_evaluateLSQEndTerm( acadoWorkspace.objValueIn, acadoWorkspace.objValueOut ); +acado_evaluateLSQEndTerm( acadoWorkspace.objValueIn, acadoWorkspace.objValueOut, TR ); acadoWorkspace.DyN[0] = acadoWorkspace.objValueOut[0] - acadoVariables.yN[0]; acadoWorkspace.DyN[1] = acadoWorkspace.objValueOut[1] - acadoVariables.yN[1]; acadoWorkspace.DyN[2] = acadoWorkspace.objValueOut[2] - acadoVariables.yN[2]; @@ -4779,4 +4779,3 @@ objVal += + acadoWorkspace.DyN[0]*tmpDyN[0] + acadoWorkspace.DyN[1]*tmpDyN[1] + objVal *= 0.5; return objVal; } - diff --git a/selfdrive/controls/lib/longitudinal_mpc/libmpc_py.py b/selfdrive/controls/lib/longitudinal_mpc/libmpc_py.py index b43773593ffaf0..a1e7be2ea276d0 100644 --- a/selfdrive/controls/lib/longitudinal_mpc/libmpc_py.py +++ b/selfdrive/controls/lib/longitudinal_mpc/libmpc_py.py @@ -28,8 +28,9 @@ def _get_libmpc(mpc_id): void init(double ttcCost, double distanceCost, double accelerationCost, double jerkCost); void init_with_simulation(double v_ego, double x_l, double v_l, double a_l, double l); + void change_tr(double ttcCost, double distanceCost, double accelerationCost, double jerkCost); int run_mpc(state_t * x0, log_t * solution, - double l, double a_l_0); + double l, double a_l_0, double TR); """) return (ffi, ffi.dlopen(libmpc_fn)) @@ -37,4 +38,4 @@ def _get_libmpc(mpc_id): mpcs = [_get_libmpc(1), _get_libmpc(2)] def get_libmpc(mpc_id): - return mpcs[mpc_id - 1] + return mpcs[mpc_id - 1] \ No newline at end of file diff --git a/selfdrive/controls/lib/longitudinal_mpc/longitudinal_mpc.c b/selfdrive/controls/lib/longitudinal_mpc/longitudinal_mpc.c index d4bfee8c8aac0d..f0c6cca85b6fe4 100644 --- a/selfdrive/controls/lib/longitudinal_mpc/longitudinal_mpc.c +++ b/selfdrive/controls/lib/longitudinal_mpc/longitudinal_mpc.c @@ -68,6 +68,25 @@ void init(double ttcCost, double distanceCost, double accelerationCost, double j } +void change_tr(double ttcCost, double distanceCost, double accelerationCost, double jerkCost){ + int i; + const int STEP_MULTIPLIER = 3; + + for (i = 0; i < N; i++) { + int f = 1; + if (i > 4){ + f = STEP_MULTIPLIER; + } + acadoVariables.W[16 * i + 0] = ttcCost * f; // exponential cost for time-to-collision (ttc) + acadoVariables.W[16 * i + 5] = distanceCost * f; // desired distance + acadoVariables.W[16 * i + 10] = accelerationCost * f; // acceleration + acadoVariables.W[16 * i + 15] = jerkCost * f; // jerk + } + acadoVariables.WN[0] = ttcCost * STEP_MULTIPLIER; // exponential cost for danger zone + acadoVariables.WN[4] = distanceCost * STEP_MULTIPLIER; // desired distance + acadoVariables.WN[8] = accelerationCost * STEP_MULTIPLIER; // acceleration +} + void init_with_simulation(double v_ego, double x_l_0, double v_l_0, double a_l_0, double l){ int i; @@ -112,7 +131,7 @@ void init_with_simulation(double v_ego, double x_l_0, double v_l_0, double a_l_0 for (i = 0; i < NYN; ++i) acadoVariables.yN[ i ] = 0.0; } -int run_mpc(state_t * x0, log_t * solution, double l, double a_l_0){ +int run_mpc(state_t * x0, log_t * solution, double l, double a_l_0, double TR){ // Calculate lead vehicle predictions int i; double t = 0.; @@ -152,7 +171,7 @@ int run_mpc(state_t * x0, log_t * solution, double l, double a_l_0){ acadoVariables.x[1] = acadoVariables.x0[1] = x0->v_ego; acadoVariables.x[2] = acadoVariables.x0[2] = x0->a_ego; - acado_preparationStep(); + acado_preparationStep(TR); acado_feedbackStep(); for (i = 0; i <= N; i++){ @@ -164,10 +183,10 @@ int run_mpc(state_t * x0, log_t * solution, double l, double a_l_0){ solution->j_ego[i] = acadoVariables.u[i]; } } - solution->cost = acado_getObjective(); + solution->cost = acado_getObjective(TR); // Dont shift states here. Current solution is closer to next timestep than if // we shift by 0.2 seconds. return acado_getNWSR(); -} +} \ No newline at end of file From d6733ddc917153ee89241a73f955cf66d0be3fc9 Mon Sep 17 00:00:00 2001 From: Shane Date: Mon, 16 Dec 2019 20:41:42 -0600 Subject: [PATCH 08/30] don't disengage if door is opened or seatbelt is unlatched, but don't allow engagement --- selfdrive/car/toyota/interface.py | 9 +++++++-- 1 file changed, 7 insertions(+), 2 deletions(-) diff --git a/selfdrive/car/toyota/interface.py b/selfdrive/car/toyota/interface.py index 865fc7cb674feb..7b742cdf467a1c 100755 --- a/selfdrive/car/toyota/interface.py +++ b/selfdrive/car/toyota/interface.py @@ -373,13 +373,18 @@ def update(self, c, can_strings): # events events = [] + if ret.cruiseState.enabled and not self.cruise_enabled_prev: # this lets us modularize which alerts we want to disable if op is engaged + can_disengage = True + else: + can_disengage = False + if self.cp_cam.can_invalid_cnt >= 200 and self.CP.enableCamera: events.append(create_event('invalidGiraffeToyota', [ET.PERMANENT])) if not ret.gearShifter == GearShifter.drive and self.CP.openpilotLongitudinalControl: events.append(create_event('wrongGear', [ET.NO_ENTRY, ET.SOFT_DISABLE])) - if ret.doorOpen: + if ret.doorOpen and can_disengage: events.append(create_event('doorOpen', [ET.NO_ENTRY, ET.SOFT_DISABLE])) - if ret.seatbeltUnlatched: + if ret.seatbeltUnlatched and can_disengage: events.append(create_event('seatbeltNotLatched', [ET.NO_ENTRY, ET.SOFT_DISABLE])) if self.CS.esp_disabled and self.CP.openpilotLongitudinalControl: events.append(create_event('espDisabled', [ET.NO_ENTRY, ET.SOFT_DISABLE])) From 7253374d666e32a718f0003986c9ef990f5c6de9 Mon Sep 17 00:00:00 2001 From: Shane Date: Mon, 16 Dec 2019 20:47:21 -0600 Subject: [PATCH 09/30] fix for steering unavailable with high steering rate --- selfdrive/car/toyota/carcontroller.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py index 0a8f7979d498e7..78f63cac811f15 100644 --- a/selfdrive/car/toyota/carcontroller.py +++ b/selfdrive/car/toyota/carcontroller.py @@ -136,7 +136,7 @@ def update(self, enabled, CS, frame, actuators, pcm_cancel_cmd, hud_alert, self.last_fault_frame = frame # Cut steering for 2s after fault - if not enabled or (frame - self.last_fault_frame < 200): + if not enabled or (frame - self.last_fault_frame < 200) or abs(CS.steeringRate) >= 100: apply_steer = 0 apply_steer_req = 0 else: From 2101ef0e95e018ab3cceba59cc69868654979189 Mon Sep 17 00:00:00 2001 From: Shane Date: Mon, 16 Dec 2019 21:31:24 -0600 Subject: [PATCH 10/30] add dynamic follow with new tuning! --- selfdrive/controls/lib/long_mpc.py | 106 ++++++++++++++++++++++++++++- 1 file changed, 104 insertions(+), 2 deletions(-) diff --git a/selfdrive/controls/lib/long_mpc.py b/selfdrive/controls/lib/long_mpc.py index 332891dca118e0..92b406f0b7f180 100644 --- a/selfdrive/controls/lib/long_mpc.py +++ b/selfdrive/controls/lib/long_mpc.py @@ -1,5 +1,6 @@ import os import math +import time import cereal.messaging as messaging from selfdrive.swaglog import cloudlog @@ -7,6 +8,9 @@ from selfdrive.controls.lib.radar_helpers import _LEAD_ACCEL_TAU from selfdrive.controls.lib.longitudinal_mpc import libmpc_py from selfdrive.controls.lib.drive_helpers import MPC_COST_LONG +from common.op_params import opParams +from common.numpy_fast import interp, clip +from common.travis_checker import travis LOG_MPC = os.environ.get('LOG_MPC', False) @@ -14,6 +18,7 @@ class LongitudinalMpc(): def __init__(self, mpc_id): self.mpc_id = mpc_id + self.MPH_TO_MS = 0.44704 self.setup_mpc() self.v_mpc = 0.0 @@ -23,9 +28,16 @@ def __init__(self, mpc_id): self.prev_lead_status = False self.prev_lead_x = 0.0 self.new_lead = False - self.last_cloudlog_t = 0.0 + self.op_params = opParams() + self.CS = None + self.lead_data = {'v_lead': None, 'x_lead': None, 'a_lead': None, 'status': False} + self.past_v_ego = 0.0 + self.df_data = {"v_leads": [], "v_egos": []} # dynamic follow data + self.last_cost = 0.0 + self.customTR = self.op_params.get('following_distance', None) + def send_mpc_solution(self, pm, qp_iterations, calculation_time): qp_iterations = max(0, qp_iterations) dat = messaging.new_message() @@ -57,8 +69,95 @@ def set_cur_state(self, v, a): self.cur_state[0].v_ego = v self.cur_state[0].a_ego = a + def get_TR(self): + if not self.lead_data['status'] or travis: + TR = 1.8 + elif self.customTR is not None: + TR = clip(self.customTR, 0.9, 2.7) + else: + self.store_lead_data() + TR = self.dynamic_follow() + + self.change_cost(TR) + return TR + + def change_cost(self, TR): + TRs = [0.9, 1.8, 2.7] + costs = [1.0, 0.1, 0.05] + cost = interp(TR, TRs, costs) + if self.last_cost != cost: + self.libmpc.change_tr(MPC_COST_LONG.TTC, cost, MPC_COST_LONG.ACCELERATION, MPC_COST_LONG.JERK) + self.last_cost = cost + + def store_lead_data(self): + v_lead_retention = 2.0 # seconds + v_ego_retention = 1.5 + + if self.lead_data['status']: + self.df_data['v_leads'] = [sample for sample in self.df_data['v_leads'] if + time.time() - sample['time'] <= v_lead_retention + and not self.new_lead] # reset when new lead + self.df_data['v_leads'].append({'v_lead': self.lead_data['v_lead'], 'time': time.time()}) + + self.df_data['v_egos'] = [sample for sample in self.df_data['v_egos'] if time.time() - sample['time'] <= v_ego_retention] + self.df_data['v_egos'].append({'v_ego': self.CS.vEgo, 'time': time.time()}) + + def accel_over_time(self): + min_consider_time = 1.5 + if len(self.df_data['v_leads']) > 0: + elapsed = self.df_data['v_leads'][-1]['time'] - self.df_data['v_leads'][0]['time'] + if elapsed > min_consider_time: + v_diff = self.df_data['v_leads'][-1]['v_lead'] - self.df_data['v_leads'][0]['v_lead'] + to_return = v_diff / elapsed + if abs(self.CS.aEgo) > abs(to_return): + return self.CS.aEgo + else: + return to_return + return 0 + + def dynamic_follow(self): + x_vel = [0.0, 1.8627, 3.7253, 5.588, 7.4507, 9.3133, 11.5598, 13.645, 22.352, 31.2928, 33.528, 35.7632, 40.2336] # velocities + y_mod = [1.102, 1.12, 1.14, 1.168, 1.21, 1.273, 1.36, 1.411, 1.543, 1.62, 1.664, 1.736, 1.853] # TRs + + sng_TR = 1.7 # stop and go parameters + sng_speed = 15.0 * self.MPH_TO_MS + + if self.CS.vEgo >= sng_speed or self.df_data['v_egos'][-1]['v_ego'] >= self.CS.vEgo: # if above 15 mph OR we're decelerating to a stop, keep shorter TR. when we reaccelerate, use 1.8s and slowly decrease + TR = interp(self.CS.vEgo, x_vel, y_mod) + else: # this allows us to get closer to the lead car when stopping, while being able to have smooth stop and go when reaccelerating + x = [sng_speed / 3.0, sng_speed] # decrease TR between 5 and 15 mph from 1.8s to defined TR above at 15mph while accelerating + y = [sng_TR, interp(sng_speed, x_vel, y_mod)] + TR = interp(self.CS.vEgo, x, y) + + # Dynamic follow modifications (the secret sauce) + x = [-15.6464, -9.8422, -6.0, -4.0, -2.68, -2.3, -1.8, -1.26, -0.61, 0, 0.61, 1.26, 2.1, 2.68] # relative velocity values + y = [.504, 0.34, 0.29, 0.25, 0.22, 0.19, 0.13, 0.053, 0.017, 0, -0.015, -0.042, -0.108, -0.163] # modification values # todo: test if these modifications are too much + TR_mod = interp(self.lead_data['v_lead'] - self.CS.vEgo, x, y) + + x = [-2.235, -1.49, -1.1, -0.67, -0.224, 0.0, 0.67, 1.1, 1.49] # lead acceleration values + y = [0.26, 0.182, 0.104, 0.06, 0.039, 0.0, -0.016, -0.032, -0.056] # modification values + # TR_mod += interp(self.accel_over_time(), x, y) # todo: test if these modifications are too much + + TR += TR_mod + + if self.CS.leftBlinker or self.CS.rightBlinker: + x = [8.9408, 22.352, 31.2928] # 20, 50, 70 mph + y = [1.0, .7, .65] # reduce TR when changing lanes + TR *= interp(self.CS.vEgo, x, y) + + # TR *= self.get_traffic_level() # modify TR based on last minute of traffic data # todo: look at getting this to work, a model could be used + + return clip(round(TR, 3), 0.9, 2.7) + + def process_lead(self, v_lead, a_lead, x_lead, status): + self.lead_data['v_lead'] = v_lead + self.lead_data['a_lead'] = a_lead + self.lead_data['x_lead'] = x_lead + self.lead_data['status'] = status + def update(self, pm, CS, lead, v_cruise_setpoint): v_ego = CS.vEgo + self.CS = CS # Setup current mpc state self.cur_state[0].x_ego = 0.0 @@ -68,10 +167,12 @@ def update(self, pm, CS, lead, v_cruise_setpoint): v_lead = max(0.0, lead.vLead) a_lead = lead.aLeadK - if (v_lead < 0.1 or -a_lead / 2.0 > v_lead): + if v_lead < 0.1 or -a_lead / 2.0 > v_lead: v_lead = 0.0 a_lead = 0.0 + self.process_lead(v_lead, a_lead, x_lead, lead.status) + self.a_lead_tau = lead.aLeadTau self.new_lead = False if not self.prev_lead_status or abs(x_lead - self.prev_lead_x) > 2.5: @@ -83,6 +184,7 @@ def update(self, pm, CS, lead, v_cruise_setpoint): self.cur_state[0].x_l = x_lead self.cur_state[0].v_l = v_lead else: + self.process_lead(None, None, None, False) self.prev_lead_status = False # Fake a fast lead car, so mpc keeps running self.cur_state[0].x_l = 50.0 From 398f9dada26278c7c0b34c503958ad3f0a8c2ba3 Mon Sep 17 00:00:00 2001 From: Shane Date: Mon, 16 Dec 2019 21:34:57 -0600 Subject: [PATCH 11/30] check for travis --- selfdrive/car/toyota/interface.py | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/selfdrive/car/toyota/interface.py b/selfdrive/car/toyota/interface.py index 7b742cdf467a1c..6704d0261eedb4 100755 --- a/selfdrive/car/toyota/interface.py +++ b/selfdrive/car/toyota/interface.py @@ -8,6 +8,7 @@ from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness, is_ecu_disconnected, gen_empty_fingerprint from selfdrive.swaglog import cloudlog from selfdrive.car.interfaces import CarInterfaceBase +from common.travis_checker import travis ButtonType = car.CarState.ButtonEvent.Type GearShifter = car.CarState.GearShifter @@ -373,7 +374,7 @@ def update(self, c, can_strings): # events events = [] - if ret.cruiseState.enabled and not self.cruise_enabled_prev: # this lets us modularize which alerts we want to disable if op is engaged + if (ret.cruiseState.enabled and not self.cruise_enabled_prev) or travis: # this lets us modularize which alerts we want to disable if op is engaged can_disengage = True else: can_disengage = False From 99a14394ac5a844fb4ce9ef622779894b2e48526 Mon Sep 17 00:00:00 2001 From: Shane Date: Mon, 16 Dec 2019 21:36:29 -0600 Subject: [PATCH 12/30] it's not steeringRate --- selfdrive/car/toyota/carcontroller.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py index 78f63cac811f15..48a7fbbd011e75 100644 --- a/selfdrive/car/toyota/carcontroller.py +++ b/selfdrive/car/toyota/carcontroller.py @@ -136,7 +136,7 @@ def update(self, enabled, CS, frame, actuators, pcm_cancel_cmd, hud_alert, self.last_fault_frame = frame # Cut steering for 2s after fault - if not enabled or (frame - self.last_fault_frame < 200) or abs(CS.steeringRate) >= 100: + if not enabled or (frame - self.last_fault_frame < 200) or abs(CS.angle_steers_rate) >= 100: apply_steer = 0 apply_steer_req = 0 else: From 1bda32e14a601a0c2725db34443c2bf8504e2712 Mon Sep 17 00:00:00 2001 From: Shane Date: Mon, 16 Dec 2019 21:40:04 -0600 Subject: [PATCH 13/30] is a travis check required here? --- selfdrive/car/toyota/carcontroller.py | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py index 48a7fbbd011e75..573b5516048087 100644 --- a/selfdrive/car/toyota/carcontroller.py +++ b/selfdrive/car/toyota/carcontroller.py @@ -6,6 +6,7 @@ create_acc_cancel_command, create_fcw_command from selfdrive.car.toyota.values import CAR, ECU, STATIC_MSGS, SteerLimitParams from opendbc.can.packer import CANPacker +from common.travis_checker import travis VisualAlert = car.CarControl.HUDControl.VisualAlert @@ -136,7 +137,7 @@ def update(self, enabled, CS, frame, actuators, pcm_cancel_cmd, hud_alert, self.last_fault_frame = frame # Cut steering for 2s after fault - if not enabled or (frame - self.last_fault_frame < 200) or abs(CS.angle_steers_rate) >= 100: + if not enabled or (frame - self.last_fault_frame < 200) or (abs(CS.angle_steers_rate) >= 100 and not travis): apply_steer = 0 apply_steer_req = 0 else: From 045592860d0bfd63751bd60ba484a395acae1f9b Mon Sep 17 00:00:00 2001 From: Shane Date: Mon, 16 Dec 2019 22:04:22 -0600 Subject: [PATCH 14/30] update relative velocity tuning values! --- selfdrive/controls/lib/long_mpc.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/selfdrive/controls/lib/long_mpc.py b/selfdrive/controls/lib/long_mpc.py index 92b406f0b7f180..1bda80d94c33e0 100644 --- a/selfdrive/controls/lib/long_mpc.py +++ b/selfdrive/controls/lib/long_mpc.py @@ -130,8 +130,8 @@ def dynamic_follow(self): TR = interp(self.CS.vEgo, x, y) # Dynamic follow modifications (the secret sauce) - x = [-15.6464, -9.8422, -6.0, -4.0, -2.68, -2.3, -1.8, -1.26, -0.61, 0, 0.61, 1.26, 2.1, 2.68] # relative velocity values - y = [.504, 0.34, 0.29, 0.25, 0.22, 0.19, 0.13, 0.053, 0.017, 0, -0.015, -0.042, -0.108, -0.163] # modification values # todo: test if these modifications are too much + x = [-20, -15.655, -11.1702, -7.8235, -4.6665, -2.5663, -1.1843, 0, 1.0107, 1.89, 2.6909] # relative velocity values + y = [0.65, 0.525, 0.44, 0.341, 0.26, 0.159, 0.049, 0, -0.082, -0.18, -0.28] # modification values TR_mod = interp(self.lead_data['v_lead'] - self.CS.vEgo, x, y) x = [-2.235, -1.49, -1.1, -0.67, -0.224, 0.0, 0.67, 1.1, 1.49] # lead acceleration values From 718697da8e50ccb3a32a0f18ff5fd689bf395980 Mon Sep 17 00:00:00 2001 From: Shane Date: Mon, 16 Dec 2019 22:05:36 -0600 Subject: [PATCH 15/30] remove toyota tssp2 check for now --- selfdrive/test/process_replay/test_processes.py | 1 + 1 file changed, 1 insertion(+) diff --git a/selfdrive/test/process_replay/test_processes.py b/selfdrive/test/process_replay/test_processes.py index 6832aba33cca9c..8af87cf0bb7602 100755 --- a/selfdrive/test/process_replay/test_processes.py +++ b/selfdrive/test/process_replay/test_processes.py @@ -17,6 +17,7 @@ "b6e1317e1bfbefa6|2019-07-06--04-05-26--5", # CHRYSLER.JEEP_CHEROKEE "7873afaf022d36e2|2019-07-03--18-46-44--0", # SUBARU.IMPREZA ] +segments.remove("cce908f7eb8db67d|2019-08-02--15-09-51--3") def get_segment(segment_name): route_name, segment_num = segment_name.rsplit("--", 1) From 6e39e3d584397b277f2831d0564dbe6015479b0b Mon Sep 17 00:00:00 2001 From: Shane Date: Mon, 16 Dec 2019 22:45:07 -0600 Subject: [PATCH 16/30] can add back toyota check now. also more tuning for dynamic follow --- selfdrive/controls/lib/long_mpc.py | 62 ++++++++++++------- .../test/process_replay/test_processes.py | 2 +- 2 files changed, 41 insertions(+), 23 deletions(-) diff --git a/selfdrive/controls/lib/long_mpc.py b/selfdrive/controls/lib/long_mpc.py index 1bda80d94c33e0..3636ac24c046c7 100644 --- a/selfdrive/controls/lib/long_mpc.py +++ b/selfdrive/controls/lib/long_mpc.py @@ -19,6 +19,7 @@ class LongitudinalMpc(): def __init__(self, mpc_id): self.mpc_id = mpc_id self.MPH_TO_MS = 0.44704 + self.op_params = opParams() self.setup_mpc() self.v_mpc = 0.0 @@ -30,10 +31,9 @@ def __init__(self, mpc_id): self.new_lead = False self.last_cloudlog_t = 0.0 - self.op_params = opParams() self.CS = None + self.car_data = {'v_ego': 0.0, 'a_ego': 0.0} self.lead_data = {'v_lead': None, 'x_lead': None, 'a_lead': None, 'status': False} - self.past_v_ego = 0.0 self.df_data = {"v_leads": [], "v_egos": []} # dynamic follow data self.last_cost = 0.0 self.customTR = self.op_params.get('following_distance', None) @@ -100,20 +100,19 @@ def store_lead_data(self): self.df_data['v_leads'].append({'v_lead': self.lead_data['v_lead'], 'time': time.time()}) self.df_data['v_egos'] = [sample for sample in self.df_data['v_egos'] if time.time() - sample['time'] <= v_ego_retention] - self.df_data['v_egos'].append({'v_ego': self.CS.vEgo, 'time': time.time()}) + self.df_data['v_egos'].append({'v_ego': self.car_data['v_ego'], 'time': time.time()}) - def accel_over_time(self): - min_consider_time = 1.5 - if len(self.df_data['v_leads']) > 0: + def lead_accel_over_time(self): + min_consider_time = 1.0 # minimum amount of time required to consider calculation + a_lead = self.lead_data['a_lead'] + if len(self.df_data['v_leads']): # if not empty elapsed = self.df_data['v_leads'][-1]['time'] - self.df_data['v_leads'][0]['time'] - if elapsed > min_consider_time: + if elapsed > min_consider_time: # if greater than min time (not 0) v_diff = self.df_data['v_leads'][-1]['v_lead'] - self.df_data['v_leads'][0]['v_lead'] - to_return = v_diff / elapsed - if abs(self.CS.aEgo) > abs(to_return): - return self.CS.aEgo - else: - return to_return - return 0 + calculated_accel = v_diff / elapsed + if abs(calculated_accel) > abs(a_lead): # if a_lead is greater than calculated accel (over last 1.5s, use that) + a_lead = calculated_accel + return a_lead # if above doesn't execute, we'll return a_lead from radar def dynamic_follow(self): x_vel = [0.0, 1.8627, 3.7253, 5.588, 7.4507, 9.3133, 11.5598, 13.645, 22.352, 31.2928, 33.528, 35.7632, 40.2336] # velocities @@ -122,28 +121,31 @@ def dynamic_follow(self): sng_TR = 1.7 # stop and go parameters sng_speed = 15.0 * self.MPH_TO_MS - if self.CS.vEgo >= sng_speed or self.df_data['v_egos'][-1]['v_ego'] >= self.CS.vEgo: # if above 15 mph OR we're decelerating to a stop, keep shorter TR. when we reaccelerate, use 1.8s and slowly decrease - TR = interp(self.CS.vEgo, x_vel, y_mod) + if self.car_data['v_ego'] >= sng_speed or self.df_data['v_egos'][-1]['v_ego'] >= self.car_data['v_ego']: # if above 15 mph OR we're decelerating to a stop, keep shorter TR. when we reaccelerate, use 1.8s and slowly decrease + TR = interp(self.car_data['v_ego'], x_vel, y_mod) else: # this allows us to get closer to the lead car when stopping, while being able to have smooth stop and go when reaccelerating x = [sng_speed / 3.0, sng_speed] # decrease TR between 5 and 15 mph from 1.8s to defined TR above at 15mph while accelerating y = [sng_TR, interp(sng_speed, x_vel, y_mod)] - TR = interp(self.CS.vEgo, x, y) + TR = interp(self.car_data['v_ego'], x, y) # Dynamic follow modifications (the secret sauce) x = [-20, -15.655, -11.1702, -7.8235, -4.6665, -2.5663, -1.1843, 0, 1.0107, 1.89, 2.6909] # relative velocity values y = [0.65, 0.525, 0.44, 0.341, 0.26, 0.159, 0.049, 0, -0.082, -0.18, -0.28] # modification values - TR_mod = interp(self.lead_data['v_lead'] - self.CS.vEgo, x, y) + TR_mod = interp(self.lead_data['v_lead'] - self.car_data['v_ego'], x, y) - x = [-2.235, -1.49, -1.1, -0.67, -0.224, 0.0, 0.67, 1.1, 1.49] # lead acceleration values - y = [0.26, 0.182, 0.104, 0.06, 0.039, 0.0, -0.016, -0.032, -0.056] # modification values - # TR_mod += interp(self.accel_over_time(), x, y) # todo: test if these modifications are too much + x = [-4.4704, -1.77, -0.3145, 0, 0.446, 1.3411] # lead acceleration values + y = [0.237, 0.12, 0.027, 0, -0.105, -0.195] # modification values + TR_mod += interp(self.lead_accel_over_time(), x, y) # todo: test if these modifications are too much TR += TR_mod if self.CS.leftBlinker or self.CS.rightBlinker: + old_TR = float(TR) x = [8.9408, 22.352, 31.2928] # 20, 50, 70 mph y = [1.0, .7, .65] # reduce TR when changing lanes - TR *= interp(self.CS.vEgo, x, y) + TR *= interp(self.car_data['v_ego'], x, y) + with open('/data/blinker_debug', 'a') as f: + f.write('{}\n'.format([self.CS.leftBlinker, self.CS.rightBlinker, self.car_data['v_ego'], old_TR, TR])) # TR *= self.get_traffic_level() # modify TR based on last minute of traffic data # todo: look at getting this to work, a model could be used @@ -155,9 +157,26 @@ def process_lead(self, v_lead, a_lead, x_lead, status): self.lead_data['x_lead'] = x_lead self.lead_data['status'] = status + # def get_traffic_level(self, lead_vels): # generate a value to modify TR by based on fluctuations in lead speed + # if len(lead_vels) < 60: + # return 1.0 # if less than 30 seconds of traffic data do nothing to TR + # lead_vel_diffs = [] + # for idx, vel in enumerate(lead_vels): + # try: + # lead_vel_diffs.append(abs(vel - lead_vels[idx - 1])) + # except: + # pass + # + # x = [0, len(lead_vels)] + # y = [1.15, .9] # min and max values to modify TR by, need to tune + # traffic = interp(sum(lead_vel_diffs), x, y) + # + # return traffic + def update(self, pm, CS, lead, v_cruise_setpoint): v_ego = CS.vEgo self.CS = CS + self.car_data = {'v_ego': CS.vEgo, 'a_ego': CS.aEgo} # Setup current mpc state self.cur_state[0].x_ego = 0.0 @@ -170,7 +189,6 @@ def update(self, pm, CS, lead, v_cruise_setpoint): if v_lead < 0.1 or -a_lead / 2.0 > v_lead: v_lead = 0.0 a_lead = 0.0 - self.process_lead(v_lead, a_lead, x_lead, lead.status) self.a_lead_tau = lead.aLeadTau diff --git a/selfdrive/test/process_replay/test_processes.py b/selfdrive/test/process_replay/test_processes.py index 8af87cf0bb7602..748f4e5f9417a2 100755 --- a/selfdrive/test/process_replay/test_processes.py +++ b/selfdrive/test/process_replay/test_processes.py @@ -17,7 +17,7 @@ "b6e1317e1bfbefa6|2019-07-06--04-05-26--5", # CHRYSLER.JEEP_CHEROKEE "7873afaf022d36e2|2019-07-03--18-46-44--0", # SUBARU.IMPREZA ] -segments.remove("cce908f7eb8db67d|2019-08-02--15-09-51--3") +# segments.remove("cce908f7eb8db67d|2019-08-02--15-09-51--3") def get_segment(segment_name): route_name, segment_num = segment_name.rsplit("--", 1) From 35c404902c24342920ebdbb788234ae8a44f76aa Mon Sep 17 00:00:00 2001 From: Shane Date: Mon, 16 Dec 2019 23:03:55 -0600 Subject: [PATCH 17/30] -recover quicker from deceleration (from Arne's fork) -added parameter to change the minimum speed the model will slow you down to for curves (min_model_speed, default is 20 mph) --- common/op_params.py | 3 ++- selfdrive/controls/lib/planner.py | 4 +++- selfdrive/controls/lib/speed_smoother.py | 4 ++-- 3 files changed, 7 insertions(+), 4 deletions(-) diff --git a/common/op_params.py b/common/op_params.py index fef9d765de9b03..121b4e9187db31 100644 --- a/common/op_params.py +++ b/common/op_params.py @@ -37,7 +37,8 @@ def __init__(self): 'following_distance': {'default': None, 'allowed_types': [type(None), float], 'description': 'None has no effect, while setting this to a float will let you change the TR'}, 'alca_nudge_required': {'default': True, 'allowed_types': [bool], 'description': ('Whether to wait for applied torque to the wheel (nudge) before making lane changes. ' 'If False, lane change will occur IMMEDIATELY after signaling')}, - 'alca_min_speed': {'default': 30.0, 'allowed_types': [float, int], 'description': 'The minimum speed allowed for an automatic lane change (in MPH)'}} + 'alca_min_speed': {'default': 30.0, 'allowed_types': [float, int], 'description': 'The minimum speed allowed for an automatic lane change (in MPH)'}, + 'min_model_speed': {'default': 20.0, 'allowed_types': [float, int], 'description': 'The minimum speed the model will be allowed to slow down for curves (in MPH)'}} self.params = {} self.params_file = "/data/op_params.json" diff --git a/selfdrive/controls/lib/planner.py b/selfdrive/controls/lib/planner.py index 6646e134546628..6e3b45a813d2bb 100755 --- a/selfdrive/controls/lib/planner.py +++ b/selfdrive/controls/lib/planner.py @@ -13,6 +13,7 @@ from selfdrive.controls.lib.longcontrol import LongCtrlState, MIN_CAN_SPEED from selfdrive.controls.lib.fcw import FCWChecker from selfdrive.controls.lib.long_mpc import LongitudinalMpc +from common.op_params import opParams MAX_SPEED = 255.0 @@ -80,6 +81,7 @@ def __init__(self, CP): self.path_x = np.arange(192) self.params = Params() + self.min_model_speed = opParams().get('min_model_speed', default=20.0) def choose_solution(self, v_cruise_setpoint, enabled): if enabled: @@ -137,7 +139,7 @@ def update(self, sm, pm, CP, VM, PP): a_y_max = 2.975 - v_ego * 0.0375 # ~1.85 @ 75mph, ~2.6 @ 25mph v_curvature = np.sqrt(a_y_max / np.clip(np.abs(curv), 1e-4, None)) model_speed = np.min(v_curvature) - model_speed = max(20.0 * CV.MPH_TO_MS, model_speed) # Don't slow down below 20mph + model_speed = max(self.min_model_speed * CV.MPH_TO_MS, model_speed) # Don't slow down below 20mph else: model_speed = MAX_SPEED diff --git a/selfdrive/controls/lib/speed_smoother.py b/selfdrive/controls/lib/speed_smoother.py index 1ee7ec6b18cb5d..96a9366dd357cc 100644 --- a/selfdrive/controls/lib/speed_smoother.py +++ b/selfdrive/controls/lib/speed_smoother.py @@ -18,9 +18,9 @@ def speed_smoother(vEgo, aEgo, vT, aMax, aMin, jMax, jMin, ts): # recover quickly if dV is positive and aEgo is negative or viceversa if dV > 0. and aEgo < 0.: - jMax *= 3. + jMax *= 6. # stock: 3. elif dV < 0. and aEgo > 0.: - jMin *= 3. + jMin *= 4.5 # stock: 3. tDelta = get_delta_out_limits(aEgo, aMax, aMin, jMax, jMin) From ff526ec21473136e20c87a630d02b65ea0bccaf9 Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Mon, 16 Dec 2019 23:08:23 -0600 Subject: [PATCH 18/30] Update README.md --- README.md | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/README.md b/README.md index d307f39377bcb2..1a70bff079257f 100644 --- a/README.md +++ b/README.md @@ -30,6 +30,6 @@ Highlight Features cd /data/openpilot python op_edit.py ``` - A list of parameters that you can change are located [here](https://github.com/ShaneSmiskol/openpilot/blob/stock_additions-07-final/common/op_params.py#L29). + A list of parameters that you can change are located [here](https://github.com/ShaneSmiskol/openpilot/blob/stock_additions-07/common/op_params.py#L29). - Parameters are stored at `/data/op_params.json` \ No newline at end of file + Parameters are stored at `/data/op_params.json` From 1b467d0d7d3291d782cc48ec6ed08adbce0e96b7 Mon Sep 17 00:00:00 2001 From: Shane Date: Mon, 16 Dec 2019 23:24:00 -0600 Subject: [PATCH 19/30] add lead debugging --- selfdrive/controls/lib/long_mpc.py | 3 ++ selfdrive/controls/lib/longcontrol.py | 44 +++++++++++++++++++++++++++ 2 files changed, 47 insertions(+) diff --git a/selfdrive/controls/lib/long_mpc.py b/selfdrive/controls/lib/long_mpc.py index 3636ac24c046c7..a85b34f2aaff36 100644 --- a/selfdrive/controls/lib/long_mpc.py +++ b/selfdrive/controls/lib/long_mpc.py @@ -215,6 +215,9 @@ def update(self, pm, CS, lead, v_cruise_setpoint): n_its = self.libmpc.run_mpc(self.cur_state, self.mpc_solution, self.a_lead_tau, a_lead, 1.8) duration = int((sec_since_boot() - t) * 1e9) + with open('/data/mpc_debug.{}'.format(self.mpc_id), 'a') as f: + f.write('{}\n'.format(self.lead_data)) + if LOG_MPC: self.send_mpc_solution(pm, n_its, duration) diff --git a/selfdrive/controls/lib/longcontrol.py b/selfdrive/controls/lib/longcontrol.py index 8910747cdbaaf1..dc9c7f5dcdb047 100644 --- a/selfdrive/controls/lib/longcontrol.py +++ b/selfdrive/controls/lib/longcontrol.py @@ -65,12 +65,56 @@ def __init__(self, CP, compute_gb): convert=compute_gb) self.v_pid = 0.0 self.last_output_gb = 0.0 + self.lead_data = {'v_rel': None, 'a_lead': None, 'x_lead': None, 'status': False} + self.v_ego = 0.0 + self.gas_pressed = False def reset(self, v_pid): """Reset PID controller and change setpoint""" self.pid.reset() self.v_pid = v_pid + def dynamic_gas(self): + x = [0.0, 1.4082, 2.80311, 4.22661, 5.38271, 6.16561, 7.24781, 8.28308, 10.24465, 12.96402, 15.42303, 18.11903, + 20.11703, 24.46614, 29.05805, 32.71015, 35.76326] + # y = [0.2, 0.20443, 0.21592, 0.23334, 0.25734, 0.27916, 0.3229, 0.34784, 0.36765, 0.38, 0.396, 0.409, 0.425, 0.478, 0.55, 0.621, 0.7] + # y = [0.175, 0.178, 0.185, 0.195, 0.209, 0.222, 0.249, 0.264, 0.276, 0.283, 0.293, 0.3, 0.31, 0.342, 0.385, 0.428, 0.475] # todo: elvaluate if this is better + y = [0.2, 0.20443, 0.21592, 0.23334, 0.25734, 0.27916, 0.3229, 0.35, 0.368, 0.377, 0.389, 0.399, 0.411, 0.45, 0.504, + 0.558, 0.617] # todo: this is the average of the above, only above the 8th index (about .75 reduction) + + gas = interp(self.v_ego, x, y) + + if self.lead_data['status']: # if lead + with open('/data/lead_data', 'a') as f: + f.write(str(self.lead_data) + '\n') + if self.v_ego <= 8.9408: # if under 20 mph + # TR = 1.8 # desired TR, might need to switch this to hardcoded distance values + # current_TR = self.lead_data['x_lead'] / self.v_ego if self.v_ego > 0 else TR + + x = [0.0, 0.24588812499999999, 0.432818589, 0.593044697, 0.730381365, 1.050833588, 1.3965, + 1.714627481] # relative velocity mod + y = [-(gas / 1.01), -(gas / 1.105), -(gas / 1.243), -(gas / 1.6), -(gas / 2.32), -(gas / 4.8), -(gas / 15), 0] + gas_mod = interp(self.lead_data['v_rel'], x, y) + + # x = [0.0, 0.22, 0.44518483, 0.675, 1.0, 1.76361684] # lead accel mod + # y = [0.0, (gas * 0.08), (gas * 0.20), (gas * 0.4), (gas * 0.52), (gas * 0.6)] + # gas_mod += interp(a_lead, x, y) + + # x = [TR * 0.5, TR, TR * 1.5] # as lead gets further from car, lessen gas mod + # y = [gas_mod * 1.5, gas_mod, gas_mod * 0.5] + # gas_mod += (interp(current_TR, x, y)) + new_gas = gas + gas_mod # (interp(current_TR, x, y)) + + x = [1.78816, 6.0, 8.9408] # slowly ramp mods down as we approach 20 mph + y = [new_gas, (new_gas * 0.8 + gas * 0.2), gas] + gas = interp(self.v_ego, x, y) + else: + x = [-0.89408, 0, 2.0] # need to tune this + y = [-.17, -.08, .01] + gas += interp(self.lead_data['v_rel'], x, y) + + return round(clip(gas, 0.0, 1.0), 4) + def update(self, active, v_ego, brake_pressed, standstill, cruise_standstill, v_cruise, v_target, v_target_future, a_target, CP): """Update longitudinal control. This updates the state machine and runs a PID loop""" # Actuation limits From 49a861f4a0e600714f8b6f88d02909b9099c37ec Mon Sep 17 00:00:00 2001 From: Shane Date: Mon, 16 Dec 2019 23:24:35 -0600 Subject: [PATCH 20/30] travis check --- selfdrive/controls/lib/long_mpc.py | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/selfdrive/controls/lib/long_mpc.py b/selfdrive/controls/lib/long_mpc.py index a85b34f2aaff36..9f0fdd0a615abb 100644 --- a/selfdrive/controls/lib/long_mpc.py +++ b/selfdrive/controls/lib/long_mpc.py @@ -215,8 +215,9 @@ def update(self, pm, CS, lead, v_cruise_setpoint): n_its = self.libmpc.run_mpc(self.cur_state, self.mpc_solution, self.a_lead_tau, a_lead, 1.8) duration = int((sec_since_boot() - t) * 1e9) - with open('/data/mpc_debug.{}'.format(self.mpc_id), 'a') as f: - f.write('{}\n'.format(self.lead_data)) + if not travis: + with open('/data/mpc_debug.{}'.format(self.mpc_id), 'a') as f: + f.write('{}\n'.format(self.lead_data)) if LOG_MPC: self.send_mpc_solution(pm, n_its, duration) From 910d89ac3bcd99971775b2caba6253ac22961d3d Mon Sep 17 00:00:00 2001 From: Shane Date: Mon, 16 Dec 2019 23:28:35 -0600 Subject: [PATCH 21/30] add travis check --- selfdrive/controls/lib/speed_smoother.py | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/selfdrive/controls/lib/speed_smoother.py b/selfdrive/controls/lib/speed_smoother.py index 96a9366dd357cc..432f5cd0738858 100644 --- a/selfdrive/controls/lib/speed_smoother.py +++ b/selfdrive/controls/lib/speed_smoother.py @@ -1,4 +1,5 @@ import numpy as np +from common.travis_checker import travis def get_delta_out_limits(aEgo, aMax, aMin, jMax, jMin): @@ -12,15 +13,17 @@ def get_delta_out_limits(aEgo, aMax, aMin, jMax, jMin): return tDelta +# noinspection PyChainedComparisons,PyRedundantParentheses def speed_smoother(vEgo, aEgo, vT, aMax, aMin, jMax, jMin, ts): dV = vT - vEgo # recover quickly if dV is positive and aEgo is negative or viceversa + recovery_factor = 5.25 if not travis else 3.0 if dV > 0. and aEgo < 0.: - jMax *= 6. # stock: 3. + jMax *= recovery_factor elif dV < 0. and aEgo > 0.: - jMin *= 4.5 # stock: 3. + jMin *= recovery_factor tDelta = get_delta_out_limits(aEgo, aMax, aMin, jMax, jMin) From a438dc298bb9e4ec6dc8af4468d49b59693e1056 Mon Sep 17 00:00:00 2001 From: Shane Date: Mon, 16 Dec 2019 23:45:52 -0600 Subject: [PATCH 22/30] add dynamic gas! --- selfdrive/controls/controlsd.py | 12 +++++----- selfdrive/controls/lib/longcontrol.py | 34 +++++++++++++++++---------- 2 files changed, 28 insertions(+), 18 deletions(-) diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index f6a27902bd5f20..4d1d4c50f92306 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -74,8 +74,6 @@ def data_sample(CI, CC, sm, can_sock, driver_status, state, mismatch_counter, pa can_strs = messaging.drain_sock_raw(can_sock, wait_for_one=True) CS = CI.update(CC, can_strs) - sm.update(0) - events = list(CS.events) add_lane_change_event(events, sm['pathPlan']) enabled = isEnabled(state) @@ -238,7 +236,7 @@ def state_transition(frame, CS, CP, state, events, soft_disable_timer, v_cruise_ def state_control(frame, rcv_frame, plan, path_plan, CS, CP, state, events, v_cruise_kph, v_cruise_kph_last, - AM, rk, driver_status, LaC, LoC, read_only, is_metric, cal_perc, last_blinker_frame): + AM, rk, driver_status, LaC, LoC, read_only, is_metric, cal_perc, last_blinker_frame, lead_one): """Given the state, this function returns an actuators packet""" actuators = car.CarControl.Actuators.new_message() @@ -285,8 +283,9 @@ def state_control(frame, rcv_frame, plan, path_plan, CS, CP, state, events, v_cr v_acc_sol = plan.vStart + dt * (a_acc_sol + plan.aStart) / 2.0 # Gas/Brake PID loop + passable = {'lead_one': lead_one, 'gas_pressed': CS.gasPressed, 'has_lead': plan.hasLead} actuators.gas, actuators.brake = LoC.update(active, CS.vEgo, CS.brakePressed, CS.standstill, CS.cruiseState.standstill, - v_cruise_kph, v_acc_sol, plan.vTargetFuture, a_acc_sol, CP) + v_cruise_kph, v_acc_sol, plan.vTargetFuture, a_acc_sol, CP, passable) # Steering PID loop and lateral MPC actuators.steer, actuators.steerAngle, lac_log = LaC.update(active, CS.vEgo, CS.steeringAngle, CS.steeringRate, CS.steeringTorqueEps, CS.steeringPressed, CS.steeringRateLimited, CP, path_plan) @@ -477,7 +476,7 @@ def controlsd_thread(sm=None, pm=None, can_sock=None): if sm is None: sm = messaging.SubMaster(['thermal', 'health', 'liveCalibration', 'driverMonitoring', 'plan', 'pathPlan', \ - 'model', 'gpsLocation'], ignore_alive=['gpsLocation']) + 'model', 'gpsLocation', 'radarState'], ignore_alive=['gpsLocation']) if can_sock is None: @@ -549,6 +548,7 @@ def controlsd_thread(sm=None, pm=None, can_sock=None): prof = Profiler(False) # off by default while True: + sm.update(0) start_time = sec_since_boot() prof.checkpoint("Ratekeeper", ignore=True) @@ -593,7 +593,7 @@ def controlsd_thread(sm=None, pm=None, can_sock=None): # Compute actuators (runs PID loops and lateral MPC) actuators, v_cruise_kph, driver_status, v_acc, a_acc, lac_log, last_blinker_frame = \ state_control(sm.frame, sm.rcv_frame, sm['plan'], sm['pathPlan'], CS, CP, state, events, v_cruise_kph, v_cruise_kph_last, AM, rk, - driver_status, LaC, LoC, read_only, is_metric, cal_perc, last_blinker_frame) + driver_status, LaC, LoC, read_only, is_metric, cal_perc, last_blinker_frame, sm['radarState'].leadOne) prof.checkpoint("State Control") diff --git a/selfdrive/controls/lib/longcontrol.py b/selfdrive/controls/lib/longcontrol.py index dc9c7f5dcdb047..f35b502a86ca96 100644 --- a/selfdrive/controls/lib/longcontrol.py +++ b/selfdrive/controls/lib/longcontrol.py @@ -1,6 +1,7 @@ from cereal import log from common.numpy_fast import clip, interp from selfdrive.controls.lib.pid import PIController +from common.travis_checker import travis LongCtrlState = log.ControlsState.LongControlState @@ -75,32 +76,31 @@ def reset(self, v_pid): self.v_pid = v_pid def dynamic_gas(self): - x = [0.0, 1.4082, 2.80311, 4.22661, 5.38271, 6.16561, 7.24781, 8.28308, 10.24465, 12.96402, 15.42303, 18.11903, - 20.11703, 24.46614, 29.05805, 32.71015, 35.76326] + x = [0.0, 1.4082, 2.80311, 4.22661, 5.38271, 6.16561, 7.24781, 8.28308, 10.24465, 12.96402, 15.42303, 18.11903, 20.11703, 24.46614, 29.05805, 32.71015, 35.76326] # y = [0.2, 0.20443, 0.21592, 0.23334, 0.25734, 0.27916, 0.3229, 0.34784, 0.36765, 0.38, 0.396, 0.409, 0.425, 0.478, 0.55, 0.621, 0.7] # y = [0.175, 0.178, 0.185, 0.195, 0.209, 0.222, 0.249, 0.264, 0.276, 0.283, 0.293, 0.3, 0.31, 0.342, 0.385, 0.428, 0.475] # todo: elvaluate if this is better - y = [0.2, 0.20443, 0.21592, 0.23334, 0.25734, 0.27916, 0.3229, 0.35, 0.368, 0.377, 0.389, 0.399, 0.411, 0.45, 0.504, - 0.558, 0.617] # todo: this is the average of the above, only above the 8th index (about .75 reduction) + y = [0.2, 0.20443, 0.21592, 0.23334, 0.25734, 0.27916, 0.3229, 0.35, 0.368, 0.377, 0.389, 0.399, 0.411, 0.45, 0.504, 0.558, 0.617] # todo: this is the average of the above, only above the 8th index (about .75 reduction) gas = interp(self.v_ego, x, y) - if self.lead_data['status']: # if lead + if not travis: with open('/data/lead_data', 'a') as f: f.write(str(self.lead_data) + '\n') + + if self.lead_data['status']: # if lead if self.v_ego <= 8.9408: # if under 20 mph # TR = 1.8 # desired TR, might need to switch this to hardcoded distance values # current_TR = self.lead_data['x_lead'] / self.v_ego if self.v_ego > 0 else TR - x = [0.0, 0.24588812499999999, 0.432818589, 0.593044697, 0.730381365, 1.050833588, 1.3965, - 1.714627481] # relative velocity mod + x = [0.0, 0.24588812499999999, 0.432818589, 0.593044697, 0.730381365, 1.050833588, 1.3965, 1.714627481] # relative velocity mod y = [-(gas / 1.01), -(gas / 1.105), -(gas / 1.243), -(gas / 1.6), -(gas / 2.32), -(gas / 4.8), -(gas / 15), 0] gas_mod = interp(self.lead_data['v_rel'], x, y) - # x = [0.0, 0.22, 0.44518483, 0.675, 1.0, 1.76361684] # lead accel mod + # x = [0.0, 0.22, 0.44518483, 0.675, 1.0, 1.76361684] # lead accel mod # todo: this # y = [0.0, (gas * 0.08), (gas * 0.20), (gas * 0.4), (gas * 0.52), (gas * 0.6)] # gas_mod += interp(a_lead, x, y) - # x = [TR * 0.5, TR, TR * 1.5] # as lead gets further from car, lessen gas mod + # x = [TR * 0.5, TR, TR * 1.5] # as lead gets further from car, lessen gas mod # todo: this # y = [gas_mod * 1.5, gas_mod, gas_mod * 0.5] # gas_mod += (interp(current_TR, x, y)) new_gas = gas + gas_mod # (interp(current_TR, x, y)) @@ -113,12 +113,22 @@ def dynamic_gas(self): y = [-.17, -.08, .01] gas += interp(self.lead_data['v_rel'], x, y) - return round(clip(gas, 0.0, 1.0), 4) + return clip(gas, 0.0, 1.0) - def update(self, active, v_ego, brake_pressed, standstill, cruise_standstill, v_cruise, v_target, v_target_future, a_target, CP): + def handle_passable(self, passable): + self.gas_pressed = passable['gas_pressed'] + self.lead_data['v_rel'] = passable['lead_one'].vRel + self.lead_data['a_lead'] = passable['lead_one'].aLeadK + self.lead_data['x_lead'] = passable['lead_one'].dRel + self.lead_data['status'] = passable['has_lead'] # this fixes radarstate always reporting a lead, thanks to arne + + def update(self, active, v_ego, brake_pressed, standstill, cruise_standstill, v_cruise, v_target, v_target_future, a_target, CP, passable): """Update longitudinal control. This updates the state machine and runs a PID loop""" + self.handle_passable(passable) + self.v_ego = v_ego + # Actuation limits - gas_max = interp(v_ego, CP.gasMaxBP, CP.gasMaxV) + gas_max = self.dynamic_gas() brake_max = interp(v_ego, CP.brakeMaxBP, CP.brakeMaxV) # Update state machine From 42eed665e484bfa36261a9549448a0c4f263d0a2 Mon Sep 17 00:00:00 2001 From: Shane Date: Mon, 16 Dec 2019 23:47:13 -0600 Subject: [PATCH 23/30] update max torque and torque error --- panda/board/safety/safety_toyota.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/panda/board/safety/safety_toyota.h b/panda/board/safety/safety_toyota.h index 0576a463816f62..948bc18e52da9e 100644 --- a/panda/board/safety/safety_toyota.h +++ b/panda/board/safety/safety_toyota.h @@ -1,11 +1,11 @@ // global torque limit -const int TOYOTA_MAX_TORQUE = 1500; // max torque cmd allowed ever +const int TOYOTA_MAX_TORQUE = 1900; // max torque cmd allowed ever // rate based torque limit + stay within actually applied // packet is sent at 100hz, so this limit is 1000/sec const int TOYOTA_MAX_RATE_UP = 10; // ramp up slow const int TOYOTA_MAX_RATE_DOWN = 25; // ramp down fast -const int TOYOTA_MAX_TORQUE_ERROR = 350; // max torque cmd in excess of torque motor +const int TOYOTA_MAX_TORQUE_ERROR = 425; // max torque cmd in excess of torque motor // real time torque limit to prevent controls spamming // the real time limit is 1500/sec From 4cd2bda3a968b5175bb9ac2a5e54bf57803fe24c Mon Sep 17 00:00:00 2001 From: Shane Date: Mon, 16 Dec 2019 23:56:52 -0600 Subject: [PATCH 24/30] this check might be required --- selfdrive/controls/lib/longcontrol.py | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/selfdrive/controls/lib/longcontrol.py b/selfdrive/controls/lib/longcontrol.py index f35b502a86ca96..a80e11bb9c6053 100644 --- a/selfdrive/controls/lib/longcontrol.py +++ b/selfdrive/controls/lib/longcontrol.py @@ -128,7 +128,10 @@ def update(self, active, v_ego, brake_pressed, standstill, cruise_standstill, v_ self.v_ego = v_ego # Actuation limits - gas_max = self.dynamic_gas() + if not travis: + gas_max = self.dynamic_gas() + else: + gas_max = interp(v_ego, CP.gasMaxBP, CP.gasMaxV) brake_max = interp(v_ego, CP.brakeMaxBP, CP.brakeMaxV) # Update state machine From 06d6a5bdbb5fcc69dc7b2b2c5ec4e76c1e68d047 Mon Sep 17 00:00:00 2001 From: Shane Date: Tue, 17 Dec 2019 01:38:28 -0600 Subject: [PATCH 25/30] maybe move back update? --- selfdrive/controls/controlsd.py | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 4d1d4c50f92306..41d837d7c44a7d 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -74,6 +74,8 @@ def data_sample(CI, CC, sm, can_sock, driver_status, state, mismatch_counter, pa can_strs = messaging.drain_sock_raw(can_sock, wait_for_one=True) CS = CI.update(CC, can_strs) + sm.update(0) + events = list(CS.events) add_lane_change_event(events, sm['pathPlan']) enabled = isEnabled(state) @@ -548,7 +550,6 @@ def controlsd_thread(sm=None, pm=None, can_sock=None): prof = Profiler(False) # off by default while True: - sm.update(0) start_time = sec_since_boot() prof.checkpoint("Ratekeeper", ignore=True) From 84510f5967ba07edb50f1b751d341c75ffc3a555 Mon Sep 17 00:00:00 2001 From: Shane Date: Tue, 17 Dec 2019 15:51:34 -0600 Subject: [PATCH 26/30] travis check on sm['radarState'] --- selfdrive/controls/controlsd.py | 13 +++++++++---- selfdrive/controls/lib/longcontrol.py | 7 +++---- 2 files changed, 12 insertions(+), 8 deletions(-) diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 41d837d7c44a7d..5423ec500d90d8 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -27,6 +27,7 @@ from selfdrive.controls.lib.planner import LON_MPC_STEP from selfdrive.controls.lib.gps_helpers import is_rhd_region from selfdrive.locationd.calibration_helpers import Calibration, Filter +from common.travis_checker import travis LANE_DEPARTURE_THRESHOLD = 0.1 @@ -238,7 +239,7 @@ def state_transition(frame, CS, CP, state, events, soft_disable_timer, v_cruise_ def state_control(frame, rcv_frame, plan, path_plan, CS, CP, state, events, v_cruise_kph, v_cruise_kph_last, - AM, rk, driver_status, LaC, LoC, read_only, is_metric, cal_perc, last_blinker_frame, lead_one): + AM, rk, driver_status, LaC, LoC, read_only, is_metric, cal_perc, last_blinker_frame, passable_state_control): """Given the state, this function returns an actuators packet""" actuators = car.CarControl.Actuators.new_message() @@ -285,9 +286,9 @@ def state_control(frame, rcv_frame, plan, path_plan, CS, CP, state, events, v_cr v_acc_sol = plan.vStart + dt * (a_acc_sol + plan.aStart) / 2.0 # Gas/Brake PID loop - passable = {'lead_one': lead_one, 'gas_pressed': CS.gasPressed, 'has_lead': plan.hasLead} + passable_loc = {'lead_one': passable_state_control['lead_one'], 'gas_pressed': CS.gasPressed, 'has_lead': plan.hasLead} actuators.gas, actuators.brake = LoC.update(active, CS.vEgo, CS.brakePressed, CS.standstill, CS.cruiseState.standstill, - v_cruise_kph, v_acc_sol, plan.vTargetFuture, a_acc_sol, CP, passable) + v_cruise_kph, v_acc_sol, plan.vTargetFuture, a_acc_sol, CP, passable_loc) # Steering PID loop and lateral MPC actuators.steer, actuators.steerAngle, lac_log = LaC.update(active, CS.vEgo, CS.steeringAngle, CS.steeringRate, CS.steeringTorqueEps, CS.steeringPressed, CS.steeringRateLimited, CP, path_plan) @@ -592,9 +593,13 @@ def controlsd_thread(sm=None, pm=None, can_sock=None): prof.checkpoint("State transition") # Compute actuators (runs PID loops and lateral MPC) + if not travis: + passable_state_control = {'lead_one': sm['radarState'].leadOne} + else: + passable_state_control = {'lead_one': None} actuators, v_cruise_kph, driver_status, v_acc, a_acc, lac_log, last_blinker_frame = \ state_control(sm.frame, sm.rcv_frame, sm['plan'], sm['pathPlan'], CS, CP, state, events, v_cruise_kph, v_cruise_kph_last, AM, rk, - driver_status, LaC, LoC, read_only, is_metric, cal_perc, last_blinker_frame, sm['radarState'].leadOne) + driver_status, LaC, LoC, read_only, is_metric, cal_perc, last_blinker_frame, passable_state_control) prof.checkpoint("State Control") diff --git a/selfdrive/controls/lib/longcontrol.py b/selfdrive/controls/lib/longcontrol.py index a80e11bb9c6053..023c83e6eeaa19 100644 --- a/selfdrive/controls/lib/longcontrol.py +++ b/selfdrive/controls/lib/longcontrol.py @@ -83,9 +83,8 @@ def dynamic_gas(self): gas = interp(self.v_ego, x, y) - if not travis: - with open('/data/lead_data', 'a') as f: - f.write(str(self.lead_data) + '\n') + with open('/data/lead_data', 'a') as f: + f.write(str(self.lead_data) + '\n') if self.lead_data['status']: # if lead if self.v_ego <= 8.9408: # if under 20 mph @@ -124,11 +123,11 @@ def handle_passable(self, passable): def update(self, active, v_ego, brake_pressed, standstill, cruise_standstill, v_cruise, v_target, v_target_future, a_target, CP, passable): """Update longitudinal control. This updates the state machine and runs a PID loop""" - self.handle_passable(passable) self.v_ego = v_ego # Actuation limits if not travis: + self.handle_passable(passable) # so travis doesn't call vRel... on None gas_max = self.dynamic_gas() else: gas_max = interp(v_ego, CP.gasMaxBP, CP.gasMaxV) From 5dbc1b88409eb21d52b256a86ba2c618683db896 Mon Sep 17 00:00:00 2001 From: Shane Date: Tue, 17 Dec 2019 15:53:25 -0600 Subject: [PATCH 27/30] fix for slow acceleration on hondas --- selfdrive/controls/lib/longcontrol.py | 11 +++++++---- 1 file changed, 7 insertions(+), 4 deletions(-) diff --git a/selfdrive/controls/lib/longcontrol.py b/selfdrive/controls/lib/longcontrol.py index 023c83e6eeaa19..7769145ed8c628 100644 --- a/selfdrive/controls/lib/longcontrol.py +++ b/selfdrive/controls/lib/longcontrol.py @@ -58,6 +58,8 @@ def long_control_state_trans(active, long_control_state, v_ego, v_target, v_pid, class LongControl(): def __init__(self, CP, compute_gb): + self.gas_interceptor = CP.enableGasInterceptor + self.long_control_state = LongCtrlState.off # initialized to off self.pid = PIController((CP.longitudinalTuning.kpBP, CP.longitudinalTuning.kpV), (CP.longitudinalTuning.kiBP, CP.longitudinalTuning.kiV), @@ -76,10 +78,11 @@ def reset(self, v_pid): self.v_pid = v_pid def dynamic_gas(self): - x = [0.0, 1.4082, 2.80311, 4.22661, 5.38271, 6.16561, 7.24781, 8.28308, 10.24465, 12.96402, 15.42303, 18.11903, 20.11703, 24.46614, 29.05805, 32.71015, 35.76326] - # y = [0.2, 0.20443, 0.21592, 0.23334, 0.25734, 0.27916, 0.3229, 0.34784, 0.36765, 0.38, 0.396, 0.409, 0.425, 0.478, 0.55, 0.621, 0.7] - # y = [0.175, 0.178, 0.185, 0.195, 0.209, 0.222, 0.249, 0.264, 0.276, 0.283, 0.293, 0.3, 0.31, 0.342, 0.385, 0.428, 0.475] # todo: elvaluate if this is better - y = [0.2, 0.20443, 0.21592, 0.23334, 0.25734, 0.27916, 0.3229, 0.35, 0.368, 0.377, 0.389, 0.399, 0.411, 0.45, 0.504, 0.558, 0.617] # todo: this is the average of the above, only above the 8th index (about .75 reduction) + if self.gas_interceptor: # if user has pedal (probably Toyota) + x = [0.0, 1.4082, 2.80311, 4.22661, 5.38271, 6.16561, 7.24781, 8.28308, 10.24465, 12.96402, 15.42303, 18.11903, 20.11703, 24.46614, 29.05805, 32.71015, 35.76326] + # y = [0.2, 0.20443, 0.21592, 0.23334, 0.25734, 0.27916, 0.3229, 0.34784, 0.36765, 0.38, 0.396, 0.409, 0.425, 0.478, 0.55, 0.621, 0.7] + # y = [0.175, 0.178, 0.185, 0.195, 0.209, 0.222, 0.249, 0.264, 0.276, 0.283, 0.293, 0.3, 0.31, 0.342, 0.385, 0.428, 0.475] # todo: elvaluate if this is better + y = [0.2, 0.20443, 0.21592, 0.23334, 0.25734, 0.27916, 0.3229, 0.35, 0.368, 0.377, 0.389, 0.399, 0.411, 0.45, 0.504, 0.558, 0.617] # todo: this is the average of the above, only above the 8th index (about .75 reduction) gas = interp(self.v_ego, x, y) From b007f481bed44e4ddc47dcdb2468573f2f5115ae Mon Sep 17 00:00:00 2001 From: Shane Date: Tue, 17 Dec 2019 15:55:12 -0600 Subject: [PATCH 28/30] fix for slow acceleration on hondas 2 --- selfdrive/controls/lib/longcontrol.py | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/selfdrive/controls/lib/longcontrol.py b/selfdrive/controls/lib/longcontrol.py index 7769145ed8c628..e4052b21924e95 100644 --- a/selfdrive/controls/lib/longcontrol.py +++ b/selfdrive/controls/lib/longcontrol.py @@ -58,8 +58,6 @@ def long_control_state_trans(active, long_control_state, v_ego, v_target, v_pid, class LongControl(): def __init__(self, CP, compute_gb): - self.gas_interceptor = CP.enableGasInterceptor - self.long_control_state = LongCtrlState.off # initialized to off self.pid = PIController((CP.longitudinalTuning.kpBP, CP.longitudinalTuning.kpV), (CP.longitudinalTuning.kiBP, CP.longitudinalTuning.kiV), @@ -77,12 +75,14 @@ def reset(self, v_pid): self.pid.reset() self.v_pid = v_pid - def dynamic_gas(self): - if self.gas_interceptor: # if user has pedal (probably Toyota) + def dynamic_gas(self, CP): + if CP.enableGasInterceptor: # if user has pedal (probably Toyota) x = [0.0, 1.4082, 2.80311, 4.22661, 5.38271, 6.16561, 7.24781, 8.28308, 10.24465, 12.96402, 15.42303, 18.11903, 20.11703, 24.46614, 29.05805, 32.71015, 35.76326] # y = [0.2, 0.20443, 0.21592, 0.23334, 0.25734, 0.27916, 0.3229, 0.34784, 0.36765, 0.38, 0.396, 0.409, 0.425, 0.478, 0.55, 0.621, 0.7] # y = [0.175, 0.178, 0.185, 0.195, 0.209, 0.222, 0.249, 0.264, 0.276, 0.283, 0.293, 0.3, 0.31, 0.342, 0.385, 0.428, 0.475] # todo: elvaluate if this is better y = [0.2, 0.20443, 0.21592, 0.23334, 0.25734, 0.27916, 0.3229, 0.35, 0.368, 0.377, 0.389, 0.399, 0.411, 0.45, 0.504, 0.558, 0.617] # todo: this is the average of the above, only above the 8th index (about .75 reduction) + else: + x, y = CP.gasMaxBP, CP.gasMaxV # if user doesn't have pedal, use stock params gas = interp(self.v_ego, x, y) @@ -131,7 +131,7 @@ def update(self, active, v_ego, brake_pressed, standstill, cruise_standstill, v_ # Actuation limits if not travis: self.handle_passable(passable) # so travis doesn't call vRel... on None - gas_max = self.dynamic_gas() + gas_max = self.dynamic_gas(CP) else: gas_max = interp(v_ego, CP.gasMaxBP, CP.gasMaxV) brake_max = interp(v_ego, CP.brakeMaxBP, CP.brakeMaxV) From e5f028fb43f70ca0a0f1eee9e2404d1d3d2785cd Mon Sep 17 00:00:00 2001 From: Shane Date: Tue, 17 Dec 2019 16:02:55 -0600 Subject: [PATCH 29/30] temp fix for now, only allow pedals to use dynamic gas at all --- selfdrive/controls/lib/longcontrol.py | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/selfdrive/controls/lib/longcontrol.py b/selfdrive/controls/lib/longcontrol.py index e4052b21924e95..88b647e1e3968b 100644 --- a/selfdrive/controls/lib/longcontrol.py +++ b/selfdrive/controls/lib/longcontrol.py @@ -76,13 +76,13 @@ def reset(self, v_pid): self.v_pid = v_pid def dynamic_gas(self, CP): - if CP.enableGasInterceptor: # if user has pedal (probably Toyota) + if True: # CP.enableGasInterceptor: # if user has pedal (probably Toyota) #todo: make different profiles for different toyotas, and check if toyota or not x = [0.0, 1.4082, 2.80311, 4.22661, 5.38271, 6.16561, 7.24781, 8.28308, 10.24465, 12.96402, 15.42303, 18.11903, 20.11703, 24.46614, 29.05805, 32.71015, 35.76326] # y = [0.2, 0.20443, 0.21592, 0.23334, 0.25734, 0.27916, 0.3229, 0.34784, 0.36765, 0.38, 0.396, 0.409, 0.425, 0.478, 0.55, 0.621, 0.7] # y = [0.175, 0.178, 0.185, 0.195, 0.209, 0.222, 0.249, 0.264, 0.276, 0.283, 0.293, 0.3, 0.31, 0.342, 0.385, 0.428, 0.475] # todo: elvaluate if this is better y = [0.2, 0.20443, 0.21592, 0.23334, 0.25734, 0.27916, 0.3229, 0.35, 0.368, 0.377, 0.389, 0.399, 0.411, 0.45, 0.504, 0.558, 0.617] # todo: this is the average of the above, only above the 8th index (about .75 reduction) - else: - x, y = CP.gasMaxBP, CP.gasMaxV # if user doesn't have pedal, use stock params + # else: + # x, y = CP.gasMaxBP, CP.gasMaxV # if user doesn't have pedal, use stock params gas = interp(self.v_ego, x, y) @@ -129,7 +129,7 @@ def update(self, active, v_ego, brake_pressed, standstill, cruise_standstill, v_ self.v_ego = v_ego # Actuation limits - if not travis: + if not travis and CP.enableGasInterceptor: self.handle_passable(passable) # so travis doesn't call vRel... on None gas_max = self.dynamic_gas(CP) else: From 81ad11e3c4b4848c49dc0416ee9e9747f3ab7772 Mon Sep 17 00:00:00 2001 From: Shane Date: Tue, 17 Dec 2019 20:45:45 -0600 Subject: [PATCH 30/30] add lane hugging mod to latcontrol_lqr --- selfdrive/controls/lib/latcontrol_lqr.py | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/selfdrive/controls/lib/latcontrol_lqr.py b/selfdrive/controls/lib/latcontrol_lqr.py index a23eafb5cfa957..6070c1213ebc80 100644 --- a/selfdrive/controls/lib/latcontrol_lqr.py +++ b/selfdrive/controls/lib/latcontrol_lqr.py @@ -3,6 +3,7 @@ from common.numpy_fast import clip from common.realtime import DT_CTRL from cereal import log +from selfdrive.controls.lane_hugging import LaneHugging class LatControlLQR(): @@ -23,6 +24,7 @@ def __init__(self, CP): self.sat_count_rate = 1.0 * DT_CTRL self.sat_limit = CP.steerLimitTimer + self.lane_hugging = LaneHugging() self.reset() @@ -50,7 +52,8 @@ def update(self, active, v_ego, angle_steers, angle_steers_rate, eps_torque, ste torque_scale = (0.45 + v_ego / 60.0)**2 # Scale actuator model with speed # Subtract offset. Zero angle should correspond to zero torque - self.angle_steers_des = path_plan.angleSteers - path_plan.angleOffset + # self.angle_steers_des = path_plan.angleSteers - path_plan.angleOffset + self.angle_steers_des = self.lane_hugging.offset_mod(path_plan.angleSteers - path_plan.angleOffset) angle_steers -= path_plan.angleOffset # Update Kalman filter