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 cereal import car |
||||||
from selfdrive.car import make_can_msg |
from common.numpy_fast import clip, interp |
||||||
from selfdrive.car.ford.fordcan import create_steer_command, create_lkas_ui, spam_cancel_button |
from selfdrive.car.ford import fordcan |
||||||
|
from selfdrive.car.ford.values import CarControllerParams |
||||||
from opendbc.can.packer import CANPacker |
from opendbc.can.packer import CANPacker |
||||||
|
|
||||||
VisualAlert = car.CarControl.HUDControl.VisualAlert |
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(): |
class CarController(): |
||||||
def __init__(self, dbc_name, CP, VM): |
def __init__(self, dbc_name, CP, VM): |
||||||
|
self.CP = CP |
||||||
|
self.VM = VM |
||||||
self.packer = CANPacker(dbc_name) |
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.main_on_last = False |
||||||
self.vehicle_model = VM |
self.lkas_enabled_last = False |
||||||
self.generic_toggle_last = 0 |
|
||||||
self.steer_alert_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 = [] |
can_sends = [] |
||||||
steer_alert = visual_alert in (VisualAlert.steerRequired, VisualAlert.ldw) |
|
||||||
|
|
||||||
apply_steer = actuators.steer |
|
||||||
|
|
||||||
if pcm_cancel: |
actuators = CC.actuators |
||||||
#print "CANCELING!!!!" |
hud_control = CC.hudControl |
||||||
can_sends.append(spam_cancel_button(self.packer)) |
|
||||||
|
|
||||||
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 |
# apply rate limits |
||||||
if TOGGLE_DEBUG: |
new_steer = actuators.steeringAngleDeg |
||||||
self.lkas_action += int(CS.out.genericToggle and not self.generic_toggle_last) |
apply_steer = apply_ford_steer_angle_limits(new_steer, self.apply_steer_last, CS.out.vEgo) |
||||||
self.lkas_action &= 0xf |
self.steer_rate_limited = new_steer != apply_steer |
||||||
else: |
|
||||||
self.lkas_action = 5 # 4 and 5 seem the best. 8 and 9 seem to aggressive and laggy |
|
||||||
|
|
||||||
can_sends.append(create_steer_command(self.packer, apply_steer, enabled, |
# send steering commands at 20Hz |
||||||
CS.lkas_state, CS.out.steeringAngleDeg, curvature, self.lkas_action)) |
if (frame % CarControllerParams.LKAS_STEER_STEP) == 0: |
||||||
self.generic_toggle_last = CS.out.genericToggle |
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)) |
# convert steer angle to curvature |
||||||
#can_sends.append(make_can_msg(984, b'\x00\x00\x00\x00\x80\x45\x60\x30', 0)) |
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 \ |
# TODO: get other actuators |
||||||
(self.steer_alert_last != steer_alert): |
curvature_rate = 0 |
||||||
can_sends.append(create_lkas_ui(self.packer, CS.out.cruiseState.available, enabled, steer_alert)) |
path_offset = 0 |
||||||
|
|
||||||
if (frame % 200) == 0: |
ramp_type = 3 # 0=Slow, 1=Medium, 2=Fast, 3=Immediately |
||||||
can_sends.append(make_can_msg(1875, b'\x80\xb0\x55\x55\x78\x90\x00\x00', 1)) |
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)) |
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(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)) |
|
||||||
|
|
||||||
can_sends.append(make_can_msg(1910, b'\x06\x4b\x06\x4b\x42\xd3\x11\x30', 1)) |
# send lkas ui command at 1Hz or if ui state changes |
||||||
can_sends.append(make_can_msg(1911, b'\x48\x53\x37\x54\x48\x53\x37\x54', 1)) |
if (frame % CarControllerParams.LKAS_UI_STEP) == 0 or send_ui: |
||||||
can_sends.append(make_can_msg(1912, b'\x31\x34\x47\x30\x38\x31\x43\x42', 1)) |
can_sends.append(fordcan.create_lkas_ui_command(self.packer, main_on, CC.latActive, steer_alert, CS.lkas_status_stock_values)) |
||||||
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)) |
|
||||||
|
|
||||||
static_msgs = range(1653, 1658) |
# send acc ui command at 20Hz or if ui state changes |
||||||
for addr in static_msgs: |
if (frame % CarControllerParams.ACC_UI_STEP) == 0 or send_ui: |
||||||
cnt = (frame % 10) + 1 |
can_sends.append(fordcan.create_acc_ui_command(self.packer, main_on, CC.latActive, CS.acc_tja_status_stock_values)) |
||||||
can_sends.append(make_can_msg(addr, (cnt << 4).to_bytes(1, 'little') + b'\x00\x00\x00\x00\x00\x00\x00', 1)) |
|
||||||
|
|
||||||
self.enabled_last = enabled |
self.main_on_last = main_on |
||||||
self.main_on_last = CS.out.cruiseState.available |
self.lkas_enabled_last = CC.latActive |
||||||
self.steer_alert_last = steer_alert |
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 cereal import car |
||||||
from common.conversions import Conversions as CV |
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 opendbc.can.parser import CANParser |
||||||
from selfdrive.car.interfaces import CarStateBase |
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): |
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 = car.CarState.new_message() |
||||||
|
|
||||||
ret.wheelSpeeds = self.get_wheel_speeds( |
# car speed |
||||||
cp.vl["WheelSpeed_CG1"]["WhlFl_W_Meas"], |
ret.vEgoRaw = cp.vl["EngVehicleSpThrottle2"]["Veh_V_ActlEng"] * CV.KPH_TO_MS |
||||||
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]) |
|
||||||
ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw) |
ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw) |
||||||
ret.standstill = not ret.vEgoRaw > 0.001 |
ret.yawRate = cp.vl["Yaw_Data_FD1"]["VehYaw_W_Actl"] * CV.RAD_TO_DEG |
||||||
ret.steeringAngleDeg = cp.vl["Steering_Wheel_Data_CG1"]["SteWhlRelInit_An_Sns"] |
ret.standstill = cp.vl["DesiredTorqBrk"]["VehStop_D_Stat"] == 1 |
||||||
ret.steeringPressed = not cp.vl["Lane_Keep_Assist_Status"]["LaHandsOff_B_Actl"] |
|
||||||
ret.steerFaultPermanent = cp.vl["Lane_Keep_Assist_Status"]["LaActDeny_B_Actl"] == 1 |
# gas pedal |
||||||
ret.cruiseState.speed = cp.vl["Cruise_Status"]["Set_Speed"] * CV.MPH_TO_MS |
ret.gas = cp.vl["EngVehicleSpThrottle"]["ApedPos_Pc_ActlArb"] / 100. |
||||||
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.gasPressed = ret.gas > 1e-6 |
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"]) |
# brake pedal |
||||||
# TODO: we also need raw driver torque, needed for Assisted Lane Change |
ret.brake = cp.vl["BrakeSnData_4"]["BrkTot_Tq_Actl"] / 32756. # torque in Nm |
||||||
self.lkas_state = cp.vl["Lane_Keep_Assist_Status"]["LaActAvail_D_Actl"] |
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 |
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 |
@staticmethod |
||||||
def get_can_parser(CP): |
def get_can_parser(CP): |
||||||
signals = [ |
signals = [ |
||||||
# sig_name, sig_address |
# sig_name, sig_address |
||||||
("WhlRr_W_Meas", "WheelSpeed_CG1"), |
("Veh_V_ActlEng", "EngVehicleSpThrottle2"), # ABS vehicle speed (kph) |
||||||
("WhlRl_W_Meas", "WheelSpeed_CG1"), |
("VehYaw_W_Actl", "Yaw_Data_FD1"), # ABS vehicle yaw rate (rad/s) |
||||||
("WhlFr_W_Meas", "WheelSpeed_CG1"), |
("VehStop_D_Stat", "DesiredTorqBrk"), # ABS vehicle stopped |
||||||
("WhlFl_W_Meas", "WheelSpeed_CG1"), |
("PrkBrkStatus", "DesiredTorqBrk"), # ABS park brake status |
||||||
("SteWhlRelInit_An_Sns", "Steering_Wheel_Data_CG1"), |
("ApedPos_Pc_ActlArb", "EngVehicleSpThrottle"), # PCM throttle (pct) |
||||||
("Cruise_State", "Cruise_Status"), |
("BrkTot_Tq_Actl", "BrakeSnData_4"), # ABS brake torque (Nm) |
||||||
("Set_Speed", "Cruise_Status"), |
("BpedDrvAppl_D_Actl", "EngBrakeData"), # PCM driver brake pedal pressed |
||||||
("LaActAvail_D_Actl", "Lane_Keep_Assist_Status"), |
("Veh_V_DsplyCcSet", "EngBrakeData"), # PCM ACC set speed (mph) |
||||||
("LaHandsOff_B_Actl", "Lane_Keep_Assist_Status"), |
# The units might change with IPC settings? |
||||||
("LaActDeny_B_Actl", "Lane_Keep_Assist_Status"), |
("CcStat_D_Actl", "EngBrakeData"), # PCM ACC status |
||||||
("ApedPosScal_Pc_Actl", "EngineData_14"), |
("StePinComp_An_Est", "SteeringPinion_Data"), # PSCM estimated steering angle (deg) |
||||||
("Dist_Incr", "Steering_Buttons"), |
# Calculates steering angle (and offset) from pinion |
||||||
("Brake_Drv_Appl", "Cruise_Status"), |
# angle and driving measurements. |
||||||
] |
# StePinRelInit_An_Sns is the pinion angle, initialised |
||||||
checks = [] |
# to zero at the beginning of the drive. |
||||||
return CANParser(DBC[CP.carFingerprint]["pt"], signals, checks, 0, enforce_checks=False) |
("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 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): |
def create_lkas_command(packer, angle_deg: float, curvature: float): |
||||||
"""Creates a CAN message for the Ford Steer Command.""" |
""" |
||||||
|
Creates a CAN message for the Ford LKAS Command. |
||||||
|
|
||||||
#if enabled and lkas available: |
This command can apply "Lane Keeping Aid" manoeuvres, which are subject to the |
||||||
if enabled and lkas_state in (2, 3): # and (frame % 500) >= 3: |
PSCM lockout. |
||||||
action = lkas_action |
|
||||||
else: |
|
||||||
action = 0xf |
|
||||||
angle_cmd = angle_steers/MAX_ANGLE |
|
||||||
|
|
||||||
angle_cmd = clip(angle_cmd * MAX_ANGLE, - MAX_ANGLE, MAX_ANGLE) |
Frequency is 20Hz. |
||||||
|
""" |
||||||
|
|
||||||
values = { |
values = { |
||||||
"Lkas_Action": action, |
"LkaDrvOvrrd_D_Rq": 0, # driver override level? [0|3] |
||||||
"Lkas_Alert": 0xf, # no alerts |
"LkaActvStats_D2_Req": 0, # action [0|7] |
||||||
"Lane_Curvature": clip(curvature, -0.01, 0.01), # is it just for debug? |
"LaRefAng_No_Req": angle_deg, # angle [-102.4|102.3] degrees |
||||||
#"Lane_Curvature": 0, # is it just for debug? |
"LaRampType_B_Req": 0, # Ramp speed: 0=Smooth, 1=Quick |
||||||
"Steer_Angle_Req": angle_cmd |
"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): |
Stock functionality is maintained by passing through unmodified signals. |
||||||
"""Creates a CAN message for the Ford Steer Ui.""" |
|
||||||
|
|
||||||
if not main_on: |
Frequency is 1Hz. |
||||||
lines = 0xf |
""" |
||||||
elif enabled: |
|
||||||
lines = 0x3 |
# LaActvStats_D_Dsply |
||||||
|
# TODO: get LDW state from OP |
||||||
|
if enabled: |
||||||
|
lines = 6 |
||||||
|
elif main_on: |
||||||
|
lines = 0 |
||||||
else: |
else: |
||||||
lines = 0x6 |
lines = 30 |
||||||
|
|
||||||
values = { |
values = { |
||||||
"Set_Me_X80": 0x80, |
"FeatConfigIpmaActl": stock_values["FeatConfigIpmaActl"], # [0|65535] |
||||||
"Set_Me_X45": 0x45, |
"FeatNoIpmaActl": stock_values["FeatNoIpmaActl"], # [0|65535] |
||||||
"Set_Me_X30": 0x30, |
"PersIndexIpma_D_Actl": stock_values["PersIndexIpma_D_Actl"], # [0|7] |
||||||
"Lines_Hud": lines, |
"AhbcRampingV_D_Rq": stock_values["AhbcRampingV_D_Rq"], # AHB ramping [0|3] |
||||||
"Hands_Warning_W_Chime": steer_alert, |
"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 = { |
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 |
#!/usr/bin/env python3 |
||||||
from cereal import car |
from cereal import car |
||||||
from common.conversions import Conversions as CV |
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 |
||||||
from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness, gen_empty_fingerprint, get_safety_config |
from selfdrive.car.ford.values import TransmissionType, CAR |
||||||
from selfdrive.car.interfaces import CarInterfaceBase |
from selfdrive.car.interfaces import CarInterfaceBase |
||||||
|
|
||||||
|
|
||||||
|
EventName = car.CarEvent.EventName |
||||||
|
|
||||||
|
|
||||||
class CarInterface(CarInterfaceBase): |
class CarInterface(CarInterfaceBase): |
||||||
@staticmethod |
@staticmethod |
||||||
def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=None, disable_radar=False): |
def get_params(candidate, fingerprint=gen_empty_fingerprint(), car_fw=None, disable_radar=False): |
||||||
ret = CarInterfaceBase.get_std_params(candidate, fingerprint) |
ret = CarInterfaceBase.get_std_params(candidate, fingerprint) |
||||||
|
|
||||||
ret.carName = "ford" |
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.dashcamOnly = True |
||||||
|
|
||||||
ret.wheelbase = 2.85 |
# Angle-based steering |
||||||
ret.steerRatio = 14.8 |
# TODO: use curvature control when ready |
||||||
ret.mass = 3045. * CV.LB_TO_KG + STD_CARGO_KG |
ret.steerControlType = car.CarParams.SteerControlType.angle |
||||||
ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]] |
ret.steerActuatorDelay = 0.1 |
||||||
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 |
|
||||||
ret.steerLimitTimer = 1.0 |
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.steerRateCost = 1.0 |
||||||
ret.centerToFront = ret.wheelbase * 0.44 |
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) |
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, |
ret.tireStiffnessFront, ret.tireStiffnessRear = scale_tire_stiffness(ret.mass, ret.wheelbase, ret.centerToFront, |
||||||
tire_stiffness_factor=tire_stiffness_factor) |
tire_stiffness_factor=tire_stiffness_factor) |
||||||
|
|
||||||
ret.steerControlType = car.CarParams.SteerControlType.angle |
|
||||||
|
|
||||||
return ret |
return ret |
||||||
|
|
||||||
# returns a car.CarState |
def _update(self, c): |
||||||
def update(self, c, can_strings): |
ret = self.CS.update(self.cp, self.cp_cam) |
||||||
# ******************* do can recv ******************* |
|
||||||
self.cp.update_strings(can_strings) |
|
||||||
|
|
||||||
ret = self.CS.update(self.cp) |
|
||||||
|
|
||||||
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) |
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() |
ret.events = events.to_msg() |
||||||
|
|
||||||
self.CS.out = ret.as_reader() |
return ret |
||||||
return self.CS.out |
|
||||||
|
|
||||||
# pass in a car.CarControl |
|
||||||
# to be called @ 100hz |
|
||||||
def apply(self, c): |
def apply(self, c): |
||||||
|
ret = self.CC.update(c, self.CS, self.frame) |
||||||
ret = self.CC.update(c.enabled, self.CS, self.frame, c.actuators, |
|
||||||
c.hudControl.visualAlert, c.cruiseControl.cancel) |
|
||||||
|
|
||||||
self.frame += 1 |
self.frame += 1 |
||||||
return ret |
return ret |
||||||
|
|||||||
@ -1,21 +1,82 @@ |
|||||||
|
from collections import namedtuple |
||||||
from typing import Dict, List, Union |
from typing import Dict, List, Union |
||||||
|
|
||||||
from cereal import car |
from cereal import car |
||||||
from selfdrive.car import dbc_dict |
from selfdrive.car import dbc_dict |
||||||
from selfdrive.car.docs_definitions import CarInfo |
from selfdrive.car.docs_definitions import CarInfo |
||||||
|
|
||||||
Ecu = car.CarParams.Ecu |
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: |
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_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 = { |
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