|
|
|
@ -6,7 +6,7 @@ from selfdrive.controls.lib.drive_helpers import EventTypes as ET, create_event |
|
|
|
|
from selfdrive.controls.lib.vehicle_model import VehicleModel |
|
|
|
|
from selfdrive.car.toyota.carstate import CarState, get_can_parser, get_cam_can_parser |
|
|
|
|
from selfdrive.car.toyota.values import ECU, check_ecu_msgs, CAR, NO_STOP_TIMER_CAR |
|
|
|
|
from selfdrive.car import STD_CARGO_KG |
|
|
|
|
from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness |
|
|
|
|
from selfdrive.swaglog import cloudlog |
|
|
|
|
|
|
|
|
|
|
|
|
|
@ -54,16 +54,6 @@ class CarInterface(object): |
|
|
|
|
# pedal |
|
|
|
|
ret.enableCruise = not ret.enableGasInterceptor |
|
|
|
|
|
|
|
|
|
# FIXME: hardcoding honda civic 2016 touring params so they can be used to |
|
|
|
|
# scale unknown params for other cars |
|
|
|
|
mass_civic = 2923. * CV.LB_TO_KG + STD_CARGO_KG |
|
|
|
|
wheelbase_civic = 2.70 |
|
|
|
|
centerToFront_civic = wheelbase_civic * 0.4 |
|
|
|
|
centerToRear_civic = wheelbase_civic - centerToFront_civic |
|
|
|
|
rotationalInertia_civic = 2500 |
|
|
|
|
tireStiffnessFront_civic = 192150 |
|
|
|
|
tireStiffnessRear_civic = 202500 |
|
|
|
|
|
|
|
|
|
ret.steerActuatorDelay = 0.12 # Default delay, Prius has larger delay |
|
|
|
|
if candidate != CAR.PRIUS: |
|
|
|
|
ret.lateralTuning.init('pid') |
|
|
|
@ -100,7 +90,7 @@ class CarInterface(object): |
|
|
|
|
ret.safetyParam = 100 |
|
|
|
|
ret.wheelbase = 2.70 |
|
|
|
|
ret.steerRatio = 17.8 |
|
|
|
|
tire_stiffness_factor = 0.444 |
|
|
|
|
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 |
|
|
|
@ -170,7 +160,7 @@ class CarInterface(object): |
|
|
|
|
ret.safetyParam = 73 |
|
|
|
|
ret.wheelbase = 2.63906 |
|
|
|
|
ret.steerRatio = 13.9 |
|
|
|
|
tire_stiffness_factor = 0.444 |
|
|
|
|
tire_stiffness_factor = 0.444 # not optimized yet |
|
|
|
|
ret.mass = 3060. * CV.LB_TO_KG + STD_CARGO_KG |
|
|
|
|
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.6], [0.1]] |
|
|
|
|
ret.lateralTuning.pid.kf = 0.00007818594 |
|
|
|
@ -179,8 +169,8 @@ class CarInterface(object): |
|
|
|
|
stop_and_go = True |
|
|
|
|
ret.safetyParam = 73 |
|
|
|
|
ret.wheelbase = 2.8702 |
|
|
|
|
ret.steerRatio = 16.0 #not optimized |
|
|
|
|
tire_stiffness_factor = 0.444 |
|
|
|
|
ret.steerRatio = 16.0 # not optimized |
|
|
|
|
tire_stiffness_factor = 0.444 # not optimized yet |
|
|
|
|
ret.mass = 3704. * CV.LB_TO_KG + STD_CARGO_KG |
|
|
|
|
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.6], [0.1]] |
|
|
|
|
ret.lateralTuning.pid.kf = 0.00007818594 |
|
|
|
@ -195,20 +185,14 @@ class CarInterface(object): |
|
|
|
|
# to a negative value, so it won't matter. |
|
|
|
|
ret.minEnableSpeed = -1. if (stop_and_go or ret.enableGasInterceptor) else 19. * CV.MPH_TO_MS |
|
|
|
|
|
|
|
|
|
centerToRear = ret.wheelbase - ret.centerToFront |
|
|
|
|
# TODO: get actual value, for now starting with reasonable value for |
|
|
|
|
# civic and scaling by mass and wheelbase |
|
|
|
|
ret.rotationalInertia = rotationalInertia_civic * \ |
|
|
|
|
ret.mass * ret.wheelbase**2 / (mass_civic * wheelbase_civic**2) |
|
|
|
|
ret.rotationalInertia = scale_rot_inertia(ret.mass, ret.wheelbase) |
|
|
|
|
|
|
|
|
|
# TODO: start from empirically derived lateral slip stiffness for the civic and scale by |
|
|
|
|
# mass and CG position, so all cars will have approximately similar dyn behaviors |
|
|
|
|
ret.tireStiffnessFront = (tireStiffnessFront_civic * tire_stiffness_factor) * \ |
|
|
|
|
ret.mass / mass_civic * \ |
|
|
|
|
(centerToRear / ret.wheelbase) / (centerToRear_civic / wheelbase_civic) |
|
|
|
|
ret.tireStiffnessRear = (tireStiffnessRear_civic * tire_stiffness_factor) * \ |
|
|
|
|
ret.mass / mass_civic * \ |
|
|
|
|
(ret.centerToFront / ret.wheelbase) / (centerToFront_civic / wheelbase_civic) |
|
|
|
|
ret.tireStiffnessFront, ret.tireStiffnessRear = scale_tire_stiffness(ret.mass, ret.wheelbase, ret.centerToFront, |
|
|
|
|
tire_stiffness_factor=tire_stiffness_factor) |
|
|
|
|
|
|
|
|
|
# no rear steering, at least on the listed cars above |
|
|
|
|
ret.steerRatioRear = 0. |
|
|
|
|