diff --git a/launch_chffrplus.sh b/launch_chffrplus.sh index c353b90823..2424038e45 100755 --- a/launch_chffrplus.sh +++ b/launch_chffrplus.sh @@ -19,12 +19,44 @@ function agnos_init { # Check if AGNOS update is required if [ $(< /VERSION) != "$AGNOS_VERSION" ]; then + echo "Waiting for internet..." + + timeout=0 + while [ $timeout -lt 120 ]; do + if getent hosts pypi.org >/dev/null 2>&1; then + break + fi + echo "Waiting for internet... (${timeout})" + sleep 5 + timeout=$((timeout+5)) + + done + + if python3 -c "import jeepney" > /dev/null 2>&1; then + echo "jeepney already installed." + else + echo "jeepney installing to pydeps." + python3 -m pip install --target "$PYDEPS" --upgrade jeepney + fi + AGNOS_PY="$DIR/system/hardware/tici/agnos.py" MANIFEST="$DIR/system/hardware/tici/agnos.json" + MODEL="$(tr -d '\000\r\n' 2>/dev/null < /sys/firmware/devicetree/base/model | tr '[:upper:]' '[:lower:]')" + MODEL="${MODEL#comma }" + if [ "$MODEL" = "c3" ] || [ "$MODEL" = "tici" ]; then + MANIFEST="$DIR/system/hardware/tici/agnos-tici.json" + fi if $AGNOS_PY --verify $MANIFEST; then sudo reboot fi - $DIR/system/hardware/tici/updater $AGNOS_PY $MANIFEST + echo "AGNOS_PY=${AGNOS_PY}" + echo "MANIFEST=${MANIFEST}" + echo "MODEL=${MODEL}" + if ! python3 $DIR/system/ui/updater.py $AGNOS_PY $MANIFEST; then + echo "python updater failed, falling back to bundled updater" + $DIR/system/hardware/tici/updater $AGNOS_PY $MANIFEST + fi + echo "end updater $AGNOS_PY $MANIFEST" fi } @@ -124,7 +156,6 @@ function launch { PYDEPS="$DIR/pydeps" mkdir -p "$PYDEPS" export PYTHONPATH="$PYDEPS:$PWD${PYTHONPATH:+:$PYTHONPATH}" - start_carrot_recovery # hardware specific init @@ -132,6 +163,7 @@ function launch { agnos_init fi + FORCE_REBUILD=0 invalidate_modeld_build_if_needed invalidate_native_build_if_needed diff --git a/selfdrive/pandad/SConscript b/selfdrive/pandad/SConscript index fd59db9853..343ca4b79a 100644 --- a/selfdrive/pandad/SConscript +++ b/selfdrive/pandad/SConscript @@ -1,8 +1,8 @@ Import('env', 'arch', 'common', 'messaging') if arch != "Darwin": - libs = [common, messaging, 'pthread'] - panda = env.Library('panda', ['panda.cc', 'spi.cc']) + libs = [common, messaging, 'pthread', 'usb-1.0'] + panda = env.Library('panda', ['panda.cc', 'panda_comms.cc', 'spi.cc']) env.Program('pandad', ['main.cc', 'pandad.cc', 'panda_safety.cc'], LIBS=[panda] + libs) diff --git a/selfdrive/pandad/panda.cc b/selfdrive/pandad/panda.cc index edc2228c0c..a21fb4abe8 100644 --- a/selfdrive/pandad/panda.cc +++ b/selfdrive/pandad/panda.cc @@ -2,6 +2,7 @@ #include +#include #include #include #include @@ -13,8 +14,13 @@ const bool PANDAD_MAXOUT = getenv("PANDAD_MAXOUT") != nullptr; Panda::Panda(std::string serial) { - handle = std::make_unique(serial); - LOGW("connected to %s over SPI", serial.c_str()); + try { + handle = std::make_unique(serial); + LOGW("connected to %s over USB", serial.c_str()); + } catch (const std::exception &e) { + handle = std::make_unique(serial); + LOGW("connected to %s over SPI", serial.c_str()); + } hw_type = get_hw_type(); can_reset_communications(); @@ -33,7 +39,13 @@ std::string Panda::hw_serial() { } std::vector Panda::list() { - return PandaSpiHandle::list(); + std::vector serials = PandaUsbHandle::list(); + for (const auto &s : PandaSpiHandle::list()) { + if (std::find(serials.begin(), serials.end(), s) == serials.end()) { + serials.push_back(s); + } + } + return serials; } void Panda::set_safety_model(cereal::CarParams::SafetyModel safety_model, uint16_t safety_param) { diff --git a/selfdrive/pandad/panda.h b/selfdrive/pandad/panda.h index 1a066dc5fb..979c2a5da1 100644 --- a/selfdrive/pandad/panda.h +++ b/selfdrive/pandad/panda.h @@ -45,7 +45,7 @@ struct can_frame { class Panda { private: - std::unique_ptr handle; + std::unique_ptr handle; public: Panda(std::string serial); diff --git a/selfdrive/pandad/panda_comms.cc b/selfdrive/pandad/panda_comms.cc new file mode 100644 index 0000000000..ba5da331d3 --- /dev/null +++ b/selfdrive/pandad/panda_comms.cc @@ -0,0 +1,224 @@ +#include "selfdrive/pandad/panda_comms.h" + +#include + +#include +#include + +#include "common/swaglog.h" + +static libusb_context *init_usb_ctx() { + libusb_context *context = nullptr; + int err = libusb_init(&context); + if (err != 0) { + LOGE("libusb initialization error"); + return nullptr; + } + +#if LIBUSB_API_VERSION >= 0x01000106 + libusb_set_option(context, LIBUSB_OPTION_LOG_LEVEL, LIBUSB_LOG_LEVEL_INFO); +#else + libusb_set_debug(context, 3); +#endif + return context; +} + +PandaUsbHandle::PandaUsbHandle(std::string serial) { + ssize_t num_devices; + libusb_device **dev_list = nullptr; + int err = 0; + ctx = init_usb_ctx(); + if (!ctx) { goto fail; } + + num_devices = libusb_get_device_list(ctx, &dev_list); + if (num_devices < 0) { goto fail; } + for (size_t i = 0; i < num_devices; ++i) { + libusb_device_descriptor desc; + libusb_get_device_descriptor(dev_list[i], &desc); + if (desc.idVendor == 0x3801 && desc.idProduct == 0xddcc) { + int ret = libusb_open(dev_list[i], &dev_handle); + if (dev_handle == nullptr || ret < 0) { goto fail; } + + unsigned char desc_serial[26] = {0}; + ret = libusb_get_string_descriptor_ascii(dev_handle, desc.iSerialNumber, desc_serial, sizeof(desc_serial)); + if (ret < 0) { goto fail; } + + hw_serial = std::string((char *)desc_serial, ret); + if (serial.empty() || serial == hw_serial) { + break; + } + libusb_close(dev_handle); + dev_handle = nullptr; + } + } + if (dev_handle == nullptr) { goto fail; } + libusb_free_device_list(dev_list, 1); + dev_list = nullptr; + + if (libusb_kernel_driver_active(dev_handle, 0) == 1) { + libusb_detach_kernel_driver(dev_handle, 0); + } + + err = libusb_set_configuration(dev_handle, 1); + if (err != 0) { goto fail; } + + err = libusb_claim_interface(dev_handle, 0); + if (err != 0) { goto fail; } + + return; + +fail: + if (dev_list != nullptr) { + libusb_free_device_list(dev_list, 1); + } + cleanup(); + throw std::runtime_error("Error connecting to panda over USB"); +} + +PandaUsbHandle::~PandaUsbHandle() { + std::lock_guard lk(hw_lock); + cleanup(); + connected = false; +} + +void PandaUsbHandle::cleanup() { + if (dev_handle != nullptr) { + libusb_release_interface(dev_handle, 0); + libusb_close(dev_handle); + dev_handle = nullptr; + } + + if (ctx != nullptr) { + libusb_exit(ctx); + ctx = nullptr; + } +} + +std::vector PandaUsbHandle::list() { + static std::unique_ptr context(init_usb_ctx(), libusb_exit); + ssize_t num_devices; + libusb_device **dev_list = nullptr; + std::vector serials; + if (!context) { return serials; } + + num_devices = libusb_get_device_list(context.get(), &dev_list); + if (num_devices < 0) { + LOGE("libusb can't get device list"); + goto finish; + } + + for (size_t i = 0; i < num_devices; ++i) { + libusb_device *device = dev_list[i]; + libusb_device_descriptor desc; + libusb_get_device_descriptor(device, &desc); + if (desc.idVendor == 0x3801 && desc.idProduct == 0xddcc) { + libusb_device_handle *handle = nullptr; + int ret = libusb_open(device, &handle); + if (ret < 0) { goto finish; } + + unsigned char desc_serial[26] = {0}; + ret = libusb_get_string_descriptor_ascii(handle, desc.iSerialNumber, desc_serial, sizeof(desc_serial)); + libusb_close(handle); + if (ret < 0) { goto finish; } + + serials.push_back(std::string((char *)desc_serial, ret)); + } + } + +finish: + if (dev_list != nullptr) { + libusb_free_device_list(dev_list, 1); + } + return serials; +} + +void PandaUsbHandle::handle_usb_issue(int err, const char func[]) { + LOGE_100("usb error %d \"%s\" in %s", err, libusb_strerror((enum libusb_error)err), func); + if (err == LIBUSB_ERROR_NO_DEVICE) { + LOGE("lost connection"); + connected = false; + } +} + +int PandaUsbHandle::control_write(uint8_t bRequest, uint16_t wValue, uint16_t wIndex, unsigned int timeout) { + int err; + const uint8_t bmRequestType = LIBUSB_ENDPOINT_OUT | LIBUSB_REQUEST_TYPE_VENDOR | LIBUSB_RECIPIENT_DEVICE; + + if (!connected) { + return LIBUSB_ERROR_NO_DEVICE; + } + + std::lock_guard lk(hw_lock); + do { + err = libusb_control_transfer(dev_handle, bmRequestType, bRequest, wValue, wIndex, nullptr, 0, timeout); + if (err < 0) { handle_usb_issue(err, __func__); } + } while (err < 0 && connected); + + return err; +} + +int PandaUsbHandle::control_read(uint8_t bRequest, uint16_t wValue, uint16_t wIndex, unsigned char *data, uint16_t wLength, unsigned int timeout) { + int err; + const uint8_t bmRequestType = LIBUSB_ENDPOINT_IN | LIBUSB_REQUEST_TYPE_VENDOR | LIBUSB_RECIPIENT_DEVICE; + + if (!connected) { + return LIBUSB_ERROR_NO_DEVICE; + } + + std::lock_guard lk(hw_lock); + do { + err = libusb_control_transfer(dev_handle, bmRequestType, bRequest, wValue, wIndex, data, wLength, timeout); + if (err < 0) { handle_usb_issue(err, __func__); } + } while (err < 0 && connected); + + return err; +} + +int PandaUsbHandle::bulk_write(unsigned char endpoint, unsigned char* data, int length, unsigned int timeout) { + int err; + int transferred = 0; + + if (!connected) { + return 0; + } + + std::lock_guard lk(hw_lock); + do { + // Panda can NAK when the receive buffer is full. After 5ms, drop the messages. + err = libusb_bulk_transfer(dev_handle, endpoint, data, length, &transferred, timeout); + + if (err == LIBUSB_ERROR_TIMEOUT) { + LOGW("Transmit buffer full"); + break; + } else if (err != 0 || length != transferred) { + handle_usb_issue(err, __func__); + } + } while (err != 0 && connected); + + return transferred; +} + +int PandaUsbHandle::bulk_read(unsigned char endpoint, unsigned char* data, int length, unsigned int timeout) { + int err; + int transferred = 0; + + if (!connected) { + return 0; + } + + std::lock_guard lk(hw_lock); + do { + err = libusb_bulk_transfer(dev_handle, endpoint, data, length, &transferred, timeout); + + if (err == LIBUSB_ERROR_TIMEOUT) { + break; + } else if (err == LIBUSB_ERROR_OVERFLOW) { + comms_healthy = false; + LOGE_100("overflow got 0x%x", transferred); + } else if (err != 0) { + handle_usb_issue(err, __func__); + } + } while (err != 0 && connected); + + return transferred; +} diff --git a/selfdrive/pandad/panda_comms.h b/selfdrive/pandad/panda_comms.h index cdfb5019b6..a16a65ac9a 100644 --- a/selfdrive/pandad/panda_comms.h +++ b/selfdrive/pandad/panda_comms.h @@ -6,17 +6,52 @@ #include #include +struct libusb_context; +struct libusb_device_handle; #define TIMEOUT 0 #define SPI_BUF_SIZE 2048 -class PandaSpiHandle { +class PandaCommsHandle { public: + PandaCommsHandle() {} + virtual ~PandaCommsHandle() {} + virtual void cleanup() = 0; + std::string hw_serial; std::atomic connected = true; std::atomic comms_healthy = true; + virtual int control_write(uint8_t request, uint16_t param1, uint16_t param2, unsigned int timeout=TIMEOUT) = 0; + virtual int control_read(uint8_t request, uint16_t param1, uint16_t param2, unsigned char *data, uint16_t length, unsigned int timeout=TIMEOUT) = 0; + virtual int bulk_write(unsigned char endpoint, unsigned char* data, int length, unsigned int timeout=TIMEOUT) = 0; + virtual int bulk_read(unsigned char endpoint, unsigned char* data, int length, unsigned int timeout=TIMEOUT) = 0; +}; + +class PandaUsbHandle : public PandaCommsHandle { +public: + PandaUsbHandle(std::string serial); + ~PandaUsbHandle(); + + int control_write(uint8_t request, uint16_t param1, uint16_t param2, unsigned int timeout=TIMEOUT); + int control_read(uint8_t request, uint16_t param1, uint16_t param2, unsigned char *data, uint16_t length, unsigned int timeout=TIMEOUT); + int bulk_write(unsigned char endpoint, unsigned char* data, int length, unsigned int timeout=TIMEOUT); + int bulk_read(unsigned char endpoint, unsigned char* data, int length, unsigned int timeout=TIMEOUT); + void cleanup(); + + static std::vector list(); + +private: + libusb_context *ctx = nullptr; + libusb_device_handle *dev_handle = nullptr; + std::recursive_mutex hw_lock; + + void handle_usb_issue(int err, const char func[]); +}; + +class PandaSpiHandle : public PandaCommsHandle { +public: PandaSpiHandle(std::string serial); ~PandaSpiHandle(); diff --git a/selfdrive/pandad/pandad.py b/selfdrive/pandad/pandad.py index df2b4f7ee8..730836a360 100755 --- a/selfdrive/pandad/pandad.py +++ b/selfdrive/pandad/pandad.py @@ -6,16 +6,16 @@ import signal import subprocess -from panda import Panda, PandaDFU, PandaProtocolMismatch, McuType, FW_PATH +from panda import Panda, PandaDFU, PandaProtocolMismatch, FW_PATH from openpilot.common.basedir import BASEDIR from openpilot.common.params import Params from openpilot.system.hardware import HARDWARE from openpilot.common.swaglog import cloudlog -def get_expected_signature() -> bytes: +def get_expected_signature(panda: Panda) -> bytes: try: - fn = os.path.join(FW_PATH, McuType.H7.config.app_fn) + fn = os.path.join(FW_PATH, panda.get_mcu_type().config.app_fn) return Panda.get_signature_from_firmware(fn) except Exception: cloudlog.exception("Error computing expected signature") @@ -29,7 +29,7 @@ def flash_panda(panda_serial: str) -> Panda: HARDWARE.recover_internal_panda() raise - fw_signature = get_expected_signature() + fw_signature = get_expected_signature(panda) internal_panda = panda.is_internal() panda_version = "bootstub" if panda.bootstub else panda.get_version() diff --git a/system/camerad/SConscript b/system/camerad/SConscript index c28330b32c..7f97590fc6 100644 --- a/system/camerad/SConscript +++ b/system/camerad/SConscript @@ -4,7 +4,7 @@ libs = [common, messaging, visionipc] if arch != "Darwin": camera_obj = env.Object(['cameras/camera_qcom2.cc', 'cameras/camera_common.cc', 'cameras/spectra.cc', - 'cameras/cdm.cc', 'sensors/ox03c10.cc', 'sensors/os04c10.cc']) + 'cameras/cdm.cc', 'sensors/ar0231.cc', 'sensors/ox03c10.cc', 'sensors/os04c10.cc']) env.Program('camerad', ['main.cc', camera_obj], LIBS=libs) if GetOption("extras") and arch == "x86_64": diff --git a/system/camerad/cameras/spectra.cc b/system/camerad/cameras/spectra.cc index 0b5f5327f5..5d3c728ce3 100644 --- a/system/camerad/cameras/spectra.cc +++ b/system/camerad/cameras/spectra.cc @@ -22,6 +22,13 @@ // ************** low level camera helpers **************** +int get_bps_blob_index(const SensorInfo *sensor) { + if (sensor->image_sensor == cereal::FrameData::ImageSensor::AR0231) { + return 2; + } + return sensor->num(); +} + int do_cam_control(int fd, int op_code, void *handle, int size) { struct cam_control camcontrol = {0}; camcontrol.op_code = op_code; @@ -1046,7 +1053,8 @@ bool SpectraCamera::openSensor() { }; // Figure out which sensor we have - if (!init_sensor_lambda(new OS04C10) && + if (!init_sensor_lambda(new AR0231) && + !init_sensor_lambda(new OS04C10) && !init_sensor_lambda(new OX03C10)) { LOGE("** sensor %d FAILED bringup, disabling", cc.camera_num); enabled = false; @@ -1167,11 +1175,13 @@ void SpectraCamera::configICP() { int cfg_handle; + const int bps_idx = get_bps_blob_index(sensor.get()); + LOGW("camera %d sensor %d: using BPS blob index %d", cc.camera_num, sensor->num(), bps_idx); uint32_t cfg_size = sizeof(bps_cfg[0]) / sizeof(bps_cfg[0][0]); void *cfg = alloc_w_mmu_hdl(m->video0_fd, cfg_size, (uint32_t*)&cfg_handle, 0x1, CAM_MEM_FLAG_HW_READ_WRITE | CAM_MEM_FLAG_UMD_ACCESS | CAM_MEM_FLAG_HW_SHARED_ACCESS, m->icp_device_iommu); - memcpy(cfg, bps_cfg[sensor->num()], cfg_size); + memcpy(cfg, bps_cfg[bps_idx], cfg_size); struct cam_icp_acquire_dev_info icp_info = { .scratch_mem_size = 0x0, @@ -1206,7 +1216,7 @@ void SpectraCamera::configICP() { // BPSIQSettings struct uint32_t settings_size = sizeof(bps_settings[0]) / sizeof(bps_settings[0][0]); bps_iq.init(m, settings_size, 0x20, true, m->icp_device_iommu); - memcpy(bps_iq.ptr, bps_settings[sensor->num()], settings_size); + memcpy(bps_iq.ptr, bps_settings[bps_idx], settings_size); // for cdm register writes, just make it bigger than you need bps_cdm_program_array.init(m, 0x1000, 0x20, true, m->icp_device_iommu); @@ -1214,7 +1224,7 @@ void SpectraCamera::configICP() { // striping lib output uint32_t striping_size = sizeof(bps_striping_output[0]) / sizeof(bps_striping_output[0][0]); bps_striping.init(m, striping_size, 0x20, true, m->icp_device_iommu); - memcpy(bps_striping.ptr, bps_striping_output[sensor->num()], striping_size); + memcpy(bps_striping.ptr, bps_striping_output[bps_idx], striping_size); // used internally by the BPS, we just allocate it. // size comes from the BPSStripingLib @@ -1513,7 +1523,8 @@ bool SpectraCamera::waitForFrameReady(uint64_t request_id) { } bool SpectraCamera::processFrame(int buf_idx, uint64_t request_id, uint64_t frame_id_raw, uint64_t timestamp) { - if (!syncFirstFrame(cc.camera_num, request_id, frame_id_raw, timestamp, cc.staggered_sof)) { + const bool staggered_sof = cc.staggered_sof && sensor->image_sensor != cereal::FrameData::ImageSensor::AR0231; + if (!syncFirstFrame(cc.camera_num, request_id, frame_id_raw, timestamp, staggered_sof)) { return false; } @@ -1570,7 +1581,12 @@ bool SpectraCamera::syncFirstFrame(int camera_id, uint64_t request_id, uint64_t // Timeout in case the timestamps never line up if (raw_id > 40) { - LOGE("camera first frame sync timed out"); + LOGE("camera first frame sync timed out: camera %d request %lu raw_id %lu timestamp %lu cams %zu/%d", + camera_id, request_id, raw_id, timestamp, camera_sync_data.size(), enabled_camera_count); + for (const auto &[cam, sync_data] : camera_sync_data) { + LOGE("camera %d first frame sync data: frame_id_offset %lu timestamp %lu staggered %d", + cam, sync_data.frame_id_offset, sync_data.timestamp, sync_data.staggered); + } first_frame_synced = true; } diff --git a/system/camerad/sensors/ar0231.cc b/system/camerad/sensors/ar0231.cc new file mode 100644 index 0000000000..e4ae29f079 --- /dev/null +++ b/system/camerad/sensors/ar0231.cc @@ -0,0 +1,136 @@ +#include +#include + +#include "system/camerad/sensors/sensor.h" + +namespace { + +const size_t AR0231_REGISTERS_HEIGHT = 2; +// TODO: this extra height is universal and doesn't apply per camera +const size_t AR0231_STATS_HEIGHT = 2 + 8; + +const float sensor_analog_gains_AR0231[] = { + 1.0 / 8.0, 2.0 / 8.0, 2.0 / 7.0, 3.0 / 7.0, // 0, 1, 2, 3 + 3.0 / 6.0, 4.0 / 6.0, 4.0 / 5.0, 5.0 / 5.0, // 4, 5, 6, 7 + 5.0 / 4.0, 6.0 / 4.0, 6.0 / 3.0, 7.0 / 3.0, // 8, 9, 10, 11 + 7.0 / 2.0, 8.0 / 2.0, 8.0 / 1.0}; // 12, 13, 14, 15 = bypass + +} // namespace + +AR0231::AR0231() { + image_sensor = cereal::FrameData::ImageSensor::AR0231; + bayer_pattern = CAM_ISP_PATTERN_BAYER_GRGRGR; + pixel_size_mm = 0.003; + data_word = true; + frame_width = 1928; + frame_height = 1208; + frame_stride = (frame_width * 12 / 8) + 4; + extra_height = AR0231_REGISTERS_HEIGHT + AR0231_STATS_HEIGHT; + + registers_offset = 0; + frame_offset = AR0231_REGISTERS_HEIGHT; + stats_offset = AR0231_REGISTERS_HEIGHT + frame_height; + + start_reg_array.assign(std::begin(start_reg_array_ar0231), std::end(start_reg_array_ar0231)); + init_reg_array.assign(std::begin(init_array_ar0231), std::end(init_array_ar0231)); + probe_reg_addr = 0x3000; + probe_expected_data = 0x354; + bits_per_pixel = 12; + mipi_format = CAM_FORMAT_MIPI_RAW_12; + frame_data_type = 0x12; // Changing stats to 0x2C doesn't work, so change pixels to 0x12 instead + mclk_frequency = 19200000; //Hz + + readout_time_ns = 22850000; + + dc_gain_factor = 2.5; + dc_gain_min_weight = 0; + dc_gain_max_weight = 1; + dc_gain_on_grey = 0.2; + dc_gain_off_grey = 0.3; + exposure_time_min = 2; // with HDR, fastest ss + exposure_time_max = 0x0855; // with HDR, slowest ss, 40ms + analog_gain_min_idx = 0x1; // 0.25x + analog_gain_rec_idx = 0x6; // 0.8x + analog_gain_max_idx = 0xD; // 4.0x + analog_gain_cost_delta = 0; + analog_gain_cost_low = 0.1; + analog_gain_cost_high = 5.0; + for (int i = 0; i <= analog_gain_max_idx; i++) { + sensor_analog_gains[i] = sensor_analog_gains_AR0231[i]; + } + min_ev = exposure_time_min * sensor_analog_gains[analog_gain_min_idx]; + max_ev = exposure_time_max * dc_gain_factor * sensor_analog_gains[analog_gain_max_idx]; + target_grey_factor = 1.0; + + black_level = 168; + color_correct_matrix = { + 0x000000af, 0x00000ff9, 0x00000fd8, + 0x00000fbc, 0x000000bb, 0x00000009, + 0x00000fb6, 0x00000fe0, 0x000000ea, + }; + for (int i = 0; i < 65; i++) { + float fx = i / 64.0; + const float gamma_k = 0.75; + const float gamma_b = 0.125; + const float mp = 0.01; // ideally midpoint should be adaptive + const float rk = 9 - 100*mp; + // poly approximation for s curve + fx = (fx > mp) ? + ((rk * (fx-mp) * (1-(gamma_k*mp+gamma_b)) * (1+1/(rk*(1-mp))) / (1+rk*(fx-mp))) + gamma_k*mp + gamma_b) : + ((rk * (fx-mp) * (gamma_k*mp+gamma_b) * (1+1/(rk*mp)) / (1-rk*(fx-mp))) + gamma_k*mp + gamma_b); + gamma_lut_rgb.push_back((uint32_t)(fx*1023.0 + 0.5)); + } + prepare_gamma_lut(); + linearization_lut = { + 0x02000000, 0x02000000, 0x02000000, 0x02000000, + 0x020007ff, 0x020007ff, 0x020007ff, 0x020007ff, + 0x02000bff, 0x02000bff, 0x02000bff, 0x02000bff, + 0x020017ff, 0x020017ff, 0x020017ff, 0x020017ff, + 0x02001bff, 0x02001bff, 0x02001bff, 0x02001bff, + 0x020023ff, 0x020023ff, 0x020023ff, 0x020023ff, + 0x00003fff, 0x00003fff, 0x00003fff, 0x00003fff, + 0x00003fff, 0x00003fff, 0x00003fff, 0x00003fff, + 0x00003fff, 0x00003fff, 0x00003fff, 0x00003fff, + }; + linearization_pts = {0x07ff0bff, 0x17ff1bff, 0x23ff3fff, 0x3fff3fff}; + vignetting_lut = { + 0x00eaa755, 0x00cf2679, 0x00bc05e0, 0x00acc566, 0x00a1450a, 0x009984cc, 0x0095a4ad, 0x009584ac, 0x009944ca, 0x00a0c506, 0x00ac0560, 0x00bb25d9, 0x00ce2671, 0x00e90748, 0x01112889, 0x014a2a51, 0x01984cc2, + 0x00db06d8, 0x00c30618, 0x00afe57f, 0x00a0a505, 0x009524a9, 0x008d646b, 0x0089844c, 0x0089644b, 0x008d2469, 0x0094a4a5, 0x009fe4ff, 0x00af0578, 0x00c20610, 0x00d986cc, 0x00fda7ed, 0x01320990, 0x017aebd7, + 0x00d1868c, 0x00baa5d5, 0x00a7853c, 0x009844c2, 0x008cc466, 0x0085a42d, 0x0083641b, 0x0083641b, 0x0085842c, 0x008c4462, 0x0097a4bd, 0x00a6c536, 0x00b9a5cd, 0x00d06683, 0x00f1678b, 0x01226913, 0x0167ab3d, + 0x00cd0668, 0x00b625b1, 0x00a30518, 0x0093c49e, 0x00884442, 0x00830418, 0x0080e407, 0x0080c406, 0x0082e417, 0x0087c43e, 0x00932499, 0x00a22511, 0x00b525a9, 0x00cbe65f, 0x00eb0758, 0x011a68d3, 0x015daaed, + 0x00cc4662, 0x00b565ab, 0x00a24512, 0x00930498, 0x0087843c, 0x0082a415, 0x00806403, 0x00806403, 0x00828414, 0x00870438, 0x00926493, 0x00a1850c, 0x00b465a3, 0x00cb2659, 0x00ea2751, 0x011928c9, 0x015c2ae1, + 0x00cf667b, 0x00b885c4, 0x00a5652b, 0x009624b1, 0x008aa455, 0x00846423, 0x00822411, 0x00822411, 0x00844422, 0x008a2451, 0x009564ab, 0x00a48524, 0x00b785bc, 0x00ce4672, 0x00ee6773, 0x011e88f4, 0x0162eb17, + 0x00d6c6b6, 0x00bf65fb, 0x00ac4562, 0x009d04e8, 0x0091848c, 0x0089c44e, 0x00862431, 0x00860430, 0x0089844c, 0x00910488, 0x009c64e3, 0x00ab655b, 0x00be65f3, 0x00d566ab, 0x00f847c2, 0x012b2959, 0x01726b93, + 0x00e3e71f, 0x00ca0650, 0x00b705b8, 0x00a7a53d, 0x009c24e1, 0x009484a4, 0x00908484, 0x00908484, 0x009424a1, 0x009bc4de, 0x00a70538, 0x00b625b1, 0x00c90648, 0x00e26713, 0x0108e847, 0x013fe9ff, 0x018bcc5e, + 0x00f807c0, 0x00d966cb, 0x00c5862c, 0x00b625b1, 0x00aaa555, 0x00a30518, 0x009f04f8, 0x009f04f8, 0x00a2a515, 0x00aa2551, 0x00b585ac, 0x00c4a625, 0x00d846c2, 0x00f647b2, 0x0121a90d, 0x015e4af2, 0x01b8cdc6, + 0x011548aa, 0x00f1678b, 0x00d886c4, 0x00c86643, 0x00bce5e7, 0x00b545aa, 0x00b1658b, 0x00b1458a, 0x00b505a8, 0x00bc85e4, 0x00c7c63e, 0x00d786bc, 0x00efe77f, 0x0113489a, 0x0144ea27, 0x01888c44, 0x01fdcfee, + 0x013e49f2, 0x0113e89f, 0x00f5a7ad, 0x00e0c706, 0x00d30698, 0x00cb665b, 0x00c7663b, 0x00c7663b, 0x00cb0658, 0x00d2a695, 0x00dfe6ff, 0x00f467a3, 0x01122891, 0x013be9df, 0x01750ba8, 0x01cfae7d, 0x025912c8, + 0x01766bb3, 0x01446a23, 0x011fc8fe, 0x0105e82f, 0x00f467a3, 0x00e9874c, 0x00e46723, 0x00e44722, 0x00e92749, 0x00f3a79d, 0x0104c826, 0x011e48f2, 0x01424a12, 0x01738b9c, 0x01bf6dfb, 0x023611b0, 0x02ced676, + 0x01cf8e7c, 0x01866c33, 0x015aaad5, 0x013ae9d7, 0x01250928, 0x011768bb, 0x0110a885, 0x01108884, 0x0116e8b7, 0x01242921, 0x0139a9cd, 0x0158eac7, 0x01840c20, 0x01cb0e58, 0x0233719b, 0x02b9d5ce, 0x03645b22, + }; +} + +std::vector AR0231::getExposureRegisters(int exposure_time, int new_exp_g, bool dc_gain_enabled) const { + uint16_t analog_gain_reg = 0xFF00 | (new_exp_g << 4) | new_exp_g; + return { + {0x3366, analog_gain_reg}, + {0x3362, (uint16_t)(dc_gain_enabled ? 0x1 : 0x0)}, + {0x3012, (uint16_t)exposure_time}, + }; +} + +int AR0231::getSlaveAddress(int port) const { + assert(port >= 0 && port <= 2); + return (int[]){0x20, 0x30, 0x20}[port]; +} + +float AR0231::getExposureScore(float desired_ev, int exp_t, int exp_g_idx, float exp_gain, int gain_idx) const { + // Cost of ev diff + float score = std::abs(desired_ev - (exp_t * exp_gain)) * 10; + // Cost of absolute gain + float m = exp_g_idx > analog_gain_rec_idx ? analog_gain_cost_high : analog_gain_cost_low; + score += std::abs(exp_g_idx - (int)analog_gain_rec_idx) * m; + // Cost of changing gain + score += std::abs(exp_g_idx - gain_idx) * (score + 1.0) / 10.0; + return score; +} diff --git a/system/camerad/sensors/ar0231_registers.h b/system/camerad/sensors/ar0231_registers.h new file mode 100644 index 0000000000..e0872a673a --- /dev/null +++ b/system/camerad/sensors/ar0231_registers.h @@ -0,0 +1,121 @@ +#pragma once + +const struct i2c_random_wr_payload start_reg_array_ar0231[] = {{0x301A, 0x91C}}; +const struct i2c_random_wr_payload stop_reg_array_ar0231[] = {{0x301A, 0x918}}; + +const struct i2c_random_wr_payload init_array_ar0231[] = { + {0x301A, 0x0018}, // RESET_REGISTER + + // **NOTE**: if this is changed, readout_time_ns must be updated in the Sensor config + + // CLOCK Settings + // input clock is 19.2 / 2 * 0x37 = 528 MHz + // pixclk is 528 / 6 = 88 MHz + // full roll time is 1000/(PIXCLK/(LINE_LENGTH_PCK*FRAME_LENGTH_LINES)) = 39.99 ms + // img roll time is 1000/(PIXCLK/(LINE_LENGTH_PCK*Y_OUTPUT_CONTROL)) = 22.85 ms + {0x302A, 0x0006}, // VT_PIX_CLK_DIV + {0x302C, 0x0001}, // VT_SYS_CLK_DIV + {0x302E, 0x0002}, // PRE_PLL_CLK_DIV + {0x3030, 0x0037}, // PLL_MULTIPLIER + {0x3036, 0x000C}, // OP_PIX_CLK_DIV + {0x3038, 0x0001}, // OP_SYS_CLK_DIV + + // FORMAT + {0x3040, 0xC000}, // READ_MODE + {0x3004, 0x0000}, // X_ADDR_START_ + {0x3008, 0x0787}, // X_ADDR_END_ + {0x3002, 0x0000}, // Y_ADDR_START_ + {0x3006, 0x04B7}, // Y_ADDR_END_ + {0x3032, 0x0000}, // SCALING_MODE + {0x30A2, 0x0001}, // X_ODD_INC_ + {0x30A6, 0x0001}, // Y_ODD_INC_ + {0x3402, 0x0788}, // X_OUTPUT_CONTROL + {0x3404, 0x04B8}, // Y_OUTPUT_CONTROL + {0x3064, 0x1982}, // SMIA_TEST + {0x30BA, 0x11F2}, // DIGITAL_CTRL + + // Enable external trigger and disable GPIO outputs + {0x30CE, 0x0120}, // SLAVE_SH_SYNC_MODE | FRAME_START_MODE + {0x340A, 0xE0}, // GPIO3_INPUT_DISABLE | GPIO2_INPUT_DISABLE | GPIO1_INPUT_DISABLE + {0x340C, 0x802}, // GPIO_HIDRV_EN | GPIO0_ISEL=2 + + // Readout timing + {0x300C, 0x0672}, // LINE_LENGTH_PCK (valid for 3-exposure HDR) + {0x300A, 0x0855}, // FRAME_LENGTH_LINES + {0x3042, 0x0000}, // EXTRA_DELAY + + // Readout Settings + {0x31AE, 0x0204}, // SERIAL_FORMAT, 4-lane MIPI + {0x31AC, 0x0C0C}, // DATA_FORMAT_BITS, 12 -> 12 + {0x3342, 0x1212}, // MIPI_F1_PDT_EDT + {0x3346, 0x1212}, // MIPI_F2_PDT_EDT + {0x334A, 0x1212}, // MIPI_F3_PDT_EDT + {0x334E, 0x1212}, // MIPI_F4_PDT_EDT + {0x3344, 0x0011}, // MIPI_F1_VDT_VC + {0x3348, 0x0111}, // MIPI_F2_VDT_VC + {0x334C, 0x0211}, // MIPI_F3_VDT_VC + {0x3350, 0x0311}, // MIPI_F4_VDT_VC + {0x31B0, 0x0053}, // FRAME_PREAMBLE + {0x31B2, 0x003B}, // LINE_PREAMBLE + {0x301A, 0x001C}, // RESET_REGISTER + + // Noise Corrections + {0x3092, 0x0C24}, // ROW_NOISE_CONTROL + {0x337A, 0x0C80}, // DBLC_SCALE0 + {0x3370, 0x03B1}, // DBLC + {0x3044, 0x0400}, // DARK_CONTROL + + // Enable temperature sensor + {0x30B4, 0x0007}, // TEMPSENS0_CTRL_REG + {0x30B8, 0x0007}, // TEMPSENS1_CTRL_REG + + // Enable dead pixel correction using + // the 1D line correction scheme + {0x31E0, 0x0003}, + + // HDR Settings + {0x3082, 0x0004}, // OPERATION_MODE_CTRL + {0x3238, 0x0444}, // EXPOSURE_RATIO + + {0x1008, 0x0361}, // FINE_INTEGRATION_TIME_MIN + {0x100C, 0x0589}, // FINE_INTEGRATION_TIME2_MIN + {0x100E, 0x07B1}, // FINE_INTEGRATION_TIME3_MIN + {0x1010, 0x0139}, // FINE_INTEGRATION_TIME4_MIN + + // TODO: do these have to be lower than LINE_LENGTH_PCK? + {0x3014, 0x08CB}, // FINE_INTEGRATION_TIME_ + {0x321E, 0x0894}, // FINE_INTEGRATION_TIME2 + + {0x31D0, 0x0000}, // COMPANDING, no good in 10 bit? + {0x33DA, 0x0000}, // COMPANDING + {0x318E, 0x0200}, // PRE_HDR_GAIN_EN + + // DLO Settings + {0x3100, 0x4000}, // DLO_CONTROL0 + {0x3280, 0x0CCC}, // T1 G1 + {0x3282, 0x0CCC}, // T1 R + {0x3284, 0x0CCC}, // T1 B + {0x3286, 0x0CCC}, // T1 G2 + {0x3288, 0x0FA0}, // T2 G1 + {0x328A, 0x0FA0}, // T2 R + {0x328C, 0x0FA0}, // T2 B + {0x328E, 0x0FA0}, // T2 G2 + + // Initial Gains + {0x3022, 0x0001}, // GROUPED_PARAMETER_HOLD_ + {0x3366, 0xFF77}, // ANALOG_GAIN (1x) + + {0x3060, 0x3333}, // ANALOG_COLOR_GAIN + + {0x3362, 0x0000}, // DC GAIN + + {0x305A, 0x00F8}, // red gain + {0x3058, 0x0122}, // blue gain + {0x3056, 0x009A}, // g1 gain + {0x305C, 0x009A}, // g2 gain + + {0x3022, 0x0000}, // GROUPED_PARAMETER_HOLD_ + + // Initial Integration Time + {0x3012, 0x0005}, +}; diff --git a/system/camerad/sensors/sensor.h b/system/camerad/sensors/sensor.h index 1a647d1528..e10d8a493e 100644 --- a/system/camerad/sensors/sensor.h +++ b/system/camerad/sensors/sensor.h @@ -10,6 +10,7 @@ #include "media/cam_sensor.h" #include "cereal/gen/cpp/log.capnp.h" +#include "system/camerad/sensors/ar0231_registers.h" #include "system/camerad/sensors/ox03c10_registers.h" #include "system/camerad/sensors/os04c10_registers.h" @@ -87,6 +88,14 @@ class SensorInfo { }; }; +class AR0231 : public SensorInfo { +public: + AR0231(); + std::vector getExposureRegisters(int exposure_time, int new_exp_g, bool dc_gain_enabled) const override; + float getExposureScore(float desired_ev, int exp_t, int exp_g_idx, float exp_gain, int gain_idx) const override; + int getSlaveAddress(int port) const override; +}; + class OX03C10 : public SensorInfo { public: OX03C10(); diff --git a/system/hardware/base.h b/system/hardware/base.h index 3eded659ac..5088f5dfb0 100644 --- a/system/hardware/base.h +++ b/system/hardware/base.h @@ -22,6 +22,7 @@ class HardwareNone { } static void set_ir_power(int percentage) {} + static void config_cpu_rendering(bool) {} static bool PC() { return false; } static bool TICI() { return false; } diff --git a/system/hardware/pc/hardware.h b/system/hardware/pc/hardware.h index 71f58b188b..4a0b200a50 100644 --- a/system/hardware/pc/hardware.h +++ b/system/hardware/pc/hardware.h @@ -11,4 +11,12 @@ class HardwarePC : public HardwareNone { static bool PC() { return true; } static bool TICI() { return util::getenv("TICI", 0) == 1; } static bool AGNOS() { return util::getenv("TICI", 0) == 1; } + + static void config_cpu_rendering(bool offscreen) { + if (offscreen) { + setenv("QT_QPA_PLATFORM", "offscreen", 1); + } + setenv("__GLX_VENDOR_LIBRARY_NAME", "mesa", 1); + setenv("LP_NUM_THREADS", "0", 1); + } }; diff --git a/system/hardware/tici/agnos-tici.json b/system/hardware/tici/agnos-tici.json new file mode 100644 index 0000000000..c0cce92e9e --- /dev/null +++ b/system/hardware/tici/agnos-tici.json @@ -0,0 +1,84 @@ +[ + { + "name": "xbl", + "url": "https://commadist.azureedge.net/agnosupdate/xbl-6710967ca9701f205d7ab19c3a9b0dd2f547e65b3d96048b7c2b03755aafa0f1.img.xz", + "hash": "6710967ca9701f205d7ab19c3a9b0dd2f547e65b3d96048b7c2b03755aafa0f1", + "hash_raw": "6710967ca9701f205d7ab19c3a9b0dd2f547e65b3d96048b7c2b03755aafa0f1", + "size": 3282256, + "sparse": false, + "full_check": true, + "has_ab": true, + "ondevice_hash": "003a17ab1be68a696f7efe4c9938e8be511d4aacfc2f3211fc896bdc1681d089" + }, + { + "name": "xbl_config", + "url": "https://commadist.azureedge.net/agnosupdate/xbl_config-63922cfbfdf4ab87986c4ba8f3a4df5bf28414b3f71a29ec5947336722215535.img.xz", + "hash": "63922cfbfdf4ab87986c4ba8f3a4df5bf28414b3f71a29ec5947336722215535", + "hash_raw": "63922cfbfdf4ab87986c4ba8f3a4df5bf28414b3f71a29ec5947336722215535", + "size": 98124, + "sparse": false, + "full_check": true, + "has_ab": true, + "ondevice_hash": "2a855dd636cc94718b64bea83a44d0a31741ecaa8f72a63613ff348ec7404091" + }, + { + "name": "abl", + "url": "https://commadist.azureedge.net/agnosupdate/abl-32a2174b5f764e95dfc54cf358ba01752943b1b3b90e626149c3da7d5f1830b6.img.xz", + "hash": "32a2174b5f764e95dfc54cf358ba01752943b1b3b90e626149c3da7d5f1830b6", + "hash_raw": "32a2174b5f764e95dfc54cf358ba01752943b1b3b90e626149c3da7d5f1830b6", + "size": 274432, + "sparse": false, + "full_check": true, + "has_ab": true, + "ondevice_hash": "32a2174b5f764e95dfc54cf358ba01752943b1b3b90e626149c3da7d5f1830b6" + }, + { + "name": "aop", + "url": "https://commadist.azureedge.net/agnosupdate/aop-21370172e590bd4ea907a558bcd6df20dc7a6c7d38b8e62fdde18f4a512ba9e9.img.xz", + "hash": "21370172e590bd4ea907a558bcd6df20dc7a6c7d38b8e62fdde18f4a512ba9e9", + "hash_raw": "21370172e590bd4ea907a558bcd6df20dc7a6c7d38b8e62fdde18f4a512ba9e9", + "size": 184364, + "sparse": false, + "full_check": true, + "has_ab": true, + "ondevice_hash": "c1be2f4aac5b3af49b904b027faec418d05efd7bd5144eb4fdfcba602bcf2180" + }, + { + "name": "devcfg", + "url": "https://commadist.azureedge.net/agnosupdate/devcfg-d7d7e52963bbedbbf8a7e66847579ca106a0a729ce2cf60f4b8d8ea4b535d620.img.xz", + "hash": "d7d7e52963bbedbbf8a7e66847579ca106a0a729ce2cf60f4b8d8ea4b535d620", + "hash_raw": "d7d7e52963bbedbbf8a7e66847579ca106a0a729ce2cf60f4b8d8ea4b535d620", + "size": 40336, + "sparse": false, + "full_check": true, + "has_ab": true, + "ondevice_hash": "17b229668b20305ff8fa3cd5f94716a3aaa1e5bf9d1c24117eff7f2f81ae719f" + }, + { + "name": "boot", + "url": "https://agnosupdate.carrotpilot.app/18.4/boot-a227ab7a6ec9f1f5b47c2d003ec75b7d54672e80b867843ee0b87eb735c775df.img.xz", + "hash": "a227ab7a6ec9f1f5b47c2d003ec75b7d54672e80b867843ee0b87eb735c775df", + "hash_raw": "a227ab7a6ec9f1f5b47c2d003ec75b7d54672e80b867843ee0b87eb735c775df", + "size": 17864704, + "sparse": false, + "full_check": true, + "has_ab": true, + "ondevice_hash": "e4dac469c76307917b24d72cf42723c584ef38532d15e99f2293eb44e09e8193" + }, + { + "name": "system", + "url": "https://agnosupdate.carrotpilot.app/18.4/system-edda90aeecb40a620db657521b1c38dfac77e610b06700671fca9fca82278b8d.img.xz", + "hash": "77d8fbf70aae7e6d8ce6acc6a28cd862c155a7b1bf940064ee335b90a5ee91c1", + "hash_raw": "edda90aeecb40a620db657521b1c38dfac77e610b06700671fca9fca82278b8d", + "size": 4718592000, + "sparse": true, + "full_check": false, + "has_ab": true, + "ondevice_hash": "92e8c1d099ceff3a46fe3299c74765013a56c873ce695d9c54c660fc20bbc898", + "alt": { + "hash": "edda90aeecb40a620db657521b1c38dfac77e610b06700671fca9fca82278b8d", + "url": "https://agnosupdate.carrotpilot.app/18.4/system-edda90aeecb40a620db657521b1c38dfac77e610b06700671fca9fca82278b8d.img", + "size": 4718592000 + } + } +] diff --git a/system/hardware/tici/agnos.json b/system/hardware/tici/agnos.json index 07e2079ec9..02c5fed23c 100644 --- a/system/hardware/tici/agnos.json +++ b/system/hardware/tici/agnos.json @@ -81,4 +81,4 @@ "size": 4718592000 } } -] \ No newline at end of file +] diff --git a/system/hardware/tici/amplifier.py b/system/hardware/tici/amplifier.py index 09436e6ff4..ae5508c36e 100755 --- a/system/hardware/tici/amplifier.py +++ b/system/hardware/tici/amplifier.py @@ -7,8 +7,23 @@ # https://datasheets.maximintegrated.com/en/ds/MAX98089.pdf AmpConfig = namedtuple('AmpConfig', ['name', 'value', 'register', 'offset', 'mask']) - -CONFIG = [ +EQParams = namedtuple('EQParams', ['K', 'k1', 'k2', 'c1', 'c2']) + +def configs_from_eq_params(base, eq_params): + return [ + AmpConfig("K (high)", (eq_params.K >> 8), base, 0, 0xFF), + AmpConfig("K (low)", (eq_params.K & 0xFF), base + 1, 0, 0xFF), + AmpConfig("k1 (high)", (eq_params.k1 >> 8), base + 2, 0, 0xFF), + AmpConfig("k1 (low)", (eq_params.k1 & 0xFF), base + 3, 0, 0xFF), + AmpConfig("k2 (high)", (eq_params.k2 >> 8), base + 4, 0, 0xFF), + AmpConfig("k2 (low)", (eq_params.k2 & 0xFF), base + 5, 0, 0xFF), + AmpConfig("c1 (high)", (eq_params.c1 >> 8), base + 6, 0, 0xFF), + AmpConfig("c1 (low)", (eq_params.c1 & 0xFF), base + 7, 0, 0xFF), + AmpConfig("c2 (high)", (eq_params.c2 >> 8), base + 8, 0, 0xFF), + AmpConfig("c2 (low)", (eq_params.c2 & 0xFF), base + 9, 0, 0xFF), + ] + +BASE_CONFIG = [ AmpConfig("MCLK prescaler", 0b01, 0x10, 4, 0b00110000), AmpConfig("PM: enable speakers", 0b11, 0x4D, 4, 0b00110000), AmpConfig("PM: enable DACs", 0b11, 0x4D, 0, 0b00000011), @@ -44,31 +59,47 @@ AmpConfig("Enhanced volume smoothing disabled", 0b0, 0x49, 7, 0b10000000), AmpConfig("Volume adjustment smoothing disabled", 0b0, 0x49, 6, 0b01000000), AmpConfig("Zero-crossing detection disabled", 0b0, 0x49, 5, 0b00100000), - - AmpConfig("Left speaker output from left DAC", 0b1, 0x2B, 0, 0b11111111), - AmpConfig("Right speaker output from right DAC", 0b1, 0x2C, 0, 0b11111111), - AmpConfig("Left Speaker Mixer Gain", 0b00, 0x2D, 0, 0b00000011), - AmpConfig("Right Speaker Mixer Gain", 0b00, 0x2D, 2, 0b00001100), - AmpConfig("Left speaker output volume", 0x17, 0x3D, 0, 0b00011111), - AmpConfig("Right speaker output volume", 0x17, 0x3E, 0, 0b00011111), - - AmpConfig("DAI2 EQ enable", 0b0, 0x49, 1, 0b00000010), - AmpConfig("DAI2: DC blocking", 0b0, 0x20, 0, 0b00000001), - AmpConfig("ALC enable", 0b0, 0x43, 7, 0b10000000), - AmpConfig("DAI2 EQ attenuation", 0x2, 0x32, 0, 0b00001111), - AmpConfig("Excursion limiter upper corner freq", 0b001, 0x41, 4, 0b01110000), - AmpConfig("Excursion limiter threshold", 0b100, 0x42, 0, 0b00001111), - AmpConfig("Distortion limit (THDCLP)", 0x0, 0x46, 4, 0b11110000), - AmpConfig("Distortion limiter release time constant", 0b1, 0x46, 0, 0b00000001), - AmpConfig("Left DAC input mixer: DAI1 left", 0b0, 0x22, 7, 0b10000000), - AmpConfig("Left DAC input mixer: DAI1 right", 0b0, 0x22, 6, 0b01000000), - AmpConfig("Left DAC input mixer: DAI2 left", 0b1, 0x22, 5, 0b00100000), - AmpConfig("Left DAC input mixer: DAI2 right", 0b0, 0x22, 4, 0b00010000), - AmpConfig("Right DAC input mixer: DAI2 left", 0b0, 0x22, 1, 0b00000010), - AmpConfig("Right DAC input mixer: DAI2 right", 0b1, 0x22, 0, 0b00000001), - AmpConfig("Volume adjustment smoothing disabled", 0b1, 0x49, 6, 0b01000000), ] +CONFIGS = { + "tici": [ + AmpConfig("Right speaker output from right DAC", 0b1, 0x2C, 0, 0b11111111), + AmpConfig("Right Speaker Mixer Gain", 0b00, 0x2D, 2, 0b00001100), + AmpConfig("Right speaker output volume", 0x1c, 0x3E, 0, 0b00011111), + AmpConfig("DAI2 EQ enable", 0b1, 0x49, 1, 0b00000010), + + *configs_from_eq_params(0x84, EQParams(0x274F, 0xC0FF, 0x3BF9, 0x0B3C, 0x1656)), + *configs_from_eq_params(0x8E, EQParams(0x1009, 0xC6BF, 0x2952, 0x1C97, 0x30DF)), + *configs_from_eq_params(0x98, EQParams(0x0F75, 0xCBE5, 0x0ED2, 0x2528, 0x3E42)), + *configs_from_eq_params(0xA2, EQParams(0x091F, 0x3D4C, 0xCE11, 0x1266, 0x2807)), + *configs_from_eq_params(0xAC, EQParams(0x0A9E, 0x3F20, 0xE573, 0x0A8B, 0x3A3B)), + ], + "tizi": [ + AmpConfig("Left speaker output from left DAC", 0b1, 0x2B, 0, 0b11111111), + AmpConfig("Right speaker output from right DAC", 0b1, 0x2C, 0, 0b11111111), + AmpConfig("Left Speaker Mixer Gain", 0b00, 0x2D, 0, 0b00000011), + AmpConfig("Right Speaker Mixer Gain", 0b00, 0x2D, 2, 0b00001100), + AmpConfig("Left speaker output volume", 0x17, 0x3D, 0, 0b00011111), + AmpConfig("Right speaker output volume", 0x17, 0x3E, 0, 0b00011111), + + AmpConfig("DAI2 EQ enable", 0b0, 0x49, 1, 0b00000010), + AmpConfig("DAI2: DC blocking", 0b0, 0x20, 0, 0b00000001), + AmpConfig("ALC enable", 0b0, 0x43, 7, 0b10000000), + AmpConfig("DAI2 EQ attenuation", 0x2, 0x32, 0, 0b00001111), + AmpConfig("Excursion limiter upper corner freq", 0b001, 0x41, 4, 0b01110000), + AmpConfig("Excursion limiter threshold", 0b100, 0x42, 0, 0b00001111), + AmpConfig("Distortion limit (THDCLP)", 0x0, 0x46, 4, 0b11110000), + AmpConfig("Distortion limiter release time constant", 0b1, 0x46, 0, 0b00000001), + AmpConfig("Left DAC input mixer: DAI1 left", 0b0, 0x22, 7, 0b10000000), + AmpConfig("Left DAC input mixer: DAI1 right", 0b0, 0x22, 6, 0b01000000), + AmpConfig("Left DAC input mixer: DAI2 left", 0b1, 0x22, 5, 0b00100000), + AmpConfig("Left DAC input mixer: DAI2 right", 0b0, 0x22, 4, 0b00010000), + AmpConfig("Right DAC input mixer: DAI2 left", 0b0, 0x22, 1, 0b00000010), + AmpConfig("Right DAC input mixer: DAI2 right", 0b1, 0x22, 0, 0b00000001), + AmpConfig("Volume adjustment smoothing disabled", 0b1, 0x49, 6, 0b01000000), + ], +} + class Amplifier: AMP_I2C_BUS = 0 AMP_ADDRESS = 0x10 @@ -109,15 +140,20 @@ def set_configs(self, configs: list[AmpConfig]) -> bool: def set_global_shutdown(self, amp_disabled: bool) -> bool: return self.set_configs([self._get_shutdown_config(amp_disabled), ]) - def initialize_configuration(self) -> bool: + def initialize_configuration(self, model: str) -> bool: cfgs = [ self._get_shutdown_config(True), - *CONFIG, + *BASE_CONFIG, + *CONFIGS[model], self._get_shutdown_config(False), ] return self.set_configs(cfgs) if __name__ == "__main__": + with open("/sys/firmware/devicetree/base/model") as f: + model = f.read().strip('\x00') + model = model.split('comma ')[-1] + amp = Amplifier() - amp.initialize_configuration() + amp.initialize_configuration(model) diff --git a/system/hardware/tici/hardware.h b/system/hardware/tici/hardware.h index a06ce43b35..8e318b8b8e 100644 --- a/system/hardware/tici/hardware.h +++ b/system/hardware/tici/hardware.h @@ -90,4 +90,11 @@ class HardwareTici : public HardwareNone { return ret; } + + static void config_cpu_rendering(bool offscreen) { + if (offscreen) { + setenv("QT_QPA_PLATFORM", "eglfs", 1); + } + setenv("LP_NUM_THREADS", "0", 1); + } }; diff --git a/system/hardware/tici/hardware.py b/system/hardware/tici/hardware.py index 10b6d7646c..839ca1a028 100644 --- a/system/hardware/tici/hardware.py +++ b/system/hardware/tici/hardware.py @@ -136,11 +136,12 @@ def get_network_type(self): def get_sim_info(self): ms = self.get_modem_state() sim_id = ms.get('iccid', '') + sim_state = ms.get('sim_state') or ("READY" if sim_id else "ABSENT") return { 'sim_id': sim_id, 'mcc_mnc': ms.get('mcc_mnc') or None, 'network_type': ["Unknown"], - 'sim_state': ["ABSENT"] if not sim_id else ["READY"], + 'sim_state': [sim_state], 'data_connected': ms.get('connected', False), } @@ -299,7 +300,7 @@ def set_power_save(self, powersave_enabled): if self.amplifier is not None: self.amplifier.set_global_shutdown(amp_disabled=powersave_enabled) if not powersave_enabled: - self.amplifier.initialize_configuration() + self.amplifier.initialize_configuration(self.get_device_type()) # *** CPU config *** @@ -337,7 +338,7 @@ def get_gpu_usage_percent(self): def initialize_hardware(self): if self.amplifier is not None: - self.amplifier.initialize_configuration() + self.amplifier.initialize_configuration(self.get_device_type()) # Allow hardwared to write engagement status to kmsg os.system("sudo chmod a+w /dev/kmsg") diff --git a/system/hardware/tici/modem.py b/system/hardware/tici/modem.py index 39266be122..9eac96144f 100644 --- a/system/hardware/tici/modem.py +++ b/system/hardware/tici/modem.py @@ -56,6 +56,7 @@ "state": "INITIALIZING", "connected": False, "ip_address": "", "iccid": "", "mcc_mnc": "", "imei": "", "modem_version": "", + "sim_state": "ABSENT", "signal_strength": 0, "signal_quality": 0, "network_type": "unknown", "operator": "", "band": "", "channel": 0, "registration": "unknown", "temperatures": [], "extra": "", @@ -72,6 +73,7 @@ class State(Enum): STATE_WAIT = 1.0 # seconds to wait after each state handler returns +ICCID_CHECK_INTERVAL = 60.0 class PPPSession: @@ -176,6 +178,7 @@ def __init__(self): self._sim_change = False self._apn = "" # blank = network-provided via PCO self._roaming_allowed = True + self._last_iccid_check = 0.0 self.running = True self.S = INITIAL_STATE.copy() @@ -211,7 +214,7 @@ def _publish_state(self, **kwargs): os.chmod(f.name, 0o644) os.replace(f.name, STATE_PATH) - def _at(self, cmd): + def _at(self, cmd, log_errors=True): """Send AT command, return response lines. [] on error or if LPA holds port.""" fd = os.open(AT_LOCK, os.O_CREAT | os.O_RDWR, 0o666) try: @@ -238,14 +241,15 @@ def _at(self, cmd): lines.append(line) return lines except (RuntimeError, TimeoutError, OSError) as e: - logging.info(f"AT {cmd} failed: {e}") + if log_errors: + logging.info(f"AT {cmd} failed: {e}") return [] finally: fcntl.flock(fd, fcntl.LOCK_UN) os.close(fd) - def _atv(self, cmd, pfx): - for line in self._at(cmd): + def _atv(self, cmd, pfx, log_errors=True): + for line in self._at(cmd, log_errors=log_errors): if pfx in line and ":" in line: return line.split(":", 1)[1].strip() return None @@ -258,7 +262,7 @@ def _init_at_channel(self) -> bool: return bool(r) and not r[0].startswith("AT") def _configure_modem(self, modem_version: str): - if not modem_version.startswith("EG25"): + if not modem_version.startswith(("EC20", "EG25")): return cmds = [ # clear initial EPS bearer APN (some carriers reject the default) @@ -288,7 +292,7 @@ def _do_initializing(self): return State.INITIALIZING identity = self._read_identity() - if not identity["iccid"] or not identity["imei"]: + if not identity["imei"]: logging.warning(f"identity read incomplete: {identity}, retrying") return State.INITIALIZING @@ -300,33 +304,39 @@ def _do_initializing(self): # blank APN lets the carrier supply one via PCO self._at(f'AT+CGDCONT={DIAL_CID},"IP","{self._apn}"') logging.info(f"APN '{self._apn or '(network-provided)'}' written to CID {DIAL_CID}, roaming={'on' if self._roaming_allowed else 'off'}") + if identity["sim_state"] == "ABSENT": + logging.info("SIM absent") self._sim_change = False # clear since we just re-read identity with the new SIM self._publish_state(**identity) return State.SEARCHING def _read_identity(self): - def first_line(cmd): - r = self._at(cmd) + def first_line(cmd, log_errors=True): + r = self._at(cmd, log_errors=log_errors) return r[0].strip() if r else "" imei = first_line("AT+CGSN") if not (imei.isdigit() and 14 <= len(imei) <= 17): # 3GPP TS 23.003 imei = "" - iccid = (self._atv("AT+QCCID", "+QCCID:") or "").rstrip("F") + iccid = (self._atv("AT+QCCID", "+QCCID:", log_errors=False) or "").rstrip("F") if not iccid.isdigit(): iccid = "" - imsi = first_line("AT+CIMI") + imsi = first_line("AT+CIMI", log_errors=False) mcc_mnc = imsi[:6] if imsi.isdigit() and len(imsi) >= 6 else "" + sim_state = "READY" if iccid or mcc_mnc else "ABSENT" modem_version = first_line("AT+GMR") - logging.info(f"imei={imei} iccid={iccid} mcc_mnc={mcc_mnc} ver={modem_version}") - return {"imei": imei, "iccid": iccid, "mcc_mnc": mcc_mnc, "modem_version": modem_version} + logging.info(f"imei={imei} iccid={iccid} mcc_mnc={mcc_mnc} sim_state={sim_state} ver={modem_version}") + return {"imei": imei, "iccid": iccid, "mcc_mnc": mcc_mnc, "modem_version": modem_version, "sim_state": sim_state} def _do_searching(self): + if self.S["sim_state"] == "ABSENT": + return self._searching_idle() + new_roaming = self._is_roaming_allowed() if new_roaming != self._roaming_allowed: logging.info(f"roaming changed: {self._roaming_allowed} -> {new_roaming}") @@ -391,9 +401,14 @@ def _params_changed(self) -> bool: return False def _check_iccid(self, state): - if state in (State.INITIALIZING, State.DISCONNECTING) or not self.S["iccid"]: + if state in (State.INITIALIZING, State.DISCONNECTING): return - iccid = (self._atv("AT+QCCID", "+QCCID:") or "").rstrip("F") + now = time.monotonic() + if now - self._last_iccid_check < ICCID_CHECK_INTERVAL: + return + self._last_iccid_check = now + + iccid = (self._atv("AT+QCCID", "+QCCID:", log_errors=False) or "").rstrip("F") if iccid and iccid != self.S["iccid"]: logging.warning(f"iccid changed: {self.S['iccid']} -> {iccid}") self._sim_change = True diff --git a/system/hardware/tici/tests/test_amplifier.py b/system/hardware/tici/tests/test_amplifier.py index 9ce00c3ff2..3f75436db1 100644 --- a/system/hardware/tici/tests/test_amplifier.py +++ b/system/hardware/tici/tests/test_amplifier.py @@ -5,6 +5,7 @@ from panda import Panda from openpilot.system.hardware import TICI, HARDWARE +from openpilot.system.hardware.tici.hardware import Tici from openpilot.system.hardware.tici.amplifier import Amplifier @@ -38,7 +39,7 @@ def _check_for_i2c_errors(self, expected): def test_init(self): amp = Amplifier(debug=True) - r = amp.initialize_configuration() + r = amp.initialize_configuration(Tici().get_device_type()) assert r assert self._check_for_i2c_errors(False) @@ -60,7 +61,7 @@ def test_init_while_siren_play(self): time.sleep(random.randint(0, 5)) amp = Amplifier(debug=True) - r = amp.initialize_configuration() + r = amp.initialize_configuration(Tici().get_device_type()) assert r if self._check_for_i2c_errors(True): diff --git a/system/ui/lib/application.py b/system/ui/lib/application.py index 70ce9c16fe..3671460452 100644 --- a/system/ui/lib/application.py +++ b/system/ui/lib/application.py @@ -6,6 +6,7 @@ import time import signal import sys +import struct import pyray as rl import threading import platform @@ -33,6 +34,15 @@ MAX_TOUCH_SLOTS = 2 TOUCH_HISTORY_TIMEOUT = 3.0 # Seconds before touch points fade out +TOUCH_EVENT_DEVICE = "/dev/input/by-path/platform-894000.i2c-event" +EVENT_FORMAT = "llHHi" +EVENT_SIZE = struct.calcsize(EVENT_FORMAT) +EV_SYN, EV_KEY, EV_ABS = 0x00, 0x01, 0x03 +SYN_REPORT = 0x00 +BTN_TOUCH = 0x14a +ABS_MT_SLOT = 0x2f +ABS_MT_TRACKING_ID = 0x39 + BIG_UI = os.getenv("BIG", "0") == "1" ENABLE_VSYNC = os.getenv("ENABLE_VSYNC", "0") == "1" SHOW_FPS = os.getenv("SHOW_FPS") == "1" @@ -146,6 +156,10 @@ def __init__(self, scale: float = 1.0): self._events: deque[MouseEvent] = deque(maxlen=MOUSE_THREAD_RATE) # bound event list self._prev_mouse_event: list[MouseEvent | None] = [None] * MAX_TOUCH_SLOTS + self._slot_active: list[bool] = [False] * MAX_TOUCH_SLOTS + self._cur_slot = 0 + self._saw_mt = False + self._rk = Ratekeeper(MOUSE_THREAD_RATE, print_delay_threshold=None) self._lock = threading.Lock() self._exit_event = threading.Event() @@ -169,34 +183,73 @@ def stop(self): self._thread.join() def _run_thread(self): - while not self._exit_event.is_set(): - rl.poll_input_events() - self._handle_mouse_event() - self._rk.keep_time() + touch_fd = self._open_touch_device() + try: + while not self._exit_event.is_set(): + rl.poll_input_events() + if touch_fd is not None: + self._read_touch_events(touch_fd) + else: + self._handle_mouse_event() + self._rk.keep_time() + finally: + if touch_fd is not None: + os.close(touch_fd) + + def _open_touch_device(self) -> int | None: + if PC: + return None + try: + return os.open(TOUCH_EVENT_DEVICE, os.O_RDONLY | os.O_NONBLOCK) + except OSError as e: + cloudlog.warning(f"mouse: using raylib touch, can't open {TOUCH_EVENT_DEVICE}: {e}") + return None + + def _read_touch_events(self, fd: int) -> None: + try: + data = os.read(fd, EVENT_SIZE * 64) + except (BlockingIOError, OSError): + return + + for off in range(0, len(data) - EVENT_SIZE + 1, EVENT_SIZE): + _sec, _usec, etype, code, value = struct.unpack(EVENT_FORMAT, data[off:off + EVENT_SIZE]) + if etype == EV_ABS: + if code == ABS_MT_SLOT: + self._cur_slot = value + elif code == ABS_MT_TRACKING_ID: + self._saw_mt = True + if 0 <= self._cur_slot < MAX_TOUCH_SLOTS: + self._slot_active[self._cur_slot] = value != -1 + elif etype == EV_KEY and code == BTN_TOUCH and not self._saw_mt: + self._slot_active[0] = value != 0 + elif etype == EV_SYN and code == SYN_REPORT: + for slot in range(MAX_TOUCH_SLOTS): + self._append_event(slot, self._slot_active[slot]) def _handle_mouse_event(self): - # TODO: read touch events from evdev directly to get real kernel timestamps. - # Polling at 140Hz with time.monotonic() causes timing jitter that makes scroll - # velocity oscillate (alternating high/low). Real timestamps would also let us - # detect swipe-stop-lift via event gaps instead of the fragile decel heuristic. for slot in range(MAX_TOUCH_SLOTS): + self._append_event(slot, rl.is_mouse_button_down(slot)) + + def _append_event(self, slot: int, down: bool) -> None: + prev = self._prev_mouse_event[slot] + prev_down = prev.left_down if prev is not None else False + pressed = down and not prev_down + released = prev_down and not down + + if down: mouse_pos = rl.get_touch_position(slot) x = mouse_pos.x / self._scale if self._scale != 1.0 else mouse_pos.x y = mouse_pos.y / self._scale if self._scale != 1.0 else mouse_pos.y - ev = MouseEvent( - MousePos(x, y), - slot, - rl.is_mouse_button_pressed(slot), # noqa: TID251 - rl.is_mouse_button_released(slot), # noqa: TID251 - rl.is_mouse_button_down(slot), - time.monotonic(), - ) - # Only add changes - prev = self._prev_mouse_event[slot] - if prev is None or ev[:-1] != prev[:-1]: - with self._lock: - self._events.append(ev) - self._prev_mouse_event[slot] = ev + pos = MousePos(x, y) + else: + pos = prev.pos if prev is not None else MousePos(0.0, 0.0) + + ev = MouseEvent(pos, slot, pressed, released, down, time.monotonic()) + + if prev is None or ev[:-1] != prev[:-1]: + with self._lock: + self._events.append(ev) + self._prev_mouse_event[slot] = ev class GuiApplication: diff --git a/system/updated/updated.py b/system/updated/updated.py index e47131347f..ae8c8c0541 100755 --- a/system/updated/updated.py +++ b/system/updated/updated.py @@ -221,6 +221,17 @@ def handle_agnos_update() -> None: set_offroad_alert("Offroad_NeosUpdate", True) manifest_path = os.path.join(OVERLAY_MERGED, "system/hardware/tici/agnos.json") + try: + with open("/sys/firmware/devicetree/base/model") as f: + model = f.read().replace("\x00", "").strip().lower().removeprefix("comma ") + print(f"[agnos] device model: {model}", flush=True) + + if model in ("c3", "tici"): + manifest_path = os.path.join(OVERLAY_MERGED, "system/hardware/tici/agnos-tici.json") + print(f"[agnos] manifest_path: {manifest_path}", flush=True) + except OSError as e: + print(f"[agnos] model read failed: {e}", flush=True) + pass target_slot_number = get_target_slot_number() flash_agnos_update(manifest_path, target_slot_number, cloudlog) set_offroad_alert("Offroad_NeosUpdate", False)