diff --git a/src/omv/lepton.c b/src/omv/lepton.c index 62b0e5fe8..9dfac8edf 100644 --- a/src/omv/lepton.c +++ b/src/omv/lepton.c @@ -93,7 +93,7 @@ static uint16_t lepton_calc_crc(uint8_t *buf) buf[1] &= 0xFF; buf[2] = 0; buf[3] = 0; - return CalcCRC16Bytes(VOSPI_LINE_SIZE, (char *) buf); + return CalcCRC16Bytes(VOSPI_PACKET_SIZE, (char *) buf); } static int sleep(sensor_t *sensor, int enable) @@ -344,61 +344,43 @@ void HAL_SPI_RxCpltCallback(SPI_HandleTypeDef *hspi) } } } - } static int snapshot(sensor_t *sensor, image_t *image, streaming_cb_t cb) { + fb_update_jpeg_buffer(); + if ((!h_res) || (!v_res) || (!sensor->framesize) || (!sensor->pixformat)) { return -1; } - fb_update_jpeg_buffer(); - + // The SPI DMA device is always clocking the FLIR Lepton in the background. + // The code below resets the vospi control values to let data be pulled in. + // If we need to re-sync we do it. Otherwise, after we finish pulling data + // in we exit and let the SPI bus keep running. Then on the next call to + // snapshot we read in more data and pull in the next frame. + HAL_NVIC_DisableIRQ(LEPTON_SPI_DMA_IRQn); vospi_pid = VOSPI_FIRST_PACKET; vospi_seg = VOSPI_FIRST_SEGMENT; + HAL_NVIC_EnableIRQ(LEPTON_SPI_DMA_IRQn); + do { if (vospi_resync == true) { lepton_sync(); } __WFI(); - } while (vospi_pid < vospi_packets); + } while (vospi_pid < vospi_packets); // only checking one volatile var so atomic. - image->w = MAIN_FB()->w; - image->h = MAIN_FB()->h; - image->bpp = MAIN_FB()->bpp; // invalid - image->data = MAIN_FB()->pixels; // valid - - uint16_t *src = (uint16_t*) vospi_buffer; - - for (int y=0; y>8; - switch (sensor->pixformat) { - case PIXFORMAT_RGB565: { - IMAGE_PUT_RGB565_PIXEL(image, x, y, rainbow_table[val]); - break; - } - case PIXFORMAT_GRAYSCALE: { - IMAGE_PUT_GRAYSCALE_PIXEL(image, x, y, val); - break; - } - default: { - break; - } - } - } - } + MAIN_FB()->w = MAIN_FB()->u; + MAIN_FB()->h = MAIN_FB()->v; switch (sensor->pixformat) { - case PIXFORMAT_GRAYSCALE: { - MAIN_FB()->bpp = 1; + case PIXFORMAT_RGB565: { + MAIN_FB()->bpp = sizeof(uint16_t); break; } - case PIXFORMAT_RGB565: { - MAIN_FB()->bpp = 2; + case PIXFORMAT_GRAYSCALE: { + MAIN_FB()->bpp = sizeof(uint8_t); break; } default: { @@ -406,7 +388,58 @@ static int snapshot(sensor_t *sensor, image_t *image, streaming_cb_t cb) } } - image->bpp = MAIN_FB()->bpp; + image->w = MAIN_FB()->u; + image->h = MAIN_FB()->v; + image->bpp = MAIN_FB()->bpp; // invalid + image->data = MAIN_FB()->pixels; // valid + + uint16_t *src = (uint16_t*) vospi_buffer; + + float x_scale = resolution[sensor->framesize][0] / ((float) h_res); + float y_scale = resolution[sensor->framesize][1] / ((float) v_res); + // MAX == KeepAspectRationByExpanding - MIN == KeepAspectRatio + float scale = IM_MAX(x_scale, y_scale), scale_inv = 1.0f / scale; + int x_offset = (resolution[sensor->framesize][0] - (h_res * scale)) / 2; + int y_offset = (resolution[sensor->framesize][1] - (v_res * scale)) / 2; + // The code below upscales the source image to the requested frame size + // and then crops it to the window set by the user. + + for (int y = y_offset, yy = fast_ceilf(v_res * scale) + y_offset; y < yy; y++) { + if ((MAIN_FB()->y <= y) && (y < (MAIN_FB()->y + MAIN_FB()->v))) { // user window cropping + + uint16_t *row_ptr = src + (fast_floorf(y * scale_inv) * h_res); + + for (int x = x_offset, xx = fast_ceilf(h_res * scale) + x_offset; x < xx; x++) { + if ((MAIN_FB()->x <= x) && (x < (MAIN_FB()->x + MAIN_FB()->u))) { // user window cropping + + // Value is the 14-bit value from the FLIR IR camera. + // However, with AGC enabled only the bottom 8-bits are non-zero. + int value = __REV16(row_ptr[fast_floorf(x * scale_inv)]) & 0x3FFF; + + int t_x = x - MAIN_FB()->x; + int t_y = y - MAIN_FB()->y; + + if (h_mirror) t_x = MAIN_FB()->u - t_x - 1; + if (v_flip) t_y = MAIN_FB()->v - t_y - 1; + + switch (sensor->pixformat) { + case PIXFORMAT_RGB565: { + IMAGE_PUT_RGB565_PIXEL(image, t_x, t_y, rainbow_table[value & 0xFF]); + break; + } + case PIXFORMAT_GRAYSCALE: { + IMAGE_PUT_GRAYSCALE_PIXEL(image, t_x, t_y, value & 0xFF); + break; + } + default: { + break; + } + } + } + } + } + } + return 0; }