diff --git a/Makefile b/Makefile index d26ead5..fad0db7 100644 --- a/Makefile +++ b/Makefile @@ -254,7 +254,7 @@ endif .PHONY: upload-usb upload-usb: archives all @echo Attempting to flash $(notdir $(LEGACY_PX4)) PX4FLOW board via USB - @python -u Tools/px_uploader.py $(LEGACY_PX4) --baud 921600 --port $(SERIAL_PORTS) + @python2 -u Tools/px_uploader.py $(LEGACY_PX4) --baud 921600 --port $(SERIAL_PORTS) # diff --git a/Tools/make_can_boot_descriptor.py b/Tools/make_can_boot_descriptor.py index 8065086..d505a42 100644 --- a/Tools/make_can_boot_descriptor.py +++ b/Tools/make_can_boot_descriptor.py @@ -258,7 +258,7 @@ def app_descriptor(self, value): try: options.vcs_commit = int(GitWrapper.command("rev-list HEAD --max-count=1 --abbrev=8 --abbrev-commit"),16) except Exception as e: - print "Git Command failed "+ str(e) +"- Exiting!" + print("Git Command failed "+ str(e) +"- Exiting!") quit() if args: diff --git a/makefiles/baremetal/baremetal_px4.mk b/makefiles/baremetal/baremetal_px4.mk index 96f652f..69a7ce9 100644 --- a/makefiles/baremetal/baremetal_px4.mk +++ b/makefiles/baremetal/baremetal_px4.mk @@ -63,7 +63,7 @@ $(PRODUCT_BIN): $(PRODUCT_ELF) ifdef MKUAVCANBL ifdef MAKE_UAVCAN_BOOT_LOADABLE_ID $(info %% Generating UAVCAN Bootable $(MAKE_UAVCAN_BOOT_LOADABLE_ID) as $(IMAGE_DIR)$(MAKE_UAVCAN_BOOT_LOADABLE_ID).$(UAVCAN_BL_EXT)) - $(Q) python $(MKUAVCANBL) -v --use-git-hash $@ $(IMAGE_DIR)$(MAKE_UAVCAN_BOOT_LOADABLE_ID).$(UAVCAN_BL_EXT) + $(Q) python2 $(MKUAVCANBL) -v --use-git-hash $@ $(IMAGE_DIR)$(MAKE_UAVCAN_BOOT_LOADABLE_ID).$(UAVCAN_BL_EXT) endif endif diff --git a/makefiles/baremetal/toolchain_gnu-arm-eabi.mk b/makefiles/baremetal/toolchain_gnu-arm-eabi.mk index 2cd036e..a90d4a4 100644 --- a/makefiles/baremetal/toolchain_gnu-arm-eabi.mk +++ b/makefiles/baremetal/toolchain_gnu-arm-eabi.mk @@ -50,7 +50,7 @@ OBJDUMP = $(CROSSDEV)objdump # Check if the right version of the toolchain is available # -CROSSDEV_VER_SUPPORTED = 4.7.4 4.7.5 4.7.6 4.8.4 4.9.3 5.4.1 +CROSSDEV_VER_SUPPORTED = 4.7.4 4.7.5 4.7.6 4.8.2 4.8.4 4.9.3 5.4.1 7.1.0 7.2.0 CROSSDEV_VER_FOUND = $(shell $(CC) -dumpversion) ifeq (,$(findstring $(CROSSDEV_VER_FOUND), $(CROSSDEV_VER_SUPPORTED))) @@ -277,4 +277,3 @@ define SYM_TO_BIN @$(MKDIR) -p $(dir $2) $(Q) $(OBJCOPY) -O binary $1 $2 endef - diff --git a/src/drivers/boards/px4flow-v1/px4flow_init.c b/src/drivers/boards/px4flow-v1/px4flow_init.c index 143855f..bd8c419 100644 --- a/src/drivers/boards/px4flow-v1/px4flow_init.c +++ b/src/drivers/boards/px4flow-v1/px4flow_init.c @@ -74,6 +74,11 @@ /**************************************************************************** * Public Functions ****************************************************************************/ +static int errn; +int *__errno _PARAMS ((void)) +{ + return &errn; +} /**************************************************************************** * Name: board_initialize diff --git a/src/include/dcmi.h b/src/include/dcmi.h index b127273..24e92b9 100644 --- a/src/include/dcmi.h +++ b/src/include/dcmi.h @@ -49,6 +49,11 @@ void dma_copy_image_buffers(uint8_t ** current_image, uint8_t ** previous_image, */ void send_calibration_image(uint8_t ** image_buffer_fast_1, uint8_t ** image_buffer_fast_2); +/** + * @brief Whiten image assuming pixels are iid Gaussian + */ +void whitened_image(uint8_t *source, uint8_t *dest, uint16_t image_size); + /** * @brief Initialize DCMI DMA and enable image capturing */ diff --git a/src/include/mt9v034.h b/src/include/mt9v034.h index c3c16b5..dde4fe1 100644 --- a/src/include/mt9v034.h +++ b/src/include/mt9v034.h @@ -103,6 +103,8 @@ #define MTV_HDR_ENABLE_REG 0x0F #define MTV_ADC_RES_CTRL_REG 0x1C +#define MTV_BLACK_LEVEL_CTRL_REG 0x47 +#define MTV_BLACK_LEVEL_CALIBRATION_REG 0x48 #define MTV_ROW_NOISE_CORR_CTRL_REG 0x70 #define MTV_TEST_PATTERN_REG 0x7F #define MTV_TILED_DIGITAL_GAIN_REG 0x80 diff --git a/src/include/settings.h b/src/include/settings.h index dcbe4b2..1bcd892 100644 --- a/src/include/settings.h +++ b/src/include/settings.h @@ -42,6 +42,14 @@ #define BOTTOM_FLOW_IMAGE_WIDTH 64 #define BOTTOM_FLOW_SEARCH_WINDOW_SIZE 8 +/* Translate normalized pixels to have their mean around 127. */ +#define WHITENING_MEAN 127.0f + +/* Rescale normalized pixels so 32 is one standard deviation, + * giving a range of four sigmas. + */ +#define WHITENING_STDDEV 32.0f + /****************************************************************** * ALL TYPE DEFINITIONS */ @@ -88,6 +96,18 @@ typedef enum DPS2000 = 2000 /*!< 2000 dps */ } GyroSensitivity_TypeDef; +/** + * @brief gyro sensitivity enumeration + */ +typedef enum +{ + IMAGE_WHITENING_DISABLED = 0, /*!< do not perform image whitening */ + IMAGE_WHITENING_ALWAYS = 1, /*!< always perform image whitening */ + IMAGE_WHITENING_AUTO = 2, /*!< perform image whitening, fall back to non-whitened images + if flow quality is lower than PARAM_IMAGE_WHITEN_QUALITY_THRESHOLD */ +} ImageWhitening_TypeDef; + + /****************************************************************** * ALL SETTINGS VARIABLES */ @@ -117,6 +137,8 @@ enum global_param_id_t PARAM_MAX_FLOW_PIXEL, PARAM_IMAGE_LOW_LIGHT, PARAM_IMAGE_ROW_NOISE_CORR, + PARAM_IMAGE_WHITENING, + PARAM_IMAGE_WHITENING_QUALITY_THRESHOLD, PARAM_IMAGE_TEST_PATTERN, PARAM_GYRO_SENSITIVITY_DPS, PARAM_GYRO_COMPENSATION_THRESHOLD, @@ -124,6 +146,7 @@ enum global_param_id_t PARAM_SONAR_KALMAN_L1, PARAM_SONAR_KALMAN_L2, + PARAM_USB_DRAW_FLOW, PARAM_USB_SEND_VIDEO, PARAM_USB_SEND_FLOW, PARAM_USB_SEND_GYRO, diff --git a/src/modules/flow/dcmi.c b/src/modules/flow/dcmi.c index f66cc20..92c13ed 100644 --- a/src/modules/flow/dcmi.c +++ b/src/modules/flow/dcmi.c @@ -251,35 +251,55 @@ void dma_copy_image_buffers(uint8_t ** current_image, uint8_t ** previous_image, *current_image = *previous_image; *previous_image = tmp_image; -TODO(NB dma_copy_image_buffers is calling uavcan_run()); - /* wait for new image if needed */ - while(image_counter < image_step) { - PROBE_1(false); - uavcan_run(); - PROBE_1(true); - } + while(image_counter < image_step); image_counter = 0; /* time between images */ time_between_images = time_between_next_images; + uint8_t *source = NULL; + /* copy image */ - if (dcmi_image_buffer_unused == 1) - { - for (uint16_t pixel = 0; pixel < image_size; pixel++) - (*current_image)[pixel] = (uint8_t)(dcmi_image_buffer_8bit_1[pixel]); + if (dcmi_image_buffer_unused == 1) { + source = dcmi_image_buffer_8bit_1; + } else if (dcmi_image_buffer_unused == 2) { + source = dcmi_image_buffer_8bit_2; + } else { + source = dcmi_image_buffer_8bit_3; } - else if (dcmi_image_buffer_unused == 2) - { - for (uint16_t pixel = 0; pixel < image_size; pixel++) - (*current_image)[pixel] = (uint8_t)(dcmi_image_buffer_8bit_2[pixel]); + + for (uint16_t pixel = 0; pixel < image_size; pixel++) + (*current_image)[pixel] = source[pixel]; +} + +/** + * @brief Whiten image by mean shift and scaling by standard deviation + * + * @param source Source image + * @param dest Destination image (may be the same as source) + * @param image_size Size of source and dest images. Must be <= 256. + */ +void whitened_image(uint8_t *source, uint8_t *dest, uint16_t image_size) { + uint32_t sum = 0; + for (uint16_t pixel = 0; pixel < image_size; pixel++) + sum += source[pixel]; + uint8_t mean = sum / image_size; + uint32_t rss = 0; + for (uint16_t pixel = 0; pixel < image_size; pixel++) { + int16_t v = source[pixel] - mean; + rss += v*v; } - else - { - for (uint16_t pixel = 0; pixel < image_size; pixel++) - (*current_image)[pixel] = (uint8_t)(dcmi_image_buffer_8bit_3[pixel]); + float stddev = sqrtf(rss/(image_size - 1)) / WHITENING_STDDEV; + + for (uint16_t pixel = 0; pixel < image_size; pixel++) { + float v = WHITENING_MEAN + (source[pixel] - mean)/stddev; + if (v < 0.0f) + v = 0; + if (v > 255.0f) + v = 255; + dest[pixel] = (uint8_t)v; } } @@ -304,16 +324,11 @@ void send_calibration_image(uint8_t ** image_buffer_fast_1, uint8_t ** image_buf 100); uint16_t frame = 0; - uint8_t image = 0; uint8_t frame_buffer[MAVLINK_MSG_ENCAPSULATED_DATA_FIELD_DATA_LEN]; for (int i = 0; i < FULL_IMAGE_SIZE * 4; i++) { - - if (i % FULL_IMAGE_SIZE == 0 && i != 0) - { - image++; - } + uint8_t image = i / FULL_IMAGE_SIZE; if (i % MAVLINK_MSG_ENCAPSULATED_DATA_FIELD_DATA_LEN == 0 && i != 0) { @@ -322,44 +337,39 @@ void send_calibration_image(uint8_t ** image_buffer_fast_1, uint8_t ** image_buf delay(2); } - if (image == 0 ) - { - frame_buffer[i % MAVLINK_MSG_ENCAPSULATED_DATA_FIELD_DATA_LEN] = (uint8_t)(*image_buffer_fast_1)[i % FULL_IMAGE_SIZE]; - } - else if (image == 1 ) - { - frame_buffer[i % MAVLINK_MSG_ENCAPSULATED_DATA_FIELD_DATA_LEN] = (uint8_t)(*image_buffer_fast_2)[i % FULL_IMAGE_SIZE]; - } - else if (image == 2) - { + // The whole camera capture is stored in five parts: two buffers given + // as arguments, and three internal DMA buffers. + uint8_t *source; + + if (image == 0) + source = *image_buffer_fast_1; + else if (image == 1) + source = *image_buffer_fast_2; + else if (image == 2) { if (calibration_unused == 1) - frame_buffer[i % MAVLINK_MSG_ENCAPSULATED_DATA_FIELD_DATA_LEN] = dcmi_image_buffer_8bit_1[i % FULL_IMAGE_SIZE]; + source = dcmi_image_buffer_8bit_1; else if (calibration_unused == 2) - frame_buffer[i % MAVLINK_MSG_ENCAPSULATED_DATA_FIELD_DATA_LEN] = dcmi_image_buffer_8bit_2[i % FULL_IMAGE_SIZE]; + source = dcmi_image_buffer_8bit_2; else - frame_buffer[i % MAVLINK_MSG_ENCAPSULATED_DATA_FIELD_DATA_LEN] = dcmi_image_buffer_8bit_3[i % FULL_IMAGE_SIZE]; - } - else - { - if (calibration_used) - { + source = dcmi_image_buffer_8bit_3; + } else { + if (calibration_used) { if (calibration_mem0 == 1) - frame_buffer[i % MAVLINK_MSG_ENCAPSULATED_DATA_FIELD_DATA_LEN] = dcmi_image_buffer_8bit_1[i % FULL_IMAGE_SIZE]; + source = dcmi_image_buffer_8bit_1; else if (calibration_mem0 == 2) - frame_buffer[i % MAVLINK_MSG_ENCAPSULATED_DATA_FIELD_DATA_LEN] = dcmi_image_buffer_8bit_2[i % FULL_IMAGE_SIZE]; + source = dcmi_image_buffer_8bit_2; else - frame_buffer[i % MAVLINK_MSG_ENCAPSULATED_DATA_FIELD_DATA_LEN] = dcmi_image_buffer_8bit_3[i % FULL_IMAGE_SIZE]; - } - else - { + source = dcmi_image_buffer_8bit_3; + } else { if (calibration_mem1 == 1) - frame_buffer[i % MAVLINK_MSG_ENCAPSULATED_DATA_FIELD_DATA_LEN] = dcmi_image_buffer_8bit_1[i % FULL_IMAGE_SIZE]; + source = dcmi_image_buffer_8bit_1; else if (calibration_mem1 == 2) - frame_buffer[i % MAVLINK_MSG_ENCAPSULATED_DATA_FIELD_DATA_LEN] = dcmi_image_buffer_8bit_2[i % FULL_IMAGE_SIZE]; + source = dcmi_image_buffer_8bit_2; else - frame_buffer[i % MAVLINK_MSG_ENCAPSULATED_DATA_FIELD_DATA_LEN] = dcmi_image_buffer_8bit_3[i % FULL_IMAGE_SIZE]; + source = dcmi_image_buffer_8bit_3; } } + frame_buffer[i % MAVLINK_MSG_ENCAPSULATED_DATA_FIELD_DATA_LEN] = source[i % FULL_IMAGE_SIZE]; } mavlink_msg_encapsulated_data_send(MAVLINK_COMM_2, frame, frame_buffer); diff --git a/src/modules/flow/flow.c b/src/modules/flow/flow.c index 59a4d85..0ad987e 100644 --- a/src/modules/flow/flow.c +++ b/src/modules/flow/flow.c @@ -404,35 +404,43 @@ uint8_t compute_flow(uint8_t *image1, uint8_t *image2, float x_rate, float y_rat const uint16_t hist_size = 2*(winmax-winmin+1)+1; /* variables */ - uint16_t pixLo = SEARCH_SIZE + 1; - uint16_t pixHi = FRAME_SIZE - (SEARCH_SIZE + 1) - TILE_SIZE; - uint16_t pixStep = (pixHi - pixLo) / NUM_BLOCKS + 1; + uint16_t pixLo = SEARCH_SIZE + 1; + uint16_t pixHi = FRAME_SIZE - (SEARCH_SIZE + 1) - TILE_SIZE; + uint16_t pixStep = (pixHi - pixLo) / NUM_BLOCKS + 1; uint16_t i, j; uint32_t acc[8]; // subpixels uint16_t histx[hist_size]; // counter for x shift uint16_t histy[hist_size]; // counter for y shift - int8_t dirsx[64]; // shift directions in x - int8_t dirsy[64]; // shift directions in y - uint8_t subdirs[64]; // shift directions of best subpixels + int8_t dirsx[NUM_BLOCKS*NUM_BLOCKS]; // shift directions in x + int8_t dirsy[NUM_BLOCKS*NUM_BLOCKS]; // shift directions in y + uint8_t subdirs[NUM_BLOCKS*NUM_BLOCKS]; // shift directions of best subpixels + bool used[NUM_BLOCKS*NUM_BLOCKS]; // store which blocks where used float meanflowx = 0.0f; float meanflowy = 0.0f; uint16_t meancount = 0; float histflowx = 0.0f; float histflowy = 0.0f; + float stddev_flowx = 0.0f; + float stddev_flowy = 0.0f; /* initialize with 0 */ for (j = 0; j < hist_size; j++) { histx[j] = 0; histy[j] = 0; } /* iterate over all patterns */ + uint32_t block_id = 0; // id of this block for (j = pixLo; j < pixHi; j += pixStep) { for (i = pixLo; i < pixHi; i += pixStep) { + used[block_id] = false; + /* test pixel if it is suitable for flow tracking */ uint32_t diff = compute_diff(image1, i, j, (uint16_t) global_data.param[PARAM_IMAGE_WIDTH]); if (diff < global_data.param[PARAM_BOTTOM_FLOW_FEATURE_THRESHOLD]) { + /* Next block */ + block_id++; continue; } @@ -463,6 +471,7 @@ uint8_t compute_flow(uint8_t *image1, uint8_t *image2, float x_rate, float y_rat /* acceptance SAD distance threshhold */ if (dist < global_data.param[PARAM_BOTTOM_FLOW_VALUE_THRESHOLD]) { + used[block_id] = true; meanflowx += (float) sumx; meanflowy += (float) sumy; @@ -495,25 +504,48 @@ uint8_t compute_flow(uint8_t *image1, uint8_t *image2, float x_rate, float y_rat histy[hist_index_y]++; } + + /* Next block */ + block_id++; } } /* create flow image if needed (image1 is not needed anymore) * -> can be used for debugging purpose */ - if (FLOAT_AS_BOOL(global_data.param[PARAM_USB_SEND_VIDEO]))//&& global_data.param[PARAM_VIDEO_USB_MODE] == FLOW_VIDEO) + if (FLOAT_AS_BOOL(global_data.param[PARAM_USB_SEND_VIDEO])) { - for (j = pixLo; j < pixHi; j += pixStep) + uint8_t offset = TILE_SIZE / 2 + 1; + block_id = 0; + uint32_t used_block_id = 0; + for (j = pixLo + offset; j < pixHi + offset; j += pixStep) { - for (i = pixLo; i < pixHi; i += pixStep) + for (i = pixLo + offset; i < pixHi + offset; i += pixStep) { - - uint32_t diff = compute_diff(image1, i, j, (uint16_t) global_data.param[PARAM_IMAGE_WIDTH]); - if (diff > global_data.param[PARAM_BOTTOM_FLOW_FEATURE_THRESHOLD]) + if (used[block_id]) { + if (FLOAT_AS_BOOL(global_data.param[PARAM_USB_DRAW_FLOW])) + { + // Draw optic flow vector + uint8_t steps = fmax(abs(dirsx[used_block_id]), abs(dirsy[used_block_id])); + for (int8_t k = 0; k < steps; k++) + { + // Draw black segment to represent optic flow vector + int8_t dx = (k * dirsx[used_block_id]) / steps; + int8_t dy = (k * dirsy[used_block_id]) / steps; + image1[(j + dy) * ((uint16_t) global_data.param[PARAM_IMAGE_WIDTH]) + i + dx] = 0; + } + } + + // Draw white dot if block was used image1[j * ((uint16_t) global_data.param[PARAM_IMAGE_WIDTH]) + i] = 255; + + // Next used block + used_block_id++; } + // Next block + block_id++; } } } @@ -638,24 +670,37 @@ uint8_t compute_flow(uint8_t *image1, uint8_t *image2, float x_rate, float y_rat /* use average of accepted flow values */ uint32_t meancount_x = 0; uint32_t meancount_y = 0; + float histflowx_sum = 0.0f; + float histflowy_sum = 0.0f; + float histflowx_sum2 = 0.0f; + float histflowy_sum2 = 0.0f; for (uint8_t h = 0; h < meancount; h++) { float subdirx = 0.0f; if (subdirs[h] == 0 || subdirs[h] == 1 || subdirs[h] == 7) subdirx = 0.5f; if (subdirs[h] == 3 || subdirs[h] == 4 || subdirs[h] == 5) subdirx = -0.5f; - histflowx += (float)dirsx[h] + subdirx; + float flow_x = (float)dirsx[h] + subdirx; + histflowx_sum += flow_x; + histflowx_sum2 += flow_x * flow_x; meancount_x++; float subdiry = 0.0f; if (subdirs[h] == 5 || subdirs[h] == 6 || subdirs[h] == 7) subdiry = -0.5f; if (subdirs[h] == 1 || subdirs[h] == 2 || subdirs[h] == 3) subdiry = 0.5f; - histflowy += (float)dirsy[h] + subdiry; + float flow_y = (float)dirsy[h] + subdiry; + histflowy_sum += flow_y; + histflowy_sum2 += flow_y * flow_y; meancount_y++; } - histflowx /= meancount_x; - histflowy /= meancount_y; + // Compute standart deviation of accepted optic flow values + stddev_flowx = sqrtf((histflowx_sum2 - (histflowx_sum * histflowx_sum) / meancount_x) / meancount_x); + stddev_flowy = sqrtf((histflowy_sum2 - (histflowy_sum * histflowy_sum) / meancount_y) / meancount_y); + + // Compute average of accepted optic flow values + histflowx = histflowx_sum / meancount_x; + histflowy = histflowy_sum / meancount_y; } @@ -741,8 +786,13 @@ uint8_t compute_flow(uint8_t *image1, uint8_t *image2, float x_rate, float y_rat return 0; } - /* calc quality */ + /* calc quality from ratio of used & unused blocks*/ uint8_t qual = (uint8_t)(meancount * 255 / (NUM_BLOCKS*NUM_BLOCKS)); + /* Adapt quality based on standard deviation of flow in used blocks */ + float stddev = sqrtf(stddev_flowx * stddev_flowx + stddev_flowy * stddev_flowy); + float qual_scaling = fmaxf(0.0f, (SEARCH_SIZE - 2.0f*stddev) / SEARCH_SIZE); + qual *= qual_scaling; + return qual; } diff --git a/src/modules/flow/main.c b/src/modules/flow/main.c index 2293bab..64fee45 100644 --- a/src/modules/flow/main.c +++ b/src/modules/flow/main.c @@ -88,6 +88,8 @@ __ALIGN_BEGIN USB_OTG_CORE_HANDLE USB_OTG_dev __ALIGN_END; /* fast image buffers for calculations */ uint8_t image_buffer_8bit_1[FULL_IMAGE_SIZE] __attribute__((section(".ccm"))); uint8_t image_buffer_8bit_2[FULL_IMAGE_SIZE] __attribute__((section(".ccm"))); +uint8_t image_bottom_buffer_8bit_1[BOTTOM_FLOW_IMAGE_WIDTH * BOTTOM_FLOW_IMAGE_HEIGHT] __attribute__((section(".ccm"))); +uint8_t image_bottom_buffer_8bit_2[BOTTOM_FLOW_IMAGE_WIDTH * BOTTOM_FLOW_IMAGE_HEIGHT] __attribute__((section(".ccm"))); uint8_t buffer_reset_needed; /* boot time in milliseconds ticks */ @@ -108,10 +110,10 @@ volatile uint32_t boot_time10_us = 0; #define TIMER_LPOS 8 #define MS_TIMER_COUNT 100 /* steps in 10 microseconds ticks */ #define LED_TIMER_COUNT 500 /* steps in milliseconds ticks */ -#define SONAR_TIMER_COUNT 100 /* steps in milliseconds ticks */ +#define SONAR_TIMER_COUNT 100 /* steps in milliseconds ticks */ #define SYSTEM_STATE_COUNT 1000/* steps in milliseconds ticks */ #define PARAMS_COUNT 100 /* steps in milliseconds ticks */ -#define LPOS_TIMER_COUNT 100 /* steps in milliseconds ticks */ +#define LPOS_TIMER_COUNT 100 /* steps in milliseconds ticks */ static volatile unsigned timer[NTIMERS]; static volatile unsigned timer_ms = MS_TIMER_COUNT; @@ -251,14 +253,14 @@ int main(void) LEDOff(LED_ACT); LEDOff(LED_COM); LEDOff(LED_ERR); - board_led_rgb(255,255,255, 1); - board_led_rgb( 0, 0,255, 0); - board_led_rgb( 0, 0, 0, 0); - board_led_rgb(255, 0, 0, 1); - board_led_rgb(255, 0, 0, 2); - board_led_rgb(255, 0, 0, 3); - board_led_rgb( 0,255, 0, 3); - board_led_rgb( 0, 0,255, 4); + board_led_rgb(255,255,255, 1); + board_led_rgb( 0, 0,255, 0); + board_led_rgb( 0, 0, 0, 0); + board_led_rgb(255, 0, 0, 1); + board_led_rgb(255, 0, 0, 2); + board_led_rgb(255, 0, 0, 3); + board_led_rgb( 0,255, 0, 3); + board_led_rgb( 0, 0,255, 4); /* enable FPU on Cortex-M4F core */ SCB_CPACR |= ((3UL << 10 * 2) | (3UL << 11 * 2)); /* set CP10 Full Access and set CP11 Full Access */ @@ -294,14 +296,22 @@ int main(void) image_buffer_8bit_2[i] = 0; } + for (int i = 0; i < BOTTOM_FLOW_IMAGE_WIDTH * BOTTOM_FLOW_IMAGE_HEIGHT; i++) + { + image_bottom_buffer_8bit_1[i] = 0; + image_bottom_buffer_8bit_2[i] = 0; + } + uint8_t * current_image = image_buffer_8bit_1; uint8_t * previous_image = image_buffer_8bit_2; + uint8_t * current_image_whitened = image_bottom_buffer_8bit_1; + uint8_t * previous_image_whitened = image_bottom_buffer_8bit_2; /* usart config*/ usart_init(); - /* i2c config*/ - i2c_init(); + /* i2c config*/ + i2c_init(); /* sonar config*/ float sonar_distance_filtered = 0.0f; // distance in meter @@ -319,6 +329,7 @@ int main(void) /* variables */ uint32_t counter = 0; uint8_t qual = 0; + bool used_whitened_image = false; /* bottom flow variables */ float pixel_flow_x = 0.0f; @@ -348,9 +359,10 @@ int main(void) /* main loop */ while (1) { - PROBE_1(false); - uavcan_run(); - PROBE_1(true); + PROBE_1(false); + uavcan_run(); + PROBE_1(true); + /* reset flow buffers if needed */ if(buffer_reset_needed) { @@ -360,6 +372,13 @@ int main(void) image_buffer_8bit_1[i] = 0; image_buffer_8bit_2[i] = 0; } + + for (int i = 0; i < BOTTOM_FLOW_IMAGE_WIDTH * BOTTOM_FLOW_IMAGE_HEIGHT; i++) + { + image_bottom_buffer_8bit_1[i] = 0; + image_bottom_buffer_8bit_2[i] = 0; + } + delay(500); continue; } @@ -428,8 +447,39 @@ int main(void) /* copy recent image to faster ram */ dma_copy_image_buffers(¤t_image, &previous_image, image_size, 1); - /* compute optical flow */ - qual = compute_flow(previous_image, current_image, x_rate, y_rate, z_rate, &pixel_flow_x, &pixel_flow_y); + if (FLOAT_EQ_INT(global_data.param[PARAM_IMAGE_WHITENING], IMAGE_WHITENING_DISABLED)) { + /* compute optical flow */ + qual = compute_flow(previous_image, current_image, x_rate, y_rate, z_rate, &pixel_flow_x, &pixel_flow_y); + used_whitened_image = false; + } else { + /* swap whitened image buffers */ + uint8_t * tmp_image = current_image_whitened; + current_image_whitened = previous_image_whitened; + previous_image_whitened = tmp_image; + + /* whiten image */ + whitened_image(current_image, current_image_whitened, image_size); + used_whitened_image = true; + + /* compute optical flow on whitened images*/ + qual = compute_flow(previous_image_whitened, current_image_whitened, x_rate, y_rate, z_rate, &pixel_flow_x, &pixel_flow_y); + + if (FLOAT_EQ_INT(global_data.param[PARAM_IMAGE_WHITENING], IMAGE_WHITENING_AUTO) && (qual < global_data.param[PARAM_IMAGE_WHITENING_QUALITY_THRESHOLD])) { + float pixel_flow_x_no_whiten = 0.0f; + float pixel_flow_y_no_whiten = 0.0f; + + /* compute optical flow on non-whitened images */ + uint8_t qual_no_whiten = compute_flow(previous_image, current_image, x_rate, y_rate, z_rate, &pixel_flow_x_no_whiten, &pixel_flow_y_no_whiten); + + /* keep best optical flow (from whitened or non-whitened images) */ + if (qual_no_whiten > qual) { + qual = qual_no_whiten; + pixel_flow_x = pixel_flow_x_no_whiten; + pixel_flow_y = pixel_flow_y_no_whiten; + used_whitened_image = false; + } + } + } /* * real point P (X,Y,Z), image plane projection p (x,y,z), focal-length f, distance-to-scene Z @@ -505,20 +555,12 @@ int main(void) float ground_distance = 0.0f; + ground_distance = FLOAT_AS_BOOL(global_data.param[PARAM_SONAR_FILTERED]) ? sonar_distance_filtered : sonar_distance_raw; - if(FLOAT_AS_BOOL(global_data.param[PARAM_SONAR_FILTERED])) - { - ground_distance = sonar_distance_filtered; - } - else - { - ground_distance = sonar_distance_raw; - } - - uavcan_define_export(i2c_data, legacy_12c_data_t, ccm); - uavcan_define_export(range_data, range_data_t, ccm); + uavcan_define_export(i2c_data, legacy_12c_data_t, ccm); + uavcan_define_export(range_data, range_data_t, ccm); uavcan_timestamp_export(i2c_data); - uavcan_assign(range_data.time_stamp_utc, i2c_data.time_stamp_utc); + uavcan_assign(range_data.time_stamp_utc, i2c_data.time_stamp_utc); //update I2C transmitbuffer if(valid_frame_count>0) { @@ -530,13 +572,13 @@ int main(void) update_TX_buffer(pixel_flow_x, pixel_flow_y, 0.0f, 0.0f, qual, ground_distance, x_rate, y_rate, z_rate, gyro_temp, uavcan_use_export(i2c_data)); } - PROBE_2(false); - uavcan_publish(range, 40, range_data); - PROBE_2(true); + PROBE_2(false); + uavcan_publish(range, 40, range_data); + PROBE_2(true); - PROBE_3(false); - uavcan_publish(flow, 40, i2c_data); - PROBE_3(true); + PROBE_3(false); + uavcan_publish(flow, 40, i2c_data); + PROBE_3(true); //serial mavlink + usb mavlink output throttled uint32_t now = get_boot_time_us(); @@ -612,6 +654,8 @@ int main(void) accumulated_gyro_x, accumulated_gyro_y, accumulated_gyro_z, gyro_temp, accumulated_quality/accumulated_framecount, time_since_last_sonar_update,ground_distance); + + mavlink_msg_named_value_float_send(MAVLINK_COMM_2, get_boot_time_us(), "DT", integration_timespan / accumulated_framecount); } @@ -707,7 +751,12 @@ int main(void) uint16_t frame = 0; for (frame = 0; frame < image_size_send / MAVLINK_MSG_ENCAPSULATED_DATA_FIELD_DATA_LEN + 1; frame++) { - mavlink_msg_encapsulated_data_send(MAVLINK_COMM_2, frame, &((uint8_t *) previous_image)[frame * MAVLINK_MSG_ENCAPSULATED_DATA_FIELD_DATA_LEN]); + // send whitened image if it was used + if (used_whitened_image) { + mavlink_msg_encapsulated_data_send(MAVLINK_COMM_2, frame, &((uint8_t *) previous_image_whitened)[frame * MAVLINK_MSG_ENCAPSULATED_DATA_FIELD_DATA_LEN]); + } else { + mavlink_msg_encapsulated_data_send(MAVLINK_COMM_2, frame, &((uint8_t *) previous_image)[frame * MAVLINK_MSG_ENCAPSULATED_DATA_FIELD_DATA_LEN]); + } } send_image_now = false; diff --git a/src/modules/flow/mt9v034.c b/src/modules/flow/mt9v034.c index ebfd6b9..9bfa06a 100644 --- a/src/modules/flow/mt9v034.c +++ b/src/modules/flow/mt9v034.c @@ -127,6 +127,9 @@ void mt9v034_context_configuration(void) if (hdr_enable_flag) { hdr_enabled = 0x0100; } + uint16_t analog_gain_control = 16; + uint16_t black_level_control = 0x01; + uint16_t black_level_calibration = 0x50; bool aec_enable_flag = global_data.param[PARAM_AEC] > 0; uint16_t aec_agc_enabled = 0x0000; @@ -182,6 +185,7 @@ void mt9v034_context_configuration(void) mt9v034_WriteReg16(MTV_COARSE_SW_2_REG_A, coarse_sw2); mt9v034_WriteReg16(MTV_COARSE_SW_CTRL_REG_A, shutter_width_ctrl); mt9v034_WriteReg16(MTV_COARSE_SW_TOTAL_REG_A, total_shutter_width); + mt9v034_WriteReg16(MTV_ANALOG_GAIN_CTRL_REG_A, analog_gain_control); /* Context B */ @@ -196,6 +200,7 @@ void mt9v034_context_configuration(void) mt9v034_WriteReg16(MTV_COARSE_SW_2_REG_B, coarse_sw2); mt9v034_WriteReg16(MTV_COARSE_SW_CTRL_REG_B, shutter_width_ctrl); mt9v034_WriteReg16(MTV_COARSE_SW_TOTAL_REG_B, total_shutter_width); + mt9v034_WriteReg16(MTV_ANALOG_GAIN_CTRL_REG_B, analog_gain_control); /* General Settings */ mt9v034_WriteReg16(MTV_ROW_NOISE_CORR_CTRL_REG, row_noise_correction); @@ -215,6 +220,9 @@ void mt9v034_context_configuration(void) mt9v034_WriteReg16(MTV_AGC_UPDATE_REG,agc_update_freq); mt9v034_WriteReg16(MTV_AGC_LOWPASS_REG,agc_low_pass); + mt9v034_WriteReg16(MTV_BLACK_LEVEL_CTRL_REG,black_level_control); + mt9v034_WriteReg16(MTV_BLACK_LEVEL_CALIBRATION_REG,black_level_calibration); + /* Reset */ mt9v034_WriteReg16(MTV_SOFT_RESET_REG, 0x01); } diff --git a/src/modules/flow/settings.c b/src/modules/flow/settings.c index 21f2d5d..f715632 100644 --- a/src/modules/flow/settings.c +++ b/src/modules/flow/settings.c @@ -111,6 +111,14 @@ void global_data_reset_param_defaults(void){ strcpy(global_data.param_name[PARAM_IMAGE_ROW_NOISE_CORR], "IMAGE_NOISE_C"); global_data.param_access[PARAM_IMAGE_ROW_NOISE_CORR] = READ_WRITE; + global_data.param[PARAM_IMAGE_WHITENING] = IMAGE_WHITENING_AUTO; + strcpy(global_data.param_name[PARAM_IMAGE_WHITENING], "IMAGE_WHITEN"); + global_data.param_access[PARAM_IMAGE_WHITENING] = READ_WRITE; + + global_data.param[PARAM_IMAGE_WHITENING_QUALITY_THRESHOLD] = 50; + strcpy(global_data.param_name[PARAM_IMAGE_WHITENING_QUALITY_THRESHOLD], "IMAGE_WHIT_QTH"); + global_data.param_access[PARAM_IMAGE_WHITENING_QUALITY_THRESHOLD] = READ_WRITE; + global_data.param[PARAM_IMAGE_TEST_PATTERN] = 0; strcpy(global_data.param_name[PARAM_IMAGE_TEST_PATTERN], "IMAGE_TEST_PAT"); global_data.param_access[PARAM_IMAGE_TEST_PATTERN] = READ_WRITE; @@ -135,6 +143,10 @@ void global_data_reset_param_defaults(void){ strcpy(global_data.param_name[PARAM_SONAR_KALMAN_L2], "SONAR_KAL_L2"); global_data.param_access[PARAM_SONAR_KALMAN_L2] = READ_WRITE; + global_data.param[PARAM_USB_DRAW_FLOW] = 1; // draw flow vectors on the video sent over USB + strcpy(global_data.param_name[PARAM_USB_DRAW_FLOW], "USB_DRAW_FLOW"); + global_data.param_access[PARAM_USB_DRAW_FLOW] = READ_WRITE; + global_data.param[PARAM_USB_SEND_VIDEO] = 1; // send video over USB strcpy(global_data.param_name[PARAM_USB_SEND_VIDEO], "USB_SEND_VIDEO"); global_data.param_access[PARAM_USB_SEND_VIDEO] = READ_WRITE; diff --git a/src/modules/flow/system_stm32f4xx.c b/src/modules/flow/system_stm32f4xx.c index a7e7563..b31938a 100644 --- a/src/modules/flow/system_stm32f4xx.c +++ b/src/modules/flow/system_stm32f4xx.c @@ -349,7 +349,7 @@ static void SetSysClock(void) RCC->CFGR |= RCC_CFGR_SW_PLL; /* Wait till the main PLL is used as system clock source */ - while ((RCC->CFGR & (uint32_t)RCC_CFGR_SWS ) != RCC_CFGR_SWS_PLL); + while ((RCC->CFGR & (uint32_t)RCC_CFGR_SWS ) != RCC_CFGR_SWS_PLL) { } }