From 4e0a408055751f43e95ee2acc370a87c04cb68bd Mon Sep 17 00:00:00 2001 From: iabdalkader Date: Tue, 5 Mar 2019 21:08:55 +0200 Subject: [PATCH] Add FLIR Lepton IOCTLs --- src/omv/lepton.c | 86 +++++++++++++++++++++++++++++++++++----- src/omv/py/py_sensor.c | 78 ++++++++++++++++++++++++++++++++++-- src/omv/py/qstrdefsomv.h | 8 ++++ src/omv/sensor.h | 8 ++++ 4 files changed, 168 insertions(+), 12 deletions(-) diff --git a/src/omv/lepton.c b/src/omv/lepton.c index 23b571a6f..19f29ab93 100644 --- a/src/omv/lepton.c +++ b/src/omv/lepton.c @@ -47,7 +47,7 @@ static bool v_flip = false; static SPI_HandleTypeDef SPIHandle; static DMA_HandleTypeDef DMAHandle; - +LEP_CAMERA_PORT_DESC_T LEPHandle; extern uint8_t _line_buf; extern uint8_t _vospi_buf; @@ -219,6 +219,73 @@ static int set_lens_correction(sensor_t *sensor, int enable, int radi, int coef) return 0; } +static int ioctl(sensor_t *sensor, int request, va_list ap) +{ + int ret = 0; + + if ((!h_res) || (!v_res)) { + return -1; + } + + switch (request) { + case IOCTL_LEPTON_GET_TYPE: { + int *type = va_arg(ap, int *); + *type = (h_res == 80) ? 1 : 3; + break; + } + case IOCTL_LEPTON_GET_REFRESH: { + int *refresh = va_arg(ap, int *); + *refresh = (h_res == 80) ? 27 : 9; + break; + } + case IOCTL_LEPTON_GET_RESOLUTION: { + int *resolution = va_arg(ap, int *); + *resolution = 14; + break; + } + case IOCTL_LEPTON_RUN_COMMAND: { + int command = va_arg(ap, int); + ret = (LEP_RunCommand(&LEPHandle, command) == LEP_OK) ? 0 : -1; + break; + } + case IOCTL_LEPTON_SET_ATTRIBUTE: { + int command = va_arg(ap, int); + uint16_t *data = va_arg(ap, uint16_t *); + size_t data_len = va_arg(ap, size_t); + ret = (LEP_SetAttribute(&LEPHandle, command, (LEP_ATTRIBUTE_T_PTR) data, data_len) == LEP_OK) ? 0 : -1; + break; + } + case IOCTL_LEPTON_GET_ATTRIBUTE: { + int command = va_arg(ap, int); + uint16_t *data = va_arg(ap, uint16_t *); + size_t data_len = va_arg(ap, size_t); + ret = (LEP_GetAttribute(&LEPHandle, command, (LEP_ATTRIBUTE_T_PTR) data, data_len) == LEP_OK) ? 0 : -1; + break; + } + case IOCTL_LEPTON_GET_FPA_TEMPERATURE: { + int *temp = va_arg(ap, int *); + LEP_SYS_FPA_TEMPERATURE_KELVIN_T tfpa; + ret = (LEP_GetSysFpaTemperatureKelvin(&LEPHandle, &tfpa) == LEP_OK) ? 0 : -1; + *temp = tfpa; + break; + } + case IOCTL_LEPTON_GET_AUX_TEMPERATURE: { + int *temp = va_arg(ap, int *); + LEP_SYS_AUX_TEMPERATURE_KELVIN_T taux; + ret = (LEP_GetSysAuxTemperatureKelvin(&LEPHandle, &taux) == LEP_OK) ? 0 : -1; + *temp = taux; + break; + } + default: { + ret = -1; + break; + } + } + + return ret; +} + + static int reset(sensor_t *sensor) { DCMI_PWDN_LOW(); @@ -234,11 +301,11 @@ static int reset(sensor_t *sensor) systick_sleep(1000); LEP_AGC_ROI_T roi; - LEP_CAMERA_PORT_DESC_T handle = {0}; h_res = v_res = h_mirror = v_flip = 0; + memset(&LEPHandle, 0, sizeof(LEP_CAMERA_PORT_DESC_T)); for (uint32_t start = HAL_GetTick(); ;systick_sleep(1)) { - if (LEP_OpenPort(0, LEP_CCI_TWI, 0, &handle) == LEP_OK) { + if (LEP_OpenPort(0, LEP_CCI_TWI, 0, &LEPHandle) == LEP_OK) { break; } if (HAL_GetTick() - start >= LEPTON_TIMEOUT) { @@ -248,7 +315,7 @@ static int reset(sensor_t *sensor) for (uint32_t start = HAL_GetTick(); ;systick_sleep(1)) { LEP_SDK_BOOT_STATUS_E status; - if (LEP_GetCameraBootStatus(&handle, &status) != LEP_OK) { + if (LEP_GetCameraBootStatus(&LEPHandle, &status) != LEP_OK) { return -1; } if (status == LEP_BOOT_STATUS_BOOTED) { @@ -261,7 +328,7 @@ static int reset(sensor_t *sensor) for (uint32_t start = HAL_GetTick(); ;systick_sleep(1)) { LEP_UINT16 status; - if (LEP_DirectReadRegister(&handle, LEP_I2C_STATUS_REG, &status) != LEP_OK) { + if (LEP_DirectReadRegister(&LEPHandle, LEP_I2C_STATUS_REG, &status) != LEP_OK) { return -1; } if (!(status & LEP_I2C_STATUS_BUSY_BIT_MASK)) { @@ -272,10 +339,10 @@ static int reset(sensor_t *sensor) } } - if (LEP_SetRadEnableState(&handle, LEP_RAD_DISABLE) != LEP_OK - || LEP_GetAgcROI(&handle, &roi) != LEP_OK - || LEP_SetAgcEnableState(&handle, LEP_AGC_ENABLE) != LEP_OK - || LEP_SetAgcCalcEnableState(&handle, LEP_AGC_ENABLE) != LEP_OK) { + if (LEP_SetRadEnableState(&LEPHandle, LEP_RAD_DISABLE) != LEP_OK + || LEP_GetAgcROI(&LEPHandle, &roi) != LEP_OK + || LEP_SetAgcEnableState(&LEPHandle, LEP_AGC_ENABLE) != LEP_OK + || LEP_SetAgcCalcEnableState(&LEPHandle, LEP_AGC_ENABLE) != LEP_OK) { return -1; } @@ -468,6 +535,7 @@ int lepton_init(sensor_t *sensor) sensor->set_hmirror = set_hmirror; sensor->set_vflip = set_vflip; sensor->set_lens_correction = set_lens_correction; + sensor->ioctl = ioctl; SENSOR_HW_FLAGS_SET(sensor, SENSOR_HW_FLAGS_VSYNC, 1); SENSOR_HW_FLAGS_SET(sensor, SENSOR_HW_FLAGS_HSYNC, 0); diff --git a/src/omv/py/py_sensor.c b/src/omv/py/py_sensor.c index ce0a3727e..1fa78ec1a 100644 --- a/src/omv/py/py_sensor.c +++ b/src/omv/py/py_sensor.c @@ -409,7 +409,7 @@ static mp_obj_t py_sensor_set_vsync_output(mp_obj_t pin_obj) { static mp_obj_t py_sensor_ioctl(uint n_args, const mp_obj_t *args) { - mp_obj_t ret_obj = mp_const_true; + mp_obj_t ret_obj = mp_const_none; int request = mp_obj_get_int(args[0]); switch (request) { @@ -428,6 +428,70 @@ static mp_obj_t py_sensor_ioctl(uint n_args, const mp_obj_t *args) break; } + case IOCTL_LEPTON_GET_TYPE: { + int type; + if (sensor_ioctl(request, &type) != 0) { + nlr_raise(mp_obj_new_exception_msg(&mp_type_ValueError, "Sensor control failed!")); + } + ret_obj = mp_obj_new_int(type); + break; + } + + case IOCTL_LEPTON_GET_REFRESH: { + int refresh; + if (sensor_ioctl(request, &refresh) != 0) { + nlr_raise(mp_obj_new_exception_msg(&mp_type_ValueError, "Sensor control failed!")); + } + ret_obj = mp_obj_new_int(refresh); + break; + } + + case IOCTL_LEPTON_GET_RESOLUTION: { + int resolution; + if (sensor_ioctl(request, &resolution) != 0) { + nlr_raise(mp_obj_new_exception_msg(&mp_type_ValueError, "Sensor control failed!")); + } + ret_obj = mp_obj_new_int(resolution); + break; + } + + case IOCTL_LEPTON_RUN_COMMAND: { + if (n_args < 2 || sensor_ioctl(request, mp_obj_get_int(args[1])) != 0) { + nlr_raise(mp_obj_new_exception_msg(&mp_type_ValueError, "Sensor control failed!")); + } + break; + } + case IOCTL_LEPTON_SET_ATTRIBUTE: { + size_t data_len; + int command = mp_obj_get_int(args[0]); + uint16_t *data = (uint16_t *) mp_obj_str_get_data(args[1], &data_len); + PY_ASSERT_TRUE_MSG(data_len > 0, "0 bytes transferred!"); + if (sensor_ioctl(request, command, data, data_len / sizeof(uint16_t)) != 0) { + nlr_raise(mp_obj_new_exception_msg(&mp_type_ValueError, "Sensor control failed!")); + } + break; + } + + case IOCTL_LEPTON_GET_ATTRIBUTE: { + int command = mp_obj_get_int(args[0]); + size_t data_len = mp_obj_get_int(args[1]); + PY_ASSERT_TRUE_MSG(data_len > 0, "0 bytes transferred!"); + uint16_t *data = xalloc(data_len * sizeof(uint16_t)); + if (sensor_ioctl(request, command, data, data_len) != 0) { + nlr_raise(mp_obj_new_exception_msg(&mp_type_ValueError, "Sensor control failed!")); + } + ret_obj = mp_obj_new_bytearray_by_ref(data_len, data); + break; + } + + case IOCTL_LEPTON_GET_FPA_TEMPERATURE: + case IOCTL_LEPTON_GET_AUX_TEMPERATURE: { + int temp; + if (sensor_ioctl(request, &temp) != 0) { + nlr_raise(mp_obj_new_exception_msg(&mp_type_ValueError, "Sensor control failed!")); + } + ret_obj = mp_obj_new_int(temp); + } default: nlr_raise(mp_obj_new_exception_msg(&mp_type_ValueError, "Operation not supported!")); break; @@ -556,8 +620,16 @@ STATIC const mp_map_elem_t globals_dict_table[] = { { MP_OBJ_NEW_QSTR(MP_QSTR_PALETTE_IRONBOW), MP_OBJ_NEW_SMALL_INT(COLOR_PALETTE_IRONBOW)}, // IOCTLs - { MP_OBJ_NEW_QSTR(MP_QSTR_IOCTL_SET_TRIGGERED_MODE), MP_OBJ_NEW_SMALL_INT(IOCTL_SET_TRIGGERED_MODE)}, - { MP_OBJ_NEW_QSTR(MP_QSTR_IOCTL_GET_TRIGGERED_MODE), MP_OBJ_NEW_SMALL_INT(IOCTL_GET_TRIGGERED_MODE)}, + { MP_OBJ_NEW_QSTR(MP_QSTR_IOCTL_SET_TRIGGERED_MODE), MP_OBJ_NEW_SMALL_INT(IOCTL_SET_TRIGGERED_MODE)}, + { MP_OBJ_NEW_QSTR(MP_QSTR_IOCTL_GET_TRIGGERED_MODE), MP_OBJ_NEW_SMALL_INT(IOCTL_GET_TRIGGERED_MODE)}, + { MP_OBJ_NEW_QSTR(MP_QSTR_IOCTL_LEPTON_GET_TYPE), MP_OBJ_NEW_SMALL_INT(IOCTL_LEPTON_GET_TYPE)}, + { MP_OBJ_NEW_QSTR(MP_QSTR_IOCTL_LEPTON_GET_REFRESH), MP_OBJ_NEW_SMALL_INT(IOCTL_LEPTON_GET_REFRESH)}, + { MP_OBJ_NEW_QSTR(MP_QSTR_IOCTL_LEPTON_GET_RESOLUTION), MP_OBJ_NEW_SMALL_INT(IOCTL_LEPTON_GET_RESOLUTION)}, + { MP_OBJ_NEW_QSTR(MP_QSTR_IOCTL_LEPTON_RUN_COMMAND), MP_OBJ_NEW_SMALL_INT(IOCTL_LEPTON_RUN_COMMAND)}, + { MP_OBJ_NEW_QSTR(MP_QSTR_IOCTL_LEPTON_SET_ATTRIBUTE), MP_OBJ_NEW_SMALL_INT(IOCTL_LEPTON_SET_ATTRIBUTE)}, + { MP_OBJ_NEW_QSTR(MP_QSTR_IOCTL_LEPTON_GET_ATTRIBUTE), MP_OBJ_NEW_SMALL_INT(IOCTL_LEPTON_GET_ATTRIBUTE)}, + { MP_OBJ_NEW_QSTR(MP_QSTR_IOCTL_LEPTON_GET_FPA_TEMPERATURE), MP_OBJ_NEW_SMALL_INT(IOCTL_LEPTON_GET_FPA_TEMPERATURE)}, + { MP_OBJ_NEW_QSTR(MP_QSTR_IOCTL_LEPTON_GET_AUX_TEMPERATURE), MP_OBJ_NEW_SMALL_INT(IOCTL_LEPTON_GET_AUX_TEMPERATURE)}, // Sensor functions { MP_OBJ_NEW_QSTR(MP_QSTR_reset), (mp_obj_t)&py_sensor_reset_obj }, diff --git a/src/omv/py/qstrdefsomv.h b/src/omv/py/qstrdefsomv.h index acc0c3f61..d76375adb 100644 --- a/src/omv/py/qstrdefsomv.h +++ b/src/omv/py/qstrdefsomv.h @@ -205,6 +205,14 @@ Q(NEGATIVE) //IOCTLs Q(IOCTL_SET_TRIGGERED_MODE) Q(IOCTL_GET_TRIGGERED_MODE) +Q(IOCTL_LEPTON_GET_TYPE) +Q(IOCTL_LEPTON_GET_REFRESH) +Q(IOCTL_LEPTON_GET_RESOLUTION) +Q(IOCTL_LEPTON_RUN_COMMAND) +Q(IOCTL_LEPTON_SET_ATTRIBUTE) +Q(IOCTL_LEPTON_GET_ATTRIBUTE) +Q(IOCTL_LEPTON_GET_FPA_TEMPERATURE) +Q(IOCTL_LEPTON_GET_AUX_TEMPERATURE) // Color Palettes Q(PALETTE_RAINBOW) diff --git a/src/omv/sensor.h b/src/omv/sensor.h index 7e2fefc85..0a6d5b2d4 100644 --- a/src/omv/sensor.h +++ b/src/omv/sensor.h @@ -99,6 +99,14 @@ typedef enum { typedef enum { IOCTL_SET_TRIGGERED_MODE, IOCTL_GET_TRIGGERED_MODE, + IOCTL_LEPTON_GET_TYPE, + IOCTL_LEPTON_GET_REFRESH, + IOCTL_LEPTON_GET_RESOLUTION, + IOCTL_LEPTON_RUN_COMMAND, + IOCTL_LEPTON_SET_ATTRIBUTE, + IOCTL_LEPTON_GET_ATTRIBUTE, + IOCTL_LEPTON_GET_FPA_TEMPERATURE, + IOCTL_LEPTON_GET_AUX_TEMPERATURE } ioctl_t; #define SENSOR_HW_FLAGS_VSYNC (0) // vertical sync polarity.