diff --git a/release/files_common b/release/files_common index 7257fb820d..f8b424683a 100644 --- a/release/files_common +++ b/release/files_common @@ -319,6 +319,7 @@ system/camerad/snapshot/* system/camerad/cameras/camera_common.h system/camerad/cameras/camera_common.cc system/camerad/cameras/sensor2_i2c.h +system/camerad/sensors/ar0231.cc selfdrive/manager/__init__.py selfdrive/manager/build.py diff --git a/system/camerad/SConscript b/system/camerad/SConscript index 83f93344d8..d814ebf1b0 100644 --- a/system/camerad/SConscript +++ b/system/camerad/SConscript @@ -2,7 +2,8 @@ Import('env', 'arch', 'cereal', 'messaging', 'common', 'gpucommon', 'visionipc') libs = ['m', 'pthread', common, 'jpeg', 'OpenCL', 'yuv', cereal, messaging, 'zmq', 'capnp', 'kj', visionipc, gpucommon, 'atomic'] -camera_obj = env.Object(['cameras/camera_qcom2.cc', 'cameras/camera_common.cc', 'cameras/camera_util.cc']) +camera_obj = env.Object(['cameras/camera_qcom2.cc', 'cameras/camera_common.cc', 'cameras/camera_util.cc', + 'sensors/ar0231.cc']) env.Program('camerad', ['main.cc', camera_obj], LIBS=libs) if GetOption("extras") and arch == "x86_64": diff --git a/system/camerad/cameras/camera_common.h b/system/camerad/cameras/camera_common.h index 23bfc8b7d5..9d9e42a5ed 100644 --- a/system/camerad/cameras/camera_common.h +++ b/system/camerad/cameras/camera_common.h @@ -103,3 +103,6 @@ void cameras_close(MultiCameraState *s); void camerad_thread(); int open_v4l_by_name_and_index(const char name[], int index = 0, int flags = O_RDWR | O_NONBLOCK); + +void ar0231_process_registers(MultiCameraState *s, CameraState *c, cereal::FrameData::Builder &framed); + diff --git a/system/camerad/cameras/camera_qcom2.cc b/system/camerad/cameras/camera_qcom2.cc index 8d20ed27c8..13fd60f4d2 100644 --- a/system/camerad/cameras/camera_qcom2.cc +++ b/system/camerad/cameras/camera_qcom2.cc @@ -907,70 +907,6 @@ void cameras_close(MultiCameraState *s) { delete s->pm; } -std::map> CameraState::ar0231_build_register_lut(uint8_t *data) { - // This function builds a lookup table from register address, to a pair of indices in the - // buffer where to read this address. The buffer contains padding bytes, - // as well as markers to indicate the type of the next byte. - // - // 0xAA is used to indicate the MSB of the address, 0xA5 for the LSB of the address. - // Every byte of data (MSB and LSB) is preceded by 0x5A. Specifying an address is optional - // for contiguous ranges. See page 27-29 of the AR0231 Developer guide for more information. - - int max_i[] = {1828 / 2 * 3, 1500 / 2 * 3}; - auto get_next_idx = [](int cur_idx) { - return (cur_idx % 3 == 1) ? cur_idx + 2 : cur_idx + 1; // Every third byte is padding - }; - - std::map> registers; - for (int register_row = 0; register_row < 2; register_row++) { - uint8_t *registers_raw = data + ci.frame_stride * register_row; - assert(registers_raw[0] == 0x0a); // Start of line - - int value_tag_count = 0; - int first_val_idx = 0; - uint16_t cur_addr = 0; - - for (int i = 1; i <= max_i[register_row]; i = get_next_idx(get_next_idx(i))) { - int val_idx = get_next_idx(i); - - uint8_t tag = registers_raw[i]; - uint16_t val = registers_raw[val_idx]; - - if (tag == 0xAA) { // Register MSB tag - cur_addr = val << 8; - } else if (tag == 0xA5) { // Register LSB tag - cur_addr |= val; - cur_addr -= 2; // Next value tag will increment address again - } else if (tag == 0x5A) { // Value tag - - // First tag - if (value_tag_count % 2 == 0) { - cur_addr += 2; - first_val_idx = val_idx; - } else { - registers[cur_addr] = std::make_pair(first_val_idx + ci.frame_stride * register_row, val_idx + ci.frame_stride * register_row); - } - - value_tag_count++; - } - } - } - return registers; -} - -std::map CameraState::ar0231_parse_registers(uint8_t *data, std::initializer_list addrs) { - if (ar0231_register_lut.empty()) { - ar0231_register_lut = ar0231_build_register_lut(data); - } - - std::map registers; - for (uint16_t addr : addrs) { - auto offset = ar0231_register_lut[addr]; - registers[addr] = ((uint16_t)data[offset.first] << 8) | data[offset.second]; - } - return registers; -} - void CameraState::handle_camera_event(void *evdat) { if (!enabled) return; struct cam_req_mgr_message *event_data = (struct cam_req_mgr_message *)evdat; @@ -1180,32 +1116,6 @@ void CameraState::set_camera_exposure(float grey_frac) { } } -static float ar0231_parse_temp_sensor(uint16_t calib1, uint16_t calib2, uint16_t data_reg) { - // See AR0231 Developer Guide - page 36 - float slope = (125.0 - 55.0) / ((float)calib1 - (float)calib2); - float t0 = 55.0 - slope * (float)calib2; - return t0 + slope * (float)data_reg; -} - -static void ar0231_process_registers(MultiCameraState *s, CameraState *c, cereal::FrameData::Builder &framed){ - const uint8_t expected_preamble[] = {0x0a, 0xaa, 0x55, 0x20, 0xa5, 0x55}; - uint8_t *data = (uint8_t*)c->buf.cur_camera_buf->addr + c->ci.registers_offset; - - if (memcmp(data, expected_preamble, std::size(expected_preamble)) != 0){ - LOGE("unexpected register data found"); - return; - } - - auto registers = c->ar0231_parse_registers(data, {0x2000, 0x2002, 0x20b0, 0x20b2, 0x30c6, 0x30c8, 0x30ca, 0x30cc}); - - uint32_t frame_id = ((uint32_t)registers[0x2000] << 16) | registers[0x2002]; - framed.setFrameIdSensor(frame_id); - - float temp_0 = ar0231_parse_temp_sensor(registers[0x30c6], registers[0x30c8], registers[0x20b0]); - float temp_1 = ar0231_parse_temp_sensor(registers[0x30ca], registers[0x30cc], registers[0x20b2]); - framed.setTemperaturesC({temp_0, temp_1}); -} - static void process_driver_camera(MultiCameraState *s, CameraState *c, int cnt) { c->set_camera_exposure(set_exposure_target(&c->buf, 96, 1832, 2, 242, 1148, 4)); diff --git a/system/camerad/cameras/camera_qcom2.h b/system/camerad/cameras/camera_qcom2.h index 1e7cbfc104..87cdd091f6 100644 --- a/system/camerad/cameras/camera_qcom2.h +++ b/system/camerad/cameras/camera_qcom2.h @@ -94,7 +94,6 @@ public: CameraBuf buf; MemoryManager mm; -private: void config_isp(int io_mem_handle, int fence, int request_id, int buf0_mem_handle, int buf0_offset); void enqueue_req_multi(int start, int n, bool dp); void enqueue_buffer(int i, bool dp); @@ -106,8 +105,8 @@ private: // Register parsing std::map> ar0231_register_lut; - std::map> ar0231_build_register_lut(uint8_t *data); +private: // for debugging Params params; }; diff --git a/system/camerad/sensors/ar0231.cc b/system/camerad/sensors/ar0231.cc new file mode 100644 index 0000000000..7d7a454f7e --- /dev/null +++ b/system/camerad/sensors/ar0231.cc @@ -0,0 +1,98 @@ +#include + +#include "system/camerad/cameras/camera_common.h" +#include "system/camerad/cameras/camera_qcom2.h" + +namespace { + +std::map> ar0231_build_register_lut(CameraState *c, uint8_t *data) { + // This function builds a lookup table from register address, to a pair of indices in the + // buffer where to read this address. The buffer contains padding bytes, + // as well as markers to indicate the type of the next byte. + // + // 0xAA is used to indicate the MSB of the address, 0xA5 for the LSB of the address. + // Every byte of data (MSB and LSB) is preceded by 0x5A. Specifying an address is optional + // for contiguous ranges. See page 27-29 of the AR0231 Developer guide for more information. + + int max_i[] = {1828 / 2 * 3, 1500 / 2 * 3}; + auto get_next_idx = [](int cur_idx) { + return (cur_idx % 3 == 1) ? cur_idx + 2 : cur_idx + 1; // Every third byte is padding + }; + + std::map> registers; + for (int register_row = 0; register_row < 2; register_row++) { + uint8_t *registers_raw = data + c->ci.frame_stride * register_row; + assert(registers_raw[0] == 0x0a); // Start of line + + int value_tag_count = 0; + int first_val_idx = 0; + uint16_t cur_addr = 0; + + for (int i = 1; i <= max_i[register_row]; i = get_next_idx(get_next_idx(i))) { + int val_idx = get_next_idx(i); + + uint8_t tag = registers_raw[i]; + uint16_t val = registers_raw[val_idx]; + + if (tag == 0xAA) { // Register MSB tag + cur_addr = val << 8; + } else if (tag == 0xA5) { // Register LSB tag + cur_addr |= val; + cur_addr -= 2; // Next value tag will increment address again + } else if (tag == 0x5A) { // Value tag + + // First tag + if (value_tag_count % 2 == 0) { + cur_addr += 2; + first_val_idx = val_idx; + } else { + registers[cur_addr] = std::make_pair(first_val_idx + c->ci.frame_stride * register_row, val_idx + c->ci.frame_stride * register_row); + } + + value_tag_count++; + } + } + } + return registers; +} + +std::map ar0231_parse_registers(CameraState *c, uint8_t *data, std::initializer_list addrs) { + if (c->ar0231_register_lut.empty()) { + c->ar0231_register_lut = ar0231_build_register_lut(c, data); + } + + std::map registers; + for (uint16_t addr : addrs) { + auto offset = c->ar0231_register_lut[addr]; + registers[addr] = ((uint16_t)data[offset.first] << 8) | data[offset.second]; + } + return registers; +} + +float ar0231_parse_temp_sensor(uint16_t calib1, uint16_t calib2, uint16_t data_reg) { + // See AR0231 Developer Guide - page 36 + float slope = (125.0 - 55.0) / ((float)calib1 - (float)calib2); + float t0 = 55.0 - slope * (float)calib2; + return t0 + slope * (float)data_reg; +} + +} // namespace + +void ar0231_process_registers(MultiCameraState *s, CameraState *c, cereal::FrameData::Builder &framed) { + const uint8_t expected_preamble[] = {0x0a, 0xaa, 0x55, 0x20, 0xa5, 0x55}; + uint8_t *data = (uint8_t *)c->buf.cur_camera_buf->addr + c->ci.registers_offset; + + if (memcmp(data, expected_preamble, std::size(expected_preamble)) != 0) { + LOGE("unexpected register data found"); + return; + } + + auto registers = ar0231_parse_registers(c, data, {0x2000, 0x2002, 0x20b0, 0x20b2, 0x30c6, 0x30c8, 0x30ca, 0x30cc}); + + uint32_t frame_id = ((uint32_t)registers[0x2000] << 16) | registers[0x2002]; + framed.setFrameIdSensor(frame_id); + + float temp_0 = ar0231_parse_temp_sensor(registers[0x30c6], registers[0x30c8], registers[0x20b0]); + float temp_1 = ar0231_parse_temp_sensor(registers[0x30ca], registers[0x30cc], registers[0x20b2]); + framed.setTemperaturesC({temp_0, temp_1}); +}