Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Added lateral acceleration model to test script #217

Closed
wants to merge 1 commit into from
Closed

Added lateral acceleration model to test script #217

Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
21 changes: 13 additions & 8 deletions selfdrive/test/plant/plant.py
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,6 +1,7 @@
#!/usr/bin/env python
import os
import struct
import math

import zmq
import numpy as np
Expand Down Expand Up @@ -28,6 +29,7 @@

def car_plant(pos, speed, grade, gas, brake):
# vehicle parameters
wheelbase = 2.70002
mass = 1700
aero_cd = 0.3
force_peak = mass*3.
Expand All @@ -36,7 +38,7 @@ def car_plant(pos, speed, grade, gas, brake):
speed_base = power_peak/force_peak
rolling_res = 0.01
g = 9.81
#frontal_area = 2.2 TODO: use it!
frontal_area = 2.2
air_density = 1.225
gas_to_peak_linear_slope = 3.33
brake_to_peak_linear_slope = 0.3
Expand All @@ -56,12 +58,15 @@ def car_plant(pos, speed, grade, gas, brake):
creep_accel = np.interp(speed, creep_accel_bp, creep_accel_v)
force_creep = creep_accel * mass

force_resistance = -(rolling_res * mass * g + 0.5 * speed**2 * aero_cd * air_density)
force_resistance = -(rolling_res * mass * g + 0.5 * speed**2 * aero_cd * air_density * frontal_area)
force = force_gas + force_brake + force_resistance + force_grade + force_creep
acceleration = force / mass

# TODO: lateral model
return speed, acceleration
turn_radius = wheelbase / math.sin(math.pi/180 * steering_angle)
lateral_acceleration = speed**2/turn_radius

return speed, acceleration, lateral_acceleration

def get_car_can_parser():
dbc_f = 'honda_civic_touring_2016_can_generated.dbc'
Expand Down Expand Up @@ -186,17 +191,17 @@ def step(self, v_lead=0.0, cruise_buttons=None, grade=0.0, publish_model = True)

distance_lead = self.distance_lead_prev + v_lead * self.ts

# ******** lateral ********
self.angle_steer -= (steer_torque/10.0) * self.ts

# ******** run the car ********
speed, acceleration = car_plant(self.distance_prev, self.speed_prev, grade, gas, brake)
speed, acceleration, lateral_acceleration = car_plant(self.distance_prev, self.speed_prev, grade, gas, brake)
distance = self.distance_prev + speed * self.ts
speed = self.speed_prev + self.ts * acceleration
if speed <= 0:
speed = 0
acceleration = 0

# ******** lateral ********
self.angle_steer -= (steer_torque/10.0) * self.ts

# *** radar model ***
if self.lead_relevancy:
d_rel = np.maximum(0., distance_lead - distance)
Expand All @@ -208,7 +213,7 @@ def step(self, v_lead=0.0, cruise_buttons=None, grade=0.0, publish_model = True)

# print at 5hz
if (self.rk.frame%(self.rate/5)) == 0:
print "%6.2f m %6.2f m/s %6.2f m/s2 %.2f ang gas: %.2f brake: %.2f steer: %5.2f lead_rel: %6.2f m %6.2f m/s" % (distance, speed, acceleration, self.angle_steer, gas, brake, steer_torque, d_rel, v_rel)
print "%6.2f m %6.2f m/s %6.2f m/s2 %6.2f m/s2 %.2f ang gas: %.2f brake: %.2f steer: %5.2f lead_rel: %6.2f m %6.2f m/s" % (distance, speed, acceleration, lateral_acceleration, self.angle_steer, gas, brake, steer_torque, d_rel, v_rel)

# ******** publish the car ********
vls = [self.speed_sensor(speed), self.speed_sensor(speed), self.speed_sensor(speed), self.speed_sensor(speed), self.speed_sensor(speed),
Expand Down