From ee88862a18bcbadf6e13db3b3de13fe980ffb275 Mon Sep 17 00:00:00 2001 From: James Goppert Date: Tue, 6 Sep 2016 17:21:32 -0400 Subject: [PATCH] Improve performance for indoors and auto exposure/gain, add more params. --- .gitignore | 1 + notebooks/flow_analysis.ipynb | 78 ++++++++++++++++++++++++++++++++ src/include/settings.h | 13 +++++- src/modules/flow/communication.c | 17 +++++++ src/modules/flow/flow.c | 34 +++++++------- src/modules/flow/main.c | 42 ++++++++++------- src/modules/flow/mt9v034.c | 59 ++++++++++-------------- src/modules/flow/settings.c | 48 +++++++++++++++++--- 8 files changed, 214 insertions(+), 78 deletions(-) create mode 100644 notebooks/flow_analysis.ipynb diff --git a/.gitignore b/.gitignore index 8524587..4dd2158 100644 --- a/.gitignore +++ b/.gitignore @@ -46,3 +46,4 @@ xcode src/platforms/posix/px4_messages/ src/platforms/posix-arm/px4_messages/ src/platforms/qurt/px4_messages/ +*.ipynb_checkpoints diff --git a/notebooks/flow_analysis.ipynb b/notebooks/flow_analysis.ipynb new file mode 100644 index 0000000..3b20e01 --- /dev/null +++ b/notebooks/flow_analysis.ipynb @@ -0,0 +1,78 @@ +{ + "cells": [ + { + "cell_type": "code", + "execution_count": 1, + "metadata": { + "collapsed": false + }, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "Using matplotlib backend: Qt5Agg\n", + "Populating the interactive namespace from numpy and matplotlib\n" + ] + } + ], + "source": [ + "%pylab" + ] + }, + { + "cell_type": "code", + "execution_count": 20, + "metadata": { + "collapsed": false + }, + "outputs": [ + { + "data": { + "text/plain": [ + "183.23012438744959" + ] + }, + "execution_count": 20, + "metadata": {}, + "output_type": "execute_result" + } + ], + "source": [ + "s = 64 # image size\n", + "win = 4 # search area, pixels\n", + "vel = 1 # velocity\n", + "d = 1 # distance above ground\n", + "rate = 100 # hz\n", + "\n", + "fov = np.deg2rad(5)\n", + "f = s/(2*(tan(fov/2))) # focal length, pixels\n", + "x = win * d/f\n", + "rate = vel/x #rate, hz\n", + "rate" + ] + } + ], + "metadata": { + "anaconda-cloud": {}, + "kernelspec": { + "display_name": "Python [default]", + "language": "python", + "name": "python3" + }, + "language_info": { + "codemirror_mode": { + "name": "ipython", + "version": 3 + }, + "file_extension": ".py", + "mimetype": "text/x-python", + "name": "python", + "nbconvert_exporter": "python", + "pygments_lexer": "ipython3", + "version": "3.5.2" + } + }, + "nbformat": 4, + "nbformat_minor": 1 +} diff --git a/src/include/settings.h b/src/include/settings.h index 0139530..dcbe4b2 100644 --- a/src/include/settings.h +++ b/src/include/settings.h @@ -40,7 +40,7 @@ #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 /****************************************************************** * ALL TYPE DEFINITIONS @@ -139,9 +139,18 @@ enum global_param_id_t PARAM_BOTTOM_FLOW_GYRO_COMPENSATION, PARAM_BOTTOM_FLOW_LP_FILTERED, PARAM_BOTTOM_FLOW_WEIGHT_NEW, - PARAM_BOTTOM_FLOW_SERIAL_THROTTLE_FACTOR, + PARAM_BOTTOM_FLOW_PUB_RATE, PARAM_SENSOR_POSITION, + PARAM_GAIN_MAX, + PARAM_EXPOSURE_MAX, + PARAM_SHTR_W_1, + PARAM_SHTR_W_2, + PARAM_SHTR_W_TOT, + PARAM_HDR, + PARAM_AEC, + PARAM_AGC, + PARAM_BRIGHT, DEBUG_VARIABLE, ONBOARD_PARAM_COUNT diff --git a/src/modules/flow/communication.c b/src/modules/flow/communication.c index 206e5d2..1955cba 100644 --- a/src/modules/flow/communication.c +++ b/src/modules/flow/communication.c @@ -273,6 +273,23 @@ void handle_mavlink_message(mavlink_channel_t chan, l3gd20_config(); } + /* shutter/ exposure params */ + else if(i == PARAM_SHTR_W_1 || + i == PARAM_SHTR_W_2 || + i == PARAM_SHTR_W_TOT || + i == PARAM_EXPOSURE_MAX || + i == PARAM_HDR || + i == PARAM_AEC || + i == PARAM_AGC || + i == PARAM_BRIGHT || + i == PARAM_GAIN_MAX + ) + { + mt9v034_context_configuration(); + dma_reconfigure(); + buffer_reset(); + } + else { debug_int_message_buffer("Parameter received, param id =", i); diff --git a/src/modules/flow/flow.c b/src/modules/flow/flow.c index 9255f2a..db962f9 100644 --- a/src/modules/flow/flow.c +++ b/src/modules/flow/flow.c @@ -501,23 +501,23 @@ uint8_t compute_flow(uint8_t *image1, uint8_t *image2, float x_rate, float y_rat /* create flow image if needed (image1 is not needed anymore) * -> can be used for debugging purpose */ -// if (global_data.param[PARAM_USB_SEND_VIDEO] )//&& global_data.param[PARAM_VIDEO_USB_MODE] == FLOW_VIDEO) -// { -// -// for (j = pixLo; j < pixHi; j += pixStep) -// { -// for (i = pixLo; i < pixHi; 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]) -// { -// image1[j * ((uint16_t) global_data.param[PARAM_IMAGE_WIDTH]) + i] = 255; -// } -// -// } -// } -// } + if (global_data.param[PARAM_USB_SEND_VIDEO] )//&& global_data.param[PARAM_VIDEO_USB_MODE] == FLOW_VIDEO) + { + + for (j = pixLo; j < pixHi; j += pixStep) + { + for (i = pixLo; i < pixHi; 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]) + { + image1[j * ((uint16_t) global_data.param[PARAM_IMAGE_WIDTH]) + i] = 255; + } + + } + } + } /* evaluate flow calculation */ if (meancount > 10) diff --git a/src/modules/flow/main.c b/src/modules/flow/main.c index fb11e9f..2293bab 100644 --- a/src/modules/flow/main.c +++ b/src/modules/flow/main.c @@ -342,6 +342,7 @@ int main(void) static uint32_t integration_timespan = 0; static uint32_t lasttime = 0; uint32_t time_since_last_sonar_update= 0; + uint32_t time_last_pub= 0; uavcan_start(); /* main loop */ @@ -438,6 +439,22 @@ int main(void) float flow_compx = pixel_flow_x / focal_length_px / (get_time_between_images() / 1000000.0f); float flow_compy = pixel_flow_y / focal_length_px / (get_time_between_images() / 1000000.0f); + if (qual > 0) + { + valid_frame_count++; + + uint32_t deltatime = (get_boot_time_us() - lasttime); + integration_timespan += deltatime; + accumulated_flow_x += pixel_flow_y / focal_length_px * 1.0f; //rad axis swapped to align x flow around y axis + accumulated_flow_y += pixel_flow_x / focal_length_px * -1.0f;//rad + accumulated_gyro_x += x_rate * deltatime / 1000000.0f; //rad + accumulated_gyro_y += y_rate * deltatime / 1000000.0f; //rad + accumulated_gyro_z += z_rate * deltatime / 1000000.0f; //rad + accumulated_framecount++; + accumulated_quality += qual; + } + + /* integrate velocity and output values only if distance is valid */ if (distance_valid) { @@ -451,17 +468,6 @@ int main(void) { velocity_x_sum += new_velocity_x; velocity_y_sum += new_velocity_y; - valid_frame_count++; - - uint32_t deltatime = (get_boot_time_us() - lasttime); - integration_timespan += deltatime; - accumulated_flow_x += pixel_flow_y / focal_length_px * 1.0f; //rad axis swapped to align x flow around y axis - accumulated_flow_y += pixel_flow_x / focal_length_px * -1.0f;//rad - accumulated_gyro_x += x_rate * deltatime / 1000000.0f; //rad - accumulated_gyro_y += y_rate * deltatime / 1000000.0f; //rad - accumulated_gyro_z += z_rate * deltatime / 1000000.0f; //rad - accumulated_framecount++; - accumulated_quality += qual; /* lowpass velocity output */ velocity_x_lp = global_data.param[PARAM_BOTTOM_FLOW_WEIGHT_NEW] * new_velocity_x + @@ -533,8 +539,11 @@ int main(void) PROBE_3(true); //serial mavlink + usb mavlink output throttled - if (counter % (uint32_t)global_data.param[PARAM_BOTTOM_FLOW_SERIAL_THROTTLE_FACTOR] == 0)//throttling factor + uint32_t now = get_boot_time_us(); + uint32_t time_since_last_pub = now - time_last_pub; + if (time_since_last_pub > (1.0e6f/global_data.param[PARAM_BOTTOM_FLOW_PUB_RATE])) { + time_last_pub = now; float flow_comp_m_x = 0.0f; float flow_comp_m_y = 0.0f; @@ -577,10 +586,9 @@ 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 */ - lpos.vx = 0; - lpos.vy = 0; - lpos.vz = 0; + lpos.vx = ground_distance*accumulated_flow_x/integration_timespan; + lpos.vy = ground_distance*accumulated_flow_y/integration_timespan; + lpos.vz = 0; // no direct measurement, just ignore } else { /* toggling param allows user reset */ @@ -699,7 +707,7 @@ 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 *) current_image)[frame * MAVLINK_MSG_ENCAPSULATED_DATA_FIELD_DATA_LEN]); + 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 33d02a6..ebfd6b9 100644 --- a/src/modules/flow/mt9v034.c +++ b/src/modules/flow/mt9v034.c @@ -79,7 +79,7 @@ void mt9v034_context_configuration(void) uint16_t new_height_context_b = FULL_IMAGE_COLUMN_SIZE * 4; /* blanking settings */ - uint16_t new_hor_blanking_context_a = 709 + MINIMUM_HORIZONTAL_BLANKING;// 709 is minimum value without distortions + uint16_t new_hor_blanking_context_a = 425 + MINIMUM_HORIZONTAL_BLANKING;// 709 is minimum value without distortions uint16_t new_ver_blanking_context_a = 10; // this value is the first without image errors (dark lines) uint16_t new_hor_blanking_context_b = MAX_IMAGE_WIDTH - new_width_context_b + MINIMUM_HORIZONTAL_BLANKING; if (new_hor_blanking_context_b < 800) { @@ -109,51 +109,38 @@ void mt9v034_context_configuration(void) * so we set max on 64 (lines) = 0x40 */ uint16_t min_exposure = 0x0001; // default - uint16_t max_exposure = 0x01E0; // default - uint16_t new_max_gain = 64; // VALID RANGE: 16-64 (default) uint16_t pixel_count = 4096; //64x64 take all pixels to estimate exposure time // VALID RANGE: 1-65535 - - uint16_t desired_brightness = 58; // default - uint16_t resolution_ctrl = 0x0203; // default - uint16_t hdr_enabled = 0x0100; // default - uint16_t aec_agc_enabled = 0x0003; // default - uint16_t coarse_sw1 = 0x01BB; // default from context A - uint16_t coarse_sw2 = 0x01D9; // default from context A uint16_t shutter_width_ctrl = 0x0164; // default from context A - uint16_t total_shutter_width = 0x01E0; // default from context A uint16_t aec_update_freq = 0x02; // default Number of frames to skip between changes in AEC VALID RANGE: 0-15 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 resolution_ctrl = 0x0303; // 12 bit adc for low light + uint16_t max_gain = global_data.param[PARAM_GAIN_MAX]; + uint16_t max_exposure = global_data.param[PARAM_EXPOSURE_MAX]; + uint16_t coarse_sw1 = global_data.param[PARAM_SHTR_W_1]; + uint16_t coarse_sw2 = global_data.param[PARAM_SHTR_W_2]; + uint16_t total_shutter_width = global_data.param[PARAM_SHTR_W_TOT]; + uint16_t hdr_enabled = 0x0000; + bool hdr_enable_flag = global_data.param[PARAM_HDR] > 0; + if (hdr_enable_flag) { + hdr_enabled = 0x0100; + } - if (FLOAT_AS_BOOL(global_data.param[PARAM_IMAGE_LOW_LIGHT])) - { - min_exposure = 0x0001; - max_exposure = 0x0040; - desired_brightness = 58; // VALID RANGE: 8-64 - resolution_ctrl = 0x0202;//10 bit linear - hdr_enabled = 0x0000; // off - aec_agc_enabled = 0x0303; // on - coarse_sw1 = 0x01BB; // default from context A - coarse_sw2 = 0x01D9; // default from context A - shutter_width_ctrl = 0x0164; // default from context A - total_shutter_width = 0x01E0; // default from context A + bool aec_enable_flag = global_data.param[PARAM_AEC] > 0; + uint16_t aec_agc_enabled = 0x0000; + if (aec_enable_flag) { + aec_agc_enabled |= (1 << 0); } - else - { - min_exposure = 0x0001; - max_exposure = 0x0080; - desired_brightness = 16; // VALID RANGE: 8-64 - resolution_ctrl = 0x0202;//10bit linear - hdr_enabled = 0x0000; // off - aec_agc_enabled = 0x0303; // on - coarse_sw1 = 0x01BB; // default from context A - coarse_sw2 = 0x01D9; // default from context A - shutter_width_ctrl = 0x0164; // default from context A - total_shutter_width = 0x01E0; // default from context A + + bool agc_enable_flag = global_data.param[PARAM_AGC] > 0; + if (agc_enable_flag) { + aec_agc_enabled |= (1 << 1); } + uint16_t desired_brightness = global_data.param[PARAM_BRIGHT]; + uint16_t row_noise_correction = 0x0000; // default uint16_t test_data = 0x0000; // default @@ -216,7 +203,7 @@ void mt9v034_context_configuration(void) mt9v034_WriteReg16(MTV_HDR_ENABLE_REG, hdr_enabled); // disable HDR on both contexts mt9v034_WriteReg16(MTV_MIN_EXPOSURE_REG, min_exposure); mt9v034_WriteReg16(MTV_MAX_EXPOSURE_REG, max_exposure); - mt9v034_WriteReg16(MTV_MAX_GAIN_REG, new_max_gain); + mt9v034_WriteReg16(MTV_MAX_GAIN_REG, max_gain); mt9v034_WriteReg16(MTV_AGC_AEC_PIXEL_COUNT_REG, pixel_count); mt9v034_WriteReg16(MTV_AGC_AEC_DESIRED_BIN_REG, desired_brightness); mt9v034_WriteReg16(MTV_ADC_RES_CTRL_REG, resolution_ctrl); // here is the way to regulate darkness :) diff --git a/src/modules/flow/settings.c b/src/modules/flow/settings.c index d367d5b..4b0488e 100644 --- a/src/modules/flow/settings.c +++ b/src/modules/flow/settings.c @@ -73,7 +73,7 @@ void global_data_reset_param_defaults(void){ strcpy(global_data.param_name[PARAM_SYSTEM_SEND_STATE], "SYS_SEND_STATE"); global_data.param_access[PARAM_SYSTEM_SEND_STATE] = READ_WRITE; - global_data.param[PARAM_SYSTEM_SEND_LPOS] = 0; + global_data.param[PARAM_SYSTEM_SEND_LPOS] = 1; strcpy(global_data.param_name[PARAM_SYSTEM_SEND_LPOS], "SYS_SEND_LPOS"); global_data.param_access[PARAM_SYSTEM_SEND_LPOS] = READ_WRITE; @@ -159,7 +159,7 @@ void global_data_reset_param_defaults(void){ strcpy(global_data.param_name[PARAM_VIDEO_ONLY], "VIDEO_ONLY"); global_data.param_access[PARAM_VIDEO_ONLY] = READ_WRITE; - global_data.param[PARAM_VIDEO_RATE] = 150; + global_data.param[PARAM_VIDEO_RATE] = 50; strcpy(global_data.param_name[PARAM_VIDEO_RATE], "VIDEO_RATE"); global_data.param_access[PARAM_VIDEO_RATE] = READ_WRITE; @@ -173,7 +173,7 @@ void global_data_reset_param_defaults(void){ global_data.param_access[PARAM_BOTTOM_FLOW_VALUE_THRESHOLD] = READ_WRITE; // global_data.param[PARAM_BOTTOM_FLOW_FEATURE_THRESHOLD] = 100; - global_data.param[PARAM_BOTTOM_FLOW_FEATURE_THRESHOLD] = 30; // threshold is irrelevant with this value + global_data.param[PARAM_BOTTOM_FLOW_FEATURE_THRESHOLD] = 20; // threshold is irrelevant with this value strcpy(global_data.param_name[PARAM_BOTTOM_FLOW_FEATURE_THRESHOLD], "BFLOW_F_THLD"); global_data.param_access[PARAM_BOTTOM_FLOW_FEATURE_THRESHOLD] = READ_WRITE; @@ -195,9 +195,45 @@ void global_data_reset_param_defaults(void){ strcpy(global_data.param_name[PARAM_BOTTOM_FLOW_WEIGHT_NEW], "BFLOW_W_NEW"); global_data.param_access[PARAM_BOTTOM_FLOW_WEIGHT_NEW] = READ_WRITE; - global_data.param[PARAM_BOTTOM_FLOW_SERIAL_THROTTLE_FACTOR] = 10.0f; - strcpy(global_data.param_name[PARAM_BOTTOM_FLOW_SERIAL_THROTTLE_FACTOR], "BFLOW_THROTT"); - global_data.param_access[PARAM_BOTTOM_FLOW_SERIAL_THROTTLE_FACTOR] = READ_WRITE; + global_data.param[PARAM_BOTTOM_FLOW_PUB_RATE] = 10.0f; + strcpy(global_data.param_name[PARAM_BOTTOM_FLOW_PUB_RATE], "BFLOW_RATE"); + global_data.param_access[PARAM_BOTTOM_FLOW_PUB_RATE] = READ_WRITE; + + global_data.param[PARAM_EXPOSURE_MAX] = 2000; + strcpy(global_data.param_name[PARAM_EXPOSURE_MAX], "EXPOSURE_MAX"); + global_data.param_access[PARAM_EXPOSURE_MAX] = READ_WRITE; + + global_data.param[PARAM_GAIN_MAX] = 16; + strcpy(global_data.param_name[PARAM_GAIN_MAX], "GAIN_MAX"); + global_data.param_access[PARAM_GAIN_MAX] = READ_WRITE; + + global_data.param[PARAM_SHTR_W_1] = 443; + strcpy(global_data.param_name[PARAM_SHTR_W_1], "SHTR_W_1"); + global_data.param_access[PARAM_SHTR_W_1] = READ_WRITE; + + global_data.param[PARAM_SHTR_W_2] = 473; + strcpy(global_data.param_name[PARAM_SHTR_W_2], "SHTR_W_2"); + global_data.param_access[PARAM_SHTR_W_2] = READ_WRITE; + + global_data.param[PARAM_SHTR_W_TOT] = 480; + strcpy(global_data.param_name[PARAM_SHTR_W_TOT], "SHTR_W_TOT"); + global_data.param_access[PARAM_SHTR_W_TOT] = READ_WRITE; + + global_data.param[PARAM_HDR] = 1; + strcpy(global_data.param_name[PARAM_HDR], "HDR"); + global_data.param_access[PARAM_HDR] = READ_WRITE; + + global_data.param[PARAM_AEC] = 1; + strcpy(global_data.param_name[PARAM_AEC], "AEC"); + global_data.param_access[PARAM_AEC] = READ_WRITE; + + global_data.param[PARAM_AGC] = 1; + strcpy(global_data.param_name[PARAM_AGC], "AGC"); + global_data.param_access[PARAM_AGC] = READ_WRITE; + + global_data.param[PARAM_BRIGHT] = 15; + strcpy(global_data.param_name[PARAM_BRIGHT], "BRIGHT"); + global_data.param_access[PARAM_BRIGHT] = READ_WRITE; global_data.param[DEBUG_VARIABLE] = 1; strcpy(global_data.param_name[DEBUG_VARIABLE], "DEBUG");