Merge branch 'master' into no-highlander
cydia2020
commit
6f7bdc35ce
|
|
@ -22,8 +22,8 @@ Route: [a route with the bug fix]
|
|||
<!--- ***** Template: Car port *****
|
||||
|
||||
**Checklist**
|
||||
- [ ] added to README
|
||||
- [ ] test route added to [test_routes.py](https://github.com/commaai/openpilot/blob/master/selfdrive/test/test_routes.py)
|
||||
- [ ] added entry to CarInfo in selfdrive/car/*/values.py and ran `selfdrive/car/docs.py` to generate new docs
|
||||
- [ ] test route added to [routes.py](https://github.com/commaai/openpilot/blob/master/selfdrive/car/tests/routes.py)
|
||||
- [ ] route with openpilot:
|
||||
- [ ] route with stock system:
|
||||
|
||||
|
|
|
|||
|
|
@ -383,7 +383,7 @@ jobs:
|
|||
uses: actions/cache@v2
|
||||
with:
|
||||
path: /tmp/comma_download_cache
|
||||
key: car_models-${{ hashFiles('selfdrive/car/tests/test_models.py', 'selfdrive/test/test_routes.py') }}-${{ matrix.job }}
|
||||
key: car_models-${{ hashFiles('selfdrive/car/tests/test_models.py', 'selfdrive/car/tests/routes.py') }}-${{ matrix.job }}
|
||||
- name: Cache scons
|
||||
id: scons-cache
|
||||
# TODO: Change the version to the released version when https://github.com/actions/cache/pull/489 (or 571) is merged.
|
||||
|
|
|
|||
|
|
@ -1,6 +1,7 @@
|
|||
Version 0.8.14 (2022-0X-XX)
|
||||
========================
|
||||
* bigmodel!
|
||||
* Toyota Avalon Hybrid 2022 support
|
||||
|
||||
Version 0.8.13 (2022-02-18)
|
||||
========================
|
||||
|
|
|
|||
|
|
@ -53,6 +53,7 @@ How We Rate The Cars
|
|||
|Lexus|UX Hybrid 2019-21|All|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|
|
||||
|Toyota|Alphard 2019-20|All|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|
|
||||
|Toyota|Avalon 2022|All|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|
|
||||
|Toyota|Avalon Hybrid 2022|All|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|
|
||||
|Toyota|Camry 2021-22|All|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>[<sup>4</sup>](#Footnotes)|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|
|
||||
|Toyota|Camry Hybrid 2021-22|All|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|
|
||||
|Toyota|Corolla 2020-22|All|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|<a href="#"><img valign="top" src="assets/icon-star-full.svg" width="22" /></a>|
|
||||
|
|
|
|||
2
panda
2
panda
|
|
@ -1 +1 @@
|
|||
Subproject commit ba10911dbc0a6f2c5c04886ca793f6ac38400cd3
|
||||
Subproject commit 499906f3244712c1edd7f1cc8e92112d11f2b505
|
||||
|
|
@ -1,15 +1,13 @@
|
|||
#!/usr/bin/env python3
|
||||
import os
|
||||
from collections import Counter
|
||||
from pprint import pprint
|
||||
|
||||
from common.basedir import BASEDIR
|
||||
from selfdrive.car.docs import get_tier_car_rows
|
||||
|
||||
with open(os.path.join(BASEDIR, "docs/CARS.md")) as f:
|
||||
lines = f.readlines()
|
||||
cars = [l for l in lines if l.strip().startswith("|") and l.strip().endswith("|") and
|
||||
"Make" not in l and any(c.isalpha() for c in l)]
|
||||
if __name__ == "__main__":
|
||||
tiers = list(get_tier_car_rows())
|
||||
cars = [car for tier_cars in tiers for car in tier_cars[1]]
|
||||
|
||||
make_count = Counter(l.split('|')[1].split('|')[0].strip() for l in cars)
|
||||
print("\n", "*"*20, len(cars), "total", "*"*20, "\n")
|
||||
make_count = Counter(l[0] for l in cars)
|
||||
print("\n", "*" * 20, len(cars), "total", "*" * 20, "\n")
|
||||
pprint(make_count)
|
||||
|
|
|
|||
|
|
@ -52,7 +52,7 @@ public:
|
|||
CL_CHECK(clReleaseProgram(prg_debayer));
|
||||
}
|
||||
|
||||
void queue(cl_command_queue q, cl_mem cam_buf_cl, cl_mem buf_cl, int width, int height, float gain, cl_event *debayer_event) {
|
||||
void queue(cl_command_queue q, cl_mem cam_buf_cl, cl_mem buf_cl, int width, int height, float gain, float black_level, cl_event *debayer_event) {
|
||||
CL_CHECK(clSetKernelArg(krnl_, 0, sizeof(cl_mem), &cam_buf_cl));
|
||||
CL_CHECK(clSetKernelArg(krnl_, 1, sizeof(cl_mem), &buf_cl));
|
||||
|
||||
|
|
@ -62,6 +62,7 @@ public:
|
|||
const size_t globalWorkSize[] = {size_t(width), size_t(height)};
|
||||
const size_t localWorkSize[] = {debayer_local_worksize, debayer_local_worksize};
|
||||
CL_CHECK(clSetKernelArg(krnl_, 2, localMemSize, 0));
|
||||
CL_CHECK(clSetKernelArg(krnl_, 3, sizeof(float), &black_level));
|
||||
CL_CHECK(clEnqueueNDRangeKernel(q, krnl_, 2, NULL, globalWorkSize, localWorkSize, 0, 0, debayer_event));
|
||||
} else {
|
||||
if (hdr_) {
|
||||
|
|
@ -165,13 +166,15 @@ bool CameraBuf::acquire() {
|
|||
|
||||
if (debayer) {
|
||||
float gain = 0.0;
|
||||
|
||||
float black_level = 42.0;
|
||||
#ifndef QCOM2
|
||||
gain = camera_state->digital_gain;
|
||||
if ((int)gain == 0) gain = 1.0;
|
||||
#else
|
||||
if (camera_state->camera_id == CAMERA_ID_IMX390) black_level = 64.0;
|
||||
#endif
|
||||
|
||||
debayer->queue(q, camrabuf_cl, cur_rgb_buf->buf_cl, rgb_width, rgb_height, gain, &event);
|
||||
debayer->queue(q, camrabuf_cl, cur_rgb_buf->buf_cl, rgb_width, rgb_height, gain, black_level, &event);
|
||||
} else {
|
||||
assert(rgb_stride == camera_state->ci.frame_stride);
|
||||
CL_CHECK(clEnqueueCopyBuffer(q, camrabuf_cl, cur_rgb_buf->buf_cl, 0, 0, cur_rgb_buf->len, 0, 0, &event));
|
||||
|
|
|
|||
|
|
@ -25,7 +25,8 @@
|
|||
#define CAMERA_ID_LGC920 6
|
||||
#define CAMERA_ID_LGC615 7
|
||||
#define CAMERA_ID_AR0231 8
|
||||
#define CAMERA_ID_MAX 9
|
||||
#define CAMERA_ID_IMX390 9
|
||||
#define CAMERA_ID_MAX 10
|
||||
|
||||
const int UI_BUF_COUNT = 4;
|
||||
const int YUV_BUFFER_COUNT = Hardware::EON() ? 100 : 40;
|
||||
|
|
|
|||
|
|
@ -22,6 +22,9 @@
|
|||
#include "selfdrive/common/swaglog.h"
|
||||
#include "selfdrive/camerad/cameras/sensor2_i2c.h"
|
||||
|
||||
// For debugging:
|
||||
// echo "4294967295" > /sys/module/cam_debug_util/parameters/debug_mdl
|
||||
|
||||
extern ExitHandler do_exit;
|
||||
|
||||
const size_t FRAME_WIDTH = 1928;
|
||||
|
|
@ -39,6 +42,14 @@ CameraInfo cameras_supported[CAMERA_ID_MAX] = {
|
|||
.bayer_flip = 1,
|
||||
.hdr = false
|
||||
},
|
||||
[CAMERA_ID_IMX390] = {
|
||||
.frame_width = FRAME_WIDTH,
|
||||
.frame_height = FRAME_HEIGHT,
|
||||
.frame_stride = FRAME_STRIDE,
|
||||
.bayer = true,
|
||||
.bayer_flip = 1,
|
||||
.hdr = false
|
||||
},
|
||||
};
|
||||
|
||||
const float DC_GAIN = 2.5;
|
||||
|
|
@ -160,8 +171,15 @@ void clear_req_queue(int fd, int32_t session_hdl, int32_t link_hdl) {
|
|||
// ************** high level camera helpers ****************
|
||||
|
||||
void CameraState::sensors_start() {
|
||||
int start_reg_len = sizeof(start_reg_array) / sizeof(struct i2c_random_wr_payload);
|
||||
sensors_i2c(start_reg_array, start_reg_len, CAM_SENSOR_PACKET_OPCODE_SENSOR_CONFIG);
|
||||
if (camera_id == CAMERA_ID_AR0231) {
|
||||
int start_reg_len = sizeof(start_reg_array_ar0231) / sizeof(struct i2c_random_wr_payload);
|
||||
sensors_i2c(start_reg_array_ar0231, start_reg_len, CAM_SENSOR_PACKET_OPCODE_SENSOR_CONFIG, true);
|
||||
} else if (camera_id == CAMERA_ID_IMX390) {
|
||||
int start_reg_len = sizeof(start_reg_array_imx390) / sizeof(struct i2c_random_wr_payload);
|
||||
sensors_i2c(start_reg_array_imx390, start_reg_len, CAM_SENSOR_PACKET_OPCODE_SENSOR_CONFIG, false);
|
||||
} else {
|
||||
assert(false);
|
||||
}
|
||||
}
|
||||
|
||||
void CameraState::sensors_poke(int request_id) {
|
||||
|
|
@ -181,7 +199,7 @@ void CameraState::sensors_poke(int request_id) {
|
|||
release_fd(multi_cam_state->video0_fd, cam_packet_handle);
|
||||
}
|
||||
|
||||
void CameraState::sensors_i2c(struct i2c_random_wr_payload* dat, int len, int op_code) {
|
||||
void CameraState::sensors_i2c(struct i2c_random_wr_payload* dat, int len, int op_code, bool data_word) {
|
||||
// LOGD("sensors_i2c: %d", len);
|
||||
uint32_t cam_packet_handle = 0;
|
||||
int size = sizeof(struct cam_packet)+sizeof(struct cam_cmd_buf_desc)*1;
|
||||
|
|
@ -199,7 +217,7 @@ void CameraState::sensors_i2c(struct i2c_random_wr_payload* dat, int len, int op
|
|||
i2c_random_wr->header.count = len;
|
||||
i2c_random_wr->header.op_code = 1;
|
||||
i2c_random_wr->header.cmd_type = CAMERA_SENSOR_CMD_TYPE_I2C_RNDM_WR;
|
||||
i2c_random_wr->header.data_type = CAMERA_SENSOR_I2C_TYPE_WORD;
|
||||
i2c_random_wr->header.data_type = data_word ? CAMERA_SENSOR_I2C_TYPE_WORD : CAMERA_SENSOR_I2C_TYPE_BYTE;
|
||||
i2c_random_wr->header.addr_type = CAMERA_SENSOR_I2C_TYPE_WORD;
|
||||
memcpy(i2c_random_wr->random_wr_payload, dat, len*sizeof(struct i2c_random_wr_payload));
|
||||
|
||||
|
|
@ -220,7 +238,7 @@ static cam_cmd_power *power_set_wait(cam_cmd_power *power, int16_t delay_ms) {
|
|||
return (struct cam_cmd_power *)(unconditional_wait + 1);
|
||||
};
|
||||
|
||||
void CameraState::sensors_init() {
|
||||
int CameraState::sensors_init() {
|
||||
int video0_fd = multi_cam_state->video0_fd;
|
||||
uint32_t cam_packet_handle = 0;
|
||||
int size = sizeof(struct cam_packet)+sizeof(struct cam_cmd_buf_desc)*2;
|
||||
|
|
@ -239,17 +257,17 @@ void CameraState::sensors_init() {
|
|||
switch (camera_num) {
|
||||
case 0:
|
||||
// port 0
|
||||
i2c_info->slave_addr = 0x20;
|
||||
i2c_info->slave_addr = (camera_id == CAMERA_ID_AR0231) ? 0x20 : 0x34;
|
||||
probe->camera_id = 0;
|
||||
break;
|
||||
case 1:
|
||||
// port 1
|
||||
i2c_info->slave_addr = 0x30;
|
||||
i2c_info->slave_addr = (camera_id == CAMERA_ID_AR0231) ? 0x30 : 0x36;
|
||||
probe->camera_id = 1;
|
||||
break;
|
||||
case 2:
|
||||
// port 2
|
||||
i2c_info->slave_addr = 0x20;
|
||||
i2c_info->slave_addr = (camera_id == CAMERA_ID_AR0231) ? 0x20 : 0x34;
|
||||
probe->camera_id = 2;
|
||||
break;
|
||||
}
|
||||
|
|
@ -263,8 +281,15 @@ void CameraState::sensors_init() {
|
|||
probe->addr_type = CAMERA_SENSOR_I2C_TYPE_WORD;
|
||||
probe->op_code = 3; // don't care?
|
||||
probe->cmd_type = CAMERA_SENSOR_CMD_TYPE_PROBE;
|
||||
probe->reg_addr = 0x3000; //0x300a; //0x300b;
|
||||
probe->expected_data = 0x354; //0x7750; //0x885a;
|
||||
if (camera_id == CAMERA_ID_AR0231) {
|
||||
probe->reg_addr = 0x3000;
|
||||
probe->expected_data = 0x354;
|
||||
} else if (camera_id == CAMERA_ID_IMX390) {
|
||||
probe->reg_addr = 0x330;
|
||||
probe->expected_data = 0x1538;
|
||||
} else {
|
||||
assert(false);
|
||||
}
|
||||
probe->data_mask = 0;
|
||||
|
||||
//buf_desc[1].size = buf_desc[1].length = 148;
|
||||
|
|
@ -293,7 +318,7 @@ void CameraState::sensors_init() {
|
|||
power->count = 1;
|
||||
power->cmd_type = CAMERA_SENSOR_CMD_TYPE_PWR_UP;
|
||||
power->power_settings[0].power_seq_type = 0;
|
||||
power->power_settings[0].config_val_low = 19200000; //Hz
|
||||
power->power_settings[0].config_val_low = (camera_id == CAMERA_ID_AR0231) ? 19200000 : 24000000; //Hz
|
||||
power = power_set_wait(power, 10);
|
||||
|
||||
// 8,1 is this reset?
|
||||
|
|
@ -341,7 +366,6 @@ void CameraState::sensors_init() {
|
|||
|
||||
LOGD("probing the sensor");
|
||||
int ret = do_cam_control(sensor_fd, CAM_SENSOR_PROBE_CMD, (void *)(uintptr_t)cam_packet_handle, 0);
|
||||
assert(ret == 0);
|
||||
|
||||
munmap(i2c_info, buf_desc[0].size);
|
||||
release_fd(video0_fd, buf_desc[0].mem_handle);
|
||||
|
|
@ -349,6 +373,8 @@ void CameraState::sensors_init() {
|
|||
release_fd(video0_fd, buf_desc[1].mem_handle);
|
||||
munmap(pkt, size);
|
||||
release_fd(video0_fd, cam_packet_handle);
|
||||
|
||||
return ret;
|
||||
}
|
||||
|
||||
void CameraState::config_isp(int io_mem_handle, int fence, int request_id, int buf0_mem_handle, int buf0_offset) {
|
||||
|
|
@ -561,9 +587,10 @@ void CameraState::enqueue_req_multi(int start, int n, bool dp) {
|
|||
|
||||
// ******************* camera *******************
|
||||
|
||||
void CameraState::camera_init(MultiCameraState *multi_cam_state_, VisionIpcServer * v, int camera_id, int camera_num_, unsigned int fps, cl_device_id device_id, cl_context ctx, VisionStreamType rgb_type, VisionStreamType yuv_type) {
|
||||
void CameraState::camera_init(MultiCameraState *multi_cam_state_, VisionIpcServer * v, int camera_id_, int camera_num_, unsigned int fps, cl_device_id device_id, cl_context ctx, VisionStreamType rgb_type, VisionStreamType yuv_type) {
|
||||
LOGD("camera init %d", camera_num);
|
||||
multi_cam_state = multi_cam_state_;
|
||||
camera_id = camera_id_;
|
||||
assert(camera_id < std::size(cameras_supported));
|
||||
ci = cameras_supported[camera_id];
|
||||
assert(ci.frame_width != 0);
|
||||
|
|
@ -586,17 +613,24 @@ void CameraState::camera_init(MultiCameraState *multi_cam_state_, VisionIpcServe
|
|||
}
|
||||
|
||||
void CameraState::camera_open() {
|
||||
int ret;
|
||||
sensor_fd = open_v4l_by_name_and_index("cam-sensor-driver", camera_num);
|
||||
assert(sensor_fd >= 0);
|
||||
LOGD("opened sensor for %d", camera_num);
|
||||
|
||||
// probe the sensor
|
||||
LOGD("-- Probing sensor %d", camera_num);
|
||||
sensors_init();
|
||||
ret = sensors_init();
|
||||
if (ret != 0) {
|
||||
LOGD("AR0231 init failed, trying IMX390");
|
||||
camera_id = CAMERA_ID_IMX390;
|
||||
ret = sensors_init();
|
||||
}
|
||||
assert(ret == 0);
|
||||
|
||||
// create session
|
||||
struct cam_req_mgr_session_info session_info = {};
|
||||
int ret = do_cam_control(multi_cam_state->video0_fd, CAM_REQ_MGR_CREATE_SESSION, &session_info, sizeof(session_info));
|
||||
ret = do_cam_control(multi_cam_state->video0_fd, CAM_REQ_MGR_CREATE_SESSION, &session_info, sizeof(session_info));
|
||||
LOGD("get session: %d 0x%X", ret, session_info.session_hdl);
|
||||
session_handle = session_info.session_hdl;
|
||||
|
||||
|
|
@ -675,10 +709,13 @@ void CameraState::camera_open() {
|
|||
config_isp(0, 0, 1, buf0_handle, 0);
|
||||
|
||||
LOG("-- Configuring sensor");
|
||||
sensors_i2c(init_array_ar0231, std::size(init_array_ar0231), CAM_SENSOR_PACKET_OPCODE_SENSOR_CONFIG);
|
||||
//sensors_i2c(start_reg_array, std::size(start_reg_array), CAM_SENSOR_PACKET_OPCODE_SENSOR_STREAMON);
|
||||
//sensors_i2c(stop_reg_array, std::size(stop_reg_array), CAM_SENSOR_PACKET_OPCODE_SENSOR_STREAMOFF);
|
||||
|
||||
if (camera_id == CAMERA_ID_AR0231) {
|
||||
sensors_i2c(init_array_ar0231, std::size(init_array_ar0231), CAM_SENSOR_PACKET_OPCODE_SENSOR_CONFIG, true);
|
||||
} else if (camera_id == CAMERA_ID_IMX390) {
|
||||
sensors_i2c(init_array_imx390, std::size(init_array_imx390), CAM_SENSOR_PACKET_OPCODE_SENSOR_CONFIG, false);
|
||||
} else {
|
||||
assert(false);
|
||||
}
|
||||
|
||||
// config csiphy
|
||||
LOG("-- Config CSI PHY");
|
||||
|
|
@ -1008,14 +1045,28 @@ void CameraState::set_camera_exposure(float grey_frac) {
|
|||
}
|
||||
// LOGE("ae - camera %d, cur_t %.5f, sof %.5f, dt %.5f", camera_num, 1e-9 * nanos_since_boot(), 1e-9 * buf.cur_frame_data.timestamp_sof, 1e-9 * (nanos_since_boot() - buf.cur_frame_data.timestamp_sof));
|
||||
|
||||
uint16_t analog_gain_reg = 0xFF00 | (new_g << 4) | new_g;
|
||||
struct i2c_random_wr_payload exp_reg_array[] = {
|
||||
{0x3366, analog_gain_reg},
|
||||
{0x3362, (uint16_t)(dc_gain_enabled ? 0x1 : 0x0)},
|
||||
{0x3012, (uint16_t)exposure_time},
|
||||
};
|
||||
sensors_i2c(exp_reg_array, sizeof(exp_reg_array)/sizeof(struct i2c_random_wr_payload),
|
||||
CAM_SENSOR_PACKET_OPCODE_SENSOR_CONFIG);
|
||||
if (camera_id == CAMERA_ID_AR0231) {
|
||||
uint16_t analog_gain_reg = 0xFF00 | (new_g << 4) | new_g;
|
||||
struct i2c_random_wr_payload exp_reg_array[] = {
|
||||
{0x3366, analog_gain_reg},
|
||||
{0x3362, (uint16_t)(dc_gain_enabled ? 0x1 : 0x0)},
|
||||
{0x3012, (uint16_t)exposure_time},
|
||||
};
|
||||
sensors_i2c(exp_reg_array, sizeof(exp_reg_array)/sizeof(struct i2c_random_wr_payload), CAM_SENSOR_PACKET_OPCODE_SENSOR_CONFIG, true);
|
||||
} else if (camera_id == CAMERA_ID_IMX390) {
|
||||
// if gain is sub 1, we have to use exposure to mimic sub 1 gains
|
||||
uint32_t real_exposure_time = (gain < 1.0) ? (exposure_time*gain) : exposure_time;
|
||||
// invert real_exposure_time, max exposure is 2
|
||||
real_exposure_time = (exposure_time >= 0x7cf) ? 2 : (0x7cf - exposure_time);
|
||||
uint32_t real_gain = int((10*log10(fmax(1.0, gain)))/0.3);
|
||||
//printf("%d expose: %d gain: %f = %d\n", camera_num, exposure_time, gain, real_gain);
|
||||
struct i2c_random_wr_payload exp_reg_array[] = {
|
||||
{0x000c, real_exposure_time&0xFF}, {0x000d, real_exposure_time>>8},
|
||||
{0x0010, real_exposure_time&0xFF}, {0x0011, real_exposure_time>>8},
|
||||
{0x0018, real_gain&0xFF}, {0x0019, real_gain>>8},
|
||||
};
|
||||
sensors_i2c(exp_reg_array, sizeof(exp_reg_array)/sizeof(struct i2c_random_wr_payload), CAM_SENSOR_PACKET_OPCODE_SENSOR_CONFIG, false);
|
||||
}
|
||||
}
|
||||
|
||||
void camera_autoexposure(CameraState *s, float grey_frac) {
|
||||
|
|
|
|||
|
|
@ -40,8 +40,8 @@ public:
|
|||
|
||||
void sensors_start();
|
||||
void sensors_poke(int request_id);
|
||||
void sensors_i2c(struct i2c_random_wr_payload* dat, int len, int op_code);
|
||||
void sensors_init();
|
||||
void sensors_i2c(struct i2c_random_wr_payload* dat, int len, int op_code, bool data_word);
|
||||
int sensors_init();
|
||||
|
||||
void camera_open();
|
||||
void camera_init(MultiCameraState *multi_cam_state, VisionIpcServer * v, int camera_id, int camera_num, unsigned int fps, cl_device_id device_id, cl_context ctx, VisionStreamType rgb_type, VisionStreamType yuv_type);
|
||||
|
|
@ -62,6 +62,7 @@ public:
|
|||
int frame_id_last;
|
||||
int idx_offset;
|
||||
bool skipped;
|
||||
int camera_id;
|
||||
|
||||
CameraBuf buf;
|
||||
};
|
||||
|
|
@ -73,7 +74,6 @@ typedef struct MultiCameraState {
|
|||
int device_iommu;
|
||||
int cdm_iommu;
|
||||
|
||||
|
||||
CameraState road_cam;
|
||||
CameraState wide_road_cam;
|
||||
CameraState driver_cam;
|
||||
|
|
|
|||
|
|
@ -1,7 +1,5 @@
|
|||
#pragma OPENCL EXTENSION cl_khr_fp16 : enable
|
||||
|
||||
const half black_level = 42.0;
|
||||
|
||||
const __constant half3 color_correction[3] = {
|
||||
// post wb CCM
|
||||
(half3)(1.82717181, -0.31231438, 0.07307673),
|
||||
|
|
@ -39,7 +37,7 @@ half3 color_correct(half3 rgb) {
|
|||
return ret;
|
||||
}
|
||||
|
||||
half val_from_10(const uchar * source, int gx, int gy) {
|
||||
inline half val_from_10(const uchar * source, int gx, int gy, half black_level) {
|
||||
// parse 12bit
|
||||
int start = gy * FRAME_STRIDE + (3 * (gx / 2));
|
||||
int offset = gx % 2;
|
||||
|
|
@ -49,7 +47,7 @@ half val_from_10(const uchar * source, int gx, int gy) {
|
|||
|
||||
// normalize
|
||||
pv = max(0.0h, pv - black_level);
|
||||
pv *= 0.00101833h; // /= (1024.0f - black_level);
|
||||
pv /= (1024.0f - black_level);
|
||||
|
||||
// correct vignetting
|
||||
if (CAM_NUM == 1) { // fcamera
|
||||
|
|
@ -89,7 +87,8 @@ half phi(half x) {
|
|||
|
||||
__kernel void debayer10(const __global uchar * in,
|
||||
__global uchar * out,
|
||||
__local half * cached
|
||||
__local half * cached,
|
||||
float black_level
|
||||
)
|
||||
{
|
||||
const int x_global = get_global_id(0);
|
||||
|
|
@ -102,7 +101,7 @@ __kernel void debayer10(const __global uchar * in,
|
|||
|
||||
int out_idx = 3 * x_global + 3 * y_global * RGB_WIDTH;
|
||||
|
||||
half pv = val_from_10(in, x_global, y_global);
|
||||
half pv = val_from_10(in, x_global, y_global, black_level);
|
||||
cached[localOffset] = pv;
|
||||
|
||||
// don't care
|
||||
|
|
@ -118,22 +117,22 @@ __kernel void debayer10(const __global uchar * in,
|
|||
if (x_local < 1) {
|
||||
localColOffset = x_local;
|
||||
globalColOffset = -1;
|
||||
cached[(y_local + 1) * localRowLen + x_local] = val_from_10(in, x_global-1, y_global);
|
||||
cached[(y_local + 1) * localRowLen + x_local] = val_from_10(in, x_global-1, y_global, black_level);
|
||||
} else if (x_local >= get_local_size(0) - 1) {
|
||||
localColOffset = x_local + 2;
|
||||
globalColOffset = 1;
|
||||
cached[localOffset + 1] = val_from_10(in, x_global+1, y_global);
|
||||
cached[localOffset + 1] = val_from_10(in, x_global+1, y_global, black_level);
|
||||
}
|
||||
|
||||
if (y_local < 1) {
|
||||
cached[y_local * localRowLen + x_local + 1] = val_from_10(in, x_global, y_global-1);
|
||||
cached[y_local * localRowLen + x_local + 1] = val_from_10(in, x_global, y_global-1, black_level);
|
||||
if (localColOffset != -1) {
|
||||
cached[y_local * localRowLen + localColOffset] = val_from_10(in, x_global+globalColOffset, y_global-1);
|
||||
cached[y_local * localRowLen + localColOffset] = val_from_10(in, x_global+globalColOffset, y_global-1, black_level);
|
||||
}
|
||||
} else if (y_local >= get_local_size(1) - 1) {
|
||||
cached[(y_local + 2) * localRowLen + x_local + 1] = val_from_10(in, x_global, y_global+1);
|
||||
cached[(y_local + 2) * localRowLen + x_local + 1] = val_from_10(in, x_global, y_global+1, black_level);
|
||||
if (localColOffset != -1) {
|
||||
cached[(y_local + 2) * localRowLen + localColOffset] = val_from_10(in, x_global+globalColOffset, y_global+1);
|
||||
cached[(y_local + 2) * localRowLen + localColOffset] = val_from_10(in, x_global+globalColOffset, y_global+1, black_level);
|
||||
}
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -1,5 +1,49 @@
|
|||
struct i2c_random_wr_payload start_reg_array[] = {{0x301A, 0x91C}};
|
||||
struct i2c_random_wr_payload stop_reg_array[] = {{0x301A, 0x918}};
|
||||
struct i2c_random_wr_payload start_reg_array_ar0231[] = {{0x301A, 0x91C}};
|
||||
struct i2c_random_wr_payload stop_reg_array_ar0231[] = {{0x301A, 0x918}};
|
||||
struct i2c_random_wr_payload start_reg_array_imx390[] = {{0x0, 0}};
|
||||
struct i2c_random_wr_payload stop_reg_array_imx390[] = {{0x0, 1}};
|
||||
|
||||
struct i2c_random_wr_payload init_array_imx390[] = {
|
||||
{0x2008, 0xd0}, {0x2009, 0x07}, {0x200a, 0x00}, // MODE_VMAX = time between frames
|
||||
{0x200C, 0xe4}, {0x200D, 0x0c}, // MODE_HMAX
|
||||
|
||||
// crop
|
||||
{0x3410, 0x88}, {0x3411, 0x7}, // CROP_H_SIZE
|
||||
{0x3418, 0xb8}, {0x3419, 0x4}, // CROP_V_SIZE
|
||||
{0x0078, 1}, {0x03c0, 1},
|
||||
|
||||
// external trigger (off)
|
||||
// while images still come in, they are blank with this
|
||||
{0x3650, 0}, // CU_MODE
|
||||
|
||||
// exposure
|
||||
{0x000c, 0xc0}, {0x000d, 0x07},
|
||||
{0x0010, 0xc0}, {0x0011, 0x07},
|
||||
|
||||
// WUXGA mode
|
||||
// not in datasheet, from https://github.com/bogsen/STLinux-Kernel/blob/master/drivers/media/platform/tegra/imx185.c
|
||||
{0x0086, 0xc4}, {0x0087, 0xff}, // WND_SHIFT_V = -60
|
||||
{0x03c6, 0xc4}, {0x03c7, 0xff}, // SM_WND_SHIFT_V_APL = -60
|
||||
|
||||
{0x201c, 0xe1}, {0x201d, 0x12}, // image read amount
|
||||
{0x21ee, 0xc4}, {0x21ef, 0x04}, // image send amount (1220 is the end)
|
||||
{0x21f0, 0xc4}, {0x21f1, 0x04}, // image processing amount
|
||||
|
||||
// disable a bunch of errors causing blanking
|
||||
{0x0390, 0x00}, {0x0391, 0x00}, {0x0392, 0x00},
|
||||
|
||||
// flip bayer
|
||||
{0x2D64, 0x64 + 2},
|
||||
|
||||
// color correction
|
||||
{0x0030, 0xf8}, {0x0031, 0x00}, // red gain
|
||||
{0x0032, 0x9a}, {0x0033, 0x00}, // gr gain
|
||||
{0x0034, 0x9a}, {0x0035, 0x00}, // gb gain
|
||||
{0x0036, 0x22}, {0x0037, 0x01}, // blue gain
|
||||
|
||||
// hdr enable (noise with this on for now)
|
||||
{0x00f9, 0}
|
||||
};
|
||||
|
||||
struct i2c_random_wr_payload init_array_ar0231[] = {
|
||||
{0x301A, 0x0018}, // RESET_REGISTER
|
||||
|
|
|
|||
|
|
@ -7,6 +7,7 @@ from opendbc.can.packer import CANPacker
|
|||
|
||||
class CarController():
|
||||
def __init__(self, dbc_name, CP, VM):
|
||||
self.CP = CP
|
||||
self.apply_steer_last = 0
|
||||
self.ccframe = 0
|
||||
self.prev_frame = -1
|
||||
|
|
@ -29,11 +30,11 @@ class CarController():
|
|||
CS.out.steeringTorqueEps, CarControllerParams)
|
||||
self.steer_rate_limited = new_steer != apply_steer
|
||||
|
||||
moving_fast = CS.out.vEgo > CS.CP.minSteerSpeed # for status message
|
||||
if CS.out.vEgo > (CS.CP.minSteerSpeed - 0.5): # for command high bit
|
||||
moving_fast = CS.out.vEgo > self.CP.minSteerSpeed # for status message
|
||||
if CS.out.vEgo > (self.CP.minSteerSpeed - 0.5): # for command high bit
|
||||
self.gone_fast_yet = True
|
||||
elif self.car_fingerprint in (CAR.PACIFICA_2019_HYBRID, CAR.PACIFICA_2020, CAR.JEEP_CHEROKEE_2019):
|
||||
if CS.out.vEgo < (CS.CP.minSteerSpeed - 3.0):
|
||||
if CS.out.vEgo < (self.CP.minSteerSpeed - 3.0):
|
||||
self.gone_fast_yet = False # < 14.5m/s stock turns off this bit, but fine down to 13.5
|
||||
lkas_active = moving_fast and enabled
|
||||
|
||||
|
|
|
|||
|
|
@ -1,18 +1,21 @@
|
|||
from collections import namedtuple
|
||||
|
||||
from cereal import car
|
||||
from common.realtime import DT_CTRL
|
||||
from selfdrive.controls.lib.drive_helpers import rate_limit
|
||||
from common.conversions import Conversions as CV
|
||||
from common.numpy_fast import clip, interp
|
||||
from common.realtime import DT_CTRL
|
||||
from opendbc.can.packer import CANPacker
|
||||
from selfdrive.car import create_gas_interceptor_command
|
||||
from selfdrive.car.honda import hondacan
|
||||
from selfdrive.car.honda.values import CruiseButtons, VISUAL_HUD, HONDA_BOSCH, HONDA_NIDEC_ALT_PCM_ACCEL, CarControllerParams
|
||||
from opendbc.can.packer import CANPacker
|
||||
from selfdrive.controls.lib.drive_helpers import rate_limit
|
||||
|
||||
VisualAlert = car.CarControl.HUDControl.VisualAlert
|
||||
LongCtrlState = car.CarControl.Actuators.LongControlState
|
||||
|
||||
|
||||
def compute_gb_honda_bosch(accel, speed):
|
||||
#TODO returns 0s, is unused
|
||||
# TODO returns 0s, is unused
|
||||
return 0.0, 0.0
|
||||
|
||||
|
||||
|
|
@ -33,14 +36,14 @@ def compute_gas_brake(accel, speed, fingerprint):
|
|||
return compute_gb_honda_nidec(accel, speed)
|
||||
|
||||
|
||||
#TODO not clear this does anything useful
|
||||
def actuator_hystereses(brake, braking, brake_steady, v_ego, car_fingerprint):
|
||||
# TODO not clear this does anything useful
|
||||
def actuator_hysteresis(brake, braking, brake_steady, v_ego, car_fingerprint):
|
||||
# hyst params
|
||||
brake_hyst_on = 0.02 # to activate brakes exceed this value
|
||||
brake_hyst_off = 0.005 # to deactivate brakes below this value
|
||||
brake_hyst_gap = 0.01 # don't change brake command for small oscillations within this value
|
||||
brake_hyst_on = 0.02 # to activate brakes exceed this value
|
||||
brake_hyst_off = 0.005 # to deactivate brakes below this value
|
||||
brake_hyst_gap = 0.01 # don't change brake command for small oscillations within this value
|
||||
|
||||
#*** hysteresis logic to avoid brake blinking. go above 0.1 to trigger
|
||||
# *** hysteresis logic to avoid brake blinking. go above 0.1 to trigger
|
||||
if (brake < brake_hyst_on and not braking) or brake < brake_hyst_off:
|
||||
brake = 0.
|
||||
braking = brake > 0.
|
||||
|
|
@ -93,164 +96,170 @@ def process_hud_alert(hud_alert):
|
|||
|
||||
HUDData = namedtuple("HUDData",
|
||||
["pcm_accel", "v_cruise", "car",
|
||||
"lanes", "fcw", "acc_alert", "steer_required"])
|
||||
"lanes", "fcw", "acc_alert", "steer_required"])
|
||||
|
||||
|
||||
class CarController():
|
||||
class CarController:
|
||||
def __init__(self, dbc_name, CP, VM):
|
||||
self.CP = CP
|
||||
self.packer = CANPacker(dbc_name)
|
||||
self.params = CarControllerParams(CP)
|
||||
self.frame = 0
|
||||
|
||||
self.braking = False
|
||||
self.brake_steady = 0.
|
||||
self.brake_last = 0.
|
||||
self.apply_brake_last = 0
|
||||
self.last_pump_ts = 0.
|
||||
self.packer = CANPacker(dbc_name)
|
||||
|
||||
self.accel = 0
|
||||
self.speed = 0
|
||||
self.gas = 0
|
||||
self.brake = 0
|
||||
|
||||
self.params = CarControllerParams(CP)
|
||||
def update(self, CC, CS):
|
||||
actuators = CC.actuators
|
||||
hud_control = CC.hudControl
|
||||
hud_v_cruise = hud_control.setSpeed * CV.MS_TO_KPH if hud_control.speedVisible else 255
|
||||
pcm_cancel_cmd = CC.cruiseControl.cancel
|
||||
|
||||
def update(self, c, CS, frame, actuators, pcm_cancel_cmd,
|
||||
hud_v_cruise, hud_show_lanes, hud_show_car, hud_alert):
|
||||
|
||||
P = self.params
|
||||
|
||||
if c.longActive:
|
||||
if CC.longActive:
|
||||
accel = actuators.accel
|
||||
gas, brake = compute_gas_brake(actuators.accel, CS.out.vEgo, CS.CP.carFingerprint)
|
||||
gas, brake = compute_gas_brake(actuators.accel, CS.out.vEgo, self.CP.carFingerprint)
|
||||
else:
|
||||
accel = 0.0
|
||||
gas, brake = 0.0, 0.0
|
||||
|
||||
# *** apply brake hysteresis ***
|
||||
pre_limit_brake, self.braking, self.brake_steady = actuator_hystereses(brake, self.braking, self.brake_steady, CS.out.vEgo, CS.CP.carFingerprint)
|
||||
pre_limit_brake, self.braking, self.brake_steady = actuator_hysteresis(brake, self.braking, self.brake_steady,
|
||||
CS.out.vEgo, self.CP.carFingerprint)
|
||||
|
||||
# *** rate limit after the enable check ***
|
||||
self.brake_last = rate_limit(pre_limit_brake, self.brake_last, -2., DT_CTRL)
|
||||
|
||||
# vehicle hud display, wait for one update from 10Hz 0x304 msg
|
||||
if hud_show_lanes:
|
||||
if hud_control.lanesVisible:
|
||||
hud_lanes = 1
|
||||
else:
|
||||
hud_lanes = 0
|
||||
|
||||
if c.enabled:
|
||||
if hud_show_car:
|
||||
if CC.enabled:
|
||||
if hud_control.leadVisible:
|
||||
hud_car = 2
|
||||
else:
|
||||
hud_car = 1
|
||||
else:
|
||||
hud_car = 0
|
||||
|
||||
fcw_display, steer_required, acc_alert = process_hud_alert(hud_alert)
|
||||
|
||||
fcw_display, steer_required, acc_alert = process_hud_alert(hud_control.visualAlert)
|
||||
|
||||
# **** process the car messages ****
|
||||
|
||||
# steer torque is converted back to CAN reference (positive when steering right)
|
||||
apply_steer = int(interp(-actuators.steer * P.STEER_MAX, P.STEER_LOOKUP_BP, P.STEER_LOOKUP_V))
|
||||
apply_steer = int(interp(-actuators.steer * self.params.STEER_MAX,
|
||||
self.params.STEER_LOOKUP_BP, self.params.STEER_LOOKUP_V))
|
||||
|
||||
# Send CAN commands.
|
||||
# Send CAN commands
|
||||
can_sends = []
|
||||
|
||||
# tester present - w/ no response (keeps radar disabled)
|
||||
if CS.CP.carFingerprint in HONDA_BOSCH and CS.CP.openpilotLongitudinalControl:
|
||||
if (frame % 10) == 0:
|
||||
if self.CP.carFingerprint in HONDA_BOSCH and self.CP.openpilotLongitudinalControl:
|
||||
if self.frame % 10 == 0:
|
||||
can_sends.append((0x18DAB0F1, 0, b"\x02\x3E\x80\x00\x00\x00\x00\x00", 1))
|
||||
|
||||
# Send steering command.
|
||||
idx = frame % 4
|
||||
can_sends.append(hondacan.create_steering_control(self.packer, apply_steer,
|
||||
c.latActive, CS.CP.carFingerprint, idx, CS.CP.openpilotLongitudinalControl))
|
||||
idx = self.frame % 4
|
||||
can_sends.append(hondacan.create_steering_control(self.packer, apply_steer, CC.latActive, self.CP.carFingerprint,
|
||||
idx, CS.CP.openpilotLongitudinalControl))
|
||||
|
||||
stopping = actuators.longControlState == LongCtrlState.stopping
|
||||
|
||||
# wind brake from air resistance decel at high speed
|
||||
wind_brake = interp(CS.out.vEgo, [0.0, 2.3, 35.0], [0.001, 0.002, 0.15])
|
||||
# all of this is only relevant for HONDA NIDEC
|
||||
max_accel = interp(CS.out.vEgo, P.NIDEC_MAX_ACCEL_BP, P.NIDEC_MAX_ACCEL_V)
|
||||
max_accel = interp(CS.out.vEgo, self.params.NIDEC_MAX_ACCEL_BP, self.params.NIDEC_MAX_ACCEL_V)
|
||||
# TODO this 1.44 is just to maintain previous behavior
|
||||
pcm_speed_BP = [-wind_brake,
|
||||
-wind_brake*(3/4),
|
||||
0.0,
|
||||
0.5]
|
||||
-wind_brake * (3 / 4),
|
||||
0.0,
|
||||
0.5]
|
||||
# The Honda ODYSSEY seems to have different PCM_ACCEL
|
||||
# msgs, is it other cars too?
|
||||
if CS.CP.enableGasInterceptor:
|
||||
if self.CP.enableGasInterceptor:
|
||||
pcm_speed = 0.0
|
||||
pcm_accel = int(0.0)
|
||||
elif CS.CP.carFingerprint in HONDA_NIDEC_ALT_PCM_ACCEL:
|
||||
elif self.CP.carFingerprint in HONDA_NIDEC_ALT_PCM_ACCEL:
|
||||
pcm_speed_V = [0.0,
|
||||
clip(CS.out.vEgo - 3.0, 0.0, 100.0),
|
||||
clip(CS.out.vEgo + 0.0, 0.0, 100.0),
|
||||
clip(CS.out.vEgo + 5.0, 0.0, 100.0)]
|
||||
pcm_speed = interp(gas-brake, pcm_speed_BP, pcm_speed_V)
|
||||
pcm_accel = int((1.0) * 0xc6)
|
||||
pcm_speed = interp(gas - brake, pcm_speed_BP, pcm_speed_V)
|
||||
pcm_accel = int(1.0 * 0xc6)
|
||||
else:
|
||||
pcm_speed_V = [0.0,
|
||||
clip(CS.out.vEgo - 2.0, 0.0, 100.0),
|
||||
clip(CS.out.vEgo + 2.0, 0.0, 100.0),
|
||||
clip(CS.out.vEgo + 5.0, 0.0, 100.0)]
|
||||
pcm_speed = interp(gas-brake, pcm_speed_BP, pcm_speed_V)
|
||||
pcm_accel = int(clip((accel/1.44)/max_accel, 0.0, 1.0) * 0xc6)
|
||||
pcm_speed = interp(gas - brake, pcm_speed_BP, pcm_speed_V)
|
||||
pcm_accel = int(clip((accel / 1.44) / max_accel, 0.0, 1.0) * 0xc6)
|
||||
|
||||
if not CS.CP.openpilotLongitudinalControl:
|
||||
if (frame % 2) == 0:
|
||||
idx = frame // 2
|
||||
can_sends.append(hondacan.create_bosch_supplemental_1(self.packer, CS.CP.carFingerprint, idx))
|
||||
if not self.CP.openpilotLongitudinalControl:
|
||||
if self.frame % 2 == 0:
|
||||
idx = self.frame // 2
|
||||
can_sends.append(hondacan.create_bosch_supplemental_1(self.packer, self.CP.carFingerprint, idx))
|
||||
# If using stock ACC, spam cancel command to kill gas when OP disengages.
|
||||
if pcm_cancel_cmd:
|
||||
can_sends.append(hondacan.spam_buttons_command(self.packer, CruiseButtons.CANCEL, idx, CS.CP.carFingerprint))
|
||||
can_sends.append(hondacan.spam_buttons_command(self.packer, CruiseButtons.CANCEL, idx, self.CP.carFingerprint))
|
||||
elif CS.out.cruiseState.standstill:
|
||||
can_sends.append(hondacan.spam_buttons_command(self.packer, CruiseButtons.RES_ACCEL, idx, CS.CP.carFingerprint))
|
||||
can_sends.append(hondacan.spam_buttons_command(self.packer, CruiseButtons.RES_ACCEL, idx, self.CP.carFingerprint))
|
||||
|
||||
else:
|
||||
# Send gas and brake commands.
|
||||
if (frame % 2) == 0:
|
||||
idx = frame // 2
|
||||
ts = frame * DT_CTRL
|
||||
if self.frame % 2 == 0:
|
||||
idx = self.frame // 2
|
||||
ts = self.frame * DT_CTRL
|
||||
|
||||
if CS.CP.carFingerprint in HONDA_BOSCH:
|
||||
self.accel = clip(accel, P.BOSCH_ACCEL_MIN, P.BOSCH_ACCEL_MAX)
|
||||
self.gas = interp(accel, P.BOSCH_GAS_LOOKUP_BP, P.BOSCH_GAS_LOOKUP_V)
|
||||
can_sends.extend(hondacan.create_acc_commands(self.packer, c.enabled, c.longActive, accel, self.gas, idx, stopping, CS.CP.carFingerprint))
|
||||
if self.CP.carFingerprint in HONDA_BOSCH:
|
||||
self.accel = clip(accel, self.params.BOSCH_ACCEL_MIN, self.params.BOSCH_ACCEL_MAX)
|
||||
self.gas = interp(accel, self.params.BOSCH_GAS_LOOKUP_BP, self.params.BOSCH_GAS_LOOKUP_V)
|
||||
can_sends.extend(hondacan.create_acc_commands(self.packer, CC.enabled, CC.longActive, accel, self.gas,
|
||||
idx, stopping, self.CP.carFingerprint))
|
||||
else:
|
||||
apply_brake = clip(self.brake_last - wind_brake, 0.0, 1.0)
|
||||
apply_brake = int(clip(apply_brake * P.NIDEC_BRAKE_MAX, 0, P.NIDEC_BRAKE_MAX - 1))
|
||||
apply_brake = int(clip(apply_brake * self.params.NIDEC_BRAKE_MAX, 0, self.params.NIDEC_BRAKE_MAX - 1))
|
||||
pump_on, self.last_pump_ts = brake_pump_hysteresis(apply_brake, self.apply_brake_last, self.last_pump_ts, ts)
|
||||
|
||||
pcm_override = True
|
||||
can_sends.append(hondacan.create_brake_command(self.packer, apply_brake, pump_on,
|
||||
pcm_override, pcm_cancel_cmd, fcw_display, idx, CS.CP.carFingerprint, CS.stock_brake))
|
||||
pcm_override, pcm_cancel_cmd, fcw_display, idx,
|
||||
self.CP.carFingerprint, CS.stock_brake))
|
||||
self.apply_brake_last = apply_brake
|
||||
self.brake = apply_brake / P.NIDEC_BRAKE_MAX
|
||||
self.brake = apply_brake / self.params.NIDEC_BRAKE_MAX
|
||||
|
||||
if CS.CP.enableGasInterceptor:
|
||||
if self.CP.enableGasInterceptor:
|
||||
# way too aggressive at low speed without this
|
||||
gas_mult = interp(CS.out.vEgo, [0., 10.], [0.4, 1.0])
|
||||
# send exactly zero if apply_gas is zero. Interceptor will send the max between read value and apply_gas.
|
||||
# This prevents unexpected pedal range rescaling
|
||||
# Sending non-zero gas when OP is not enabled will cause the PCM not to respond to throttle as expected
|
||||
# when you do enable.
|
||||
if c.longActive:
|
||||
self.gas = clip(gas_mult * (gas - brake + wind_brake*3/4), 0., 1.)
|
||||
if CC.longActive:
|
||||
self.gas = clip(gas_mult * (gas - brake + wind_brake * 3 / 4), 0., 1.)
|
||||
else:
|
||||
self.gas = 0.0
|
||||
can_sends.append(create_gas_interceptor_command(self.packer, self.gas, idx))
|
||||
|
||||
# Send dashboard UI commands.
|
||||
if (frame % 10) == 0:
|
||||
idx = (frame//10) % 4
|
||||
if self.frame % 10 == 0:
|
||||
idx = (self.frame // 10) % 4
|
||||
hud = HUDData(int(pcm_accel), int(round(hud_v_cruise)), hud_car,
|
||||
hud_lanes, fcw_display, acc_alert, steer_required)
|
||||
can_sends.extend(hondacan.create_ui_commands(self.packer, CS.CP, pcm_speed, hud, CS.is_metric, idx, CS.stock_hud))
|
||||
can_sends.extend(hondacan.create_ui_commands(self.packer, self.CP, pcm_speed, hud, CS.is_metric, idx, CS.stock_hud))
|
||||
|
||||
if (CS.CP.openpilotLongitudinalControl) and (CS.CP.carFingerprint not in HONDA_BOSCH):
|
||||
if self.CP.openpilotLongitudinalControl and self.CP.carFingerprint not in HONDA_BOSCH:
|
||||
self.speed = pcm_speed
|
||||
|
||||
if not CS.CP.enableGasInterceptor:
|
||||
if not self.CP.enableGasInterceptor:
|
||||
self.gas = pcm_accel / 0xc6
|
||||
|
||||
new_actuators = actuators.copy()
|
||||
|
|
@ -259,4 +268,5 @@ class CarController():
|
|||
new_actuators.gas = self.gas
|
||||
new_actuators.brake = self.brake
|
||||
|
||||
self.frame += 1
|
||||
return new_actuators, can_sends
|
||||
|
|
|
|||
|
|
@ -418,17 +418,5 @@ class CarInterface(CarInterfaceBase):
|
|||
# pass in a car.CarControl
|
||||
# to be called @ 100hz
|
||||
def apply(self, c):
|
||||
hud_control = c.hudControl
|
||||
if hud_control.speedVisible:
|
||||
hud_v_cruise = hud_control.setSpeed * CV.MS_TO_KPH
|
||||
else:
|
||||
hud_v_cruise = 255
|
||||
|
||||
ret = self.CC.update(c, self.CS, self.frame,
|
||||
c.actuators, c.cruiseControl.cancel,
|
||||
hud_v_cruise, hud_control.lanesVisible,
|
||||
hud_show_car=hud_control.leadVisible,
|
||||
hud_alert=hud_control.visualAlert)
|
||||
|
||||
self.frame += 1
|
||||
ret = self.CC.update(c, self.CS)
|
||||
return ret
|
||||
|
|
|
|||
|
|
@ -11,34 +11,35 @@ VisualAlert = car.CarControl.HUDControl.VisualAlert
|
|||
LongCtrlState = car.CarControl.Actuators.LongControlState
|
||||
|
||||
|
||||
def process_hud_alert(enabled, fingerprint, visual_alert, left_lane,
|
||||
right_lane, left_lane_depart, right_lane_depart):
|
||||
sys_warning = (visual_alert in (VisualAlert.steerRequired, VisualAlert.ldw))
|
||||
def process_hud_alert(enabled, fingerprint, hud_control):
|
||||
sys_warning = (hud_control.visualAlert in (VisualAlert.steerRequired, VisualAlert.ldw))
|
||||
|
||||
# initialize to no line visible
|
||||
sys_state = 1
|
||||
if left_lane and right_lane or sys_warning: # HUD alert only display when LKAS status is active
|
||||
if hud_control.leftLaneVisible and hud_control.rightLaneVisible or sys_warning: # HUD alert only display when LKAS status is active
|
||||
sys_state = 3 if enabled or sys_warning else 4
|
||||
elif left_lane:
|
||||
elif hud_control.leftLaneVisible:
|
||||
sys_state = 5
|
||||
elif right_lane:
|
||||
elif hud_control.rightLaneVisible:
|
||||
sys_state = 6
|
||||
|
||||
# initialize to no warnings
|
||||
left_lane_warning = 0
|
||||
right_lane_warning = 0
|
||||
if left_lane_depart:
|
||||
if hud_control.leftLaneDepart:
|
||||
left_lane_warning = 1 if fingerprint in (CAR.GENESIS_G90, CAR.GENESIS_G80) else 2
|
||||
if right_lane_depart:
|
||||
if hud_control.rightLaneDepart:
|
||||
right_lane_warning = 1 if fingerprint in (CAR.GENESIS_G90, CAR.GENESIS_G80) else 2
|
||||
|
||||
return sys_warning, sys_state, left_lane_warning, right_lane_warning
|
||||
|
||||
|
||||
class CarController():
|
||||
class CarController:
|
||||
def __init__(self, dbc_name, CP, VM):
|
||||
self.p = CarControllerParams(CP)
|
||||
self.CP = CP
|
||||
self.params = CarControllerParams(CP)
|
||||
self.packer = CANPacker(dbc_name)
|
||||
self.frame = 0
|
||||
|
||||
self.apply_steer_last = 0
|
||||
self.car_fingerprint = CP.carFingerprint
|
||||
|
|
@ -46,47 +47,49 @@ class CarController():
|
|||
self.last_resume_frame = 0
|
||||
self.accel = 0
|
||||
|
||||
def update(self, c, CS, frame, actuators, pcm_cancel_cmd, visual_alert, hud_speed,
|
||||
left_lane, right_lane, left_lane_depart, right_lane_depart):
|
||||
def update(self, CC, CS):
|
||||
actuators = CC.actuators
|
||||
hud_control = CC.hudControl
|
||||
pcm_cancel_cmd = CC.cruiseControl.cancel
|
||||
|
||||
# Steering Torque
|
||||
new_steer = int(round(actuators.steer * self.p.STEER_MAX))
|
||||
apply_steer = apply_std_steer_torque_limits(new_steer, self.apply_steer_last, CS.out.steeringTorque, self.p)
|
||||
new_steer = int(round(actuators.steer * self.params.STEER_MAX))
|
||||
apply_steer = apply_std_steer_torque_limits(new_steer, self.apply_steer_last, CS.out.steeringTorque, self.params)
|
||||
self.steer_rate_limited = new_steer != apply_steer
|
||||
|
||||
if not c.latActive:
|
||||
if not CC.latActive:
|
||||
apply_steer = 0
|
||||
|
||||
self.apply_steer_last = apply_steer
|
||||
|
||||
sys_warning, sys_state, left_lane_warning, right_lane_warning = \
|
||||
process_hud_alert(c.enabled, self.car_fingerprint, visual_alert,
|
||||
left_lane, right_lane, left_lane_depart, right_lane_depart)
|
||||
sys_warning, sys_state, left_lane_warning, right_lane_warning = process_hud_alert(CC.enabled, self.car_fingerprint,
|
||||
hud_control)
|
||||
|
||||
can_sends = []
|
||||
|
||||
# tester present - w/ no response (keeps radar disabled)
|
||||
if CS.CP.openpilotLongitudinalControl:
|
||||
if (frame % 100) == 0:
|
||||
if self.CP.openpilotLongitudinalControl:
|
||||
if self.frame % 100 == 0:
|
||||
can_sends.append([0x7D0, 0, b"\x02\x3E\x80\x00\x00\x00\x00\x00", 0])
|
||||
|
||||
can_sends.append(create_lkas11(self.packer, frame, self.car_fingerprint, apply_steer, c.latActive,
|
||||
CS.lkas11, sys_warning, sys_state, c.enabled,
|
||||
left_lane, right_lane,
|
||||
can_sends.append(create_lkas11(self.packer, self.frame, self.car_fingerprint, apply_steer, CC.latActive,
|
||||
CS.lkas11, sys_warning, sys_state, CC.enabled,
|
||||
hud_control.leftLaneVisible, hud_control.rightLaneVisible,
|
||||
left_lane_warning, right_lane_warning))
|
||||
|
||||
if not CS.CP.openpilotLongitudinalControl:
|
||||
if not self.CP.openpilotLongitudinalControl:
|
||||
if pcm_cancel_cmd:
|
||||
can_sends.append(create_clu11(self.packer, frame, CS.clu11, Buttons.CANCEL))
|
||||
can_sends.append(create_clu11(self.packer, self.frame, CS.clu11, Buttons.CANCEL))
|
||||
elif CS.out.cruiseState.standstill:
|
||||
# send resume at a max freq of 10Hz
|
||||
if (frame - self.last_resume_frame) * DT_CTRL > 0.1:
|
||||
if (self.frame - self.last_resume_frame) * DT_CTRL > 0.1:
|
||||
# send 25 messages at a time to increases the likelihood of resume being accepted
|
||||
can_sends.extend([create_clu11(self.packer, frame, CS.clu11, Buttons.RES_ACCEL)] * 25)
|
||||
self.last_resume_frame = frame
|
||||
can_sends.extend([create_clu11(self.packer, self.frame, CS.clu11, Buttons.RES_ACCEL)] * 25)
|
||||
self.last_resume_frame = self.frame
|
||||
|
||||
if frame % 2 == 0 and CS.CP.openpilotLongitudinalControl:
|
||||
if self.frame % 2 == 0 and self.CP.openpilotLongitudinalControl:
|
||||
lead_visible = False
|
||||
accel = actuators.accel if c.longActive else 0
|
||||
accel = actuators.accel if CC.longActive else 0
|
||||
|
||||
jerk = clip(2.0 * (accel - CS.out.aEgo), -12.7, 12.7)
|
||||
|
||||
|
|
@ -96,27 +99,29 @@ class CarController():
|
|||
accel = clip(accel, CarControllerParams.ACCEL_MIN, CarControllerParams.ACCEL_MAX)
|
||||
|
||||
stopping = (actuators.longControlState == LongCtrlState.stopping)
|
||||
set_speed_in_units = hud_speed * (CV.MS_TO_MPH if CS.clu11["CF_Clu_SPEED_UNIT"] == 1 else CV.MS_TO_KPH)
|
||||
can_sends.extend(create_acc_commands(self.packer, c.enabled, accel, jerk, int(frame / 2), lead_visible, set_speed_in_units, stopping))
|
||||
set_speed_in_units = hud_control.setSpeed * (CV.MS_TO_MPH if CS.clu11["CF_Clu_SPEED_UNIT"] == 1 else CV.MS_TO_KPH)
|
||||
can_sends.extend(create_acc_commands(self.packer, CC.enabled, accel, jerk, int(self.frame / 2), lead_visible,
|
||||
set_speed_in_units, stopping, CS.out.gasPressed))
|
||||
self.accel = accel
|
||||
|
||||
# 20 Hz LFA MFA message
|
||||
if frame % 5 == 0 and self.car_fingerprint in (CAR.SONATA, CAR.PALISADE, CAR.IONIQ, CAR.KIA_NIRO_EV, CAR.KIA_NIRO_HEV_2021,
|
||||
CAR.IONIQ_EV_2020, CAR.IONIQ_PHEV, CAR.KIA_CEED, CAR.KIA_SELTOS, CAR.KONA_EV,
|
||||
CAR.ELANTRA_2021, CAR.ELANTRA_HEV_2021, CAR.SONATA_HYBRID, CAR.KONA_HEV, CAR.SANTA_FE_2022,
|
||||
CAR.KIA_K5_2021, CAR.IONIQ_HEV_2022, CAR.SANTA_FE_HEV_2022, CAR.GENESIS_G70_2020, CAR.SANTA_FE_PHEV_2022):
|
||||
can_sends.append(create_lfahda_mfc(self.packer, c.enabled))
|
||||
if self.frame % 5 == 0 and self.car_fingerprint in (CAR.SONATA, CAR.PALISADE, CAR.IONIQ, CAR.KIA_NIRO_EV, CAR.KIA_NIRO_HEV_2021,
|
||||
CAR.IONIQ_EV_2020, CAR.IONIQ_PHEV, CAR.KIA_CEED, CAR.KIA_SELTOS, CAR.KONA_EV,
|
||||
CAR.ELANTRA_2021, CAR.ELANTRA_HEV_2021, CAR.SONATA_HYBRID, CAR.KONA_HEV, CAR.SANTA_FE_2022,
|
||||
CAR.KIA_K5_2021, CAR.IONIQ_HEV_2022, CAR.SANTA_FE_HEV_2022, CAR.GENESIS_G70_2020, CAR.SANTA_FE_PHEV_2022):
|
||||
can_sends.append(create_lfahda_mfc(self.packer, CC.enabled))
|
||||
|
||||
# 5 Hz ACC options
|
||||
if frame % 20 == 0 and CS.CP.openpilotLongitudinalControl:
|
||||
if self.frame % 20 == 0 and self.CP.openpilotLongitudinalControl:
|
||||
can_sends.extend(create_acc_opt(self.packer))
|
||||
|
||||
# 2 Hz front radar options
|
||||
if frame % 50 == 0 and CS.CP.openpilotLongitudinalControl:
|
||||
if self.frame % 50 == 0 and self.CP.openpilotLongitudinalControl:
|
||||
can_sends.append(create_frt_radar_opt(self.packer))
|
||||
|
||||
new_actuators = actuators.copy()
|
||||
new_actuators.steer = apply_steer / self.p.STEER_MAX
|
||||
new_actuators.steer = apply_steer / self.params.STEER_MAX
|
||||
new_actuators.accel = self.accel
|
||||
|
||||
self.frame += 1
|
||||
return new_actuators, can_sends
|
||||
|
|
|
|||
|
|
@ -78,7 +78,7 @@ def create_lfahda_mfc(packer, enabled, hda_set_speed=0):
|
|||
}
|
||||
return packer.make_can_msg("LFAHDA_MFC", 0, values)
|
||||
|
||||
def create_acc_commands(packer, enabled, accel, jerk, idx, lead_visible, set_speed, stopping):
|
||||
def create_acc_commands(packer, enabled, accel, jerk, idx, lead_visible, set_speed, stopping, gas_pressed):
|
||||
commands = []
|
||||
|
||||
scc11_values = {
|
||||
|
|
@ -95,7 +95,7 @@ def create_acc_commands(packer, enabled, accel, jerk, idx, lead_visible, set_spe
|
|||
commands.append(packer.make_can_msg("SCC11", 0, scc11_values))
|
||||
|
||||
scc12_values = {
|
||||
"ACCMode": 1 if enabled else 0,
|
||||
"ACCMode": 2 if enabled and gas_pressed else 1 if enabled else 0,
|
||||
"StopReq": 1 if enabled and stopping else 0,
|
||||
"aReqRaw": accel if enabled else 0,
|
||||
"aReqValue": accel if enabled else 0, # stock ramps up and down respecting jerk limit until it reaches aReqRaw
|
||||
|
|
@ -111,7 +111,7 @@ def create_acc_commands(packer, enabled, accel, jerk, idx, lead_visible, set_spe
|
|||
"ComfortBandLower": 0.0, # stock usually is 0 but sometimes uses higher values
|
||||
"JerkUpperLimit": max(jerk, 1.0) if (enabled and not stopping) else 0, # stock usually is 1.0 but sometimes uses higher values
|
||||
"JerkLowerLimit": max(-jerk, 1.0) if enabled else 0, # stock usually is 0.5 but sometimes uses higher values
|
||||
"ACCMode": 1 if enabled else 4, # stock will always be 4 instead of 0 after first disengage
|
||||
"ACCMode": 2 if enabled and gas_pressed else 1 if enabled else 4, # stock will always be 4 instead of 0 after first disengage
|
||||
"ObjGap": 2 if lead_visible else 0, # 5: >30, m, 4: 25-30 m, 3: 20-25 m, 2: < 20 m, 0: no lead
|
||||
}
|
||||
commands.append(packer.make_can_msg("SCC14", 0, scc14_values))
|
||||
|
|
|
|||
|
|
@ -348,9 +348,5 @@ class CarInterface(CarInterfaceBase):
|
|||
return self.CS.out
|
||||
|
||||
def apply(self, c):
|
||||
hud_control = c.hudControl
|
||||
ret = self.CC.update(c, self.CS, self.frame, c.actuators, c.cruiseControl.cancel, hud_control.visualAlert,
|
||||
hud_control.setSpeed, hud_control.leftLaneVisible, hud_control.rightLaneVisible,
|
||||
hud_control.leftLaneDepart, hud_control.rightLaneDepart)
|
||||
self.frame += 1
|
||||
ret = self.CC.update(c, self.CS)
|
||||
return ret
|
||||
|
|
|
|||
|
|
@ -8,6 +8,7 @@ VisualAlert = car.CarControl.HUDControl.VisualAlert
|
|||
|
||||
class CarController():
|
||||
def __init__(self, dbc_name, CP, VM):
|
||||
self.CP = CP
|
||||
self.apply_steer_last = 0
|
||||
self.packer = CANPacker(dbc_name)
|
||||
self.steer_rate_limited = False
|
||||
|
|
@ -30,7 +31,7 @@ class CarController():
|
|||
# Mazda Stop and Go requires a RES button (or gas) press if the car stops more than 3 seconds
|
||||
# Send Resume button at 20hz if we're engaged at standstill to support full stop and go!
|
||||
# TODO: improve the resume trigger logic by looking at actual radar data
|
||||
can_sends.append(mazdacan.create_button_cmd(self.packer, CS.CP.carFingerprint, CS.crz_btns_counter, Buttons.RESUME))
|
||||
can_sends.append(mazdacan.create_button_cmd(self.packer, self.CP.carFingerprint, CS.crz_btns_counter, Buttons.RESUME))
|
||||
|
||||
if c.cruiseControl.cancel:
|
||||
# If brake is pressed, let us wait >70ms before trying to disable crz to avoid
|
||||
|
|
@ -41,7 +42,7 @@ class CarController():
|
|||
if frame % 10 == 0 and not (CS.out.brakePressed and self.brake_counter < 7):
|
||||
# Cancel Stock ACC if it's enabled while OP is disengaged
|
||||
# Send at a rate of 10hz until we sync with stock ACC state
|
||||
can_sends.append(mazdacan.create_button_cmd(self.packer, CS.CP.carFingerprint, CS.crz_btns_counter, Buttons.CANCEL))
|
||||
can_sends.append(mazdacan.create_button_cmd(self.packer, self.CP.carFingerprint, CS.crz_btns_counter, Buttons.CANCEL))
|
||||
else:
|
||||
self.brake_counter = 0
|
||||
|
||||
|
|
@ -56,7 +57,7 @@ class CarController():
|
|||
can_sends.append(mazdacan.create_alert_command(self.packer, CS.cam_laneinfo, ldw, steer_required))
|
||||
|
||||
# send steering command
|
||||
can_sends.append(mazdacan.create_steering_control(self.packer, CS.CP.carFingerprint,
|
||||
can_sends.append(mazdacan.create_steering_control(self.packer, self.CP.carFingerprint,
|
||||
frame, apply_steer, CS.cam_lkas))
|
||||
|
||||
new_actuators = c.actuators.copy()
|
||||
|
|
|
|||
|
|
@ -6,6 +6,7 @@ from opendbc.can.packer import CANPacker
|
|||
|
||||
class CarController():
|
||||
def __init__(self, dbc_name, CP, VM):
|
||||
self.CP = CP
|
||||
self.apply_steer_last = 0
|
||||
self.es_distance_cnt = -1
|
||||
self.es_lkas_cnt = -1
|
||||
|
|
@ -33,7 +34,7 @@ class CarController():
|
|||
if not c.latActive:
|
||||
apply_steer = 0
|
||||
|
||||
if CS.CP.carFingerprint in PREGLOBAL_CARS:
|
||||
if self.CP.carFingerprint in PREGLOBAL_CARS:
|
||||
can_sends.append(subarucan.create_preglobal_steering_control(self.packer, apply_steer, frame, self.p.STEER_STEP))
|
||||
else:
|
||||
can_sends.append(subarucan.create_steering_control(self.packer, apply_steer, frame, self.p.STEER_STEP))
|
||||
|
|
@ -43,7 +44,7 @@ class CarController():
|
|||
|
||||
# *** alerts and pcm cancel ***
|
||||
|
||||
if CS.CP.carFingerprint in PREGLOBAL_CARS:
|
||||
if self.CP.carFingerprint in PREGLOBAL_CARS:
|
||||
if self.es_distance_cnt != CS.es_distance_msg["Counter"]:
|
||||
# 1 = main, 2 = set shallow, 3 = set deep, 4 = resume shallow, 5 = resume deep
|
||||
# disengage ACC when OP is disengaged
|
||||
|
|
|
|||
|
|
@ -106,6 +106,7 @@ routes = [
|
|||
TestRoute("0bb588106852abb7|2021-05-26--12-22-01", TOYOTA.AVALON_2019),
|
||||
TestRoute("87bef2930af86592|2021-05-30--09-40-54", TOYOTA.AVALONH_2019),
|
||||
TestRoute("e9966711cfb04ce3|2022-01-11--07-59-43", TOYOTA.AVALON_TSS2),
|
||||
TestRoute("eca1080a91720a54|2022-03-17--13-32-29", TOYOTA.AVALONH_TSS2),
|
||||
TestRoute("6cdecc4728d4af37|2020-02-23--15-44-18", TOYOTA.CAMRY),
|
||||
TestRoute("3456ad0cd7281b24|2020-12-13--17-45-56", TOYOTA.CAMRY_TSS2),
|
||||
TestRoute("ffccc77938ddbc44|2021-01-04--16-55-41", TOYOTA.CAMRYH_TSS2),
|
||||
|
|
|
|||
|
|
@ -12,6 +12,7 @@ VisualAlert = car.CarControl.HUDControl.VisualAlert
|
|||
|
||||
class CarController:
|
||||
def __init__(self, dbc_name, CP, VM):
|
||||
self.CP = CP
|
||||
self.frame = 0
|
||||
self.last_steer = 0
|
||||
self.alert_active = False
|
||||
|
|
@ -29,12 +30,12 @@ class CarController:
|
|||
pcm_cancel_cmd = CC.cruiseControl.cancel
|
||||
|
||||
# gas and brake
|
||||
if CS.CP.enableGasInterceptor and CC.longActive:
|
||||
if self.CP.enableGasInterceptor and CC.longActive:
|
||||
MAX_INTERCEPTOR_GAS = 0.5
|
||||
# RAV4 has very sensitive gas pedal
|
||||
if CS.CP.carFingerprint in (CAR.RAV4,):
|
||||
if self.CP.carFingerprint in (CAR.RAV4,):
|
||||
PEDAL_SCALE = interp(CS.out.vEgo, [0.0, MIN_ACC_SPEED, MIN_ACC_SPEED + PEDAL_TRANSITION], [0.15, 0.3, 0.0])
|
||||
elif CS.CP.carFingerprint in (CAR.COROLLA,):
|
||||
elif self.CP.carFingerprint in (CAR.COROLLA,):
|
||||
PEDAL_SCALE = interp(CS.out.vEgo, [0.0, MIN_ACC_SPEED, MIN_ACC_SPEED + PEDAL_TRANSITION], [0.3, 0.4, 0.0])
|
||||
else:
|
||||
PEDAL_SCALE = interp(CS.out.vEgo, [0.0, MIN_ACC_SPEED, MIN_ACC_SPEED + PEDAL_TRANSITION], [0.4, 0.5, 0.0])
|
||||
|
|
@ -64,7 +65,7 @@ class CarController:
|
|||
pcm_cancel_cmd = 1
|
||||
|
||||
# on entering standstill, send standstill request
|
||||
if CS.out.standstill and not self.last_standstill and CS.CP.carFingerprint not in NO_STOP_TIMER_CAR:
|
||||
if CS.out.standstill and not self.last_standstill and self.CP.carFingerprint not in NO_STOP_TIMER_CAR:
|
||||
self.standstill_req = True
|
||||
if CS.pcm_acc_status != 8:
|
||||
# pcm entered standstill or it's disabled
|
||||
|
|
@ -82,7 +83,7 @@ class CarController:
|
|||
# sending it at 100Hz seem to allow a higher rate limit, as the rate limit seems imposed
|
||||
# on consecutive messages
|
||||
can_sends.append(create_steer_command(self.packer, apply_steer, apply_steer_req, self.frame))
|
||||
if self.frame % 2 == 0 and CS.CP.carFingerprint in TSS2_CAR:
|
||||
if self.frame % 2 == 0 and self.CP.carFingerprint in TSS2_CAR:
|
||||
can_sends.append(create_lta_steer_command(self.packer, 0, 0, self.frame // 2))
|
||||
|
||||
# LTA mode. Set ret.steerControlType = car.CarParams.SteerControlType.angle and whitelist 0x191 in the panda
|
||||
|
|
@ -91,19 +92,19 @@ class CarController:
|
|||
# can_sends.append(create_lta_steer_command(self.packer, actuators.steeringAngleDeg, apply_steer_req, self.frame // 2))
|
||||
|
||||
# we can spam can to cancel the system even if we are using lat only control
|
||||
if (self.frame % 3 == 0 and CS.CP.openpilotLongitudinalControl) or pcm_cancel_cmd:
|
||||
if (self.frame % 3 == 0 and self.CP.openpilotLongitudinalControl) or pcm_cancel_cmd:
|
||||
lead = hud_control.leadVisible or CS.out.vEgo < 12. # at low speed we always assume the lead is present so ACC can be engaged
|
||||
|
||||
# Lexus IS uses a different cancellation message
|
||||
if pcm_cancel_cmd and CS.CP.carFingerprint in (CAR.LEXUS_IS, CAR.LEXUS_RC):
|
||||
if pcm_cancel_cmd and self.CP.carFingerprint in (CAR.LEXUS_IS, CAR.LEXUS_RC):
|
||||
can_sends.append(create_acc_cancel_command(self.packer))
|
||||
elif CS.CP.openpilotLongitudinalControl:
|
||||
elif self.CP.openpilotLongitudinalControl:
|
||||
can_sends.append(create_accel_command(self.packer, pcm_accel_cmd, pcm_cancel_cmd, self.standstill_req, lead, CS.acc_type))
|
||||
self.accel = pcm_accel_cmd
|
||||
else:
|
||||
can_sends.append(create_accel_command(self.packer, 0, pcm_cancel_cmd, False, lead, CS.acc_type))
|
||||
|
||||
if self.frame % 2 == 0 and CS.CP.enableGasInterceptor and CS.CP.openpilotLongitudinalControl:
|
||||
if self.frame % 2 == 0 and self.CP.enableGasInterceptor and self.CP.openpilotLongitudinalControl:
|
||||
# send exactly zero if gas cmd is zero. Interceptor will send the max between read value and gas cmd.
|
||||
# This prevents unexpected pedal range rescaling
|
||||
can_sends.append(create_gas_interceptor_command(self.packer, interceptor_gas_cmd, self.frame // 2))
|
||||
|
|
@ -129,12 +130,12 @@ class CarController:
|
|||
hud_control.rightLaneVisible, hud_control.leftLaneDepart,
|
||||
hud_control.rightLaneDepart, CC.enabled))
|
||||
|
||||
if self.frame % 100 == 0 and CS.CP.enableDsu:
|
||||
if self.frame % 100 == 0 and self.CP.enableDsu:
|
||||
can_sends.append(create_fcw_command(self.packer, fcw_alert))
|
||||
|
||||
# *** static msgs ***
|
||||
for addr, cars, bus, fr_step, vl in STATIC_DSU_MSGS:
|
||||
if self.frame % fr_step == 0 and CS.CP.enableDsu and CS.CP.carFingerprint in cars:
|
||||
if self.frame % fr_step == 0 and self.CP.enableDsu and self.CP.carFingerprint in cars:
|
||||
can_sends.append(make_can_msg(addr, vl, bus))
|
||||
|
||||
new_actuators = actuators.copy()
|
||||
|
|
|
|||
|
|
@ -102,7 +102,7 @@ class CarInterface(CarInterfaceBase):
|
|||
ret.mass = 4607. * CV.LB_TO_KG + STD_CARGO_KG # mean between normal and hybrid limited
|
||||
set_lat_tune(ret.lateralTuning, LatTunes.PID_G)
|
||||
|
||||
elif candidate in (CAR.AVALON, CAR.AVALON_2019, CAR.AVALONH_2019, CAR.AVALON_TSS2):
|
||||
elif candidate in (CAR.AVALON, CAR.AVALON_2019, CAR.AVALONH_2019, CAR.AVALON_TSS2, CAR.AVALONH_TSS2):
|
||||
# starting from 2019, all Avalon variants have stop and go
|
||||
# https://engage.toyota.com/static/images/toyota_safety_sense/TSS_Applicability_Chart.pdf
|
||||
stop_and_go = candidate != CAR.AVALON
|
||||
|
|
|
|||
|
|
@ -34,6 +34,7 @@ class CAR:
|
|||
AVALON_2019 = "TOYOTA AVALON 2019"
|
||||
AVALONH_2019 = "TOYOTA AVALON HYBRID 2019"
|
||||
AVALON_TSS2 = "TOYOTA AVALON 2022" # TSS 2.5
|
||||
AVALONH_TSS2 = "TOYOTA AVALON HYBRID 2022"
|
||||
CAMRY = "TOYOTA CAMRY 2018"
|
||||
CAMRYH = "TOYOTA CAMRY HYBRID 2018"
|
||||
CAMRY_TSS2 = "TOYOTA CAMRY 2021" # TSS 2.5
|
||||
|
|
@ -55,7 +56,7 @@ class CAR:
|
|||
RAV4H = "TOYOTA RAV4 HYBRID 2017"
|
||||
RAV4_TSS2 = "TOYOTA RAV4 2019"
|
||||
RAV4H_TSS2 = "TOYOTA RAV4 HYBRID 2019"
|
||||
MIRAI = "TOYOTA MIRAI 2021" # TSS 2.5
|
||||
MIRAI = "TOYOTA MIRAI 2021" # TSS 2.5
|
||||
SIENNA = "TOYOTA SIENNA 2018"
|
||||
|
||||
# Lexus
|
||||
|
|
@ -100,6 +101,7 @@ CAR_INFO: Dict[str, Union[ToyotaCarInfo, List[ToyotaCarInfo]]] = {
|
|||
CAR.AVALON_2019: ToyotaCarInfo("Toyota Avalon 2019-21", "TSS-P", footnotes=[Footnote.DSU]),
|
||||
CAR.AVALONH_2019: ToyotaCarInfo("Toyota Avalon Hybrid 2019-21", "TSS-P", footnotes=[Footnote.DSU]),
|
||||
CAR.AVALON_TSS2: ToyotaCarInfo("Toyota Avalon 2022"),
|
||||
CAR.AVALONH_TSS2: ToyotaCarInfo("Toyota Avalon Hybrid 2022"),
|
||||
CAR.CAMRY: ToyotaCarInfo("Toyota Camry 2018-20", video_link="https://www.youtube.com/watch?v=fkcjviZY9CM", footnotes=[Footnote.CAMRY]),
|
||||
CAR.CAMRYH: ToyotaCarInfo("Toyota Camry Hybrid 2018-20", video_link="https://www.youtube.com/watch?v=Q2DYY0AWKgk", footnotes=[Footnote.CAMRY]),
|
||||
CAR.CAMRY_TSS2: ToyotaCarInfo("Toyota Camry 2021-22", footnotes=[Footnote.CAMRY]),
|
||||
|
|
@ -271,6 +273,23 @@ FW_VERSIONS = {
|
|||
b'\x028646F4104100\x00\x00\x00\x008646G5301200\x00\x00\x00\x00',
|
||||
],
|
||||
},
|
||||
CAR.AVALONH_TSS2: {
|
||||
(Ecu.esp, 0x7b0, None): [
|
||||
b'F152641080\x00\x00\x00\x00\x00\x00',
|
||||
],
|
||||
(Ecu.eps, 0x7a1, None): [
|
||||
b'8965B41110\x00\x00\x00\x00\x00\x00',
|
||||
],
|
||||
(Ecu.engine, 0x700, None): [
|
||||
b'\x018966306Q6000\x00\x00\x00\x00',
|
||||
],
|
||||
(Ecu.fwdRadar, 0x750, 0xf): [
|
||||
b'\x018821F6201200\x00\x00\x00\x00',
|
||||
],
|
||||
(Ecu.fwdCamera, 0x750, 0x6d): [
|
||||
b'\x028646F4104100\x00\x00\x00\x008646G5301200\x00\x00\x00\x00',
|
||||
],
|
||||
},
|
||||
CAR.CAMRY: {
|
||||
(Ecu.engine, 0x700, None): [
|
||||
b'\x018966306L3100\x00\x00\x00\x00',
|
||||
|
|
@ -1774,6 +1793,7 @@ DBC = {
|
|||
CAR.AVALON_2019: dbc_dict('toyota_nodsu_pt_generated', 'toyota_adas'),
|
||||
CAR.AVALONH_2019: dbc_dict('toyota_nodsu_pt_generated', 'toyota_adas'),
|
||||
CAR.AVALON_TSS2: dbc_dict('toyota_nodsu_pt_generated', 'toyota_tss2_adas'),
|
||||
CAR.AVALONH_TSS2: dbc_dict('toyota_nodsu_pt_generated', 'toyota_tss2_adas'),
|
||||
CAR.RAV4_TSS2: dbc_dict('toyota_nodsu_pt_generated', 'toyota_tss2_adas'),
|
||||
CAR.COROLLA_TSS2: dbc_dict('toyota_nodsu_pt_generated', 'toyota_tss2_adas'),
|
||||
CAR.COROLLAH_TSS2: dbc_dict('toyota_nodsu_pt_generated', 'toyota_tss2_adas'),
|
||||
|
|
@ -1798,11 +1818,11 @@ EPS_SCALE = defaultdict(lambda: 73, {CAR.PRIUS: 66, CAR.COROLLA: 88, CAR.LEXUS_I
|
|||
# Toyota/Lexus Safety Sense 2.0 and 2.5
|
||||
TSS2_CAR = {CAR.RAV4_TSS2, CAR.COROLLA_TSS2, CAR.COROLLAH_TSS2, CAR.LEXUS_ES_TSS2, CAR.LEXUS_ESH_TSS2, CAR.RAV4H_TSS2,
|
||||
CAR.LEXUS_RX_TSS2, CAR.LEXUS_RXH_TSS2, CAR.HIGHLANDER_TSS2, CAR.HIGHLANDERH_TSS2, CAR.PRIUS_TSS2, CAR.CAMRY_TSS2, CAR.CAMRYH_TSS2,
|
||||
CAR.MIRAI, CAR.LEXUS_NX_TSS2, CAR.ALPHARD_TSS2, CAR.AVALON_TSS2}
|
||||
CAR.MIRAI, CAR.LEXUS_NX_TSS2, CAR.ALPHARD_TSS2, CAR.AVALON_TSS2, CAR.AVALONH_TSS2}
|
||||
|
||||
NO_DSU_CAR = TSS2_CAR | {CAR.CHR, CAR.CHRH, CAR.CAMRY, CAR.CAMRYH}
|
||||
|
||||
EV_HYBRID_CAR = {CAR.AVALONH_2019, CAR.CAMRYH, CAR.CAMRYH_TSS2, CAR.CHRH, CAR.COROLLAH_TSS2, CAR.HIGHLANDERH, CAR.HIGHLANDERH_TSS2, CAR.PRIUS,
|
||||
EV_HYBRID_CAR = {CAR.AVALONH_2019, CAR.AVALONH_TSS2, CAR.CAMRYH, CAR.CAMRYH_TSS2, CAR.CHRH, CAR.COROLLAH_TSS2, CAR.HIGHLANDERH, CAR.HIGHLANDERH_TSS2, CAR.PRIUS,
|
||||
CAR.PRIUS_V, CAR.RAV4H, CAR.RAV4H_TSS2, CAR.LEXUS_CTH, CAR.MIRAI, CAR.LEXUS_ESH, CAR.LEXUS_ESH_TSS2, CAR.LEXUS_NXH, CAR.LEXUS_RXH,
|
||||
CAR.LEXUS_RXH_TSS2, CAR.PRIUS_TSS2}
|
||||
|
||||
|
|
|
|||
|
|
@ -9,6 +9,7 @@ VisualAlert = car.CarControl.HUDControl.VisualAlert
|
|||
class CarController():
|
||||
def __init__(self, dbc_name, CP, VM):
|
||||
self.apply_steer_last = 0
|
||||
self.CP = CP
|
||||
|
||||
self.packer_pt = CANPacker(DBC_FILES.mqb)
|
||||
|
||||
|
|
@ -86,7 +87,7 @@ class CarController():
|
|||
|
||||
# FIXME: this entire section is in desperate need of refactoring
|
||||
|
||||
if CS.CP.pcmCruise:
|
||||
if self.CP.pcmCruise:
|
||||
if frame > self.graMsgStartFramePrev + P.GRA_VBP_STEP:
|
||||
if c.cruiseControl.cancel:
|
||||
# Cancel ACC if it's engaged with OP disengaged.
|
||||
|
|
|
|||
|
|
@ -70,7 +70,7 @@ if __name__ == "__main__":
|
|||
to_sync = sys.argv[1:]
|
||||
|
||||
if not len(to_sync):
|
||||
# sync routes from test_routes and process replay
|
||||
# sync routes from the car tests routes and process replay
|
||||
to_sync.extend([rt.route for rt in test_car_models_routes])
|
||||
to_sync.extend([s[1].rsplit('--', 1)[0] for s in replay_segments])
|
||||
|
||||
|
|
|
|||
|
|
@ -1,27 +1,34 @@
|
|||
<?xml version='1.0' encoding='UTF-8'?>
|
||||
<root>
|
||||
<tabbed_widget name="Main Window" parent="main_window">
|
||||
<tabbed_widget parent="main_window" name="Main Window">
|
||||
<Tab containers="1" tab_name="tab1">
|
||||
<Container>
|
||||
<DockSplitter sizes="0.332965;0.334071;0.332965" orientation="-" count="3">
|
||||
<DockSplitter orientation="-" sizes="0.249729;0.250814;0.249729;0.249729" count="4">
|
||||
<DockArea name="...">
|
||||
<plot style="Lines" mode="TimeSeries">
|
||||
<range left="0.000000" top="1.025000" bottom="-0.025000" right="59.900659"/>
|
||||
<plot flip_y="false" flip_x="false" style="Lines" mode="TimeSeries">
|
||||
<range bottom="-0.025000" left="0.000000" right="119.992934" top="1.025000"/>
|
||||
<limitY/>
|
||||
<curve name="/controlsState/enabled" color="#1f77b4"/>
|
||||
<curve name="/pandaStates/0/controlsAllowed" color="#d62728"/>
|
||||
</plot>
|
||||
</DockArea>
|
||||
<DockArea name="...">
|
||||
<plot style="Lines" mode="TimeSeries">
|
||||
<range left="0.000000" top="-10.627637" bottom="-15.150799" right="59.900659"/>
|
||||
<plot flip_y="false" flip_x="false" style="Lines" mode="TimeSeries">
|
||||
<range bottom="-8.840137" left="0.000000" right="119.992934" top="12.881991"/>
|
||||
<limitY/>
|
||||
<curve name="/controlsState/cumLagMs" color="#9467bd"/>
|
||||
</plot>
|
||||
</DockArea>
|
||||
<DockArea name="...">
|
||||
<plot style="Lines" mode="TimeSeries">
|
||||
<range left="0.000447" top="1.025000" bottom="-0.025000" right="59.900939"/>
|
||||
<plot flip_y="false" flip_x="false" style="Lines" mode="TimeSeries">
|
||||
<range bottom="-0.100000" left="0.000000" right="119.992934" top="0.100000"/>
|
||||
<limitY/>
|
||||
<curve name="/pandaStates/0/blockedCnt" color="#d62728"/>
|
||||
</plot>
|
||||
</DockArea>
|
||||
<DockArea name="...">
|
||||
<plot flip_y="false" flip_x="false" style="Lines" mode="TimeSeries">
|
||||
<range bottom="-0.025000" left="0.000000" right="119.992934" top="1.025000"/>
|
||||
<limitY/>
|
||||
<curve name="/carState/gasPressed" color="#1ac938"/>
|
||||
<curve name="/carState/brakePressed" color="#ff7f0e"/>
|
||||
|
|
|
|||
|
|
@ -2,16 +2,16 @@
|
|||
|
||||
# Requires nvidia docker - https://github.com/NVIDIA/nvidia-docker
|
||||
if ! $(apt list --installed | grep -q nvidia-container-toolkit); then
|
||||
if [ -z "$INSTALL" ]; then
|
||||
echo "Nvidia docker is required. Re-run with INSTALL=1 to automatically install."
|
||||
exit 0
|
||||
else
|
||||
read -p "Nvidia docker is required. Do you want to install it now? (y/n)";
|
||||
if [ "${REPLY}" == "y" ]; then
|
||||
distribution=$(. /etc/os-release;echo $ID$VERSION_ID)
|
||||
echo $distribution
|
||||
curl -s -L https://nvidia.github.io/nvidia-docker/gpgkey | sudo apt-key add -
|
||||
curl -s -L https://nvidia.github.io/nvidia-docker/$distribution/nvidia-docker.list | sudo tee /etc/apt/sources.list.d/nvidia-docker.list
|
||||
sudo apt-get update && sudo apt-get install -y nvidia-container-toolkit
|
||||
sudo apt-get update && sudo apt-get install -y nvidia-docker2 # Also installs docker-ce and nvidia-container-toolkit
|
||||
sudo systemctl restart docker
|
||||
else
|
||||
exit 0
|
||||
fi
|
||||
fi
|
||||
|
||||
|
|
|
|||
Loading…
Reference in New Issue