From b42965693e1b3d995e9025581a633c61fb1b61bd Mon Sep 17 00:00:00 2001 From: kegman <8837066+kegman@users.noreply.github.com> Date: Fri, 9 Nov 2018 10:24:13 -0500 Subject: [PATCH] Adjustable Follow Distance w/HUD (#12) * TR * TR * TR * TR * TR * TR * TR * TR * remove readdistancelines - causes boot error * copying cereal * arne's cereal * HUD distance lines display * capnpp add * revert * revert * Set ACC HUD to KPH * Test - set HUD_ACC distance line to 2 * revert cruise_kph LKAS error * TR * TR * TR * TR * TR * TR * TR * TR * remove readdistancelines - causes boot error * copying cereal * HUD distance lines display * capnpp add * revert * revert * Set ACC HUD to KPH * Test - set HUD_ACC distance line to 2 * revert cruise_kph LKAS error * Remove custom btn var line could be crashing the code when button pressed * HUD distance lines * HUD distance lines * passing CS.CP.readdistancelines * Hardcode hud object dist_lines var * Initializes CS.readdistancelines before passing to hud obj * use integer read_distance_lines instead * Add HUD_DISTANCE_3 for horizontal HUD bars on dash * Default to 2 bars follow distance (1.8s) --- cereal/Makefile | 8 +-- cereal/__init__.py | 1 + cereal/car.capnp | 7 ++ cereal/ui.capnp | 64 ++++++++++++++++++ selfdrive/car/honda/carcontroller.py | 11 ++- selfdrive/car/honda/carstate.py | 22 +++++- selfdrive/car/honda/hondacan.py | 3 +- selfdrive/car/honda/interface.py | 2 + selfdrive/controls/lib/drive_helpers.py | 2 +- .../controls/lib/longitudinal_mpc/Makefile | 2 +- .../lib/longitudinal_mpc/fixdistancevar.sh | 2 + .../lib_mpc_export/acado_solver.c | 64 +++++++++--------- .../lib_mpc_export/acado_solver.o | Bin 307280 -> 306984 bytes .../controls/lib/longitudinal_mpc/libmpc1.so | Bin 446096 -> 446096 bytes .../lib/longitudinal_mpc/libmpc_py.py | 2 +- .../lib/longitudinal_mpc/longitudinal_mpc.c | 6 +- .../lib/longitudinal_mpc/longitudinal_mpc.o | Bin 3976 -> 4000 bytes selfdrive/controls/lib/planner.py | 38 ++++++++++- 18 files changed, 184 insertions(+), 50 deletions(-) create mode 100644 cereal/ui.capnp create mode 100644 selfdrive/controls/lib/longitudinal_mpc/fixdistancevar.sh diff --git a/cereal/Makefile b/cereal/Makefile index dc6b7f9d51fa03..e13b059f633059 100644 --- a/cereal/Makefile +++ b/cereal/Makefile @@ -1,14 +1,14 @@ PWD := $(shell pwd) -SRCS := log.capnp car.capnp +SRCS := log.capnp car.capnp ui.capnp -GENS := gen/cpp/car.capnp.c++ gen/cpp/log.capnp.c++ -JS := gen/js/car.capnp.js gen/js/log.capnp.js +GENS := gen/cpp/car.capnp.c++ gen/cpp/log.capnp.c++ gen/cpp/ui.capnp.c++ +JS := gen/js/car.capnp.js gen/js/log.capnp.js gen/js/ui.capnp.js UNAME_M ?= $(shell uname -m) # only generate C++ for docker tests ifneq ($(OPTEST),1) - GENS += gen/c/car.capnp.c gen/c/log.capnp.c gen/c/include/c++.capnp.h gen/c/include/java.capnp.h + GENS += gen/c/car.capnp.c gen/c/log.capnp.c gen/c/ui.capnp.c gen/c/include/c++.capnp.h gen/c/include/java.capnp.h ifeq ($(UNAME_M),x86_64) ifneq (, $(shell which capnpc-java)) diff --git a/cereal/__init__.py b/cereal/__init__.py index 2d3b48526b2c34..2638b0c07520fd 100644 --- a/cereal/__init__.py +++ b/cereal/__init__.py @@ -6,3 +6,4 @@ log = capnp.load(os.path.join(CEREAL_PATH, "log.capnp")) car = capnp.load(os.path.join(CEREAL_PATH, "car.capnp")) +ui = capnp.load(os.path.join(CEREAL_PATH, "ui.capnp")) diff --git a/cereal/car.capnp b/cereal/car.capnp index bf982f8aba0da6..58a834b8b0670e 100644 --- a/cereal/car.capnp +++ b/cereal/car.capnp @@ -116,6 +116,13 @@ struct CarState { leftBlinker @20 :Bool; rightBlinker @21 :Bool; genericToggle @23 :Bool; + blindspot @26 :Bool; + distanceToggle @27 :Float32; + laneDepartureToggle @28 :Bool; + accSlowToggle @29 :Bool; + blindspotside @30 :Float32; + readdistancelines @31 :Float32; + gasbuttonstatus @32 :Float32; # lock info doorOpen @24 :Bool; diff --git a/cereal/ui.capnp b/cereal/ui.capnp new file mode 100644 index 00000000000000..82db24287d30ee --- /dev/null +++ b/cereal/ui.capnp @@ -0,0 +1,64 @@ +using Cxx = import "./include/c++.capnp"; +$Cxx.namespace("cereal"); + +using Java = import "./include/java.capnp"; +$Java.package("ai.comma.openpilot.cereal"); +$Java.outerClassname("Ui"); + +using Car = import "car.capnp"; + +@0xce6ca45dddcd5317; + +struct UIButtonInfo { + # button ID 0..5 + btnId @0 :Int8; + # internal button name + btnName @1 :Text; + # display label for button (3 chars) + btnLabel @2 :Text; + # buttons status: 0 = DISABLED, 1 = AVAILABLE, 2 = ENABLED, 3 = WARNING, 9 = NOT AVAILABLE + btnStatus @3 :Int16; + # small font label shows below the main label, max 7 chars + btnLabel2 @4 :Text; +} + +struct UIButtonStatus { + # button ID 0..5 + btnId @0 :Int8; + # buttons status: 0 = DISABLED, 1 = AVAILABLE, 2 = ENABLED, 3 = WARNING, 9 = NOT AVAILABLE + btnStatus @1 :Int16; +} + +struct UICustomAlert { + caStatus @0 :Int8; + caText @1 :Text; +} + +struct UISetCar { + icCarFolder @0 :Text; + icCarName @1 :Text; +} + +struct UIPlaySound { + sndSound @0 :Int8; +} + +struct UIUpdate { + uiDoUpdate @0 :Int8; + uiStatus @1 :Int8; + uiCanDisplayMessage @2 :Int8; +} + +#struct UIEvent { +# # in nanoseconds? +# logMonoTime @0 :UInt64; +# +# union { +# uiButtonInfo @1 :UIButtonInfo; +# uiCustomAlert @2 :UICustomAlert; +# uiSetCar @3 :UISetCar; +# uiButtonStatus @4 :UIButtonStatus; +# uiUpdate @5 :UIUpdate; +# uiPlaySound @6 :UIPlaySound; +# } +#} diff --git a/selfdrive/car/honda/carcontroller.py b/selfdrive/car/honda/carcontroller.py index 1cb9830e39f26e..f09b6ab81cbd0a 100644 --- a/selfdrive/car/honda/carcontroller.py +++ b/selfdrive/car/honda/carcontroller.py @@ -70,7 +70,7 @@ def process_hud_alert(hud_alert): HUDData = namedtuple("HUDData", ["pcm_accel", "v_cruise", "mini_car", "car", "X4", - "lanes", "beep", "chime", "fcw", "acc_alert", "steer_required"]) + "lanes", "beep", "chime", "fcw", "acc_alert", "steer_required", "dist_lines"]) class CarController(object): @@ -118,7 +118,12 @@ def update(self, sendcan, enabled, CS, frame, actuators, \ hud_car = 1 else: hud_car = 0 - + + #if CS.readdistancelines == 0 or CS.readdistancelines == 1 or CS.readdistancelines == 2 or CS.readdistancelines == 3: + # distance_lines = CS.readdistancelines + #else: + # distance_lines = 1 + # For lateral control-only, send chimes as a beep since we don't send 0x1fa if CS.CP.radarOffCan: snd_beep = snd_beep if snd_beep is not 0 else snd_chime @@ -127,7 +132,7 @@ def update(self, sendcan, enabled, CS, frame, actuators, \ fcw_display, steer_required, acc_alert = process_hud_alert(hud_alert) hud = HUDData(int(pcm_accel), int(round(hud_v_cruise)), 1, hud_car, - 0xc1, hud_lanes, int(snd_beep), snd_chime, fcw_display, acc_alert, steer_required) + 0xc1, hud_lanes, int(snd_beep), snd_chime, fcw_display, acc_alert, steer_required, CS.read_distance_lines) # **** process the car messages **** diff --git a/selfdrive/car/honda/carstate.py b/selfdrive/car/honda/carstate.py index 99d4a2214300c2..7523b32a193784 100644 --- a/selfdrive/car/honda/carstate.py +++ b/selfdrive/car/honda/carstate.py @@ -143,6 +143,9 @@ def get_cam_can_parser(CP): class CarState(object): def __init__(self, CP): + self.trLabels = ["0.9","1.8","2.7"] + self.trMode = 1 + self.read_distance_lines_prev = 3 self.CP = CP self.can_define = CANDefine(DBC[CP.carFingerprint]['pt']) self.shifter_values = self.can_define.dv["GEARBOX"]["GEAR_SHIFTER"] @@ -183,7 +186,7 @@ def update(self, cp, cp_cam): # update prevs, update must run once per loop self.prev_cruise_buttons = self.cruise_buttons - self.prev_cruise_setting = self.cruise_setting + #self.prev_cruise_setting = self.cruise_setting self.prev_blinker_on = self.blinker_on self.prev_left_blinker_on = self.left_blinker_on @@ -242,7 +245,7 @@ def update(self, cp, cp_cam): self.angle_steers = cp.vl["STEERING_SENSORS"]['STEER_ANGLE'] self.angle_steers_rate = cp.vl["STEERING_SENSORS"]['STEER_ANGLE_RATE'] - self.cruise_setting = cp.vl["SCM_BUTTONS"]['CRUISE_SETTING'] + #self.cruise_setting = cp.vl["SCM_BUTTONS"]['CRUISE_SETTING'] self.cruise_buttons = cp.vl["SCM_BUTTONS"]['CRUISE_BUTTONS'] self.blinker_on = cp.vl["SCM_FEEDBACK"]['LEFT_BLINKER'] or cp.vl["SCM_FEEDBACK"]['RIGHT_BLINKER'] @@ -303,6 +306,21 @@ def update(self, cp, cp_cam): self.user_brake = cp.vl["VSA_STATUS"]['USER_BRAKE'] self.pcm_acc_status = cp.vl["POWERTRAIN_DATA"]['ACC_STATUS'] self.hud_lead = cp.vl["ACC_HUD"]['HUD_LEAD'] + + if self.cruise_setting == 3: + if cp.vl["SCM_BUTTONS"]["CRUISE_SETTING"] == 0: + self.trMode = (self.trMode + 1 ) % 3 + self.prev_cruise_setting = self.cruise_setting + self.cruise_setting = cp.vl["SCM_BUTTONS"]['CRUISE_SETTING'] + self.read_distance_lines = self.trMode + 1 + if self.read_distance_lines <> self.read_distance_lines_prev: + # if self.read_distance_lines == 1: + # self.UE.custom_alert_message(2,"Following distance set to 0.9s",200,3) + # if self.read_distance_lines == 2: + # self.UE.custom_alert_message(2,"Following distance set to 1.8s",200,3) + # if self.read_distance_lines == 3: + # self.UE.custom_alert_message(2,"Following distance set to 2.7s",200,3) + self.read_distance_lines_prev = self.read_distance_lines # carstate standalone tester diff --git a/selfdrive/car/honda/hondacan.py b/selfdrive/car/honda/hondacan.py index 3d92d24a7c2924..58178acb39edef 100644 --- a/selfdrive/car/honda/hondacan.py +++ b/selfdrive/car/honda/hondacan.py @@ -77,9 +77,10 @@ def create_ui_commands(packer, pcm_speed, hud, car_fingerprint, idx): 'CRUISE_SPEED': hud.v_cruise, 'ENABLE_MINI_CAR': hud.mini_car, 'HUD_LEAD': hud.car, - 'SET_ME_X03': 0x03, + 'SET_ME_X03': hud.dist_lines, 'SET_ME_X03_2': 0x03, 'SET_ME_X01': 0x01, + 'HUD_DISTANCE_3': 1, } commands.append(packer.make_can_msg("ACC_HUD", 0, acc_hud_values, idx)) diff --git a/selfdrive/car/honda/interface.py b/selfdrive/car/honda/interface.py index 6a4a007e0c2c79..3e2cf76efaebd1 100755 --- a/selfdrive/car/honda/interface.py +++ b/selfdrive/car/honda/interface.py @@ -432,6 +432,8 @@ def update(self, c): ret.cruiseState.available = bool(self.CS.main_on) ret.cruiseState.speedOffset = self.CS.cruise_speed_offset ret.cruiseState.standstill = False + + ret.readdistancelines = self.CS.read_distance_lines # TODO: button presses buttonEvents = [] diff --git a/selfdrive/controls/lib/drive_helpers.py b/selfdrive/controls/lib/drive_helpers.py index 9d78c966743569..1faefc4dd08f31 100644 --- a/selfdrive/controls/lib/drive_helpers.py +++ b/selfdrive/controls/lib/drive_helpers.py @@ -18,7 +18,7 @@ class MPC_COST_LAT: class MPC_COST_LONG: TTC = 5.0 - DISTANCE = 0.1 + DISTANCE = 0.8 ACCELERATION = 10.0 JERK = 20.0 diff --git a/selfdrive/controls/lib/longitudinal_mpc/Makefile b/selfdrive/controls/lib/longitudinal_mpc/Makefile index 17f78eada990df..93cb0a880ef403 100644 --- a/selfdrive/controls/lib/longitudinal_mpc/Makefile +++ b/selfdrive/controls/lib/longitudinal_mpc/Makefile @@ -72,7 +72,7 @@ lib_qp/%.o: $(PHONELIBS)/qpoases/SRC/%.cpp -c -o '$@' '$<' generator: generator.cpp - $(CXX) -Wall -std=c++11 \ + $(CXX) -v -Wall -std=c++11 \ generator.cpp \ -o generator \ $(ACADO_FLAGS) \ diff --git a/selfdrive/controls/lib/longitudinal_mpc/fixdistancevar.sh b/selfdrive/controls/lib/longitudinal_mpc/fixdistancevar.sh new file mode 100644 index 00000000000000..2098f2ab08851a --- /dev/null +++ b/selfdrive/controls/lib/longitudinal_mpc/fixdistancevar.sh @@ -0,0 +1,2 @@ +#!/usr/bin/env bash +sed -i -e 's/1\.2345678[^)]*)/TR)/g' -e 's/out)/out, double TR)/g' -e 's/Objective( )/Objective(double TR)/g' -e 's/ValueOut )/ValueOut, TR )/g' -e 's/ionStep( )/ionStep(double TR)/g' -e 's/Objective(double TR);/Objective(TR);/g' mpc_export/acado_solver.c 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 676787bbfcbc7b..8bd1970888e5ab 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 @@ -104,7 +104,7 @@ acadoWorkspace.evGu[lRun1 * 6 + 5] = acadoWorkspace.state[47]; 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 + 6; @@ -113,31 +113,31 @@ 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))-((xd[4]-xd[1])*(real_t)(1.8000000000000000e+00)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((xd[4]*xd[4])/(real_t)(1.9620000000000001e+01)))+(real_t)(4.0000000000000000e+00))-(xd[3]-xd[0]))/(a[0]+(real_t)(1.0000000000000001e-01)))))); +a[1] = (exp(((real_t)(2.9999999999999999e-01)*(((((((xd[1]*(real_t)(TR))-((xd[4]-xd[1])*(real_t)(TR)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((xd[4]*xd[4])/(real_t)(1.9620000000000001e+01)))+(real_t)(4.0000000000000000e+00))-(xd[3]-xd[0]))/(a[0]+(real_t)(1.0000000000000001e-01)))))); a[2] = (sqrt((xd[1]+(real_t)(5.0000000000000000e-01)))); -a[3] = (exp(((real_t)(2.9999999999999999e-01)*(((((((xd[1]*(real_t)(1.8000000000000000e+00))-((xd[4]-xd[1])*(real_t)(1.8000000000000000e+00)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((xd[4]*xd[4])/(real_t)(1.9620000000000001e+01)))+(real_t)(4.0000000000000000e+00))-((real_t)(4.0000000000000000e+00)+((((xd[1]*(real_t)(1.8000000000000000e+00))-((xd[4]-xd[1])*(real_t)(1.8000000000000000e+00)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((xd[4]*xd[4])/(real_t)(1.9620000000000001e+01)))))/(a[2]+(real_t)(1.0000000000000001e-01)))))); +a[3] = (exp(((real_t)(2.9999999999999999e-01)*(((((((xd[1]*(real_t)(TR))-((xd[4]-xd[1])*(real_t)(TR)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((xd[4]*xd[4])/(real_t)(1.9620000000000001e+01)))+(real_t)(4.0000000000000000e+00))-((real_t)(4.0000000000000000e+00)+((((xd[1]*(real_t)(TR))-((xd[4]-xd[1])*(real_t)(TR)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((xd[4]*xd[4])/(real_t)(1.9620000000000001e+01)))))/(a[2]+(real_t)(1.0000000000000001e-01)))))); a[4] = ((real_t)(1.0000000000000000e+00)/(a[0]+(real_t)(1.0000000000000001e-01))); -a[5] = (exp(((real_t)(2.9999999999999999e-01)*(((((((xd[1]*(real_t)(1.8000000000000000e+00))-((xd[4]-xd[1])*(real_t)(1.8000000000000000e+00)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((xd[4]*xd[4])/(real_t)(1.9620000000000001e+01)))+(real_t)(4.0000000000000000e+00))-(xd[3]-xd[0]))/(a[0]+(real_t)(1.0000000000000001e-01)))))); +a[5] = (exp(((real_t)(2.9999999999999999e-01)*(((((((xd[1]*(real_t)(TR))-((xd[4]-xd[1])*(real_t)(TR)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((xd[4]*xd[4])/(real_t)(1.9620000000000001e+01)))+(real_t)(4.0000000000000000e+00))-(xd[3]-xd[0]))/(a[0]+(real_t)(1.0000000000000001e-01)))))); a[6] = (((real_t)(2.9999999999999999e-01)*(((real_t)(0.0000000000000000e+00)-((real_t)(0.0000000000000000e+00)-(real_t)(1.0000000000000000e+00)))*a[4]))*a[5]); a[7] = ((real_t)(1.0000000000000000e+00)/(real_t)(1.9620000000000001e+01)); a[8] = (1.0/sqrt((xd[1]+(real_t)(5.0000000000000000e-01)))); a[9] = (a[8]*(real_t)(5.0000000000000000e-01)); a[10] = (a[4]*a[4]); -a[11] = (((real_t)(2.9999999999999999e-01)*(((((real_t)(1.8000000000000000e+00)-((real_t)(-1.8000000000000000e+00)))+((xd[1]+xd[1])*a[7]))*a[4])-((((((((xd[1]*(real_t)(1.8000000000000000e+00))-((xd[4]-xd[1])*(real_t)(1.8000000000000000e+00)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((xd[4]*xd[4])/(real_t)(1.9620000000000001e+01)))+(real_t)(4.0000000000000000e+00))-(xd[3]-xd[0]))*a[9])*a[10])))*a[5]); +a[11] = (((real_t)(2.9999999999999999e-01)*(((((real_t)(TR)-((real_t)(-TR)))+((xd[1]+xd[1])*a[7]))*a[4])-((((((((xd[1]*(real_t)(TR))-((xd[4]-xd[1])*(real_t)(TR)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((xd[4]*xd[4])/(real_t)(1.9620000000000001e+01)))+(real_t)(4.0000000000000000e+00))-(xd[3]-xd[0]))*a[9])*a[10])))*a[5]); a[12] = ((real_t)(1.0000000000000000e+00)/(real_t)(1.9620000000000001e+01)); a[13] = ((real_t)(1.0000000000000000e+00)/(real_t)(1.9620000000000001e+01)); a[14] = ((real_t)(1.0000000000000000e+00)/(a[2]+(real_t)(1.0000000000000001e-01))); a[15] = (1.0/sqrt((xd[1]+(real_t)(5.0000000000000000e-01)))); a[16] = (a[15]*(real_t)(5.0000000000000000e-01)); a[17] = (a[14]*a[14]); -a[18] = (exp(((real_t)(2.9999999999999999e-01)*(((((((xd[1]*(real_t)(1.8000000000000000e+00))-((xd[4]-xd[1])*(real_t)(1.8000000000000000e+00)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((xd[4]*xd[4])/(real_t)(1.9620000000000001e+01)))+(real_t)(4.0000000000000000e+00))-((real_t)(4.0000000000000000e+00)+((((xd[1]*(real_t)(1.8000000000000000e+00))-((xd[4]-xd[1])*(real_t)(1.8000000000000000e+00)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((xd[4]*xd[4])/(real_t)(1.9620000000000001e+01)))))/(a[2]+(real_t)(1.0000000000000001e-01)))))); -a[19] = (((real_t)(2.9999999999999999e-01)*((((((real_t)(1.8000000000000000e+00)-((real_t)(-1.8000000000000000e+00)))+((xd[1]+xd[1])*a[12]))-(((real_t)(1.8000000000000000e+00)-((real_t)(-1.8000000000000000e+00)))+((xd[1]+xd[1])*a[13])))*a[14])-((((((((xd[1]*(real_t)(1.8000000000000000e+00))-((xd[4]-xd[1])*(real_t)(1.8000000000000000e+00)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((xd[4]*xd[4])/(real_t)(1.9620000000000001e+01)))+(real_t)(4.0000000000000000e+00))-((real_t)(4.0000000000000000e+00)+((((xd[1]*(real_t)(1.8000000000000000e+00))-((xd[4]-xd[1])*(real_t)(1.8000000000000000e+00)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((xd[4]*xd[4])/(real_t)(1.9620000000000001e+01)))))*a[16])*a[17])))*a[18]); +a[18] = (exp(((real_t)(2.9999999999999999e-01)*(((((((xd[1]*(real_t)(TR))-((xd[4]-xd[1])*(real_t)(TR)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((xd[4]*xd[4])/(real_t)(1.9620000000000001e+01)))+(real_t)(4.0000000000000000e+00))-((real_t)(4.0000000000000000e+00)+((((xd[1]*(real_t)(TR))-((xd[4]-xd[1])*(real_t)(TR)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((xd[4]*xd[4])/(real_t)(1.9620000000000001e+01)))))/(a[2]+(real_t)(1.0000000000000001e-01)))))); +a[19] = (((real_t)(2.9999999999999999e-01)*((((((real_t)(TR)-((real_t)(-TR)))+((xd[1]+xd[1])*a[12]))-(((real_t)(TR)-((real_t)(-TR)))+((xd[1]+xd[1])*a[13])))*a[14])-((((((((xd[1]*(real_t)(TR))-((xd[4]-xd[1])*(real_t)(TR)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((xd[4]*xd[4])/(real_t)(1.9620000000000001e+01)))+(real_t)(4.0000000000000000e+00))-((real_t)(4.0000000000000000e+00)+((((xd[1]*(real_t)(TR))-((xd[4]-xd[1])*(real_t)(TR)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((xd[4]*xd[4])/(real_t)(1.9620000000000001e+01)))))*a[16])*a[17])))*a[18]); a[20] = (((real_t)(2.9999999999999999e-01)*(((real_t)(0.0000000000000000e+00)-(real_t)(1.0000000000000000e+00))*a[4]))*a[5]); a[21] = ((real_t)(1.0000000000000000e+00)/(real_t)(1.9620000000000001e+01)); -a[22] = (((real_t)(2.9999999999999999e-01)*((((real_t)(0.0000000000000000e+00)-(real_t)(1.8000000000000000e+00))-((xd[4]+xd[4])*a[21]))*a[4]))*a[5]); +a[22] = (((real_t)(2.9999999999999999e-01)*((((real_t)(0.0000000000000000e+00)-(real_t)(TR))-((xd[4]+xd[4])*a[21]))*a[4]))*a[5]); a[23] = ((real_t)(1.0000000000000000e+00)/(real_t)(1.9620000000000001e+01)); a[24] = ((real_t)(1.0000000000000000e+00)/(real_t)(1.9620000000000001e+01)); -a[25] = (((real_t)(2.9999999999999999e-01)*(((((real_t)(0.0000000000000000e+00)-(real_t)(1.8000000000000000e+00))-((xd[4]+xd[4])*a[23]))-(((real_t)(0.0000000000000000e+00)-(real_t)(1.8000000000000000e+00))-((xd[4]+xd[4])*a[24])))*a[14]))*a[18]); +a[25] = (((real_t)(2.9999999999999999e-01)*(((((real_t)(0.0000000000000000e+00)-(real_t)(TR))-((xd[4]+xd[4])*a[23]))-(((real_t)(0.0000000000000000e+00)-(real_t)(TR))-((xd[4]+xd[4])*a[24])))*a[14]))*a[18]); a[26] = ((real_t)(1.0000000000000000e+00)/(((real_t)(5.0000000000000003e-02)*xd[1])+(real_t)(5.0000000000000000e-01))); a[27] = ((real_t)(1.0000000000000000e+00)/(real_t)(1.9620000000000001e+01)); a[28] = (a[26]*a[26]); @@ -145,7 +145,7 @@ a[29] = ((real_t)(1.0000000000000000e+00)/(real_t)(1.9620000000000001e+01)); /* Compute outputs: */ out[0] = (a[1]-a[3]); -out[1] = (((xd[3]-xd[0])-((real_t)(4.0000000000000000e+00)+((((xd[1]*(real_t)(1.8000000000000000e+00))-((xd[4]-xd[1])*(real_t)(1.8000000000000000e+00)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((xd[4]*xd[4])/(real_t)(1.9620000000000001e+01)))))/(((real_t)(5.0000000000000003e-02)*xd[1])+(real_t)(5.0000000000000000e-01))); +out[1] = (((xd[3]-xd[0])-((real_t)(4.0000000000000000e+00)+((((xd[1]*(real_t)(TR))-((xd[4]-xd[1])*(real_t)(TR)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((xd[4]*xd[4])/(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[6]; @@ -155,10 +155,10 @@ out[7] = a[20]; out[8] = (a[22]-a[25]); out[9] = (real_t)(0.0000000000000000e+00); out[10] = (((real_t)(0.0000000000000000e+00)-(real_t)(1.0000000000000000e+00))*a[26]); -out[11] = ((((real_t)(0.0000000000000000e+00)-(((real_t)(1.8000000000000000e+00)-((real_t)(-1.8000000000000000e+00)))+((xd[1]+xd[1])*a[27])))*a[26])-((((xd[3]-xd[0])-((real_t)(4.0000000000000000e+00)+((((xd[1]*(real_t)(1.8000000000000000e+00))-((xd[4]-xd[1])*(real_t)(1.8000000000000000e+00)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((xd[4]*xd[4])/(real_t)(1.9620000000000001e+01)))))*(real_t)(5.0000000000000003e-02))*a[28])); +out[11] = ((((real_t)(0.0000000000000000e+00)-(((real_t)(TR)-((real_t)(-TR)))+((xd[1]+xd[1])*a[27])))*a[26])-((((xd[3]-xd[0])-((real_t)(4.0000000000000000e+00)+((((xd[1]*(real_t)(TR))-((xd[4]-xd[1])*(real_t)(TR)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((xd[4]*xd[4])/(real_t)(1.9620000000000001e+01)))))*(real_t)(5.0000000000000003e-02))*a[28])); out[12] = (real_t)(0.0000000000000000e+00); out[13] = a[26]; -out[14] = (((real_t)(0.0000000000000000e+00)-(((real_t)(0.0000000000000000e+00)-(real_t)(1.8000000000000000e+00))-((xd[4]+xd[4])*a[29])))*a[26]); +out[14] = (((real_t)(0.0000000000000000e+00)-(((real_t)(0.0000000000000000e+00)-(real_t)(TR))-((xd[4]+xd[4])*a[29])))*a[26]); out[15] = (real_t)(0.0000000000000000e+00); out[16] = (real_t)(0.0000000000000000e+00); out[17] = (xd[2]*(real_t)(1.0000000000000001e-01)); @@ -178,7 +178,7 @@ out[30] = (real_t)(0.0000000000000000e+00); out[31] = (((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; /* Vector of auxiliary variables; number of elements: 30. */ @@ -186,31 +186,31 @@ 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))-((xd[4]-xd[1])*(real_t)(1.8000000000000000e+00)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((xd[4]*xd[4])/(real_t)(1.9620000000000001e+01)))+(real_t)(4.0000000000000000e+00))-(xd[3]-xd[0]))/(a[0]+(real_t)(1.0000000000000001e-01)))))); +a[1] = (exp(((real_t)(2.9999999999999999e-01)*(((((((xd[1]*(real_t)(TR))-((xd[4]-xd[1])*(real_t)(TR)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((xd[4]*xd[4])/(real_t)(1.9620000000000001e+01)))+(real_t)(4.0000000000000000e+00))-(xd[3]-xd[0]))/(a[0]+(real_t)(1.0000000000000001e-01)))))); a[2] = (sqrt((xd[1]+(real_t)(5.0000000000000000e-01)))); -a[3] = (exp(((real_t)(2.9999999999999999e-01)*(((((((xd[1]*(real_t)(1.8000000000000000e+00))-((xd[4]-xd[1])*(real_t)(1.8000000000000000e+00)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((xd[4]*xd[4])/(real_t)(1.9620000000000001e+01)))+(real_t)(4.0000000000000000e+00))-((real_t)(4.0000000000000000e+00)+((((xd[1]*(real_t)(1.8000000000000000e+00))-((xd[4]-xd[1])*(real_t)(1.8000000000000000e+00)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((xd[4]*xd[4])/(real_t)(1.9620000000000001e+01)))))/(a[2]+(real_t)(1.0000000000000001e-01)))))); +a[3] = (exp(((real_t)(2.9999999999999999e-01)*(((((((xd[1]*(real_t)(TR))-((xd[4]-xd[1])*(real_t)(TR)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((xd[4]*xd[4])/(real_t)(1.9620000000000001e+01)))+(real_t)(4.0000000000000000e+00))-((real_t)(4.0000000000000000e+00)+((((xd[1]*(real_t)(TR))-((xd[4]-xd[1])*(real_t)(TR)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((xd[4]*xd[4])/(real_t)(1.9620000000000001e+01)))))/(a[2]+(real_t)(1.0000000000000001e-01)))))); a[4] = ((real_t)(1.0000000000000000e+00)/(a[0]+(real_t)(1.0000000000000001e-01))); -a[5] = (exp(((real_t)(2.9999999999999999e-01)*(((((((xd[1]*(real_t)(1.8000000000000000e+00))-((xd[4]-xd[1])*(real_t)(1.8000000000000000e+00)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((xd[4]*xd[4])/(real_t)(1.9620000000000001e+01)))+(real_t)(4.0000000000000000e+00))-(xd[3]-xd[0]))/(a[0]+(real_t)(1.0000000000000001e-01)))))); +a[5] = (exp(((real_t)(2.9999999999999999e-01)*(((((((xd[1]*(real_t)(TR))-((xd[4]-xd[1])*(real_t)(TR)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((xd[4]*xd[4])/(real_t)(1.9620000000000001e+01)))+(real_t)(4.0000000000000000e+00))-(xd[3]-xd[0]))/(a[0]+(real_t)(1.0000000000000001e-01)))))); a[6] = (((real_t)(2.9999999999999999e-01)*(((real_t)(0.0000000000000000e+00)-((real_t)(0.0000000000000000e+00)-(real_t)(1.0000000000000000e+00)))*a[4]))*a[5]); a[7] = ((real_t)(1.0000000000000000e+00)/(real_t)(1.9620000000000001e+01)); a[8] = (1.0/sqrt((xd[1]+(real_t)(5.0000000000000000e-01)))); a[9] = (a[8]*(real_t)(5.0000000000000000e-01)); a[10] = (a[4]*a[4]); -a[11] = (((real_t)(2.9999999999999999e-01)*(((((real_t)(1.8000000000000000e+00)-((real_t)(-1.8000000000000000e+00)))+((xd[1]+xd[1])*a[7]))*a[4])-((((((((xd[1]*(real_t)(1.8000000000000000e+00))-((xd[4]-xd[1])*(real_t)(1.8000000000000000e+00)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((xd[4]*xd[4])/(real_t)(1.9620000000000001e+01)))+(real_t)(4.0000000000000000e+00))-(xd[3]-xd[0]))*a[9])*a[10])))*a[5]); +a[11] = (((real_t)(2.9999999999999999e-01)*(((((real_t)(TR)-((real_t)(-TR)))+((xd[1]+xd[1])*a[7]))*a[4])-((((((((xd[1]*(real_t)(TR))-((xd[4]-xd[1])*(real_t)(TR)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((xd[4]*xd[4])/(real_t)(1.9620000000000001e+01)))+(real_t)(4.0000000000000000e+00))-(xd[3]-xd[0]))*a[9])*a[10])))*a[5]); a[12] = ((real_t)(1.0000000000000000e+00)/(real_t)(1.9620000000000001e+01)); a[13] = ((real_t)(1.0000000000000000e+00)/(real_t)(1.9620000000000001e+01)); a[14] = ((real_t)(1.0000000000000000e+00)/(a[2]+(real_t)(1.0000000000000001e-01))); a[15] = (1.0/sqrt((xd[1]+(real_t)(5.0000000000000000e-01)))); a[16] = (a[15]*(real_t)(5.0000000000000000e-01)); a[17] = (a[14]*a[14]); -a[18] = (exp(((real_t)(2.9999999999999999e-01)*(((((((xd[1]*(real_t)(1.8000000000000000e+00))-((xd[4]-xd[1])*(real_t)(1.8000000000000000e+00)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((xd[4]*xd[4])/(real_t)(1.9620000000000001e+01)))+(real_t)(4.0000000000000000e+00))-((real_t)(4.0000000000000000e+00)+((((xd[1]*(real_t)(1.8000000000000000e+00))-((xd[4]-xd[1])*(real_t)(1.8000000000000000e+00)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((xd[4]*xd[4])/(real_t)(1.9620000000000001e+01)))))/(a[2]+(real_t)(1.0000000000000001e-01)))))); -a[19] = (((real_t)(2.9999999999999999e-01)*((((((real_t)(1.8000000000000000e+00)-((real_t)(-1.8000000000000000e+00)))+((xd[1]+xd[1])*a[12]))-(((real_t)(1.8000000000000000e+00)-((real_t)(-1.8000000000000000e+00)))+((xd[1]+xd[1])*a[13])))*a[14])-((((((((xd[1]*(real_t)(1.8000000000000000e+00))-((xd[4]-xd[1])*(real_t)(1.8000000000000000e+00)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((xd[4]*xd[4])/(real_t)(1.9620000000000001e+01)))+(real_t)(4.0000000000000000e+00))-((real_t)(4.0000000000000000e+00)+((((xd[1]*(real_t)(1.8000000000000000e+00))-((xd[4]-xd[1])*(real_t)(1.8000000000000000e+00)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((xd[4]*xd[4])/(real_t)(1.9620000000000001e+01)))))*a[16])*a[17])))*a[18]); +a[18] = (exp(((real_t)(2.9999999999999999e-01)*(((((((xd[1]*(real_t)(TR))-((xd[4]-xd[1])*(real_t)(TR)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((xd[4]*xd[4])/(real_t)(1.9620000000000001e+01)))+(real_t)(4.0000000000000000e+00))-((real_t)(4.0000000000000000e+00)+((((xd[1]*(real_t)(TR))-((xd[4]-xd[1])*(real_t)(TR)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((xd[4]*xd[4])/(real_t)(1.9620000000000001e+01)))))/(a[2]+(real_t)(1.0000000000000001e-01)))))); +a[19] = (((real_t)(2.9999999999999999e-01)*((((((real_t)(TR)-((real_t)(-TR)))+((xd[1]+xd[1])*a[12]))-(((real_t)(TR)-((real_t)(-TR)))+((xd[1]+xd[1])*a[13])))*a[14])-((((((((xd[1]*(real_t)(TR))-((xd[4]-xd[1])*(real_t)(TR)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((xd[4]*xd[4])/(real_t)(1.9620000000000001e+01)))+(real_t)(4.0000000000000000e+00))-((real_t)(4.0000000000000000e+00)+((((xd[1]*(real_t)(TR))-((xd[4]-xd[1])*(real_t)(TR)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((xd[4]*xd[4])/(real_t)(1.9620000000000001e+01)))))*a[16])*a[17])))*a[18]); a[20] = (((real_t)(2.9999999999999999e-01)*(((real_t)(0.0000000000000000e+00)-(real_t)(1.0000000000000000e+00))*a[4]))*a[5]); a[21] = ((real_t)(1.0000000000000000e+00)/(real_t)(1.9620000000000001e+01)); -a[22] = (((real_t)(2.9999999999999999e-01)*((((real_t)(0.0000000000000000e+00)-(real_t)(1.8000000000000000e+00))-((xd[4]+xd[4])*a[21]))*a[4]))*a[5]); +a[22] = (((real_t)(2.9999999999999999e-01)*((((real_t)(0.0000000000000000e+00)-(real_t)(TR))-((xd[4]+xd[4])*a[21]))*a[4]))*a[5]); a[23] = ((real_t)(1.0000000000000000e+00)/(real_t)(1.9620000000000001e+01)); a[24] = ((real_t)(1.0000000000000000e+00)/(real_t)(1.9620000000000001e+01)); -a[25] = (((real_t)(2.9999999999999999e-01)*(((((real_t)(0.0000000000000000e+00)-(real_t)(1.8000000000000000e+00))-((xd[4]+xd[4])*a[23]))-(((real_t)(0.0000000000000000e+00)-(real_t)(1.8000000000000000e+00))-((xd[4]+xd[4])*a[24])))*a[14]))*a[18]); +a[25] = (((real_t)(2.9999999999999999e-01)*(((((real_t)(0.0000000000000000e+00)-(real_t)(TR))-((xd[4]+xd[4])*a[23]))-(((real_t)(0.0000000000000000e+00)-(real_t)(TR))-((xd[4]+xd[4])*a[24])))*a[14]))*a[18]); a[26] = ((real_t)(1.0000000000000000e+00)/(((real_t)(5.0000000000000003e-02)*xd[1])+(real_t)(5.0000000000000000e-01))); a[27] = ((real_t)(1.0000000000000000e+00)/(real_t)(1.9620000000000001e+01)); a[28] = (a[26]*a[26]); @@ -218,7 +218,7 @@ a[29] = ((real_t)(1.0000000000000000e+00)/(real_t)(1.9620000000000001e+01)); /* Compute outputs: */ out[0] = (a[1]-a[3]); -out[1] = (((xd[3]-xd[0])-((real_t)(4.0000000000000000e+00)+((((xd[1]*(real_t)(1.8000000000000000e+00))-((xd[4]-xd[1])*(real_t)(1.8000000000000000e+00)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((xd[4]*xd[4])/(real_t)(1.9620000000000001e+01)))))/(((real_t)(5.0000000000000003e-02)*xd[1])+(real_t)(5.0000000000000000e-01))); +out[1] = (((xd[3]-xd[0])-((real_t)(4.0000000000000000e+00)+((((xd[1]*(real_t)(TR))-((xd[4]-xd[1])*(real_t)(TR)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((xd[4]*xd[4])/(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[6]; out[4] = (a[11]-a[19]); @@ -227,10 +227,10 @@ out[6] = a[20]; out[7] = (a[22]-a[25]); out[8] = (real_t)(0.0000000000000000e+00); out[9] = (((real_t)(0.0000000000000000e+00)-(real_t)(1.0000000000000000e+00))*a[26]); -out[10] = ((((real_t)(0.0000000000000000e+00)-(((real_t)(1.8000000000000000e+00)-((real_t)(-1.8000000000000000e+00)))+((xd[1]+xd[1])*a[27])))*a[26])-((((xd[3]-xd[0])-((real_t)(4.0000000000000000e+00)+((((xd[1]*(real_t)(1.8000000000000000e+00))-((xd[4]-xd[1])*(real_t)(1.8000000000000000e+00)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((xd[4]*xd[4])/(real_t)(1.9620000000000001e+01)))))*(real_t)(5.0000000000000003e-02))*a[28])); +out[10] = ((((real_t)(0.0000000000000000e+00)-(((real_t)(TR)-((real_t)(-TR)))+((xd[1]+xd[1])*a[27])))*a[26])-((((xd[3]-xd[0])-((real_t)(4.0000000000000000e+00)+((((xd[1]*(real_t)(TR))-((xd[4]-xd[1])*(real_t)(TR)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((xd[4]*xd[4])/(real_t)(1.9620000000000001e+01)))))*(real_t)(5.0000000000000003e-02))*a[28])); out[11] = (real_t)(0.0000000000000000e+00); out[12] = a[26]; -out[13] = (((real_t)(0.0000000000000000e+00)-(((real_t)(0.0000000000000000e+00)-(real_t)(1.8000000000000000e+00))-((xd[4]+xd[4])*a[29])))*a[26]); +out[13] = (((real_t)(0.0000000000000000e+00)-(((real_t)(0.0000000000000000e+00)-(real_t)(TR))-((xd[4]+xd[4])*a[29])))*a[26]); out[14] = (real_t)(0.0000000000000000e+00); out[15] = (real_t)(0.0000000000000000e+00); out[16] = (xd[2]*(real_t)(1.0000000000000001e-01)); @@ -371,7 +371,7 @@ tmpQN1[34] = + tmpQN2[15]*tmpFx[4] + tmpQN2[16]*tmpFx[10] + tmpQN2[17]*tmpFx[16] tmpQN1[35] = + tmpQN2[15]*tmpFx[5] + tmpQN2[16]*tmpFx[11] + tmpQN2[17]*tmpFx[17]; } -void acado_evaluateObjective( ) +void acado_evaluateObjective(double TR) { int runObj; for (runObj = 0; runObj < 20; ++runObj) @@ -386,7 +386,7 @@ acadoWorkspace.objValueIn[6] = acadoVariables.u[runObj]; acadoWorkspace.objValueIn[7] = acadoVariables.od[runObj * 2]; acadoWorkspace.objValueIn[8] = 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]; @@ -405,7 +405,7 @@ acadoWorkspace.objValueIn[4] = acadoVariables.x[124]; acadoWorkspace.objValueIn[5] = acadoVariables.x[125]; acadoWorkspace.objValueIn[6] = acadoVariables.od[40]; acadoWorkspace.objValueIn[7] = 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]; @@ -5158,12 +5158,12 @@ acado_multEDu( &(acadoWorkspace.E[ 1248 ]), &(acadoWorkspace.x[ 24 ]), &(acadoVa acado_multEDu( &(acadoWorkspace.E[ 1254 ]), &(acadoWorkspace.x[ 25 ]), &(acadoVariables.x[ 120 ]) ); } -int acado_preparationStep( ) +int acado_preparationStep(double TR) { int ret; ret = acado_modelSimulation(); -acado_evaluateObjective( ); +acado_evaluateObjective(TR); acado_condensePrep( ); return ret; } @@ -5313,7 +5313,7 @@ kkt += fabs(acadoWorkspace.ubA[index] * prd); return kkt; } -real_t acado_getObjective( ) +real_t acado_getObjective(double TR) { real_t objVal; @@ -5336,7 +5336,7 @@ acadoWorkspace.objValueIn[6] = acadoVariables.u[lRun1]; acadoWorkspace.objValueIn[7] = acadoVariables.od[lRun1 * 2]; acadoWorkspace.objValueIn[8] = 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]; @@ -5350,7 +5350,7 @@ acadoWorkspace.objValueIn[4] = acadoVariables.x[124]; acadoWorkspace.objValueIn[5] = acadoVariables.x[125]; acadoWorkspace.objValueIn[6] = acadoVariables.od[40]; acadoWorkspace.objValueIn[7] = 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]; diff --git a/selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_solver.o b/selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_solver.o index 8f2fcfbdf156ea49e5a8526461ab0dcbbb6a1577..b53d2fe76f5a64fd6c4fe14461ab3a9dabe8d101 100644 GIT binary patch delta 3996 zcmeHJZERE589w*=g0GzrI}Xkla2AI(u|u_zj)EJK;&_w-TNy-njYLZw$0T+jC`eO< z+6kLtK9(Rgjv*(Mf$hWrJ7P2`XbJmM(_}QNrYUXNT20lIR7LnlyKbOucyN=w$M+by zS6zRg_1~^^b$p-a{d(Se-g7-OYl#2F&}-&Xm&R&-zzgE?$hJ^3zh+k+XOl#PdLss- z)S|U@w)Qo4Y8dvc=)o=47JS6&!maSzF1qlLwH4b-_hP%b6(14ZxD|eD^)74&ya4>{ z<__ElKFI71eZ{XY^x~D^xe*YP*n$J1C$wI*euWnwE_32&rBQM(hMe7vosWUlCa}5z z79rKSRqgLKc1{+E+lB_a8z#+d*af+D77td{wc*b!E_}<>VFTw4g@qS1 z9=y`(%wP+`D!^7PGWn>wp-P9OhU$9FZaiG&kT`7%=Cv+7V|C*>OG_qV5Tfa|9$X1( z4}-GRbzQh!Y{w>0ctdQ%x5Yi!WbVSTCWmwbu!}CEbaAUgGFjX3g`dF+%1xjk06N9m zQy+c?6;Fw7A2v7t=8hc_rSy|`?BagB`5(9M@Ic1fjOoGMMo9w&x(zm|3JRQ4wV$uF zdT;;=4M5V3;MYpq41StjN1KEt&=Ro~4|N!&SVy9s`ceOt;5%IGkaGP3;QP7gqk;Bg z1VJLN7$xUon{)v?r1&9YI)2QUMi2~{JFy?$M+}riUNuTcbmGYCMk&A}$@ykur@PqJ z`5yEeLBJSvX%sqf8&=vCefeG-u(o5`5upZhL5yLD8GvpKLtNwoCseuz`xis7K3gXe zy^wGp_CpfC7BDEtU2cPSj?%OMe=%?_;4p90Nv4RyKn$h zJXhB)t1`FavDcxcR=`2cEsNZN$7m-&;qSn|B9u@s+dFg&%7EzMd36Grof*~!St{Vp z$gG1>w6aA1CRuWLUYiKdYZ4D-Zv`CM5S}jpd8%^=m~L)JiG_R z?{;`~nr|B3T^c&P(Qg^vE1E|c-gmVBAKphD-hDXooeb|jnA>k0UN|j6+>#<+W-N=@ z=o#2p!k9iB%Sy)KoUyEu$MSzLmF3?umSannZFo4AhyKf0uFV-sdv+`}iSSk^Tl8MZ zD~Of~sXQcwE>&5dpL;t_R3#&5MSR=!_q!Y3{R4d0I^y-${<%PA`DDY#d|6Mb z@~PmgDzv}4ii=%2U0uX+Tqt2)*1&Vgh7+!m;4fXeVDd(H1A=E0+?u^dn?%>qrjK|{ zPm&jVk{@qhU7c(Y zrN;D15Jd$-o{>4#(ys}b9yF#WJFeCLmF(#ke*FCP`wKhcmAWA9MSM^fq&lXZ{q5tWBOBg4 zL$3t!4G3j=C3)eDz}GHaOuZ56;}9|+9_7Q{px4YIdHplPw^zx}Q>6B+P+b;#L&!Wo zE~BWIm)*CM180SjvP-9BdK6%Kd0CWuBnmV)Gk)!?FwSxHy| zypzmDL2ol_kFwVQw77b60`V(!hSom zPb%!Q_&ph~LD>Y&%;AB;A&YmB!!dX>KWF1=h24-l1xYeS>8Tu=3D3lJlR2O{&v-f} z5T0|A;6-?JlEg(ggRYXoIQZUVqpy7FpO8kNx1<=<$6X1Q4(X`f*oLZ+y3#F$SO>^f4hqeH1 zareoJl%-9alJV>-^AK)CD0$k$5=<-TE{5M^cy`NV@o8cL_p;x!BULG7hU@^EyE*P1 z5#Tf0oFE;Sg-zUxWbCrAPMbV0*D|0bcPu%OIXL167O4Z0DDLR7FGWl Tfke*;#{N;k%ncCBs8IbMy(bNL delta 4262 zcmeHKZA@F&89vv(!Pn-?fjE4vXBa3L7HuSJf=6K3EI}dN)`03IYNO294qt(c{;8^F z3MM!WCGCQH(;T`q!F+_+EXhiW2Gc4PN|mCjn`jrUQH7?UDhfX)EeW*4OD^m^+;bSZ zYnE-={_RSaV1|FP6ZrUGiaSD^Kgvg?f2eN|AN?ob~lM8yt3k zvsJ3W0jWWL(zrD{&U_eW+Ujvznj80KSrlK4quJZ(Yr|mv$1s$pt1Rb-f0b8N4V^Bb0amz|k*CZON1rJoalxws|JAH11)q@9j zyOd+lfPo*mR+`hFS8v1zYhkfw5 zuhF9PH%3al;75J3Xc8M;iWhdo3x3(KLteNirB(_YhCX?5=y{9cj&Udh*rkMzTB6~X zEKvkv$kvSgup9l*ozROG1xaokI%ZMYbx3iaaW+Gc=0V^P2)0AlPCx+IC42!2x(yxj zLWeHE`Z)~_I}`v-FLW#bx~LGGP^u66V?gX@j%FmmrrDaX8-n;vfI&jubO!`wxO_Jz zxwZd*OX*2RQ5w>}5h-DhE82lkG}P!)`kx0bmvLE03BBl2KG)ZsM~)ioF91bI_uzZZ z=E0*b)p4nXR;aKOyzhalxzeD#Uzqm(e?0%g@od7O|H*hZVbuDSWj%v&H&g!{X6rIwU zPAZc^aL$0D^qWc&-pas7|~u-5hmW6J~~beh_Cjw+|bKd%$-C#Elwh1bPRByj@>+Zpl6$7k;^ zKON3W3?eWO_az1+tl;n?3*I-HM#MTH6WKiQyTOeGGaOs^3cYWrSgGN+AJOYU83}cZ zsfFrOoa5Uo*YDr$=$*KFB6X;YS-t!7H|Q1N&#G7Eqr5IIc8j{rnK95GxLy+wND&D~Gxl=0s zmI|W>u+pgGqr+##Q9;;58qSGjX$$cxKf~4pkf&xdWa6Bdk~W>8(s7_luR*veJwomR zEp(3>&x_v@(n?tH3^3caVyOOckg6U~*}-T#(55x?F49Kn?=u<$ZCFF^A;Xk*YiQOs zWq3$q&+##`J_ONjFj`wm4sil41k`1UJO@veci3j|7?~`_ z0>gPl9DhaK6QmViAiXrUkDUZAXJc|rWOJ{W(t1fl7czQ;;XE40$29hKX8(@Hp5uSg z*gKg09ThJ?JOC&5v#rpU<0dsUUmv5j+g{IbUgj=V(o+NT4c1g{2inUXkkEI=13bnRhrqs=Q-807#kSQ zi{|(xhHJ$)kYSK(4aBPcT&{)5y~1!V$MKIe_AfB|>l%BG-_Y3mnf;84KeKukI1v}8 zTJVHAn1h+&JRyz;*juhh^G@g>&-FuZr`OQ%AIj==o%B+=$)+OIdoA5E`fs#2N;Agwfj6#z+;V_pmP!ZPTA7 zZ9wOTPpjg554%}?T1l|4xX}?`vp|kFc+%e@te51d@4&(b_(Zk2)hF{6g^$ zk@Cx8g#$w5bm&I+y7rNEwFfm>ci-b(#ydstfEu@Vu=g9m5Yfd@%GGGdGo{e^)_6Je=`%ZlDjG3L+lU=1G0@t#t{ zC}Ye>4V%u{bPb*_-5F&JDdlXbhTShkj5daR%Gp{C`#_pF+L#hDNb&QwhJBXfhGkFt zKb7O*8a!V*3|sp-J3+&~FEzl{70y~TtYNU?!wwDJo8vVae6o}{#u&1iv)46jnY0A# zzj4<0<){7sOsW`TO!=9!2^u!=1;vx88a$EXc^W)dB4dpqm7Fctu!p30u%|g&uVL>= z8DOJ^@O|>i(|%5ttY9~Bwx@=zmFmVCQ!a5fUBl`UlrfiU@Hmc_YVaH>Vw^E#PmLlkCW0;EeQ#^n_oPCw=Q$gm|MV>GNr%A(B`PCN(6Gr;1K4cNS~To#$!|P%6lZHR z>_sUN>_yIA*RT$03D_=tM!w6R_G*|^0d^H<6Ev(%IzJvegtK`Xw(C%3E+ra#8OO^t z_!cRC0%pP4dJTI~%9vnG5r-*b3a>uxr;+2q8hk9rdus3tQXOnvNKr;h*RZ+L9kARt)i4$@Ea`v`{^&GBPG^}{q|6Uvq*WhEM!?2af*$EnUjnn}4 zP0m_0?E8|R*_h(sY>kHPIzsW}tOhr8{JI9G(h_sa>iDjF+W$*Z1#CU$eG@cn_hiM^ zR1KcQ@jMNlFOgK_YR;Bx*u7GGsxjpvXX`br`$)xBqXzHIabeZdo@Yx|*m|9_JvD5l zR0sArXVW$8m(rb7WBjk2E!D8k@ZD9V!7teQPg>*`5^b&1SS^0vt^vPxw&8) zJUaE-7oxC-TCs=b4#f!f=4SCW!#gT%3R<;UJTDn-?DMinw5`MZTwO?vwE<4&aHo$p zb7OsCq&F50Br($Eg|Rs67e#uYrH9a~t2A*=q%>iXKhX7ax=JOB%+TaHF_PO$Ok0^7 zBUxP9#g~fs=%)iLW`Bf4ZfY5Q@nR!q*9I$Y-fP8{EzV^uCVC@OVJ|9dX)!zt%Nq(y zS$SE!&Du^%n?0@CT+2%)F}A2BVXi6a!+H(pQ`D=Cnq#EdOD~Ya(twu-NDG&pb7{Lt ztaKjU zO4uv!$n+zfaJx89!E=DO#d7S{;HQDJo%@FtTx!3Q*<7$Q7w3yRhvID9#cU4Q6-vmv zwrOu57#~P~+C7xi+3xN>=|)aVM=Gk&YL8%mHv^^c>IZC$w?>1^ zdh0H0Klu>6elVMJB#@6HLDUl8TUVGwx4qTn&bv%ol zw!L(`nFV{ni34o7MZN3tvw_0%NRQ*5P z&ku8MI42!B|BL(SVa}9KZ2K=vA_dA&T~}Jny=2+=)pspJF%xT7Sj^7=uh-x=R$0v5 zfPbpNyRNgC!-0RK!QBfj=1|}#H2CR)-&@Q9AdYE>oC1s48~70oeggv;fFIQ0Yl|#q z9q|2He67XY{8%=YYw**nE#?Qn+w5u~Z2B+#38|BUzPdU;%38suztv(6#(oQKX{4l> zjmp0DZ-qw?i+SG$WkuSy!d)w}Nm&uG70%kb1E)3pNw96%<#Dpx@J zjdtRfZGhNaIQMoCw1V4qocrGCfmNAv=P2|Jcc%x`KvJiG4{f(1J)006&`<{r)~fEbu^pL^S`Z!7q$~k zWAqrZjl2nGDnC#DX8ViGBDB4UEG1ENF(EKd-zEe(hql*|0^E=d2H3P)%)2mZ&&w{G zr7WE>JO}tT;N4qr`zamK>B(vP8Fx|!+x9sgq=3o48wj&bd$%P>mh@U%G8k<7pd?;8nLg8pK3dD9`79AU2hCB)_G%21c9dqTi^%;|8#AC`1z1i{mi7DZqh(ezLR89}zw z2N5I>U~XqJfNZx{b|&d{tEKj9>hXo?6c#@Vm;&+z2F_>FTKeb9m4a@^W-Wn zrM~1MV4B;X#6i&h*gY{+?oX~jTp2*t+v!+CDvv zzMv~GgBy6z+{Gw;ThUuG*(mftCV7jc@``Wlle0)Mi~W^svJzrS4jIoHV376^?wPe06rrX z~`E0R@c;~FD6OKEoHA!%&@sZ0VJbGvrC~csOxzo2SmB@MEH?!@+xos_3P;1jkaSZ;=Rjw+2yZtgF6xSo zBhc{-I_2*q@uY}W@5JrfjsCQgM8)QQn7HVaw_)6#%8i3hMb9y1ViG#ch&1OM%!LJWPvLNVlrHIt+46q zHP_S`!7W4}dk)Am_BFq^FYC0%Z9st|fQwH>EkNRtG++S{L|8z+gi3B9jG+xEB*CUlHgO z4C$uwHGxh%MV;xp5*K9$B`o?Ft0G{$y@*l`B5@NEQ7JI_hAuD__scc0khpb+F$}T~ zgBBwi`!F!;Bn0P}EFuQmny^SPV9BBvnk+D4@dAb|mmP{N*BbX0oK@Y^9+mhDkIG+2 zbMBp$<}3>p9}8bq`oa7^Y#j1qyaEE1UD zY4i`Lul*6f41ZP>M@QB6fN!!(e)46g)_5Ja~s~=1mMx)*&&nYQo#+yhf zx|;v}loAh{YD)1aLrVGEq5sR25`>;=N@>SaO2z*vr97selp&>5&~|vG|L>%fJ>^I# zOX&IX|0<>IVRQJ`DW(4Z_mooqKTRq1|Fe{`&{Y2aCZ#O=Pf`kU7*8p#)ZyJKbp-M3 z;LlSBQc3xIo;f;m&ZK6J0;G;^{|l+Z3rQCFp{$4Sh^4Bk707A5aGxh3qd!R~tN)XP z@_eg=lB6aS&+4*Kn2lkc{g*wYBG|35jo_rE$4NgWH=I3AI+;9#M`v|jo1CYJwNcM> z;kMyM@}u885*}UmNuW)~Nqn~kz40xfQ`NY-Pd5(!@Gp2u&#xNv?gN=#*VPWbtP9!H z*!el@bv-qIK)R8=_E$e3?Fk|DwG*VnU>&I)T#{+DZpiep)}GloSip4zKAsEKnhlw2 z%@=i^n`(82O|=&`4sOu5f6C04wBZC9@7dTU!71x?)}Pitcaq?v!$)s_?xfqn5qLrW z&fVQugco*4iTl>wX^lRn=6jfub0w*E4i&8IwJPH3W6He0rx&^(x$&4@zWtb9*SGKP zT$xyX57TV)NNTRt&;IJ3kLjV>FXfm_e}3&TWqsDs^IgSQvXeTyibcfDUf4~$n*Z*gpJ~c7;^?t;P4c9w z&OTNa$Er#nD~oRx;#>XSi0@eY-<}b3NpKT_AVg>4-7Vcjd~rMBeWlT3L^GY*Lrg13 z=xMz_9?@K>t9BL%)=Qfo(?1JB>7W0e{HQ%9eqSH<*3!;Bt?%dpHr3*h#OAOGZEEp& zV|oeGy%l{e)3KWwzJ}>RieA8UJn9&}3VH(m&@>Hl+lPX+pzK4RI}uKKl5=Geo^OfO z_;2reK^SO#Fy&O$;mwbY_s5@5u-cwhb@|>#=c90P8+Vd9$OgAOPIP8-14)n;U6RY_ zY>o|Pn5baP0XR}%?#Dnc1x(`&do#>i!6tC52gCdoY&6GW7#5^pBRJNDVWA3^$gwbn znG|dg$3hv_xj+H?axjpA-4v`B$9x&qUBRB^SR01*P_Sr@i45zdV38an40~R|OdM<8 z1Z;qUg|M|@>+u@{Ur@jRj@@Ngl7jhg>?ekeP%tl!H8O0Jg1K|-dtmGi#B$cK<-9-s z!>SjsoX(Xe6)Yj%>eLI?kpAf4YLw6i>swd#92zxX)A`WEc(DU9(3$aKPaPRSx5SHy zx~7fvLcG|^Z!3mRuQXb9sTMO@cYmD*_7abi9D1Rb80$VS*%`iw>UxXMlR(>sJKi1A z+jU%2<;jwYx+Cj@-x@;mMvKesw?~Utbp@*e95S-C@DTDg103=g$ml^1c^TvnkUJo26JQXs;|R2eJP6ql|1D`5jrJY|XfWO( zv;Y6zo#l|LASW$!$X~PTr4HGES{%zO4mkm`?e7r@$UcP_7_#R&I09L`*&#PWZZ37m z!Jde~PKP`R^6N^6Tnd?2gLaTLhY=~r9UnO4XS^`(8HYTh0802-H~@L@3pfCI9P&71 z!g)9X`Sk^O4mswcLk@3)NI{N-T=flR3i;!=4tX17={1LZ6f*ic`aymK*$g@6Ci>y& z^7T&+c`D@8pB?fl$cJ|va=}3;2Ol6rkb)B)LMF&ql6LUWy-Dr@+3e9IPlasTu1PL| zT;|s#AA_6_&?H}n3=V3Nn<0;epuIOH6xJlCL+%J~l2<`)i)@lhA$_|v$p;~`A)I3~H$vGF-6RM5U{l35$s-}V(3ulOQxG2Ut{vX7L)$Q+3sm+wNUamaC?e2zCyGOK zyZvd8R54WS2j;3j9mn*ktW$gXQmPn4CzwSy@rU*Z1fkv_J$HaRjy-;i#;1x#@fa>A z1<}8-wl!waplc4I@0mptNub}DVP+d8sd#D+qtB+|aa~NuGEqerr6Ol+rW;blNZrE@ z^iQeiQA*F@l6+3@ur_0;-z2o@L!X@_hUpr+&~cN*P*P6kO~P>DbUQ?wAEOaxY)M1= zHneP#XbkY);R}Dl@$tni^$ODp5OxsQt4fuOf;AD7^Kocj6apI5D5eWmM zJNWJnavvFtHGuV6+@Q;71zKQ|=O>GONk8g21y|c>|0%AhWK0ozlNoe7z)qGT+$iWF zjBMM<_f6d&nc5E@0aIH@8>WZ@$#U9hDntT(k%`@O`&4**jJ}I2G27;l8(lthrX61t z4V^k3$DPhrzSVflt90Cy9pcSEXvok z?nSuYh2CP~AoZJuPzZGJG;kB>OeQYT-%o>YBWVSMSbNDKmx#&|T%U&Uq3bk+V>ykO z4*F69Jk`_Jr;FWa-gMC|XxI%b8Jj;2=5iJ8_aAQ2jnl;*fri`2BgnAqV1E4wt`9vo z9Y!b7pQa<8yJ^G>1bG#m$iz`vI0IhJrPVXAkpk#xc2z=unt=!&qcJnZ4nYC_O;3ek zIqvr606J}^7}{w@Al~tqtyWQFW^2%98@0}a*HlP{pxPT}NMD_NU@JSOQ83 zrNv?($)_1h#g^JZwHemXb?m2BMythxsuu{Q>v%xxKtHJ-Wwe3OY6YQ6t1khcQNNerj9Nmd(ji!DzLJP^H7Ta*v*8^ph$=4o7e~nUYsPt7U|0??|rH(sIhZ3D_7JyLKW}E z6}65~#iO{AL#@o3T1cqUFlm$85~_48SKgy{7_F8Ps`Rs5xkn@NL94ZdDjmm_ zWIB=2YB8Zo$8%*jEo8J>O{mhnxpI*nX0%#PsM61I#X%bwt=1E&bYHFvqkhXls|AHB z-JdHqnh5$yMWIR$U!S8TKn^pmG_FwEz^rMPX$q$=QNNY2rWO~fy%3fLl+TB%NC@nFxk5>x{)rRR@c~VfwuQRyJreqbyS1SrRDr9k`lEyPy zEh$v#9IkvxGeAG7DOBlPuBbJIDlX$-`UQLSDlwnLhY!FDj^@w{Z3Xog2CXT*&`v0! z6Ia8V>{F9?_vv{?FVoP< z`uPY)DY1wLNH#4j6peTdJ-8KX~|#YjAJ8d`&FGi$bHT zw|?vZkzTQg&#GOov#yPr$*EntP`@?kT0j$-_?#I_rX!2QAblI5p1Bc6j#E-3ChLTO zbX1Y}3Ry_&iZGKrUf9XQAe6e4Ho{qY2efX`DC$)#_8>hK_a_v?hlR7Bj!h5OTY@(x zUs#M0n>DnV)5GZDVsQ(tS|Giabg|5DD+Y0b9SFF`Hn~LYF|a{5FNwZNhtOqy5$Ucmv-0*bizNkJvxlBJS6R zeBMoI(9yM`_*#jtmabpi*b61mgS7lu$8Oywj@OZ^_J%)*Q;47XXsgAW7;9fjQ+B|M zj_ANG?VdZuf>uAkq1Ihu8A+${Zy*^AvRA($mf`m}_LMi#Dx{YCioXlE zju?|NbRKOeLu~NnsvNz}+CwVDDKVJ#l264rKQ*DKvX&kO?5XCH>-5&AVqedaiOMWX ZXv}B0RMSZ|oyRWKyfMRG{h4@E_wT11KVtv@ delta 13391 zcmc&*3s@9Kw(gpNX21t;22e*31w~UJ-f#Eb``z#M=kz)K z|5fLlI@ML(gJ&vsbgIC=OwTW}M1dV&Sm&(sjprASr9?=SZ^L+{$zy37M3(uI?NYs8 zESWDg<2+6B^*550QcwS7pdh@Hq{Gl_tUx50@9co8s(qjt6V-?>OF9 zga69$(HcB#h+;oesvl#N?vL>anZS_>P0Ktfe5^5K17{mF?EBKhvBs3IID1pW{>*X1 z(kDj|9Ito})8GT7dicuY>;w&4C^f^^Tb#9M*fSj8qQUQQ{E!9@k`l)mL!KL|L~=#L z&Xg8|E#R!rOHamMF4c@Pru>$(2^#j#9G|Sg$#aUYMH)OxBF`H`vN>C=VHZkqU`sjM zq+xAR7T9Z?6<&Ta&LGJOcI+_4Z(j{-ks6;jrc`n^Q^TI*_;L+?gX0w%yoVG%-WZaT zr1)yku)U;-V239t>`e{3LMjGpcM`=*$EofLuv-wJyEf2(XiMmQgKId z_K=2^q(rb1XRm1352eLmzvrya@+YHmlWM@m^YJEV*d@}rRNNt)U8G^Ja=c7~KRsL- zMYRS`mEtB~6r63+u#%KD!I*NIv%)J+#(9C`!5X~72*p=l4Zc}wgs;sh3Y)26qoiA4 zNAkWE8g`@ zlD--?TWSQmlCzl__6_M)nlbJOXDc-9PaLn+;G1oO(hGb;qO6TNi@DUtVutI0d5>iJ zy-qk(Vu_MYaF+85aW0&_hUv47+>U#YV&^kwtDV^V z*QkSM0lX3htRKwG%+tino!^;t<8&+$_mWVqr4c&ZGl-@ zF*`=GEHI<>`2}mZ-H!t-W=3ty#VfkgeT{G(sY!nT4tDk=7%+XvtPM4AI;v z#I)MFuJi_Ji+y!p2ei4cp$g|^b;8Y(MGBq^ygjC4y9WOlIJ;-xY{e!2t<2}Vt+R1HzjZjy#%;{! zux+7)Y_&~!3(43a{dW6sQe(Tl{e&CYBfVEsi)K@IVh}cI=cn#~X4D=4H1k@Z6jpbS z+1Ni8WX}HEto;iI8L2;nWY~rs>Zo_$Kf;-~S2Ei!xYvzvlD)P*hfj&j{D=O;{atW1 zlGm{uvd6aI*h7}=dB^v$;rlnRY^FAJ#d#q++cq_f)#2jO$>+#cDfHA#1iIo>fAX&M z+f&_1l889_~l<)O)(dxD=g;Tz?(Gq z)s+@=B=Ao)c#pLfa~Sa7YjF2si#ZhdaSi@)(W@470EnX+BDct5?g;!n4Sp4d48RX) z@X|FFvkv%PEnaFdKYSz`t2Ow?t1RYwz}szWC2aaX`w^_MpfA6f8)>a!!{2N%2jhMV zZf&G&E*q5l)~^j7PAuj<>y;U4-v)QhNVzg2VjG;bcLz=n4Z^pQr5DEQvJ|JeuUpKs zFqB!%t#iSGxe9y+@EIE1y~1Li41BT%F9mJ}Zf?bG5num?xE~qeZ2rKu?b0ciTJwP| z`!BuO{kiGe6F8?{UW4=b%O^r2t%;r%b8@$0^GzZI9J=fZrwMD+_Rf{R6Wre0{(6|* z-l^X&&|#Gv|64UW&b$`OOkF#LbJ`DSIPd#m6V3y!FT?rZbtBH7TxV^5y?zVsLyyLA|Pr`uC;UUr+UB-?Ffc=(4RTo(sGZcuXs9Kcyo&J=tSF?M|xT+n(n^ikSSHfiUm1e|v(>l3s34W zKzfl#`@9Y$294;O9SL?MI^3IJr=#<{T_p7;8+4wHBb*6X-dS}t#*f%YBmLHo7}av$yQ_>*f8mjXxxgd9My716GNuB+jJWDFd<9*C&+&^|$AJM!7A7y2cq6}z_H zZlBAvMA>ZUcR^$Ysi8BBWEw2|&PXs5^oG$DO{ZY;9t-tPA+AshLtUZn3MH?%32|^I zSBQV_L}CLGc#vT6D}(Lz_y|hK{W_DIFdET?dEa~uYf~O^26OLg-(SzY+ zBq^o$!pS0lv%8VOq}0B%8_8^kOMM^mHOO!K5DSU6&x|Dt^^l+SLv)Zg`jgW-r04x- z$v3!^29WcB>Fhxy7J?4K-4jjaLF5v|rNLw!UR=W2t}Z<<8L4Fm@d92xgiwfxcw{e{ zj*Tb#AnwGIT`)+8A}4siU=PyVSY?B`_lawic$tf)(lD2!Q^UwQ);EFRm4t3eV3zFm z1fs*!k$#v+PLLgRMH0vw`|%|52_A-Y?P#(aZF{AV3lRD-nAd3gfHCBA!k%}l_t-m* zBdI!;!A4|oJi(*RetJ9^!G_mv0v1O!9W}uf@xlqjOm^5$P9T?&3j5A9qGxXRrlZde z>XiY@@6zWoNM{mdpPE6I>EP)6Br=3a?f>rOt=+H2uV!d~lKMoTQ_7eL=>LszxZSN%Kn z8w2`U8~U?WdJmpl&w;+whL&6DkMTfaPt?y8x`{;`n@Nlq*r-f$nT5!n4DMyhz{a*| zHY}SA09}I-T*ZUtHmnS2LvPAvR_MNLvY&0`H6Pet$RQ;x_m^goLWoJZB$YMDbhzvh#jn8Iyy$a37k)W3ingg8~8uB$@Ue*v}v z#Eh*V_kAU1w_~ofx`sA&AxTn}lzrzya#u%sQu-U*e?93Zzaf(#x-KDU5KET8e^2_> z609z8OK=Ov(07)iWemOK683g45p0ez^s{9Qwad%M5RvqvLspTwB#Z7{MY@LMCOhX~ z=Z=|^>^yT@c);}hPZJ*WDzJaMip&V130p|_qOy9K{sQL6x1%16#y#V?HQC8zwO^n! z%PXNEXLDjfyB!LaY?x!p8kl8TGcenf13k%*ZQ3<3$CMO2&y*CMV=~kvJJ%RynOOfE zU|v4irX=tk_ar-$qUV|t(N~DhH-!afngqiF(-HW-w1squ$bwQG3Ws8XGuto0$r?q# z!3cB+H)NSg40C8~73mqf4GnAjGMv?>KxccO1*R}GEDp&u)pQGV&Jq#rhQxv%=r|G` zPovYiDiWt#T~9x(B7N!Kwh%wkgI?cCqC8lL-gU{;dm9P#*ALD^{0mK22j-eSL2rzW zrdDHv%cq6g$W!Dlt=mR=dd@LL49s+qH>ydmcKIeD7~{b>BB}3N#8^}jJEkuW6ppm5Mtb@VUgS#IX#;b+$h6y>;XKV!IFMb>h0HeX9h2c) zjr8t*A;UEtySimKJ>E@r?upEB8saKHXIvvP6FvF;+sy84!|Wo{4?d@Eha;YAhFsI= zU#{IIXs|Ck&{>-o_@HJ+pfd%7X6fx-pX}1ZkFa^c93d#rsio#_Fq93$NwA^sU>Ls> z`p&t5&Ruis;&afC_3!opf(XiR=3u^a&@Te>n}g3OLlFZOAR#%X>X!qZnV8GnrVMB8 zs=x=eYXct$FeXG7m@JsCUC2)Lia@7e$TU^24s@>33C_$n6AR#^U@tfU2G<~K>)-&> zXxf7TT|j8S13X!A$XR_BEs0~2?c@yGZa1FlnEymFJK5L zClohVTfC#-tc}TdQ06B*sD3WPxobv-vno`0zyh!Yfy)MG0yh`j%itn8*G+I%ugGwI zr=R_{kdS4v#0bu;Ohli1GO=Lqsto5okZj(oamSQK&%y}uOyXc=G$k07kJlno2}Tl$ zThAwS9!8L7dT(`xvt%G}jA#$;8v&Wf#VA?kt|9{ow0H;UG;HVE4Cii`uX^NBCq%QA zlx+%wn@oHxY^;MZRxf3omXXT1s~&mPRXy^o!`SI181cqLNg&)Khw;x~l z*a#jk{BE2xX%>DY7TEuO-JAZ&!hdyF+l9aCzgYOswpsW|>caP|s~UsY4f*y9yGTti zU3rWQq(_gDEt2WNC%<<3*Y`hy44g`0+}qv)MuBrdY?(+xvU{%OO|Q*&$Mf7K_} zHS3N0?e}DQMb|L&f-a=|hi=bUuXxZgACX8>YhU~k@h2IHcjR<#vBW zhL-L0wyw|iw(6LkZM23%Hk{rtlyRQ(O3jAsQuBG8XL*CpP~NQf->>>^_#TRyJ#&KKqr*pUf98bSfsrKK{#SQ*;~Km?JIdTQZ%@ACZMyF)Bz4Z=f;Hls zNA!+fi22>sNA&BPkEpXSvF?iAKjw}{()|Yg%rD!veg3FSZ@;Fr-$0vth`q2kbcht! z5QE(sDdL+-gS|dV9H{sGDA|d>xi!^$y@H2Jo@sR>b;O8Yy0v(R-lrFPiM^?zx7hue z+K$2HJ+u6*3G0g9P4Z|MYH%h#h`?~+e}A(+U_TqaI?OXW10S54`$7CKG`F`nHl?{m zPOs&et3~GOc;;&Vo0*$zclH)%lK>$aj|V&^hz}M4UIOTM_YniqSI1i4*9DX};IY7>t=#yCHsEQ(bSu+4D*Ee8$F|1sa;66)7MWD!ih2kKLl_vNG<=q00SxP- zU~wGtW>`N3>&-DQhCQobQ5fK%` z2shbXMuVRg57VZn#RT`?Z+Jvg?SapTm)(MAu7}6*I;XQRsqSuSV%;wu!|#W#e`K_G z9xSH&8vb~e9S?`mMPtR_Fy*R!LVwqB{zO?#<9q9Z_YdQ(PS7o5#pQY=!`^P3*sLqs z9^jC%NeaP%4*4eJUC6%R(}NuH1jsFri0KL;9c| z<1XYt$fnK?c@|{fE)KZ_aw+6)CQS~x0n#npA%E{C6bZ+mxVZ~L=k5+U8uD1ALmmy; z5CsE}l`${?ng5hSu7(WgcK!@{16(?}!0)ZILq} zkA}6#D~F2Ovuz&p=l8XbF;kfO0gtMGp4HP1UPK9t}ByzB*Af1x*hp zxKZK1Q~Rz$cPQ+0mex-cqevM2YN9wycg&BDPZLAMfnXl`(FIJO3_Z-BzMduq(M4v_ zO?1OO{*cfS)YxipCWowaQ;iw0e1BmLSenn)&nV1}E+G&~It z-&yqeG(2^8(*h>W(AUz$PUHZsOA{k>!JX-!($M1&{TY|!C+eGyHuGs*I@+Yu=W(So z^q>pU#Zb~f*Qdi=0zCrJu61^iP*ke&;Vo^5GtQW=DxXvGAw_z@D zAZB4Vs9<0vnr@xJHGs3w#uqUu_i6Ks2!90ioC0x{#!W#g!szrV;1cNZ_{ue zJx#B&t6JJ&Iudx1rc4(*1wHN8@M%8RBd9A7_YPcBBEomR@b4{->Epyn1K2_X`vWM2GFb}A__Wqp`6+b^XNMAsFl%b z^_?08^65I>r;VT=m)~(zz_?w9T7IYE*za72T7IYE*w0+Y02;m&-qiX#mBudRItuAT z(2oo7RN9v-Y5|^#qg=sts0DZ`9>A3#TF<0sdyAudQdC#rdHyqbWg7IqK%9m zo`5Z@&C!eNE9fmoTQ&4koVL;MWuVn!Jk>xfS3KxMMyu6$Djmm_X|$NpYB`=t_vea4 z>lv-qXkQ|SR*aihM=L8}#cDm{oROK2kK$0d0xJ%lT2NuG+wb44x5 zQ}Ll(Nu@Q+n_82n(!;nS(R0k( ziK1DIR?G5KdK6biQY)j?x;&Lm;mQ))2>Nkho=T79idvYb;^VlY7Urq=^IU09!&ktY zTA8QPsa#2-6B(_R=Be~Vt`yQ@(2r~LR630-YHgm1yY_utsI_@2p23w#w3&I+PSYe# zS5n_XcvGwM)ZQ<0MXkBXM&U(_w2u^=N&oMe(Lo4&=jTbv&5%=L$rnp!%>aGl;CoSSg zyzneuEgET9u^9Xh(Y^07+!1e46N~YtaK_bB-(j#wFImK=)UH=p*E5=dsa>%Ttwz^j zG?9q|%-JYnVON3rc0vU`$KdUhtPw}iqpR`4J7bNQjhEHNH5f@>UU$pWAXK=Nc0w7w z1zPtog?g2UeTa`Tgb5{xW8BOqbJNH5me3J{FD`+_Z#1--)7@!(iMR=t5=+r4GxJF! zs^0{c-_SBmSlf75h-YUh;<-rAm10zxl&poZ?={BMh&IwV(BvphSu4(@38fhO@wND> z5TK0a=2~$%OH`>CsNil452p3&VA_W^GqIfdmbtDH%aFeObz+cPJK@S08oFNWLz~LP zzWC2ZvK~ l?D(d)%v=vnL0>7v41VN77BcEZiEPmcKwKB#K?6>C0?dTxNIrGuC_ zK<8`_PvBck&vH!XG`bj~^@kj^u^g|g7wy-}@v_5olF;3Klpt#Pj5p)mj{W*Xu-Q*;68GvujzuaBI=ki#f1SXWNY~F(?8TDk zL0W&=Vz*X`sXB7k-u$LGiTJXYa&|P}eT%i9PgAxaiuSZ_3kI-~-r9mJ9k6?D6^q*Z zT7_D-iB(vXac^PC2ixo3607jz6?@9tXce-Z$BMrIxQ?DC#q>DZT!q|Vd#gsTqxO&* zaZ*pb<1hYHjP+HwjmMJK1NKyRjRt!CQ*nT2{zPSz`LyRBaH(z@Bj_Av_l; acadoVariables.x[5] = acadoVariables.x0[5] = 0.0; - acado_preparationStep(); + acado_preparationStep(TR); acado_feedbackStep(); for (i = 0; i <= N; i++){ @@ -146,7 +146,7 @@ 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. diff --git a/selfdrive/controls/lib/longitudinal_mpc/longitudinal_mpc.o b/selfdrive/controls/lib/longitudinal_mpc/longitudinal_mpc.o index e899d7b785a16fc3493732e4ba06fc5ececfd36d..479d21492ac8fdfb2429459f2d922fc94b472604 100644 GIT binary patch delta 238 zcmeB>Um!n0gK@z|O=l*V7yPn+zOpm?{Nm5J@^3ZMN)ZNz2~(IHetO6(@|)biw1tI1 zc7@+&6J}LbWgvrrfniF5AJgBz)lMtF_&cur%I@&<1-tVfpg2$?1HH7W5eV`KKscJ_&6r7;Im*{F!>>$J>$8_iv0GB7bYk2s{;VV2{=ap delta 198 zcmZ1=-yuIigRx_yrZbbsSAN+aU;G(Y{;g(ODZ;=oVG5JO&&eH3TQ)l|tFnqSFic7C zWBU8I+G*t%f5(+y*`0qdFffR0?%?>q$jGv}lB=DO(PA$jy?3uQ4Ouon=HaUP_gK@{?N`7?!C!R6q diff --git a/selfdrive/controls/lib/planner.py b/selfdrive/controls/lib/planner.py index b986740113904d..2a979cf83b7705 100755 --- a/selfdrive/controls/lib/planner.py +++ b/selfdrive/controls/lib/planner.py @@ -28,6 +28,7 @@ _DT_MPC = 0.2 # 5Hz MAX_SPEED_ERROR = 2.0 AWARENESS_DECEL = -0.2 # car smoothly decel at .2m/s^2 when user is distracted +TR=1.8 # CS.readdistancelines GPS_PLANNER_ADDR = "192.168.5.1" @@ -151,7 +152,8 @@ def __init__(self, mpc_id, live_longitudinal_mpc): self.prev_lead_status = False self.prev_lead_x = 0.0 self.new_lead = False - + + self.lastTR = 2 self.last_cloudlog_t = 0.0 def send_mpc_solution(self, qp_iterations, calculation_time): @@ -219,7 +221,39 @@ def update(self, 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) + + if CS.vEgo < 11.4: + TR=1.8 # under 41km/hr use a TR of 1.8 seconds + #if self.lastTR > 0: + #self.libmpc.init(MPC_COST_LONG.TTC, 0.1, PC_COST_LONG.ACCELERATION, MPC_COST_LONG.JERK) + #self.lastTR = 0 + else: + if CS.readdistancelines == 2: + if CS.readdistancelines == self.lastTR: + TR=1.8 # 20m at 40km/hr + else: + TR=1.8 + self.libmpc.init(MPC_COST_LONG.TTC, 0.1, MPC_COST_LONG.ACCELERATION, MPC_COST_LONG.JERK) + self.lastTR = CS.readdistancelines + elif CS.readdistancelines == 1: + if CS.readdistancelines == self.lastTR: + TR=0.9 # 10m at 40km/hr + else: + TR=0.9 + self.libmpc.init(MPC_COST_LONG.TTC, 1.0, MPC_COST_LONG.ACCELERATION, MPC_COST_LONG.JERK) + self.lastTR = CS.readdistancelines + elif CS.readdistancelines == 3: + if CS.readdistancelines == self.lastTR: + TR=2.7 + else: + TR=2.7 # 30m at 40km/hr + self.libmpc.init(MPC_COST_LONG.TTC, 0.05, MPC_COST_LONG.ACCELERATION, MPC_COST_LONG.JERK) + self.lastTR = CS.readdistancelines + else: + TR=1.8 # if readdistancelines = 0 + #print TR + + n_its = self.libmpc.run_mpc(self.cur_state, self.mpc_solution, self.a_lead_tau, a_lead, TR) duration = int((sec_since_boot() - t) * 1e9) self.send_mpc_solution(n_its, duration)