Stock Additions 0.1

README.md

@@ -1,35 +1,79 @@
-Shane's Stock Additions (0.7)
+Shane's Stock Additions 0.7 (version 0.1)
 =====
 
-This branch is simply stock openpilot with some additions to help it drive as smooth as possible on my 2017 Toyota Corolla.
+This branch is simply stock openpilot with some additions to help it drive as smooth as possible on my 2017 Toyota Corolla /w comma pedal.
 
 
 Highlight Features
-====
-
-1. **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`:
+=====
+
+* [**Dynamic gas**](#dynamic-gas)
+* [**Dynamic follow**](#dynamic-follow)
+* [**Dynamic lane speed (new!)**](#dynamic-lane-speed)
+* [**(NOT YET ADDED) Two PID loops to control gas and brakes independently**](#Two-PID-loops-to-control-gas-and-brakes-independently)
+* [**Custom wheel offset to reduce lane hugging**](#Custom-wheel-offset-to-reduce-lane-hugging)
+* [**Custom following distance**](#Custom-following-distance)
+* [**Customize this branch (opEdit Parameter class)**](#Customize-this-branch-opEdit-Parameter-class)
+* [**Live tuning support**](#Live-tuning-support)
+
+-----
+
+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. It tries to coast if the lead is only moving slowly, it doesn't use maximum gas as soon as the lead inches forward :). Better tuning for distance will be next.
+
+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) The goal is to essentially smoothen the driving experience and increase safety, braking sooner.
+
+Dynamic lane speed
+-----
+This is a new feature that reduces your cruising speed if many vehicles around you are significantly slower than you. This works with and without an openpilot-identified lead. Ex.: It will slow you down if traveling in an open lane with cars in adjacent lanes that are slower than you. Or if the lead in front of the lead is slowing down, as well as cars in other lanes far ahead. The most it will slow you down is some average of: (the set speed and the average of the surrounding cars) The more the radar points, the more weight goes to the speeds of surrounding vehicles.
+
+~~Two PID loops to control gas and brakes independently~~
+-----
+***Update**: Probably going to remove this addition, as tuning the current pedal parameters will be a more robust solution in the long run.*
+
+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.
+
+~~Custom wheel offset to reduce lane hugging~~
+-----
+***Update**: This also may be removed, I was able to get good results live tuning camera offset. Perhaps angle offset isn't needed?*
+
+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
     ```
-   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).
+    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`.
+
+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! Please note dynamic follow modifications will be disabled if you set this parameter.
+- 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!
+
+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/common/op_params.py#L29).
+
+Parameters are stored at `/data/op_params.json`
+
+Live tuning support
+-----
+This has just been added and currently only the `camera_offset` parameter is officially supported.
+- Just start opEdit with the instructions above and pick a parameter. It will let you know if it supports live tuning, if so, updates will take affect within 5 seconds!
+- Alternatively, you can use the new opTune module to live tune quicker and easier! It stays in the parameter edit view so you can more easily experiment with values. opTune show below:
 
-    Parameters are stored at `/data/op_params.json`
+<img src="gifs/op_tune.gif?raw=true" width="600">

RELEASES.md

@@ -1,3 +1,14 @@
+Stock Additions 0.7 (version 0.1)
+========================
+ * Dynamic lane speed is a new feature that reduces your cruising speed if many vehicles around you are significantly slower than you. This works with and without an openpilot-identified lead.
+ * Dynamic gas tuning. Above 20 mph we take lead velocity and the following distance into account. Possibility of different tuning for different cars in the future. (DYNAMIC GAS NOW ONLY WORKS ON TOYOTA COROLLA AND RAV4 PEDAL)
+ * Dynamic follow tuning, don't get as close when lead is accelerating.
+ * Added static_steer_ratio parameter, if True openpilot will use the steer ratio in your interface file. Default is true, false uses the openpilot learned value which can vary through your drives.
+ * Added ability to live tune parameters with `op_tune.py`. Currently only the camera offset (`camera_offset`) is supported.
+ * Some Corolla tuning.
+ * Reduce max acceleration.
+ * TO NOTE: Dynamic Lane Speed will not work with stopped cars, at any speed. There is also a margin that cars must be traveling within in order to affect your speed. Don't expect anything magical, just minor quality of drive improvements.
+
 Version 0.7 (2019-12-13)
 ========================
  * Move to SCons build system!

cereal/log.capnp

@@ -1814,6 +1814,10 @@ struct KalmanOdometry {
   rotStd @3 :List(Float32); # std rad/s in device frame
 }
 
+struct SmiskolData {
+  mpcTR @0 :Float32;
+}
+
 struct Event {
   # in nanoseconds?
   logMonoTime @0 :UInt64;
@@ -1889,5 +1893,6 @@ struct Event {
     carEvents @68: List(Car.CarEvent);
     carParams @69: Car.CarParams;
     frontFrame @70: FrameData;
+    smiskolData @71 :SmiskolData;
   }
 }

cereal/service_list.yaml

@@ -75,6 +75,7 @@ thumbnail: [8069, true, 0.2, 1]
 carEvents: [8070, true, 1., 1]
 carParams: [8071, true, 0.02, 1]
 frontFrame: [8072, true, 10.]
+smiskolData: [8073, true, 20.]
 
 testModel: [8040, false, 0.]
 testLiveLocation: [8045, false, 0.]

common/op_params.py

@@ -5,7 +5,6 @@
 import random
 from common.travis_checker import travis
 
-
 def write_params(params, params_file):
   if not travis:
     with open(params_file, "w") as f:
@@ -26,25 +25,29 @@ def read_params(params_file, default_params):
 
 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"},
+    self.default_params = {'camera_offset': {'default': 0.06, 'allowed_types': [float, int], 'description': 'Your camera offset to use in lane_planner.py', 'live': True},
+                           'awareness_factor': {'default': 2.0, 'allowed_types': [float, int], 'description': 'Multiplier for the awareness times', 'live': False},
+                           '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", 'live': False},
                            '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'},
+                                                                                                                    'Enter absolute value here, direction is determined by parameter \'lane_hug_direction\''), 'live': False},
+                           'use_car_caching': {'default': True, 'allowed_types': [bool], 'description': 'Whether to use fingerprint caching', 'live': False},
+                           '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', 'live': False},
                            '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)'}}
+                                                                                                             'If False, lane change will occur IMMEDIATELY after signaling'), 'live': False},
+                           'alca_min_speed': {'default': 30.0, 'allowed_types': [float, int], 'description': 'The minimum speed allowed for an automatic lane change (in MPH)', 'live': False},
+                           'static_steer_ratio': {'default': False, 'allowed_types': [bool], 'description': 'Whether you want openpilot to use the steering ratio in interface.py, or the automatically learned steering ratio. If True, it will use the static value in interface.py', 'live': False},
+                           'use_dynamic_lane_speed': {'default': True, 'allowed_types': [bool], 'description': 'Whether you want openpilot to adjust your speed based on surrounding vehicles', 'live': False},
+                           'min_dynamic_lane_speed': {'default': 15.0, 'allowed_types': [float, int], 'description': 'The minimum speed to allow dynamic lane speed to operate (in MPH)', 'live': False},
+                           'longkiV': {'default': 0.0, 'allowed_types': [float, int], 'description': 'This is a temp parameter', 'live': True}}
 
     self.params = {}
     self.params_file = "/data/op_params.json"
     self.kegman_file = "/data/kegman.json"
     self.last_read_time = time.time()
-    self.read_frequency = 10.0  # max frequency to read with self.get(...) (sec)
+    self.read_frequency = 5.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.to_delete = ['dynamic_lane_speed']
     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!
@@ -82,6 +85,8 @@ def run_init(self):  # does first time initializing of default params, and/or re
       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
+        if self.delete_old():  # or if old params have been deleted
+          to_write = True
       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):
@@ -97,18 +102,29 @@ def run_init(self):  # does first time initializing of default params, and/or re
     if to_write or no_params:
       write_params(self.params, self.params_file)
 
+  def delete_old(self):
+    prev_params = self.params
+    for i in self.to_delete:
+      if i in self.params:
+        del self.params[i]
+    return prev_params == self.params
+
   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_frequency 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
+    if key is None:
       return self.params
-    else:
-      return self.params[key] if key in self.params else default
+    if not travis and key in self.default_params and self.default_params[key]['live']:  # if is a live param, we want to get updates while openpilot is running
+      if time.time() - self.last_read_time >= self.read_frequency:  # make sure we aren't reading file too often
+        self.params, read_status = read_params(self.params_file, self.format_default_params())
+        if not read_status:
+          time.sleep(0.01)
+          self.params, read_status = read_params(self.params_file, self.format_default_params())  # if the file was being written to, retry once
+        self.last_read_time = time.time()
+
+    return self.params[key] if key in self.params else default
 
   def delete(self, key):
     if key in self.params: