Improve performance for indoors and auto exposure/gain, add more params.

jgoppert · jgoppert · commit 08ced78aad54 · 2016-10-12T01:36:11.000-04:00
diff --git 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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -79,13 +79,13 @@ 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) {
 		new_hor_blanking_context_b = 800;
 	}
-	uint16_t new_ver_blanking_context_b = 10;
+	uint16_t new_ver_blanking_context_b = 5;
 
 
 	/* Read Mode
@@ -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
