diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py old mode 100755 new mode 100644 index 62c7affc017e47..d940bb2b036760 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -24,6 +24,11 @@ from selfdrive.controls.lib.alertmanager import AlertManager from selfdrive.controls.lib.vehicle_model import VehicleModel from selfdrive.controls.lib.driver_monitor import DriverStatus +try: # Real-time tuning + import cPickle as pickle +except ImportError: + import pickle +import os ThermalStatus = log.ThermalData.ThermalStatus State = log.Live100Data.ControlState @@ -504,6 +509,11 @@ def controlsd_thread(gctx=None, rate=100, default_bias=0.): prof = Profiler(False) # off by default + # Setup for real-time tuning + rt_tuning_file = '/data/.openpilot_rtt_params.pkl' + rtt_params = {} + last_mod_time = 0 + while 1: prof.checkpoint("Ratekeeper", ignore=True) @@ -533,6 +543,115 @@ def controlsd_thread(gctx=None, rate=100, default_bias=0.): live100, livempc, AM, driver_status, LaC, LoC, angle_offset, passive) prof.checkpoint("Sent") + ###################### Real-Time Tuning Add-on ######################## + # TODO: Move this into it's own function to clean things up + # Run this once per second... on frame 29, of course. + if rk.frame % 100 == 29: + # Get the last update time of our real-time tuning file + #print('Real-Time Tuning: Checking tuning file modification time.') + try: + mod_time = os.path.getmtime(rt_tuning_file) + #print('RTT mod_time: {0}'.format(mod_time)) + except OSError: + # File doesn't exist or is inaccessible + mod_time = None + print('Real-Time Tuning: RT_TUNING_FILE did not exist or was inaccessible.') + + # If rt_tuning_file doesn't exist, then create it from the current CarParams: + if not mod_time: + rtt_params['steerKpBP'] = list(CP.steerKpBP) # Note that the Kp/Ki are lists! But if you reference them directly they are .. oops. + rtt_params['steerKpV'] = list(CP.steerKpV) + rtt_params['steerKiBP'] = list(CP.steerKiBP) + rtt_params['steerKiV'] = list(CP.steerKiV) + rtt_params['steerKf'] = CP.steerKf + # TODO: Give the option to link the front and rear tire stiffness changes together + rtt_params['tireStiffnessFront'] = CP.tireStiffnessFront + rtt_params['tireStiffnessRear'] = CP.tireStiffnessRear + rtt_params['steerRatio'] = CP.steerRatio + rtt_params['steerRateCost'] = CP.steerRateCost + # Write the pickle file + # TODO: try/except the open + with open(rt_tuning_file, "wb") as f_write: + pickle.dump(rtt_params, f_write, -1) # Dump to file with highest protocol (fastest) + # No need to update next time if we just wrote the file out... + last_mod_time = os.path.getmtime(rt_tuning_file) + #print('RTT Last_mod_time: {0}'.format(last_mod_time)) + + # If file exists and has been updated since the last time we read it in + elif last_mod_time != mod_time: + print('Real-Time Tuning: Reading in the modified tuning file.') + # Read in parameters from file + # TODO: try/except the open + with open(rt_tuning_file, "rb") as f_read: + rtt_params = pickle.load(f_read) + # Sanity check the data before setting it.. format is [min, max, failsafe] + # Failsafe is used if a value is not found or if the value sent is out of the range limits + rt_data_limits = { 'steerKpBP': [ 0.0 , 67.0, 0.0 ], + 'steerKpV': [ 0.0 , 1.0, 0.2 ], + 'steerKiBP': [ 0.0 , 67.0, 0.0 ], + 'steerKiV': [ 0.0 , 1.0, 0.05 ], + 'steerKf': [ 0.0, 0.001, 0.00005 ], + 'tireStiffnessFront': [ 20000, 1000000, 192150 ], + 'tireStiffnessRear': [ 20000, 1000000, 202500 ], + 'steerRatio': [ 8.0, 25.0, 14.0 ], + 'steerRateCost': [ 0.05, 1.0, 0.5 ] + } + # Do the checks and set the values + for key in rt_data_limits: + rt_val = rtt_params.get(key) + if not rt_val: + # If this key from data limits doesn't exist in our tuning data, then add it as the failsafe + # TODO: Use CP value here instead of failsafe? + rtt_params[key] = rt_data_limits[key][2] + print('Real-Time Tuning: Value did not exist. Key: ' + key) + continue + # If it does exist, then check the values. First see if it's a list + try: + # If it's an iterable list... + for i, val2 in enumerate(rt_val): + # Check each value in the list + if (val2 < rt_data_limits[key][0]) or (val2 > rt_data_limits[key][1]): + rt_val[i] = rt_data_limits[key][2] + print('Real-Time Tuning: Invalid value replaced! Key: ' + key) + except: + # Not interable, compare it and fix if necessary + if (rt_val < rt_data_limits[key][0]) or (rt_val > rt_data_limits[key][1]): + rt_val = rt_data_limits[key][2] + print('Real-Time Tuning: Invalid value replaced! Key: ' + key) + # Set it back so if anything was fixed we have the updated value + rtt_params[key] = rt_val + + # Update CP with the new params + CP.steerKpBP = rtt_params['steerKpBP'] + CP.steerKpV = rtt_params['steerKpV'] + CP.steerKiBP = rtt_params['steerKiBP'] + CP.steerKiV = rtt_params['steerKiV'] + CP.steerKf = rtt_params['steerKf'] + CP.tireStiffnessFront = rtt_params['tireStiffnessFront'] + CP.tireStiffnessRear = rtt_params['tireStiffnessRear'] + CP.steerRatio = rtt_params['steerRatio'] + if CP.steerRateCost != rtt_params['steerRateCost']: + print(CP.steerRateCost) + print(rtt_params['steerRateCost']) + CP.steerRateCost = rtt_params['steerRateCost'] + rt_mpc_flag = True + print('Real-Time Tuning: CP.steerRateCost changed - Re-initializing lateral MPC.') + else: + rt_mpc_flag = False + # TODO: try/except the open + # Write the pickle file back so if we fixed any data errors the revised values will show up on the client-side + with open(rt_tuning_file, "wb") as f_write: + pickle.dump(rtt_params, f_write, -1) # Dump to file with highest protocol (fastest) + # Set the last modified time to this write.... we don't need to read back in what we just wrote out + # Only set this if we were able to successfully make the write (once the try/except is added) + last_mod_time = os.path.getmtime(rt_tuning_file) + # Make updates in latcontrol, etc. I'm not sure if this is actually necessary, depends on if the objects are referenced or not. Anyway, one less thing to debug atm. + VM.update_rt_params(CP) + LaC.update_rt_params(VM, rt_mpc_flag) + #print('RTT Last_mod_time: {0}'.format(last_mod_time)) + + ####### END OF REAL-TIME TUNING ADD-ON ####### + # *** run loop at fixed rate *** rk.keep_time() diff --git a/selfdrive/controls/lib/latcontrol.py b/selfdrive/controls/lib/latcontrol.py index e1fd68b90df397..0dc2e00858fb78 100644 --- a/selfdrive/controls/lib/latcontrol.py +++ b/selfdrive/controls/lib/latcontrol.py @@ -30,6 +30,16 @@ def __init__(self, VM): self.last_cloudlog_t = 0.0 self.setup_mpc(VM.CP.steerRateCost) + def update_rt_params(self, VM, rt_mpc_flag): + # TODO: Is this really necessary, or is the original reference preserved through the cap n' proto setup? + # Real-time tuning: Update these values from the CP if called from real-time tuning logic in controlsd + self.pid._k_p = (VM.CP.steerKpBP, VM.CP.steerKpV) # proportional gain + self.pid._k_i = (VM.CP.steerKiBP, VM.CP.steerKiV) # integral gain + self.pid.k_f = VM.CP.steerKf # feedforward gain + # Re-init the MPC with the new steerRateCost if it changed + if rt_mpc_flag: + self.rtt_reset_mpc = True + def setup_mpc(self, steer_rate_cost): self.libmpc = libmpc_py.libmpc self.libmpc.init(MPC_COST_LAT.PATH, MPC_COST_LAT.LANE, MPC_COST_LAT.HEADING, steer_rate_cost) @@ -48,6 +58,7 @@ def setup_mpc(self, steer_rate_cost): self.angle_steers_des_mpc = 0.0 self.angle_steers_des_prev = 0.0 self.angle_steers_des_time = 0.0 + self.rtt_reset_mpc = False def reset(self): self.pid.reset() @@ -86,6 +97,13 @@ def update(self, active, v_ego, angle_steers, steer_override, d_poly, angle_offs self.angle_steers_des_time = cur_time self.mpc_updated = True + # Real-Time Tuning: Reset MPC if steerRateCost changed + # TODO: Figure out if this is the best way to accomplish the real-time change to steerRateCost + if self.rtt_reset_mpc: + self.libmpc.init(MPC_COST_LAT.PATH, MPC_COST_LAT.LANE, MPC_COST_LAT.HEADING, VM.CP.steerRateCost) + self.cur_state[0].delta = math.radians(angle_steers) / VM.CP.steerRatio + self.rtt_reset_mpc = False + # Check for infeasable MPC solution self.mpc_nans = np.any(np.isnan(list(self.mpc_solution[0].delta))) t = sec_since_boot() diff --git a/selfdrive/controls/lib/vehicle_model.py b/selfdrive/controls/lib/vehicle_model.py old mode 100755 new mode 100644 index a42b8112a78d0b..2335d90ccebea5 --- a/selfdrive/controls/lib/vehicle_model.py +++ b/selfdrive/controls/lib/vehicle_model.py @@ -61,6 +61,19 @@ def __init__(self, CP, init_state=np.asarray([[0.], [0.]])): self.sR = CP.steerRatio self.chi = CP.steerRatioRear + def update_rt_params(self, CP): + # Update parameters used in real-time tuning if called from real-time tuning logic in controlsd + # TODO: Determine if really necessary + #self.m = CP.mass + #self.j = CP.rotationalInertia + #self.l = CP.wheelbase + #self.aF = CP.centerToFront + #self.aR = CP.wheelbase - CP.centerToFront + self.cF = CP.tireStiffnessFront + self.cR = CP.tireStiffnessRear + self.sR = CP.steerRatio + #self.chi = CP.steerRatioRear + def update_state(self, state): self.state = state diff --git a/selfdrive/rtt_cl_interface.py b/selfdrive/rtt_cl_interface.py new file mode 100644 index 00000000000000..21ded9579391dc --- /dev/null +++ b/selfdrive/rtt_cl_interface.py @@ -0,0 +1,108 @@ +# Quick and dirty real-time tuning interface from the command-line +# Will only work once controlsd has had a chance to write the real-time tuning carparams the first time. +# +# v0.01 James-T1: Initial code + +import pickle +import sys + +def isDigit(x): + try: + float(x) + return True + except (ValueError, TypeError) as e: + return False + +rt_tuning_file = '/data/.openpilot_rtt_params.pkl' + +# Loop forever (ctrl-C and then Enter key to end script) +while 1: + with open(rt_tuning_file, "rb") as f_read: + rtt_params = pickle.load(f_read) + + key_list = [] + + print('') + cnt = 0 + for key in sorted(rtt_params.keys()): + print('{0}: {1}'.format(cnt, key)) + key_list.append(key) + cnt += 1 + + print('') + sys.stdin = open('/dev/tty') + entry = raw_input('Enter parameter number to modify: ') + + # Data checking + try: + int(entry) + except ValueError: + print ('Please re-enter a valid parameter number.') + continue + param_num = int(entry) + if param_num < 0 or param_num >= len(key_list): + print('Please re-enter a valid parameter number.') + continue + + print('') + print('Old value:') + key = key_list[param_num] + original_param_is_list = False + if isDigit(rtt_params[key]): + print(' {0}: {1:.6f}'.format(key, rtt_params[key])) + else: + print(' {0}: {1}'.format(key, rtt_params[key])) + original_param_is_list = True + print('') + entry = raw_input('Enter new value: ') + print('') + #print(entry) + # Check to see if a list was entered... basically anything with a comma. + if ',' in entry or ('[' in entry and ']' in entry): + if not original_param_is_list: + print('Original value was float, new value entered is a list. Try again.') + print('') + continue + entry = entry.replace('[','').replace(']','') + processed_entry = [float(s) for s in entry.split(',') if isDigit(s)] + if len(processed_entry) == 0: + print('Invalid list entry. Try again.') + print('') + continue + if len(processed_entry) != len(rtt_params[key]): + print('New list length does not match length of original list. Try again.') + print('') + continue + elif isDigit(entry): + if original_param_is_list: + print('Original value was list, new value entered is a float. Try again.') + print('') + continue + processed_entry = float(entry) + else: + print('Invalid value entered. Try again.') + print('') + continue + + print('New value:') + if isDigit(processed_entry): + print(' {0}: {1:.6f}'.format(key, processed_entry)) + else: + # must be a list. + print(' {0}: {1}'.format(key, processed_entry)) + + print('') + confirm = raw_input('Type "y" to confirm + save or any other key to escape: ') + if confirm.lower() == 'y': + print('Confirmed. Writing to real-time tuning file.') + print('') + # Set it to this value + rtt_params[key] = processed_entry + # Save the file + with open(rt_tuning_file, "wb") as f_write: + pickle.dump(rtt_params, f_write, -1) # Dump to file with highest protocol (fastest) + else: + print('Escaped!') + print('') + + diff --git a/selfdrive/rtt_pickle_print.py b/selfdrive/rtt_pickle_print.py new file mode 100644 index 00000000000000..dd2a0efb05aeea --- /dev/null +++ b/selfdrive/rtt_pickle_print.py @@ -0,0 +1,13 @@ +# Quick and dirty tool to print the real-time tuning data from the command line +# Will only work once controlsd has had a chance to write the real-time tuning carparams the first time. +# +# v0.01 James-T1: Initial code + +import pickle + +rt_tuning_file = '/data/.openpilot_rtt_params.pkl' + +with open(rt_tuning_file, "rb") as f_read: + rtt_params = pickle.load(f_read) + +print(rtt_params) \ No newline at end of file