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 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 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/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/main.c b/src/modules/flow/main.c index fb11e9f..ce3d5c9 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) { @@ -427,6 +428,11 @@ int main(void) /* copy recent image to faster ram */ dma_copy_image_buffers(¤t_image, &previous_image, image_size, 1); + 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); @@ -499,20 +505,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 +522,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 +575,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..4b998f1 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])) { @@ -134,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 @@ -144,10 +146,10 @@ 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 = 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 @@ -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); } 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;