ZwX1616
3 years ago
commit
67cfe02595
481 changed files with 2204 additions and 57437 deletions
@ -1 +1 @@ |
||||
Subproject commit d6c3cf6b33e699f82e5d78ae22c74cad978830b6 |
||||
Subproject commit 29f4fe89ef710ff86a5aeb998a357187d0619fb8 |
||||
@ -1,2 +0,0 @@ |
||||
#!/usr/bin/bash |
||||
echo "this is a compatability shim for old updaters" |
||||
@ -1 +1 @@ |
||||
Subproject commit eb56fff37a4a2738df7add08779db51a0a6f38e2 |
||||
Subproject commit e19ba095c3ee288d629284e24ec7e0deaf645f3f |
||||
@ -1 +1 @@ |
||||
Subproject commit c925407461e46adf3282494af6daa00b2626ebb8 |
||||
Subproject commit 7dd9493eb102ba8b96362c7265b06851ec1d4bac |
||||
@ -1 +0,0 @@ |
||||
README.md |
||||
@ -0,0 +1,6 @@ |
||||
#!/usr/bin/bash |
||||
|
||||
DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" >/dev/null && pwd)" |
||||
|
||||
export FINGERPRINT="TOYOTA COROLLA TSS2 2019" |
||||
$DIR/../launch_openpilot.sh |
||||
File diff suppressed because it is too large
Load Diff
@ -1,106 +0,0 @@ |
||||
#pragma once |
||||
|
||||
#include <atomic> |
||||
#include <cstdint> |
||||
#include <memory> |
||||
|
||||
#include "cereal/messaging/messaging.h" |
||||
#include "cereal/visionipc/visionbuf.h" |
||||
#include "selfdrive/camerad/cameras/camera_common.h" |
||||
#include "selfdrive/camerad/imgproc/utils.h" |
||||
#include "selfdrive/camerad/include/msm_cam_sensor.h" |
||||
#include "selfdrive/camerad/include/msmb_camera.h" |
||||
#include "selfdrive/camerad/include/msmb_isp.h" |
||||
#include "selfdrive/camerad/include/msmb_ispif.h" |
||||
#include "selfdrive/common/mat.h" |
||||
#include "selfdrive/common/util.h" |
||||
|
||||
#define FRAME_BUF_COUNT 4 |
||||
#define METADATA_BUF_COUNT 4 |
||||
|
||||
#define NUM_FOCUS 8 |
||||
|
||||
#define LP3_AF_DAC_DOWN 366 |
||||
#define LP3_AF_DAC_UP 634 |
||||
#define LP3_AF_DAC_M 440 |
||||
#define LP3_AF_DAC_3SIG 52 |
||||
|
||||
#define FOCUS_RECOVER_PATIENCE 50 // 2.5 seconds of complete blur
|
||||
#define FOCUS_RECOVER_STEPS 240 // 6 seconds
|
||||
|
||||
typedef struct CameraState CameraState; |
||||
|
||||
typedef int (*camera_apply_exposure_func)(CameraState *s, int gain, int integ_lines, uint32_t frame_length); |
||||
|
||||
typedef struct StreamState { |
||||
struct msm_isp_buf_request buf_request; |
||||
struct msm_vfe_axi_stream_request_cmd stream_req; |
||||
struct msm_isp_qbuf_info qbuf_info[FRAME_BUF_COUNT]; |
||||
VisionBuf *bufs; |
||||
} StreamState; |
||||
|
||||
typedef struct CameraState { |
||||
int camera_num; |
||||
int camera_id; |
||||
|
||||
int fps; |
||||
CameraInfo ci; |
||||
|
||||
unique_fd csid_fd; |
||||
unique_fd csiphy_fd; |
||||
unique_fd sensor_fd; |
||||
unique_fd isp_fd; |
||||
|
||||
struct msm_vfe_axi_stream_cfg_cmd stream_cfg; |
||||
|
||||
StreamState ss[3]; |
||||
CameraBuf buf; |
||||
|
||||
std::mutex frame_info_lock; |
||||
FrameMetadata frame_metadata[METADATA_BUF_COUNT]; |
||||
int frame_metadata_idx; |
||||
|
||||
// exposure
|
||||
uint32_t pixel_clock, line_length_pclk; |
||||
uint32_t frame_length; |
||||
unsigned int max_gain; |
||||
float cur_exposure_frac, cur_gain_frac; |
||||
int cur_gain, cur_integ_lines; |
||||
|
||||
float measured_grey_fraction; |
||||
float target_grey_fraction; |
||||
|
||||
std::atomic<float> digital_gain; |
||||
camera_apply_exposure_func apply_exposure; |
||||
|
||||
// rear camera only,used for focusing
|
||||
unique_fd actuator_fd; |
||||
std::atomic<float> focus_err; |
||||
std::atomic<float> lens_true_pos; |
||||
std::atomic<int> self_recover; // af recovery counter, neg is patience, pos is active
|
||||
uint16_t cur_step_pos; |
||||
uint16_t cur_lens_pos; |
||||
int16_t focus[NUM_FOCUS]; |
||||
uint8_t confidence[NUM_FOCUS]; |
||||
} CameraState; |
||||
|
||||
|
||||
typedef struct MultiCameraState { |
||||
unique_fd ispif_fd; |
||||
unique_fd msmcfg_fd; |
||||
unique_fd v4l_fd; |
||||
uint16_t lapres[(ROI_X_MAX-ROI_X_MIN+1)*(ROI_Y_MAX-ROI_Y_MIN+1)]; |
||||
|
||||
VisionBuf focus_bufs[FRAME_BUF_COUNT]; |
||||
VisionBuf stats_bufs[FRAME_BUF_COUNT]; |
||||
|
||||
CameraState road_cam; |
||||
CameraState driver_cam; |
||||
|
||||
SubMaster *sm; |
||||
PubMaster *pm; |
||||
LapConv *lap_conv; |
||||
} MultiCameraState; |
||||
|
||||
void actuator_move(CameraState *s, uint16_t target); |
||||
int sensor_write_regs(CameraState *s, struct msm_camera_i2c_reg_array* arr, size_t size, int data_type); |
||||
@ -1,140 +0,0 @@ |
||||
const __constant float3 color_correction[3] = { |
||||
// Matrix from WBraw -> sRGBD65 (normalized) |
||||
(float3)( 1.62393627, -0.2092988, 0.00119886), |
||||
(float3)(-0.45734315, 1.5534676, -0.59296798), |
||||
(float3)(-0.16659312, -0.3441688, 1.59176912), |
||||
}; |
||||
|
||||
float3 color_correct(float3 x) { |
||||
float3 ret = (0,0,0); |
||||
|
||||
// white balance of daylight |
||||
x /= (float3)(0.4609375, 1.0, 0.546875); |
||||
x = max(0.0, min(1.0, x)); |
||||
|
||||
// fix up the colors |
||||
ret += x.x * color_correction[0]; |
||||
ret += x.y * color_correction[1]; |
||||
ret += x.z * color_correction[2]; |
||||
return ret; |
||||
} |
||||
|
||||
float3 srgb_gamma(float3 p) { |
||||
// go all out and add an sRGB gamma curve |
||||
const float3 ph = (1.0f + 0.055f)*pow(p, 1/2.4f) - 0.055f; |
||||
const float3 pl = p*12.92f; |
||||
return select(ph, pl, islessequal(p, 0.0031308f)); |
||||
} |
||||
|
||||
#if HDR |
||||
|
||||
__constant int dpcm_lookup[512] = {0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16, 17, 18, 19, 20, 21, 22, 23, 24, 25, 26, 27, 28, 29, 30, 31, 0, -1, -2, -3, -4, -5, -6, -7, -8, -9, -10, -11, -12, -13, -14, -15, -16, -17, -18, -19, -20, -21, -22, -23, -24, -25, -26, -27, -28, -29, -30, -31, 935, 951, 967, 983, 999, 1015, 1031, 1047, 1063, 1079, 1095, 1111, 1127, 1143, 1159, 1175, 1191, 1207, 1223, 1239, 1255, 1271, 1287, 1303, 1319, 1335, 1351, 1367, 1383, 1399, 1415, 1431, -935, -951, -967, -983, -999, -1015, -1031, -1047, -1063, -1079, -1095, -1111, -1127, -1143, -1159, -1175, -1191, -1207, -1223, -1239, -1255, -1271, -1287, -1303, -1319, -1335, -1351, -1367, -1383, -1399, -1415, -1431, 419, 427, 435, 443, 451, 459, 467, 475, 483, 491, 499, 507, 515, 523, 531, 539, 547, 555, 563, 571, 579, 587, 595, 603, 611, 619, 627, 635, 643, 651, 659, 667, 675, 683, 691, 699, 707, 715, 723, 731, 739, 747, 755, 763, 771, 779, 787, 795, 803, 811, 819, 827, 835, 843, 851, 859, 867, 875, 883, 891, 899, 907, 915, 923, -419, -427, -435, -443, -451, -459, -467, -475, -483, -491, -499, -507, -515, -523, -531, -539, -547, -555, -563, -571, -579, -587, -595, -603, -611, -619, -627, -635, -643, -651, -659, -667, -675, -683, -691, -699, -707, -715, -723, -731, -739, -747, -755, -763, -771, -779, -787, -795, -803, -811, -819, -827, -835, -843, -851, -859, -867, -875, -883, -891, -899, -907, -915, -923, 161, 165, 169, 173, 177, 181, 185, 189, 193, 197, 201, 205, 209, 213, 217, 221, 225, 229, 233, 237, 241, 245, 249, 253, 257, 261, 265, 269, 273, 277, 281, 285, 289, 293, 297, 301, 305, 309, 313, 317, 321, 325, 329, 333, 337, 341, 345, 349, 353, 357, 361, 365, 369, 373, 377, 381, 385, 389, 393, 397, 401, 405, 409, 413, -161, -165, -169, -173, -177, -181, -185, -189, -193, -197, -201, -205, -209, -213, -217, -221, -225, -229, -233, -237, -241, -245, -249, -253, -257, -261, -265, -269, -273, -277, -281, -285, -289, -293, -297, -301, -305, -309, -313, -317, -321, -325, -329, -333, -337, -341, -345, -349, -353, -357, -361, -365, -369, -373, -377, -381, -385, -389, -393, -397, -401, -405, -409, -413, 32, 34, 36, 38, 40, 42, 44, 46, 48, 50, 52, 54, 56, 58, 60, 62, 64, 66, 68, 70, 72, 74, 76, 78, 80, 82, 84, 86, 88, 90, 92, 94, 96, 98, 100, 102, 104, 106, 108, 110, 112, 114, 116, 118, 120, 122, 124, 126, 128, 130, 132, 134, 136, 138, 140, 142, 144, 146, 148, 150, 152, 154, 156, 158, -32, -34, -36, -38, -40, -42, -44, -46, -48, -50, -52, -54, -56, -58, -60, -62, -64, -66, -68, -70, -72, -74, -76, -78, -80, -82, -84, -86, -88, -90, -92, -94, -96, -98, -100, -102, -104, -106, -108, -110, -112, -114, -116, -118, -120, -122, -124, -126, -128, -130, -132, -134, -136, -138, -140, -142, -144, -146, -148, -150, -152, -154, -156, -158}; |
||||
|
||||
inline uint4 decompress(uint4 p, uint4 pl) { |
||||
uint4 r1 = (pl + (uint4)(dpcm_lookup[p.s0], dpcm_lookup[p.s1], dpcm_lookup[p.s2], dpcm_lookup[p.s3])); |
||||
uint4 r2 = ((p-0x200)<<5) | 0xF; |
||||
r2 += select((uint4)(0,0,0,0), (uint4)(1,1,1,1), r2 <= pl); |
||||
return select(r2, r1, p < 0x200); |
||||
} |
||||
|
||||
#endif |
||||
|
||||
__kernel void debayer10(__global uchar const * const in, |
||||
__global uchar * out, float digital_gain) |
||||
{ |
||||
const int oy = get_global_id(0); |
||||
if (oy >= RGB_HEIGHT) return; |
||||
const int iy = oy * 2; |
||||
|
||||
#if HDR |
||||
uint4 pint_last; |
||||
for (int ox = 0; ox < RGB_WIDTH; ox += 2) { |
||||
#else |
||||
int ox = get_global_id(1) * 2; |
||||
{ |
||||
#endif |
||||
const int ix = (ox/2) * 5; |
||||
|
||||
// TODO: why doesn't this work for the frontview |
||||
/*const uchar8 v1 = vload8(0, &in[iy * FRAME_STRIDE + ix]); |
||||
const uchar ex1 = v1.s4; |
||||
const uchar8 v2 = vload8(0, &in[(iy+1) * FRAME_STRIDE + ix]); |
||||
const uchar ex2 = v2.s4;*/ |
||||
|
||||
const uchar4 v1 = vload4(0, &in[iy * FRAME_STRIDE + ix]); |
||||
const uchar ex1 = in[iy * FRAME_STRIDE + ix + 4]; |
||||
const uchar4 v2 = vload4(0, &in[(iy+1) * FRAME_STRIDE + ix]); |
||||
const uchar ex2 = in[(iy+1) * FRAME_STRIDE + ix + 4]; |
||||
|
||||
uint4 pinta[2]; |
||||
pinta[0] = (uint4)( |
||||
(((uint)v1.s0 << 2) + ( (ex1 >> 0) & 3)), |
||||
(((uint)v1.s1 << 2) + ( (ex1 >> 2) & 3)), |
||||
(((uint)v2.s0 << 2) + ( (ex2 >> 0) & 3)), |
||||
(((uint)v2.s1 << 2) + ( (ex2 >> 2) & 3))); |
||||
pinta[1] = (uint4)( |
||||
(((uint)v1.s2 << 2) + ( (ex1 >> 4) & 3)), |
||||
(((uint)v1.s3 << 2) + ( (ex1 >> 6) & 3)), |
||||
(((uint)v2.s2 << 2) + ( (ex2 >> 4) & 3)), |
||||
(((uint)v2.s3 << 2) + ( (ex2 >> 6) & 3))); |
||||
|
||||
#pragma unroll |
||||
for (uint px = 0; px < 2; px++) { |
||||
uint4 pint = pinta[px]; |
||||
|
||||
#if HDR |
||||
// decompress HDR |
||||
pint = (ox == 0 && px == 0) ? ((pint<<4) | 8) : decompress(pint, pint_last); |
||||
pint_last = pint; |
||||
#endif |
||||
|
||||
float4 p = convert_float4(pint); |
||||
|
||||
// 64 is the black level of the sensor, remove |
||||
// (changed to 56 for HDR) |
||||
const float black_level = 56.0f; |
||||
// TODO: switch to max here? |
||||
p = (p - black_level); |
||||
|
||||
// correct vignetting (no pow function?) |
||||
// see https://www.eecis.udel.edu/~jye/lab_research/09/JiUp.pdf the A (4th order) |
||||
const float r = ((oy - RGB_HEIGHT/2)*(oy - RGB_HEIGHT/2) + (ox - RGB_WIDTH/2)*(ox - RGB_WIDTH/2)); |
||||
const float fake_f = 700.0f; // should be 910, but this fits... |
||||
const float lil_a = (1.0f + r/(fake_f*fake_f)); |
||||
p = p * lil_a * lil_a; |
||||
|
||||
// rescale to 1.0 |
||||
#if HDR |
||||
p /= (16384.0f-black_level); |
||||
#else |
||||
p /= (1024.0f-black_level); |
||||
#endif |
||||
|
||||
// digital gain |
||||
p *= digital_gain; |
||||
|
||||
// use both green channels |
||||
#if BAYER_FLIP == 3 |
||||
float3 c1 = (float3)(p.s3, (p.s1+p.s2)/2.0f, p.s0); |
||||
#elif BAYER_FLIP == 2 |
||||
float3 c1 = (float3)(p.s2, (p.s0+p.s3)/2.0f, p.s1); |
||||
#elif BAYER_FLIP == 1 |
||||
float3 c1 = (float3)(p.s1, (p.s0+p.s3)/2.0f, p.s2); |
||||
#elif BAYER_FLIP == 0 |
||||
float3 c1 = (float3)(p.s0, (p.s1+p.s2)/2.0f, p.s3); |
||||
#endif |
||||
|
||||
// color correction |
||||
c1 = color_correct(c1); |
||||
|
||||
#if HDR |
||||
// srgb gamma isn't right for YUV, so it's disabled for now |
||||
c1 = srgb_gamma(c1); |
||||
#endif |
||||
|
||||
// output BGR |
||||
const int ooff = oy * RGB_STRIDE/3 + ox; |
||||
vstore3(convert_uchar3_sat(c1.zyx * 255.0f), ooff+px, out); |
||||
} |
||||
} |
||||
} |
||||
@ -1,86 +1,90 @@ |
||||
import math |
||||
from cereal import car |
||||
from selfdrive.car import make_can_msg |
||||
from selfdrive.car.ford.fordcan import create_steer_command, create_lkas_ui, spam_cancel_button |
||||
from common.numpy_fast import clip, interp |
||||
from selfdrive.car.ford import fordcan |
||||
from selfdrive.car.ford.values import CarControllerParams |
||||
from opendbc.can.packer import CANPacker |
||||
|
||||
VisualAlert = car.CarControl.HUDControl.VisualAlert |
||||
|
||||
MAX_STEER_DELTA = 1 |
||||
TOGGLE_DEBUG = False |
||||
|
||||
def apply_ford_steer_angle_limits(apply_steer, apply_steer_last, vEgo): |
||||
# rate limit |
||||
steer_up = apply_steer * apply_steer_last > 0. and abs(apply_steer) > abs(apply_steer_last) |
||||
rate_limit = CarControllerParams.STEER_RATE_LIMIT_UP if steer_up else CarControllerParams.STEER_RATE_LIMIT_DOWN |
||||
max_angle_diff = interp(vEgo, rate_limit.speed_points, rate_limit.max_angle_diff_points) |
||||
apply_steer = clip(apply_steer, (apply_steer_last - max_angle_diff), (apply_steer_last + max_angle_diff)) |
||||
|
||||
return apply_steer |
||||
|
||||
|
||||
class CarController(): |
||||
def __init__(self, dbc_name, CP, VM): |
||||
self.CP = CP |
||||
self.VM = VM |
||||
self.packer = CANPacker(dbc_name) |
||||
self.enabled_last = False |
||||
|
||||
self.apply_steer_last = 0 |
||||
self.steer_rate_limited = False |
||||
self.main_on_last = False |
||||
self.vehicle_model = VM |
||||
self.generic_toggle_last = 0 |
||||
self.lkas_enabled_last = False |
||||
self.steer_alert_last = False |
||||
self.lkas_action = 0 |
||||
|
||||
def update(self, enabled, CS, frame, actuators, visual_alert, pcm_cancel): |
||||
|
||||
def update(self, CC, CS, frame): |
||||
can_sends = [] |
||||
steer_alert = visual_alert in (VisualAlert.steerRequired, VisualAlert.ldw) |
||||
|
||||
apply_steer = actuators.steer |
||||
|
||||
if pcm_cancel: |
||||
#print "CANCELING!!!!" |
||||
can_sends.append(spam_cancel_button(self.packer)) |
||||
actuators = CC.actuators |
||||
hud_control = CC.hudControl |
||||
|
||||
if (frame % 3) == 0: |
||||
main_on = CS.out.cruiseState.available |
||||
steer_alert = hud_control.visualAlert in (VisualAlert.steerRequired, VisualAlert.ldw) |
||||
|
||||
curvature = self.vehicle_model.calc_curvature(math.radians(actuators.steeringAngleDeg), CS.out.vEgo, 0.0) |
||||
if CC.cruiseControl.cancel: |
||||
# cancel stock ACC |
||||
can_sends.append(fordcan.spam_cancel_button(self.packer)) |
||||
|
||||
# The use of the toggle below is handy for trying out the various LKAS modes |
||||
if TOGGLE_DEBUG: |
||||
self.lkas_action += int(CS.out.genericToggle and not self.generic_toggle_last) |
||||
self.lkas_action &= 0xf |
||||
else: |
||||
self.lkas_action = 5 # 4 and 5 seem the best. 8 and 9 seem to aggressive and laggy |
||||
# apply rate limits |
||||
new_steer = actuators.steeringAngleDeg |
||||
apply_steer = apply_ford_steer_angle_limits(new_steer, self.apply_steer_last, CS.out.vEgo) |
||||
self.steer_rate_limited = new_steer != apply_steer |
||||
|
||||
can_sends.append(create_steer_command(self.packer, apply_steer, enabled, |
||||
CS.lkas_state, CS.out.steeringAngleDeg, curvature, self.lkas_action)) |
||||
self.generic_toggle_last = CS.out.genericToggle |
||||
# send steering commands at 20Hz |
||||
if (frame % CarControllerParams.LKAS_STEER_STEP) == 0: |
||||
lca_rq = 1 if CC.latActive else 0 |
||||
|
||||
if (frame % 100) == 0: |
||||
# use LatCtlPath_An_Actl to actuate steering for now until curvature control is implemented |
||||
path_angle = apply_steer |
||||
|
||||
can_sends.append(make_can_msg(973, b'\x00\x00\x00\x00\x00\x00\x00\x00', 0)) |
||||
#can_sends.append(make_can_msg(984, b'\x00\x00\x00\x00\x80\x45\x60\x30', 0)) |
||||
# convert steer angle to curvature |
||||
curvature = self.VM.calc_curvature(apply_steer, CS.out.vEgo, 0.0) |
||||
|
||||
if (frame % 100) == 0 or (self.enabled_last != enabled) or (self.main_on_last != CS.out.cruiseState.available) or \ |
||||
(self.steer_alert_last != steer_alert): |
||||
can_sends.append(create_lkas_ui(self.packer, CS.out.cruiseState.available, enabled, steer_alert)) |
||||
# TODO: get other actuators |
||||
curvature_rate = 0 |
||||
path_offset = 0 |
||||
|
||||
if (frame % 200) == 0: |
||||
can_sends.append(make_can_msg(1875, b'\x80\xb0\x55\x55\x78\x90\x00\x00', 1)) |
||||
ramp_type = 3 # 0=Slow, 1=Medium, 2=Fast, 3=Immediately |
||||
precision = 0 # 0=Comfortable, 1=Precise |
||||
|
||||
if (frame % 10) == 0: |
||||
self.apply_steer_last = apply_steer |
||||
can_sends.append(fordcan.create_lkas_command(self.packer, apply_steer, curvature)) |
||||
can_sends.append(fordcan.create_tja_command(self.packer, lca_rq, ramp_type, precision, |
||||
path_offset, path_angle, curvature_rate, curvature)) |
||||
|
||||
can_sends.append(make_can_msg(1648, b'\x00\x00\x00\x40\x00\x00\x50\x00', 1)) |
||||
can_sends.append(make_can_msg(1649, b'\x10\x10\xf1\x70\x04\x00\x00\x00', 1)) |
||||
|
||||
can_sends.append(make_can_msg(1664, b'\x00\x00\x03\xe8\x00\x01\xa9\xb2', 1)) |
||||
can_sends.append(make_can_msg(1674, b'\x08\x00\x00\xff\x0c\xfb\x6a\x08', 1)) |
||||
can_sends.append(make_can_msg(1675, b'\x00\x00\x3b\x60\x37\x00\x00\x00', 1)) |
||||
can_sends.append(make_can_msg(1690, b'\x70\x00\x00\x55\x86\x1c\xe0\x00', 1)) |
||||
send_ui = (self.main_on_last != main_on) or (self.lkas_enabled_last != CC.latActive) or (self.steer_alert_last != steer_alert) |
||||
|
||||
can_sends.append(make_can_msg(1910, b'\x06\x4b\x06\x4b\x42\xd3\x11\x30', 1)) |
||||
can_sends.append(make_can_msg(1911, b'\x48\x53\x37\x54\x48\x53\x37\x54', 1)) |
||||
can_sends.append(make_can_msg(1912, b'\x31\x34\x47\x30\x38\x31\x43\x42', 1)) |
||||
can_sends.append(make_can_msg(1913, b'\x31\x34\x47\x30\x38\x32\x43\x42', 1)) |
||||
can_sends.append(make_can_msg(1969, b'\xf4\x40\x00\x00\x00\x00\x00\x00', 1)) |
||||
can_sends.append(make_can_msg(1971, b'\x0b\xc0\x00\x00\x00\x00\x00\x00', 1)) |
||||
# send lkas ui command at 1Hz or if ui state changes |
||||
if (frame % CarControllerParams.LKAS_UI_STEP) == 0 or send_ui: |
||||
can_sends.append(fordcan.create_lkas_ui_command(self.packer, main_on, CC.latActive, steer_alert, CS.lkas_status_stock_values)) |
||||
|
||||
static_msgs = range(1653, 1658) |
||||
for addr in static_msgs: |
||||
cnt = (frame % 10) + 1 |
||||
can_sends.append(make_can_msg(addr, (cnt << 4).to_bytes(1, 'little') + b'\x00\x00\x00\x00\x00\x00\x00', 1)) |
||||
# send acc ui command at 20Hz or if ui state changes |
||||
if (frame % CarControllerParams.ACC_UI_STEP) == 0 or send_ui: |
||||
can_sends.append(fordcan.create_acc_ui_command(self.packer, main_on, CC.latActive, CS.acc_tja_status_stock_values)) |
||||
|
||||
self.enabled_last = enabled |
||||
self.main_on_last = CS.out.cruiseState.available |
||||
self.main_on_last = main_on |
||||
self.lkas_enabled_last = CC.latActive |
||||
self.steer_alert_last = steer_alert |
||||
|
||||
return actuators, can_sends |
||||
new_actuators = actuators.copy() |
||||
new_actuators.steeringAngleDeg = apply_steer |
||||
|
||||
return new_actuators, can_sends |
||||
|
||||
@ -1,58 +1,225 @@ |
||||
from typing import Dict |
||||
|
||||
from cereal import car |
||||
from common.conversions import Conversions as CV |
||||
from common.numpy_fast import mean |
||||
from opendbc.can.can_define import CANDefine |
||||
from opendbc.can.parser import CANParser |
||||
from selfdrive.car.interfaces import CarStateBase |
||||
from selfdrive.car.ford.values import DBC |
||||
from selfdrive.car.ford.values import CANBUS, DBC |
||||
|
||||
GearShifter = car.CarState.GearShifter |
||||
TransmissionType = car.CarParams.TransmissionType |
||||
|
||||
WHEEL_RADIUS = 0.33 |
||||
|
||||
class CarState(CarStateBase): |
||||
def update(self, cp): |
||||
def __init__(self, CP): |
||||
super().__init__(CP) |
||||
can_define = CANDefine(DBC[CP.carFingerprint]["pt"]) |
||||
if CP.transmissionType == TransmissionType.automatic: |
||||
self.shifter_values = can_define.dv["Gear_Shift_by_Wire_FD1"]["TrnGear_D_RqDrv"] |
||||
|
||||
def update(self, cp, cp_cam): |
||||
ret = car.CarState.new_message() |
||||
|
||||
ret.wheelSpeeds = self.get_wheel_speeds( |
||||
cp.vl["WheelSpeed_CG1"]["WhlFl_W_Meas"], |
||||
cp.vl["WheelSpeed_CG1"]["WhlFr_W_Meas"], |
||||
cp.vl["WheelSpeed_CG1"]["WhlRl_W_Meas"], |
||||
cp.vl["WheelSpeed_CG1"]["WhlRr_W_Meas"], |
||||
unit=WHEEL_RADIUS, |
||||
) |
||||
ret.vEgoRaw = mean([ret.wheelSpeeds.rr, ret.wheelSpeeds.rl, ret.wheelSpeeds.fr, ret.wheelSpeeds.fl]) |
||||
# car speed |
||||
ret.vEgoRaw = cp.vl["EngVehicleSpThrottle2"]["Veh_V_ActlEng"] * CV.KPH_TO_MS |
||||
ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw) |
||||
ret.standstill = not ret.vEgoRaw > 0.001 |
||||
ret.steeringAngleDeg = cp.vl["Steering_Wheel_Data_CG1"]["SteWhlRelInit_An_Sns"] |
||||
ret.steeringPressed = not cp.vl["Lane_Keep_Assist_Status"]["LaHandsOff_B_Actl"] |
||||
ret.steerFaultPermanent = cp.vl["Lane_Keep_Assist_Status"]["LaActDeny_B_Actl"] == 1 |
||||
ret.cruiseState.speed = cp.vl["Cruise_Status"]["Set_Speed"] * CV.MPH_TO_MS |
||||
ret.cruiseState.enabled = not (cp.vl["Cruise_Status"]["Cruise_State"] in (0, 3)) |
||||
ret.cruiseState.available = cp.vl["Cruise_Status"]["Cruise_State"] != 0 |
||||
ret.gas = cp.vl["EngineData_14"]["ApedPosScal_Pc_Actl"] / 100. |
||||
ret.yawRate = cp.vl["Yaw_Data_FD1"]["VehYaw_W_Actl"] * CV.RAD_TO_DEG |
||||
ret.standstill = cp.vl["DesiredTorqBrk"]["VehStop_D_Stat"] == 1 |
||||
|
||||
# gas pedal |
||||
ret.gas = cp.vl["EngVehicleSpThrottle"]["ApedPos_Pc_ActlArb"] / 100. |
||||
ret.gasPressed = ret.gas > 1e-6 |
||||
ret.brakePressed = bool(cp.vl["Cruise_Status"]["Brake_Drv_Appl"]) |
||||
ret.genericToggle = bool(cp.vl["Steering_Buttons"]["Dist_Incr"]) |
||||
# TODO: we also need raw driver torque, needed for Assisted Lane Change |
||||
self.lkas_state = cp.vl["Lane_Keep_Assist_Status"]["LaActAvail_D_Actl"] |
||||
|
||||
# brake pedal |
||||
ret.brake = cp.vl["BrakeSnData_4"]["BrkTot_Tq_Actl"] / 32756. # torque in Nm |
||||
ret.brakePressed = cp.vl["EngBrakeData"]["BpedDrvAppl_D_Actl"] == 2 |
||||
ret.parkingBrake = cp.vl["DesiredTorqBrk"]["PrkBrkStatus"] in (1, 2) |
||||
|
||||
# steering wheel |
||||
ret.steeringAngleDeg = cp.vl["SteeringPinion_Data"]["StePinComp_An_Est"] |
||||
ret.steeringTorque = cp.vl["EPAS_INFO"]["SteeringColumnTorque"] |
||||
ret.steeringPressed = cp.vl["Lane_Assist_Data3_FD1"]["LaHandsOff_B_Actl"] == 0 |
||||
ret.steerFaultTemporary = cp.vl["EPAS_INFO"]["EPAS_Failure"] == 1 |
||||
ret.steerFaultPermanent = cp.vl["EPAS_INFO"]["EPAS_Failure"] in (2, 3) |
||||
# ret.espDisabled = False # TODO: find traction control signal |
||||
|
||||
# cruise state |
||||
ret.cruiseState.speed = cp.vl["EngBrakeData"]["Veh_V_DsplyCcSet"] * CV.MPH_TO_MS |
||||
ret.cruiseState.enabled = cp.vl["EngBrakeData"]["CcStat_D_Actl"] in (4, 5) |
||||
ret.cruiseState.available = cp.vl["EngBrakeData"]["CcStat_D_Actl"] in (3, 4, 5) |
||||
|
||||
# gear |
||||
if self.CP.transmissionType == TransmissionType.automatic: |
||||
gear = int(cp.vl["Gear_Shift_by_Wire_FD1"]["TrnGear_D_RqDrv"]) |
||||
ret.gearShifter = self.parse_gear_shifter(self.shifter_values.get(gear, None)) |
||||
elif self.CP.transmissionType == TransmissionType.manual: |
||||
ret.clutchPressed = cp.vl["Engine_Clutch_Data"]["CluPdlPos_Pc_Meas"] > 0 |
||||
if bool(cp.vl["BCM_Lamp_Stat_FD1"]["RvrseLghtOn_B_Stat"]): |
||||
ret.gearShifter = GearShifter.reverse |
||||
else: |
||||
ret.gearShifter = GearShifter.drive |
||||
|
||||
# safety |
||||
ret.stockFcw = bool(cp_cam.vl["ACCDATA_3"]["FcwVisblWarn_B_Rq"]) |
||||
ret.stockAeb = ret.stockFcw and ret.cruiseState.enabled |
||||
|
||||
# button presses |
||||
ret.leftBlinker = cp.vl["Steering_Data_FD1"]["TurnLghtSwtch_D_Stat"] == 1 |
||||
ret.rightBlinker = cp.vl["Steering_Data_FD1"]["TurnLghtSwtch_D_Stat"] == 2 |
||||
ret.genericToggle = bool(cp.vl["Steering_Data_FD1"]["TjaButtnOnOffPress"]) |
||||
|
||||
# lock info |
||||
ret.doorOpen = any([cp.vl["BodyInfo_3_FD1"]["DrStatDrv_B_Actl"], cp.vl["BodyInfo_3_FD1"]["DrStatPsngr_B_Actl"], |
||||
cp.vl["BodyInfo_3_FD1"]["DrStatRl_B_Actl"], cp.vl["BodyInfo_3_FD1"]["DrStatRr_B_Actl"]]) |
||||
ret.seatbeltUnlatched = cp.vl["RCMStatusMessage2_FD1"]["FirstRowBuckleDriver"] == 2 |
||||
|
||||
# blindspot sensors |
||||
if self.CP.enableBsm: |
||||
ret.leftBlindspot = cp.vl["Side_Detect_L_Stat"]["SodDetctLeft_D_Stat"] != 0 |
||||
ret.rightBlindspot = cp.vl["Side_Detect_R_Stat"]["SodDetctRight_D_Stat"] != 0 |
||||
|
||||
# Stock values from IPMA so that we can retain some stock functionality |
||||
self.acc_tja_status_stock_values = cp_cam.vl["ACCDATA_3"] |
||||
self.lkas_status_stock_values = cp_cam.vl["IPMA_Data"] |
||||
|
||||
return ret |
||||
|
||||
@staticmethod |
||||
def parse_gear_shifter(gear: str) -> car.CarState.GearShifter: |
||||
d: Dict[str, car.CarState.GearShifter] = { |
||||
'Park': GearShifter.park, 'Reverse': GearShifter.reverse, 'Neutral': GearShifter.neutral, |
||||
'Manual': GearShifter.manumatic, 'Drive': GearShifter.drive, |
||||
} |
||||
return d.get(gear, GearShifter.unknown) |
||||
|
||||
@staticmethod |
||||
def get_can_parser(CP): |
||||
signals = [ |
||||
# sig_name, sig_address |
||||
("WhlRr_W_Meas", "WheelSpeed_CG1"), |
||||
("WhlRl_W_Meas", "WheelSpeed_CG1"), |
||||
("WhlFr_W_Meas", "WheelSpeed_CG1"), |
||||
("WhlFl_W_Meas", "WheelSpeed_CG1"), |
||||
("SteWhlRelInit_An_Sns", "Steering_Wheel_Data_CG1"), |
||||
("Cruise_State", "Cruise_Status"), |
||||
("Set_Speed", "Cruise_Status"), |
||||
("LaActAvail_D_Actl", "Lane_Keep_Assist_Status"), |
||||
("LaHandsOff_B_Actl", "Lane_Keep_Assist_Status"), |
||||
("LaActDeny_B_Actl", "Lane_Keep_Assist_Status"), |
||||
("ApedPosScal_Pc_Actl", "EngineData_14"), |
||||
("Dist_Incr", "Steering_Buttons"), |
||||
("Brake_Drv_Appl", "Cruise_Status"), |
||||
] |
||||
checks = [] |
||||
return CANParser(DBC[CP.carFingerprint]["pt"], signals, checks, 0, enforce_checks=False) |
||||
("Veh_V_ActlEng", "EngVehicleSpThrottle2"), # ABS vehicle speed (kph) |
||||
("VehYaw_W_Actl", "Yaw_Data_FD1"), # ABS vehicle yaw rate (rad/s) |
||||
("VehStop_D_Stat", "DesiredTorqBrk"), # ABS vehicle stopped |
||||
("PrkBrkStatus", "DesiredTorqBrk"), # ABS park brake status |
||||
("ApedPos_Pc_ActlArb", "EngVehicleSpThrottle"), # PCM throttle (pct) |
||||
("BrkTot_Tq_Actl", "BrakeSnData_4"), # ABS brake torque (Nm) |
||||
("BpedDrvAppl_D_Actl", "EngBrakeData"), # PCM driver brake pedal pressed |
||||
("Veh_V_DsplyCcSet", "EngBrakeData"), # PCM ACC set speed (mph) |
||||
# The units might change with IPC settings? |
||||
("CcStat_D_Actl", "EngBrakeData"), # PCM ACC status |
||||
("StePinComp_An_Est", "SteeringPinion_Data"), # PSCM estimated steering angle (deg) |
||||
# Calculates steering angle (and offset) from pinion |
||||
# angle and driving measurements. |
||||
# StePinRelInit_An_Sns is the pinion angle, initialised |
||||
# to zero at the beginning of the drive. |
||||
("SteeringColumnTorque", "EPAS_INFO"), # PSCM steering column torque (Nm) |
||||
("EPAS_Failure", "EPAS_INFO"), # PSCM EPAS status |
||||
("LaHandsOff_B_Actl", "Lane_Assist_Data3_FD1"), # PSCM LKAS hands off wheel |
||||
("TurnLghtSwtch_D_Stat", "Steering_Data_FD1"), # SCCM Turn signal switch |
||||
("TjaButtnOnOffPress", "Steering_Data_FD1"), # SCCM ACC button, lane-centering/traffic jam assist toggle |
||||
("DrStatDrv_B_Actl", "BodyInfo_3_FD1"), # BCM Door open, driver |
||||
("DrStatPsngr_B_Actl", "BodyInfo_3_FD1"), # BCM Door open, passenger |
||||
("DrStatRl_B_Actl", "BodyInfo_3_FD1"), # BCM Door open, rear left |
||||
("DrStatRr_B_Actl", "BodyInfo_3_FD1"), # BCM Door open, rear right |
||||
("FirstRowBuckleDriver", "RCMStatusMessage2_FD1"), # RCM Seatbelt status, driver |
||||
] |
||||
|
||||
checks = [ |
||||
# sig_address, frequency |
||||
("EngVehicleSpThrottle2", 50), |
||||
("Yaw_Data_FD1", 100), |
||||
("DesiredTorqBrk", 50), |
||||
("EngVehicleSpThrottle", 100), |
||||
("BrakeSnData_4", 50), |
||||
("EngBrakeData", 10), |
||||
("SteeringPinion_Data", 100), |
||||
("EPAS_INFO", 50), |
||||
("Lane_Assist_Data3_FD1", 33), |
||||
("Steering_Data_FD1", 10), |
||||
("BodyInfo_3_FD1", 2), |
||||
("RCMStatusMessage2_FD1", 10), |
||||
] |
||||
|
||||
if CP.transmissionType == TransmissionType.automatic: |
||||
signals += [ |
||||
("TrnGear_D_RqDrv", "Gear_Shift_by_Wire_FD1"), # GWM transmission gear position |
||||
] |
||||
checks += [ |
||||
("Gear_Shift_by_Wire_FD1", 10), |
||||
] |
||||
elif CP.transmissionType == TransmissionType.manual: |
||||
signals += [ |
||||
("CluPdlPos_Pc_Meas", "Engine_Clutch_Data"), # PCM clutch (pct) |
||||
("RvrseLghtOn_B_Stat", "BCM_Lamp_Stat_FD1"), # BCM reverse light |
||||
] |
||||
checks += [ |
||||
("Engine_Clutch_Data", 33), |
||||
("BCM_Lamp_Stat_FD1", 1), |
||||
] |
||||
|
||||
if CP.enableBsm: |
||||
signals += [ |
||||
("SodDetctLeft_D_Stat", "Side_Detect_L_Stat"), # Blindspot sensor, left |
||||
("SodDetctRight_D_Stat", "Side_Detect_R_Stat"), # Blindspot sensor, right |
||||
] |
||||
checks += [ |
||||
("Side_Detect_L_Stat", 5), |
||||
("Side_Detect_R_Stat", 5), |
||||
] |
||||
|
||||
return CANParser(DBC[CP.carFingerprint]["pt"], signals, checks, CANBUS.main) |
||||
|
||||
@staticmethod |
||||
def get_cam_can_parser(CP): |
||||
signals = [ |
||||
# sig_name, sig_address |
||||
("HaDsply_No_Cs", "ACCDATA_3"), |
||||
("HaDsply_No_Cnt", "ACCDATA_3"), |
||||
("AccStopStat_D_Dsply", "ACCDATA_3"), # ACC stopped status message |
||||
("AccTrgDist2_D_Dsply", "ACCDATA_3"), # ACC target distance |
||||
("AccStopRes_B_Dsply", "ACCDATA_3"), |
||||
("TjaWarn_D_Rq", "ACCDATA_3"), # TJA warning |
||||
("Tja_D_Stat", "ACCDATA_3"), # TJA status |
||||
("TjaMsgTxt_D_Dsply", "ACCDATA_3"), # TJA text |
||||
("IaccLamp_D_Rq", "ACCDATA_3"), # iACC status icon |
||||
("AccMsgTxt_D2_Rq", "ACCDATA_3"), # ACC text |
||||
("FcwDeny_B_Dsply", "ACCDATA_3"), # FCW disabled |
||||
("FcwMemStat_B_Actl", "ACCDATA_3"), # FCW enabled setting |
||||
("AccTGap_B_Dsply", "ACCDATA_3"), # ACC time gap display setting |
||||
("CadsAlignIncplt_B_Actl", "ACCDATA_3"), |
||||
("AccFllwMde_B_Dsply", "ACCDATA_3"), # ACC follow mode display setting |
||||
("CadsRadrBlck_B_Actl", "ACCDATA_3"), |
||||
("CmbbPostEvnt_B_Dsply", "ACCDATA_3"), # AEB event status |
||||
("AccStopMde_B_Dsply", "ACCDATA_3"), # ACC stop mode display setting |
||||
("FcwMemSens_D_Actl", "ACCDATA_3"), # FCW sensitivity setting |
||||
("FcwMsgTxt_D_Rq", "ACCDATA_3"), # FCW text |
||||
("AccWarn_D_Dsply", "ACCDATA_3"), # ACC warning |
||||
("FcwVisblWarn_B_Rq", "ACCDATA_3"), # FCW visible alert |
||||
("FcwAudioWarn_B_Rq", "ACCDATA_3"), # FCW audio alert |
||||
("AccTGap_D_Dsply", "ACCDATA_3"), # ACC time gap |
||||
("AccMemEnbl_B_RqDrv", "ACCDATA_3"), # ACC adaptive/normal setting |
||||
("FdaMem_B_Stat", "ACCDATA_3"), # FDA enabled setting |
||||
|
||||
("FeatConfigIpmaActl", "IPMA_Data"), |
||||
("FeatNoIpmaActl", "IPMA_Data"), |
||||
("PersIndexIpma_D_Actl", "IPMA_Data"), |
||||
("AhbcRampingV_D_Rq", "IPMA_Data"), # AHB ramping |
||||
("LaActvStats_D_Dsply", "IPMA_Data"), # LKAS status (lines) |
||||
("LaDenyStats_B_Dsply", "IPMA_Data"), # LKAS error |
||||
("LaHandsOff_D_Dsply", "IPMA_Data"), # LKAS hands on chime |
||||
("CamraDefog_B_Req", "IPMA_Data"), # Windshield heater? |
||||
("CamraStats_D_Dsply", "IPMA_Data"), # Camera status |
||||
("DasAlrtLvl_D_Dsply", "IPMA_Data"), # DAS alert level |
||||
("DasStats_D_Dsply", "IPMA_Data"), # DAS status |
||||
("DasWarn_D_Dsply", "IPMA_Data"), # DAS warning |
||||
("AhbHiBeam_D_Rq", "IPMA_Data"), # AHB status |
||||
("Set_Me_X1", "IPMA_Data"), |
||||
] |
||||
|
||||
checks = [ |
||||
# sig_address, frequency |
||||
("ACCDATA_3", 5), |
||||
("IPMA_Data", 1), |
||||
] |
||||
|
||||
return CANParser(DBC[CP.carFingerprint]["pt"], signals, checks, CANBUS.camera) |
||||
|
||||
@ -1,50 +1,144 @@ |
||||
from common.numpy_fast import clip |
||||
from selfdrive.car.ford.values import MAX_ANGLE |
||||
|
||||
|
||||
def create_steer_command(packer, angle_cmd, enabled, lkas_state, angle_steers, curvature, lkas_action): |
||||
"""Creates a CAN message for the Ford Steer Command.""" |
||||
def create_lkas_command(packer, angle_deg: float, curvature: float): |
||||
""" |
||||
Creates a CAN message for the Ford LKAS Command. |
||||
|
||||
#if enabled and lkas available: |
||||
if enabled and lkas_state in (2, 3): # and (frame % 500) >= 3: |
||||
action = lkas_action |
||||
else: |
||||
action = 0xf |
||||
angle_cmd = angle_steers/MAX_ANGLE |
||||
This command can apply "Lane Keeping Aid" manoeuvres, which are subject to the |
||||
PSCM lockout. |
||||
|
||||
angle_cmd = clip(angle_cmd * MAX_ANGLE, - MAX_ANGLE, MAX_ANGLE) |
||||
Frequency is 20Hz. |
||||
""" |
||||
|
||||
values = { |
||||
"Lkas_Action": action, |
||||
"Lkas_Alert": 0xf, # no alerts |
||||
"Lane_Curvature": clip(curvature, -0.01, 0.01), # is it just for debug? |
||||
#"Lane_Curvature": 0, # is it just for debug? |
||||
"Steer_Angle_Req": angle_cmd |
||||
"LkaDrvOvrrd_D_Rq": 0, # driver override level? [0|3] |
||||
"LkaActvStats_D2_Req": 0, # action [0|7] |
||||
"LaRefAng_No_Req": angle_deg, # angle [-102.4|102.3] degrees |
||||
"LaRampType_B_Req": 0, # Ramp speed: 0=Smooth, 1=Quick |
||||
"LaCurvature_No_Calc": curvature, # curvature [-0.01024|0.01023] 1/meter |
||||
"LdwActvStats_D_Req": 0, # LDW status [0|7] |
||||
"LdwActvIntns_D_Req": 0, # LDW intensity [0|3], shake alert strength |
||||
} |
||||
return packer.make_can_msg("Lane_Keep_Assist_Control", 0, values) |
||||
return packer.make_can_msg("Lane_Assist_Data1", 0, values) |
||||
|
||||
|
||||
def create_tja_command(packer, lca_rq: int, ramp_type: int, precision: int, path_offset: float, path_angle: float, curvature_rate: float, curvature: float): |
||||
""" |
||||
Creates a CAN message for the Ford TJA/LCA Command. |
||||
|
||||
This command can apply "Lane Centering" manoeuvres: continuous lane centering |
||||
for traffic jam assist and highway driving. It is not subject to the PSCM |
||||
lockout. |
||||
|
||||
The PSCM should be configured to accept TJA/LCA commands before these |
||||
commands will be processed. This can be done using tools such as Forscan. |
||||
|
||||
Frequency is 20Hz. |
||||
""" |
||||
|
||||
values = { |
||||
"LatCtlRng_L_Max": 0, # Unknown [0|126] meter |
||||
"HandsOffCnfm_B_Rq": 0, # Unknown: 0=Inactive, 1=Active [0|1] |
||||
"LatCtl_D_Rq": lca_rq, # Mode: 0=None, 1=ContinuousPathFollowing, 2=InterventionLeft, 3=InterventionRight, 4-7=NotUsed [0|7] |
||||
"LatCtlRampType_D_Rq": ramp_type, # Ramp speed: 0=Slow, 1=Medium, 2=Fast, 3=Immediate [0|3] |
||||
"LatCtlPrecision_D_Rq": precision, # Precision: 0=Comfortable, 1=Precise, 2/3=NotUsed [0|3] |
||||
"LatCtlPathOffst_L_Actl": clip(path_offset, -5.12, 5.11), # Path offset [-5.12|5.11] meter |
||||
"LatCtlPath_An_Actl": clip(path_angle, -0.5, 0.5235), # Path angle [-0.5|0.5235] radians |
||||
"LatCtlCurv_NoRate_Actl": clip(curvature_rate, -0.001024, 0.00102375), # Curvature rate [-0.001024|0.00102375] 1/meter^2 |
||||
"LatCtlCurv_No_Actl": clip(curvature, -0.02, 0.02094), # Curvature [-0.02|0.02094] 1/meter |
||||
} |
||||
return packer.make_can_msg("LateralMotionControl", 0, values) |
||||
|
||||
|
||||
def create_lkas_ui_command(packer, main_on: bool, enabled: bool, steer_alert: bool, stock_values): |
||||
""" |
||||
Creates a CAN message for the Ford IPC IPMA/LKAS status. |
||||
|
||||
Show the LKAS status with the "driver assist" lines in the IPC. |
||||
|
||||
def create_lkas_ui(packer, main_on, enabled, steer_alert): |
||||
"""Creates a CAN message for the Ford Steer Ui.""" |
||||
Stock functionality is maintained by passing through unmodified signals. |
||||
|
||||
if not main_on: |
||||
lines = 0xf |
||||
elif enabled: |
||||
lines = 0x3 |
||||
Frequency is 1Hz. |
||||
""" |
||||
|
||||
# LaActvStats_D_Dsply |
||||
# TODO: get LDW state from OP |
||||
if enabled: |
||||
lines = 6 |
||||
elif main_on: |
||||
lines = 0 |
||||
else: |
||||
lines = 0x6 |
||||
lines = 30 |
||||
|
||||
values = { |
||||
"Set_Me_X80": 0x80, |
||||
"Set_Me_X45": 0x45, |
||||
"Set_Me_X30": 0x30, |
||||
"Lines_Hud": lines, |
||||
"Hands_Warning_W_Chime": steer_alert, |
||||
"FeatConfigIpmaActl": stock_values["FeatConfigIpmaActl"], # [0|65535] |
||||
"FeatNoIpmaActl": stock_values["FeatNoIpmaActl"], # [0|65535] |
||||
"PersIndexIpma_D_Actl": stock_values["PersIndexIpma_D_Actl"], # [0|7] |
||||
"AhbcRampingV_D_Rq": stock_values["AhbcRampingV_D_Rq"], # AHB ramping [0|3] |
||||
"LaActvStats_D_Dsply": lines, # LKAS status (lines) [0|31] |
||||
"LaDenyStats_B_Dsply": stock_values["LaDenyStats_B_Dsply"], # LKAS error [0|1] |
||||
"LaHandsOff_D_Dsply": 2 if steer_alert else 0, # 0=HandsOn, 1=Level1 (w/o chime), 2=Level2 (w/ chime), 3=Suppressed |
||||
"CamraDefog_B_Req": stock_values["CamraDefog_B_Req"], # Windshield heater? [0|1] |
||||
"CamraStats_D_Dsply": stock_values["CamraStats_D_Dsply"], # Camera status [0|3] |
||||
"DasAlrtLvl_D_Dsply": stock_values["DasAlrtLvl_D_Dsply"], # DAS alert level [0|7] |
||||
"DasStats_D_Dsply": stock_values["DasStats_D_Dsply"], # DAS status [0|3] |
||||
"DasWarn_D_Dsply": stock_values["DasWarn_D_Dsply"], # DAS warning [0|3] |
||||
"AhbHiBeam_D_Rq": stock_values["AhbHiBeam_D_Rq"], # AHB status [0|3] |
||||
"Set_Me_X1": stock_values["Set_Me_X1"], # [0|15] |
||||
} |
||||
return packer.make_can_msg("Lane_Keep_Assist_Ui", 0, values) |
||||
return packer.make_can_msg("IPMA_Data", 0, values) |
||||
|
||||
|
||||
def create_acc_ui_command(packer, main_on: bool, enabled: bool, stock_values): |
||||
""" |
||||
Creates a CAN message for the Ford IPC adaptive cruise, forward collision |
||||
warning and traffic jam assist status. |
||||
|
||||
Stock functionality is maintained by passing through unmodified signals. |
||||
|
||||
Frequency is 20Hz. |
||||
""" |
||||
|
||||
values = { |
||||
"HaDsply_No_Cs": stock_values["HaDsply_No_Cs"], # [0|255] |
||||
"HaDsply_No_Cnt": stock_values["HaDsply_No_Cnt"], # [0|15] |
||||
"AccStopStat_D_Dsply": stock_values["AccStopStat_D_Dsply"], # ACC stopped status message: 0=NoDisplay, 1=ResumeReady, 2=Stopped, 3=PressResume [0|3] |
||||
"AccTrgDist2_D_Dsply": stock_values["AccTrgDist2_D_Dsply"], # ACC target distance [0|15] |
||||
"AccStopRes_B_Dsply": stock_values["AccStopRes_B_Dsply"], # [0|1] |
||||
"TjaWarn_D_Rq": stock_values["TjaWarn_D_Rq"], # TJA warning: 0=NoWarning, 1=Cancel, 2=HardTakeOverLevel1, 3=HardTakeOverLevel2 [0|7] |
||||
"Tja_D_Stat": 2 if enabled else (1 if main_on else 0), # TJA status: 0=Off, 1=Standby, 2=Active, 3=InterventionLeft, 4=InterventionRight, 5=WarningLeft, 6=WarningRight, 7=NotUsed [0|7] |
||||
"TjaMsgTxt_D_Dsply": stock_values["TjaMsgTxt_D_Dsply"], # TJA text [0|7] |
||||
"IaccLamp_D_Rq": stock_values["IaccLamp_D_Rq"], # iACC status icon [0|3] |
||||
"AccMsgTxt_D2_Rq": stock_values["AccMsgTxt_D2_Rq"], # ACC text [0|15] |
||||
"FcwDeny_B_Dsply": stock_values["FcwDeny_B_Dsply"], # FCW disabled [0|1] |
||||
"FcwMemStat_B_Actl": stock_values["FcwMemStat_B_Actl"], # FCW enabled setting [0|1] |
||||
"AccTGap_B_Dsply": stock_values["AccTGap_B_Dsply"], # ACC time gap display setting [0|1] |
||||
"CadsAlignIncplt_B_Actl": stock_values["CadsAlignIncplt_B_Actl"], # Radar alignment? [0|1] |
||||
"AccFllwMde_B_Dsply": stock_values["AccFllwMde_B_Dsply"], # ACC follow mode display setting [0|1] |
||||
"CadsRadrBlck_B_Actl": stock_values["CadsRadrBlck_B_Actl"], # Radar blocked? [0|1] |
||||
"CmbbPostEvnt_B_Dsply": stock_values["CmbbPostEvnt_B_Dsply"], # AEB event status [0|1] |
||||
"AccStopMde_B_Dsply": stock_values["AccStopMde_B_Dsply"], # ACC stop mode display setting [0|1] |
||||
"FcwMemSens_D_Actl": stock_values["FcwMemSens_D_Actl"], # FCW sensitivity setting [0|3] |
||||
"FcwMsgTxt_D_Rq": stock_values["FcwMsgTxt_D_Rq"], # FCW text [0|7] |
||||
"AccWarn_D_Dsply": stock_values["AccWarn_D_Dsply"], # ACC warning [0|3] |
||||
"FcwVisblWarn_B_Rq": stock_values["FcwVisblWarn_B_Rq"], # FCW alert: 0=Off, 1=On [0|1] |
||||
"FcwAudioWarn_B_Rq": stock_values["FcwAudioWarn_B_Rq"], # FCW audio: 0=Off, 1=On [0|1] |
||||
"AccTGap_D_Dsply": stock_values["AccTGap_D_Dsply"], # ACC time gap: 1=Time_Gap_1, 2=Time_Gap_2, ..., 5=Time_Gap_5 [0|7] |
||||
"AccMemEnbl_B_RqDrv": stock_values["AccMemEnbl_B_RqDrv"], # ACC setting: 0=NormalCruise, 1=AdaptiveCruise [0|1] |
||||
"FdaMem_B_Stat": stock_values["FdaMem_B_Stat"], # FDA enabled setting [0|1] |
||||
} |
||||
return packer.make_can_msg("ACCDATA_3", 0, values) |
||||
|
||||
|
||||
def spam_cancel_button(packer, cancel=1): |
||||
""" |
||||
Creates a CAN message for the Ford SCCM buttons/switches. |
||||
|
||||
Includes cruise control buttons, turn lights and more. |
||||
""" |
||||
|
||||
def spam_cancel_button(packer): |
||||
values = { |
||||
"Cancel": 1 |
||||
"CcAslButtnCnclPress": cancel, # CC cancel button |
||||
} |
||||
return packer.make_can_msg("Steering_Buttons", 0, values) |
||||
return packer.make_can_msg("Steering_Data_FD1", 0, values) |
||||
|
||||
@ -1,72 +1,84 @@ |
||||
#!/usr/bin/env python3 |
||||
from cereal import car |
||||
from common.conversions import Conversions as CV |
||||
from selfdrive.car.ford.values import MAX_ANGLE |
||||
from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness, gen_empty_fingerprint, get_safety_config |
||||
from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness, gen_empty_fingerprint |
||||
from selfdrive.car.ford.values import TransmissionType, CAR |
||||
from selfdrive.car.interfaces import CarInterfaceBase |
||||
|
||||
|
||||
EventName = car.CarEvent.EventName |
||||
|
||||
|
||||
class CarInterface(CarInterfaceBase): |
||||
@staticmethod |
||||
def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=None, disable_radar=False): |
||||
ret = CarInterfaceBase.get_std_params(candidate, fingerprint) |
||||
|
||||
ret.carName = "ford" |
||||
ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.ford)] |
||||
#ret.safetyConfigs = [get_safety_config(car.CarParams.SafetyModel.ford)] |
||||
ret.dashcamOnly = True |
||||
|
||||
ret.wheelbase = 2.85 |
||||
ret.steerRatio = 14.8 |
||||
ret.mass = 3045. * CV.LB_TO_KG + STD_CARGO_KG |
||||
ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]] |
||||
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.01], [0.005]] # TODO: tune this |
||||
ret.lateralTuning.pid.kf = 1. / MAX_ANGLE # MAX Steer angle to normalize FF |
||||
ret.steerActuatorDelay = 0.1 # Default delay, not measured yet |
||||
# Angle-based steering |
||||
# TODO: use curvature control when ready |
||||
ret.steerControlType = car.CarParams.SteerControlType.angle |
||||
ret.steerActuatorDelay = 0.1 |
||||
ret.steerLimitTimer = 1.0 |
||||
|
||||
# TODO: detect stop-and-go vehicles |
||||
stop_and_go = False |
||||
|
||||
if candidate == CAR.ESCAPE_MK4: |
||||
ret.wheelbase = 2.71 |
||||
ret.steerRatio = 14.3 # Copied from Focus |
||||
tire_stiffness_factor = 0.5328 # Copied from Focus |
||||
ret.mass = 1750 + STD_CARGO_KG |
||||
|
||||
elif candidate == CAR.FOCUS_MK4: |
||||
ret.wheelbase = 2.7 |
||||
ret.steerRatio = 14.3 |
||||
tire_stiffness_factor = 0.5328 |
||||
ret.mass = 1350 + STD_CARGO_KG |
||||
|
||||
else: |
||||
raise ValueError(f"Unsupported car: ${candidate}") |
||||
|
||||
# Auto Transmission: Gear_Shift_by_Wire_FD1 |
||||
# TODO: detect transmission in car_fw? |
||||
if 0x5A in fingerprint[0]: |
||||
ret.transmissionType = TransmissionType.automatic |
||||
else: |
||||
ret.transmissionType = TransmissionType.manual |
||||
|
||||
# BSM: Side_Detect_L_Stat, Side_Detect_R_Stat |
||||
# TODO: detect bsm in car_fw? |
||||
ret.enableBsm = 0x3A6 in fingerprint[0] and 0x3A7 in fingerprint[0] |
||||
|
||||
# min speed to enable ACC. if car can do stop and go, then set enabling speed |
||||
# to a negative value, so it won't matter. |
||||
ret.minEnableSpeed = -1. if (stop_and_go) else 20. * CV.MPH_TO_MS |
||||
# LCA can steer down to zero |
||||
ret.minSteerSpeed = 0. |
||||
|
||||
ret.steerRateCost = 1.0 |
||||
ret.centerToFront = ret.wheelbase * 0.44 |
||||
tire_stiffness_factor = 0.5328 |
||||
# TODO: add minSteerSpeed |
||||
ret.minEnableSpeed = 12. * CV.MPH_TO_MS |
||||
|
||||
# TODO: get actual value, for now starting with reasonable value for |
||||
# civic and scaling by mass and wheelbase |
||||
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, ret.tireStiffnessRear = scale_tire_stiffness(ret.mass, ret.wheelbase, ret.centerToFront, |
||||
tire_stiffness_factor=tire_stiffness_factor) |
||||
|
||||
ret.steerControlType = car.CarParams.SteerControlType.angle |
||||
|
||||
return ret |
||||
|
||||
# returns a car.CarState |
||||
def update(self, c, can_strings): |
||||
# ******************* do can recv ******************* |
||||
self.cp.update_strings(can_strings) |
||||
|
||||
ret = self.CS.update(self.cp) |
||||
def _update(self, c): |
||||
ret = self.CS.update(self.cp, self.cp_cam) |
||||
|
||||
ret.canValid = self.cp.can_valid |
||||
ret.steeringRateLimited = self.CC.steer_rate_limited if self.CC is not None else False |
||||
|
||||
# events |
||||
events = self.create_common_events(ret) |
||||
|
||||
if self.CS.lkas_state not in (2, 3) and ret.vEgo > 13. * CV.MPH_TO_MS and ret.cruiseState.enabled: |
||||
events.add(car.CarEvent.EventName.steerTempUnavailable) |
||||
|
||||
ret.events = events.to_msg() |
||||
|
||||
self.CS.out = ret.as_reader() |
||||
return self.CS.out |
||||
return ret |
||||
|
||||
# pass in a car.CarControl |
||||
# to be called @ 100hz |
||||
def apply(self, c): |
||||
|
||||
ret = self.CC.update(c.enabled, self.CS, self.frame, c.actuators, |
||||
c.hudControl.visualAlert, c.cruiseControl.cancel) |
||||
|
||||
ret = self.CC.update(c, self.CS, self.frame) |
||||
self.frame += 1 |
||||
return ret |
||||
|
||||
@ -1,21 +1,82 @@ |
||||
from collections import namedtuple |
||||
from typing import Dict, List, Union |
||||
|
||||
from cereal import car |
||||
from selfdrive.car import dbc_dict |
||||
from selfdrive.car.docs_definitions import CarInfo |
||||
|
||||
Ecu = car.CarParams.Ecu |
||||
TransmissionType = car.CarParams.TransmissionType |
||||
|
||||
AngleRateLimit = namedtuple('AngleRateLimit', ['speed_points', 'max_angle_diff_points']) |
||||
|
||||
|
||||
class CarControllerParams: |
||||
# Messages: Lane_Assist_Data1, LateralMotionControl |
||||
LKAS_STEER_STEP = 5 |
||||
# Message: IPMA_Data |
||||
LKAS_UI_STEP = 100 |
||||
# Message: ACCDATA_3 |
||||
ACC_UI_STEP = 5 |
||||
|
||||
STEER_RATE_LIMIT_UP = AngleRateLimit(speed_points=[0., 5., 15.], max_angle_diff_points=[5., .8, .15]) |
||||
STEER_RATE_LIMIT_DOWN = AngleRateLimit(speed_points=[0., 5., 15.], max_angle_diff_points=[5., 3.5, 0.4]) |
||||
|
||||
MAX_ANGLE = 87. # make sure we never command the extremes (0xfff) which cause latching fault |
||||
|
||||
class CANBUS: |
||||
main = 0 |
||||
radar = 1 |
||||
camera = 2 |
||||
|
||||
|
||||
class CAR: |
||||
FUSION = "FORD FUSION 2018" |
||||
ESCAPE_MK4 = "FORD ESCAPE 4TH GEN" |
||||
FOCUS_MK4 = "FORD FOCUS 4TH GEN" |
||||
|
||||
|
||||
CAR_INFO: Dict[str, Union[CarInfo, List[CarInfo]]] = { |
||||
CAR.FUSION: CarInfo("Ford Fusion 2018", "All") |
||||
} |
||||
|
||||
|
||||
FW_VERSIONS = { |
||||
CAR.ESCAPE_MK4: { |
||||
(Ecu.eps, 0x730, None): [ |
||||
b'LX6C-14D003-AH\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', |
||||
], |
||||
(Ecu.esp, 0x760, None): [ |
||||
b'LX6C-2D053-NS\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', |
||||
], |
||||
(Ecu.fwdRadar, 0x764, None): [ |
||||
b'LB5T-14D049-AB\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', |
||||
], |
||||
(Ecu.fwdCamera, 0x706, None): [ |
||||
b'LJ6T-14F397-AD\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', |
||||
], |
||||
(Ecu.engine, 0x7E0, None): [ |
||||
b'LX6A-14C204-ESG\x00\x00\x00\x00\x00\x00\x00\x00\x00', |
||||
], |
||||
}, |
||||
CAR.FOCUS_MK4: { |
||||
(Ecu.eps, 0x730, None): [ |
||||
b'JX6C-14D003-AH\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', |
||||
], |
||||
(Ecu.esp, 0x760, None): [ |
||||
b'JX61-2D053-CJ\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', |
||||
], |
||||
(Ecu.fwdRadar, 0x764, None): [ |
||||
b'JX7T-14D049-AC\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', |
||||
], |
||||
(Ecu.fwdCamera, 0x706, None): [ |
||||
b'JX7T-14F397-AH\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00', |
||||
], |
||||
(Ecu.engine, 0x7E0, None): [ |
||||
b'JX6A-14C204-BPL\x00\x00\x00\x00\x00\x00\x00\x00\x00', |
||||
], |
||||
}, |
||||
} |
||||
|
||||
|
||||
DBC = { |
||||
CAR.FUSION: dbc_dict('ford_fusion_2018_pt', 'ford_fusion_2018_adas'), |
||||
CAR.ESCAPE_MK4: dbc_dict('ford_lincoln_base_pt', 'ford_fusion_2018_adas'), |
||||
CAR.FOCUS_MK4: dbc_dict('ford_lincoln_base_pt', 'ford_fusion_2018_adas'), |
||||
} |
||||
|
||||
@ -1,84 +0,0 @@ |
||||
import math |
||||
import numpy as np |
||||
|
||||
from common.numpy_fast import clip |
||||
from common.realtime import DT_CTRL |
||||
from cereal import log |
||||
from selfdrive.controls.lib.latcontrol import LatControl, MIN_STEER_SPEED |
||||
|
||||
|
||||
class LatControlLQR(LatControl): |
||||
def __init__(self, CP, CI): |
||||
super().__init__(CP, CI) |
||||
self.scale = CP.lateralTuning.lqr.scale |
||||
self.ki = CP.lateralTuning.lqr.ki |
||||
|
||||
self.A = np.array(CP.lateralTuning.lqr.a).reshape((2, 2)) |
||||
self.B = np.array(CP.lateralTuning.lqr.b).reshape((2, 1)) |
||||
self.C = np.array(CP.lateralTuning.lqr.c).reshape((1, 2)) |
||||
self.K = np.array(CP.lateralTuning.lqr.k).reshape((1, 2)) |
||||
self.L = np.array(CP.lateralTuning.lqr.l).reshape((2, 1)) |
||||
self.dc_gain = CP.lateralTuning.lqr.dcGain |
||||
|
||||
self.x_hat = np.array([[0], [0]]) |
||||
self.i_unwind_rate = 0.3 * DT_CTRL |
||||
self.i_rate = 1.0 * DT_CTRL |
||||
|
||||
self.reset() |
||||
|
||||
def reset(self): |
||||
super().reset() |
||||
self.i_lqr = 0.0 |
||||
|
||||
def update(self, active, CS, CP, VM, params, last_actuators, desired_curvature, desired_curvature_rate): |
||||
lqr_log = log.ControlsState.LateralLQRState.new_message() |
||||
|
||||
torque_scale = (0.45 + CS.vEgo / 60.0)**2 # Scale actuator model with speed |
||||
|
||||
# Subtract offset. Zero angle should correspond to zero torque |
||||
steering_angle_no_offset = CS.steeringAngleDeg - params.angleOffsetAverageDeg |
||||
|
||||
desired_angle = math.degrees(VM.get_steer_from_curvature(-desired_curvature, CS.vEgo, params.roll)) |
||||
|
||||
instant_offset = params.angleOffsetDeg - params.angleOffsetAverageDeg |
||||
desired_angle += instant_offset # Only add offset that originates from vehicle model errors |
||||
lqr_log.steeringAngleDesiredDeg = desired_angle |
||||
|
||||
# Update Kalman filter |
||||
angle_steers_k = float(self.C.dot(self.x_hat)) |
||||
e = steering_angle_no_offset - angle_steers_k |
||||
self.x_hat = self.A.dot(self.x_hat) + self.B.dot(CS.steeringTorqueEps / torque_scale) + self.L.dot(e) |
||||
|
||||
if CS.vEgo < MIN_STEER_SPEED or not active: |
||||
lqr_log.active = False |
||||
lqr_output = 0. |
||||
output_steer = 0. |
||||
self.reset() |
||||
else: |
||||
lqr_log.active = True |
||||
|
||||
# LQR |
||||
u_lqr = float(desired_angle / self.dc_gain - self.K.dot(self.x_hat)) |
||||
lqr_output = torque_scale * u_lqr / self.scale |
||||
|
||||
# Integrator |
||||
if CS.steeringPressed: |
||||
self.i_lqr -= self.i_unwind_rate * float(np.sign(self.i_lqr)) |
||||
else: |
||||
error = desired_angle - angle_steers_k |
||||
i = self.i_lqr + self.ki * self.i_rate * error |
||||
control = lqr_output + i |
||||
|
||||
if (error >= 0 and (control <= self.steer_max or i < 0.0)) or \ |
||||
(error <= 0 and (control >= -self.steer_max or i > 0.0)): |
||||
self.i_lqr = i |
||||
|
||||
output_steer = lqr_output + self.i_lqr |
||||
output_steer = clip(output_steer, -self.steer_max, self.steer_max) |
||||
|
||||
lqr_log.steeringAngleDeg = angle_steers_k |
||||
lqr_log.i = self.i_lqr |
||||
lqr_log.output = output_steer |
||||
lqr_log.lqrOutput = lqr_output |
||||
lqr_log.saturated = self._check_saturation(self.steer_max - abs(output_steer) < 1e-3, CS) |
||||
return output_steer, desired_angle, lqr_log |
||||
@ -0,0 +1,79 @@ |
||||
import math |
||||
from selfdrive.controls.lib.pid import PIDController |
||||
from common.numpy_fast import interp |
||||
from selfdrive.controls.lib.latcontrol import LatControl, MIN_STEER_SPEED |
||||
from selfdrive.controls.lib.vehicle_model import ACCELERATION_DUE_TO_GRAVITY |
||||
from cereal import log |
||||
|
||||
# At higher speeds (25+mph) we can assume: |
||||
# Lateral acceleration achieved by a specific car correlates to |
||||
# torque applied to the steering rack. It does not correlate to |
||||
# wheel slip, or to speed. |
||||
|
||||
# This controller applies torque to achieve desired lateral |
||||
# accelerations. To compensate for the low speed effects we |
||||
# use a LOW_SPEED_FACTOR in the error. Additionally there is |
||||
# friction in the steering wheel that needs to be overcome to |
||||
# move it at all, this is compensated for too. |
||||
|
||||
|
||||
LOW_SPEED_FACTOR = 200 |
||||
JERK_THRESHOLD = 0.2 |
||||
|
||||
|
||||
class LatControlTorque(LatControl): |
||||
def __init__(self, CP, CI): |
||||
super().__init__(CP, CI) |
||||
self.pid = PIDController(CP.lateralTuning.torque.kp, CP.lateralTuning.torque.ki, |
||||
k_f=CP.lateralTuning.torque.kf, pos_limit=1.0, neg_limit=-1.0) |
||||
self.get_steer_feedforward = CI.get_steer_feedforward_function() |
||||
self.steer_max = 1.0 |
||||
self.pid.pos_limit = self.steer_max |
||||
self.pid.neg_limit = -self.steer_max |
||||
self.use_steering_angle = CP.lateralTuning.torque.useSteeringAngle |
||||
self.friction = CP.lateralTuning.torque.friction |
||||
|
||||
def reset(self): |
||||
super().reset() |
||||
self.pid.reset() |
||||
|
||||
def update(self, active, CS, CP, VM, params, last_actuators, desired_curvature, desired_curvature_rate, llk): |
||||
pid_log = log.ControlsState.LateralTorqueState.new_message() |
||||
|
||||
if CS.vEgo < MIN_STEER_SPEED or not active: |
||||
output_torque = 0.0 |
||||
pid_log.active = False |
||||
self.pid.reset() |
||||
else: |
||||
if self.use_steering_angle: |
||||
actual_curvature = -VM.calc_curvature(math.radians(CS.steeringAngleDeg - params.angleOffsetDeg), CS.vEgo, params.roll) |
||||
else: |
||||
actual_curvature = llk.angularVelocityCalibrated.value[2] / CS.vEgo |
||||
desired_lateral_accel = desired_curvature * CS.vEgo**2 |
||||
desired_lateral_jerk = desired_curvature_rate * CS.vEgo**2 |
||||
actual_lateral_accel = actual_curvature * CS.vEgo**2 |
||||
|
||||
setpoint = desired_lateral_accel + LOW_SPEED_FACTOR * desired_curvature |
||||
measurement = actual_lateral_accel + LOW_SPEED_FACTOR * actual_curvature |
||||
error = setpoint - measurement |
||||
pid_log.error = error |
||||
|
||||
ff = desired_lateral_accel - params.roll * ACCELERATION_DUE_TO_GRAVITY |
||||
output_torque = self.pid.update(error, |
||||
override=CS.steeringPressed, feedforward=ff, |
||||
speed=CS.vEgo, |
||||
freeze_integrator=CS.steeringRateLimited) |
||||
|
||||
friction_compensation = interp(desired_lateral_jerk, [-JERK_THRESHOLD, JERK_THRESHOLD], [-self.friction, self.friction]) |
||||
output_torque += friction_compensation |
||||
|
||||
pid_log.active = True |
||||
pid_log.p = self.pid.p |
||||
pid_log.i = self.pid.i |
||||
pid_log.d = self.pid.d |
||||
pid_log.f = self.pid.f |
||||
pid_log.output = -output_torque |
||||
pid_log.saturated = self._check_saturation(self.steer_max - abs(output_torque) < 1e-3, CS) |
||||
|
||||
#TODO left is positive in this convention |
||||
return -output_torque, 0.0, pid_log |
||||
Some files were not shown because too many files have changed in this diff Show More
Loading…
Reference in new issue