From 6feb3369b7b1f7908b7705a089cb7f3994c59770 Mon Sep 17 00:00:00 2001 From: Ludvig Ericson Date: Thu, 15 Jun 2017 10:44:41 +0200 Subject: [PATCH 1/7] Improve flow quality We noticed quite heavy flickering in the image output from the camera sensor. We tested an array of approaches to mitigate the issue. Primarily we perform whitening of the camera input. This improves the flow quality and stability immensely for us. --- src/drivers/boards/px4flow-v1/px4flow_init.c | 5 + src/include/dcmi.h | 5 + src/include/mt9v034.h | 2 + src/modules/flow/dcmi.c | 108 ++++++++++--------- src/modules/flow/main.c | 64 +++++------ src/modules/flow/mt9v034.c | 9 +- 6 files changed, 104 insertions(+), 89 deletions(-) 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/modules/flow/dcmi.c b/src/modules/flow/dcmi.c index f66cc20..21441b0 100644 --- a/src/modules/flow/dcmi.c +++ b/src/modules/flow/dcmi.c @@ -251,35 +251,47 @@ 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]); - } - else - { - for (uint16_t pixel = 0; pixel < image_size; pixel++) - (*current_image)[pixel] = (uint8_t)(dcmi_image_buffer_8bit_3[pixel]); + + for (uint16_t pixel = 0; pixel < image_size; pixel++) + (*current_image)[pixel] = source[pixel]; +} + +void whitened_image(uint8_t *source, uint8_t *dest, uint16_t image_size) { + double sum = 0.0; + for (uint16_t pixel = 0; pixel < image_size; pixel++) + sum += source[pixel]; + double mean = sum / image_size; + double rss = 0.0; + for (uint16_t pixel = 0; pixel < image_size; pixel++) + rss += pow(source[pixel] - mean, 2); + double stddev = sqrt(rss/(image_size - 1)); + dest[0] = stddev; + + for (uint16_t pixel = 0; pixel < image_size; pixel++) { + double v = 127.0 + 32.0*(source[pixel] - mean)/stddev; + if (v < 0.0) + v = 0; + if (v > 255.0) + v = 255; + dest[pixel] = (uint8_t)v; } } @@ -304,16 +316,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 +329,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/main.c b/src/modules/flow/main.c index fb11e9f..ebfc517 100644 --- a/src/modules/flow/main.c +++ b/src/modules/flow/main.c @@ -108,10 +108,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 9999/* 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 +251,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 */ @@ -300,8 +300,8 @@ int main(void) /* usart config*/ usart_init(); - /* i2c config*/ - i2c_init(); + /* i2c config*/ + i2c_init(); /* sonar config*/ float sonar_distance_filtered = 0.0f; // distance in meter @@ -347,9 +347,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) { @@ -426,6 +427,7 @@ int main(void) { /* copy recent image to faster ram */ dma_copy_image_buffers(¤t_image, &previous_image, image_size, 1); + whitened_image(current_image, current_image, image_size); /* compute optical flow */ qual = compute_flow(previous_image, current_image, x_rate, y_rate, z_rate, &pixel_flow_x, &pixel_flow_y); @@ -499,20 +501,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) { @@ -524,15 +518,15 @@ 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 + //serial mavlink + usb mavlink output throttled if (counter % (uint32_t)global_data.param[PARAM_BOTTOM_FLOW_SERIAL_THROTTLE_FACTOR] == 0)//throttling factor { @@ -577,7 +571,7 @@ int main(void) lpos.x += ground_distance*accumulated_flow_x; lpos.y += ground_distance*accumulated_flow_y; lpos.z = -ground_distance; - /* velocity not directly measured and not important for testing */ + /* velocity not directly measured and not important for testing */ lpos.vx = 0; lpos.vy = 0; lpos.vz = 0; diff --git a/src/modules/flow/mt9v034.c b/src/modules/flow/mt9v034.c index 33d02a6..833e197 100644 --- a/src/modules/flow/mt9v034.c +++ b/src/modules/flow/mt9v034.c @@ -125,7 +125,9 @@ void mt9v034_context_configuration(void) uint16_t aec_low_pass = 0x01; // default VALID RANGE: 0-2 uint16_t agc_update_freq = 0x02; // default Number of frames to skip between changes in AGC VALID RANGE: 0-15 uint16_t agc_low_pass = 0x02; // default VALID RANGE: 0-2 - + uint16_t analog_gain_control = 16; + uint16_t black_level_control = 0x01; + uint16_t black_level_calibration = 0x50; if (FLOAT_AS_BOOL(global_data.param[PARAM_IMAGE_LOW_LIGHT])) { @@ -195,6 +197,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 */ @@ -209,6 +212,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); @@ -228,6 +232,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); } From 87bee294f01f39554ad54ed5bd49fd95dd37d4e6 Mon Sep 17 00:00:00 2001 From: Ludvig Ericson Date: Thu, 15 Jun 2017 11:50:23 +0200 Subject: [PATCH 2/7] Use floats for whitening --- src/modules/flow/dcmi.c | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/src/modules/flow/dcmi.c b/src/modules/flow/dcmi.c index 21441b0..998ffb4 100644 --- a/src/modules/flow/dcmi.c +++ b/src/modules/flow/dcmi.c @@ -275,21 +275,21 @@ void dma_copy_image_buffers(uint8_t ** current_image, uint8_t ** previous_image, } void whitened_image(uint8_t *source, uint8_t *dest, uint16_t image_size) { - double sum = 0.0; + float sum = 0.0f; for (uint16_t pixel = 0; pixel < image_size; pixel++) sum += source[pixel]; - double mean = sum / image_size; - double rss = 0.0; + float mean = sum / image_size; + float rss = 0.0f; for (uint16_t pixel = 0; pixel < image_size; pixel++) - rss += pow(source[pixel] - mean, 2); - double stddev = sqrt(rss/(image_size - 1)); + rss += (source[pixel] - mean)*(source[pixel] - mean); + float stddev = sqrtf(rss/(image_size - 1)); dest[0] = stddev; for (uint16_t pixel = 0; pixel < image_size; pixel++) { - double v = 127.0 + 32.0*(source[pixel] - mean)/stddev; - if (v < 0.0) + float v = 127.0f + 32.0f*(source[pixel] - mean)/stddev; + if (v < 0.0f) v = 0; - if (v > 255.0) + if (v > 255.0f) v = 255; dest[pixel] = (uint8_t)v; } From 11a961600dce1080dfdc8975a4bc876ee3566ee9 Mon Sep 17 00:00:00 2001 From: Ludvig Ericson Date: Thu, 15 Jun 2017 11:51:29 +0200 Subject: [PATCH 3/7] Add 4.8.2 as allowed compiler --- makefiles/baremetal/toolchain_gnu-arm-eabi.mk | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/makefiles/baremetal/toolchain_gnu-arm-eabi.mk b/makefiles/baremetal/toolchain_gnu-arm-eabi.mk index fca471b..d646e02 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 +CROSSDEV_VER_SUPPORTED = 4.7.4 4.7.5 4.7.6 4.8.2 4.8.4 4.9.3 CROSSDEV_VER_FOUND = $(shell $(CC) -dumpversion) ifeq (,$(findstring $(CROSSDEV_VER_FOUND), $(CROSSDEV_VER_SUPPORTED))) @@ -276,4 +276,4 @@ define SYM_TO_BIN @$(ECHO) "BIN: $2" @$(MKDIR) -p $(dir $2) $(Q) $(OBJCOPY) -O binary $1 $2 -endef \ No newline at end of file +endef From 126320616a7617dbadf5d72bc064cbff8eb228fa Mon Sep 17 00:00:00 2001 From: Ludvig Ericson Date: Mon, 19 Jun 2017 10:27:52 +0200 Subject: [PATCH 4/7] Disable automatic gain control, set gain to 1 --- src/modules/flow/mt9v034.c | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/modules/flow/mt9v034.c b/src/modules/flow/mt9v034.c index 833e197..310a645 100644 --- a/src/modules/flow/mt9v034.c +++ b/src/modules/flow/mt9v034.c @@ -125,7 +125,7 @@ void mt9v034_context_configuration(void) uint16_t aec_low_pass = 0x01; // default VALID RANGE: 0-2 uint16_t agc_update_freq = 0x02; // default Number of frames to skip between changes in AGC VALID RANGE: 0-15 uint16_t agc_low_pass = 0x02; // default VALID RANGE: 0-2 - uint16_t analog_gain_control = 16; + uint16_t analog_gain_control = 1; uint16_t black_level_control = 0x01; uint16_t black_level_calibration = 0x50; @@ -136,7 +136,7 @@ void mt9v034_context_configuration(void) desired_brightness = 58; // VALID RANGE: 8-64 resolution_ctrl = 0x0202;//10 bit linear hdr_enabled = 0x0000; // off - aec_agc_enabled = 0x0303; // on + aec_agc_enabled = 0x0101; // aec on, agc off coarse_sw1 = 0x01BB; // default from context A coarse_sw2 = 0x01D9; // default from context A shutter_width_ctrl = 0x0164; // default from context A @@ -149,7 +149,7 @@ void mt9v034_context_configuration(void) desired_brightness = 16; // VALID RANGE: 8-64 resolution_ctrl = 0x0202;//10bit linear hdr_enabled = 0x0000; // off - aec_agc_enabled = 0x0303; // on + aec_agc_enabled = 0x0101; // aec on, agc off coarse_sw1 = 0x01BB; // default from context A coarse_sw2 = 0x01D9; // default from context A shutter_width_ctrl = 0x0164; // default from context A From 30eb3d527fa7c9dde91f6e402b811e092f7ac6cb Mon Sep 17 00:00:00 2001 From: Ludvig Ericson Date: Mon, 19 Jun 2017 13:49:32 +0200 Subject: [PATCH 5/7] Add whitening parameter --- src/include/settings.h | 11 ++++++++++- src/modules/flow/main.c | 6 +++++- src/modules/flow/settings.c | 4 ++++ 3 files changed, 19 insertions(+), 2 deletions(-) diff --git a/src/include/settings.h b/src/include/settings.h index 0139530..2fc5fad 100644 --- a/src/include/settings.h +++ b/src/include/settings.h @@ -40,7 +40,15 @@ #define ONBOARD_PARAM_NAME_LENGTH 15 #define BOTTOM_FLOW_IMAGE_HEIGHT 64 #define BOTTOM_FLOW_IMAGE_WIDTH 64 -#define BOTTOM_FLOW_SEARCH_WINDOW_SIZE 4 +#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 @@ -117,6 +125,7 @@ enum global_param_id_t PARAM_MAX_FLOW_PIXEL, PARAM_IMAGE_LOW_LIGHT, PARAM_IMAGE_ROW_NOISE_CORR, + PARAM_IMAGE_WHITEN, PARAM_IMAGE_TEST_PATTERN, PARAM_GYRO_SENSITIVITY_DPS, PARAM_GYRO_COMPENSATION_THRESHOLD, diff --git a/src/modules/flow/main.c b/src/modules/flow/main.c index ebfc517..ce3d5c9 100644 --- a/src/modules/flow/main.c +++ b/src/modules/flow/main.c @@ -427,7 +427,11 @@ int main(void) { /* copy recent image to faster ram */ dma_copy_image_buffers(¤t_image, &previous_image, image_size, 1); - whitened_image(current_image, current_image, image_size); + + if (FLOAT_AS_BOOL(global_data.param[PARAM_IMAGE_WHITEN])) { + /* whiten image for improved stability */ + whitened_image(current_image, current_image, image_size); + } /* compute optical flow */ qual = compute_flow(previous_image, current_image, x_rate, y_rate, z_rate, &pixel_flow_x, &pixel_flow_y); diff --git a/src/modules/flow/settings.c b/src/modules/flow/settings.c index d367d5b..8dd1b95 100644 --- a/src/modules/flow/settings.c +++ b/src/modules/flow/settings.c @@ -111,6 +111,10 @@ 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_WHITEN] = 1; + strcpy(global_data.param_name[PARAM_IMAGE_WHITEN], "IMAGE_WHITEN"); + global_data.param_access[PARAM_IMAGE_WHITEN] = 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; From f0771eefa707486c2db093ddcafb3acb1e5f7e2e Mon Sep 17 00:00:00 2001 From: Ludvig Ericson Date: Mon, 19 Jun 2017 13:49:51 +0200 Subject: [PATCH 6/7] Optimize types of whitening variables --- src/modules/flow/dcmi.c | 24 ++++++++++++++++-------- 1 file changed, 16 insertions(+), 8 deletions(-) diff --git a/src/modules/flow/dcmi.c b/src/modules/flow/dcmi.c index 998ffb4..92c13ed 100644 --- a/src/modules/flow/dcmi.c +++ b/src/modules/flow/dcmi.c @@ -274,19 +274,27 @@ void dma_copy_image_buffers(uint8_t ** current_image, uint8_t ** previous_image, (*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) { - float sum = 0.0f; + uint32_t sum = 0; for (uint16_t pixel = 0; pixel < image_size; pixel++) sum += source[pixel]; - float mean = sum / image_size; - float rss = 0.0f; - for (uint16_t pixel = 0; pixel < image_size; pixel++) - rss += (source[pixel] - mean)*(source[pixel] - mean); - float stddev = sqrtf(rss/(image_size - 1)); - dest[0] = stddev; + 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; + } + float stddev = sqrtf(rss/(image_size - 1)) / WHITENING_STDDEV; for (uint16_t pixel = 0; pixel < image_size; pixel++) { - float v = 127.0f + 32.0f*(source[pixel] - mean)/stddev; + float v = WHITENING_MEAN + (source[pixel] - mean)/stddev; if (v < 0.0f) v = 0; if (v > 255.0f) From dd01d8de0cbe808c9f846f7d430b24577a5357ba Mon Sep 17 00:00:00 2001 From: Ludvig Ericson Date: Mon, 19 Jun 2017 13:50:05 +0200 Subject: [PATCH 7/7] Increase manual gain, desired brightness --- src/modules/flow/mt9v034.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/flow/mt9v034.c b/src/modules/flow/mt9v034.c index 310a645..4b998f1 100644 --- a/src/modules/flow/mt9v034.c +++ b/src/modules/flow/mt9v034.c @@ -125,7 +125,7 @@ void mt9v034_context_configuration(void) uint16_t aec_low_pass = 0x01; // default VALID RANGE: 0-2 uint16_t agc_update_freq = 0x02; // default Number of frames to skip between changes in AGC VALID RANGE: 0-15 uint16_t agc_low_pass = 0x02; // default VALID RANGE: 0-2 - uint16_t analog_gain_control = 1; + uint16_t analog_gain_control = 16; uint16_t black_level_control = 0x01; uint16_t black_level_calibration = 0x50; @@ -146,7 +146,7 @@ void mt9v034_context_configuration(void) { min_exposure = 0x0001; max_exposure = 0x0080; - desired_brightness = 16; // VALID RANGE: 8-64 + desired_brightness = 32; // VALID RANGE: 8-64 resolution_ctrl = 0x0202;//10bit linear hdr_enabled = 0x0000; // off aec_agc_enabled = 0x0101; // aec on, agc off