Merge pull request #69 from PX4/camera_fix

Camera fix for block matching firmware
PX4 · Dec 10, 2015 · 4a314cf · 4a314cf
2 parents 0e3b191 + aa1d6b4
commit 4a314cf
Show file tree

Hide file tree

Showing 6 changed files with 34 additions and 15 deletions.
diff --git a/src/include/utils.h b/src/include/utils.h
@@ -40,8 +40,8 @@
 /**
 * @brief  Float to Ascii string
 */
-char* ftoa(float f);
-void ltoa(char *buf, unsigned long i, int base);
-void itoa(char *buf, unsigned int i, int base);
+char* flow_ftoa(float f);
+char* flow_ltoa(char *buf, unsigned long i, int base);
+char* flow_itoa(char *buf, unsigned int i, int base);
 
 #endif /* UTILS_H_ */
diff --git a/src/modules/flow/debug.c b/src/modules/flow/debug.c
@@ -170,14 +170,14 @@ void debug_message_send_one(void)
 		case(DEBUG_INT):
 			strncat(msg, m_debug_buf_pointer[m_debug_index_read], DEBUG_MAX_LEN);
 			strncat(msg, " ", DEBUG_MAX_LEN);
-			strncat(msg, ftoa(m_debug_buf_int[m_debug_index_read]), DEBUG_MAX_LEN);
+			strncat(msg, flow_ftoa(m_debug_buf_int[m_debug_index_read]), DEBUG_MAX_LEN);
 			msg[strlen(msg) - 2] = '\0'; // TODO workaround: cut ".0" of float
 			break;
 
 		case(DEBUG_FLOAT):
 			strncat(msg, m_debug_buf_pointer[m_debug_index_read], DEBUG_MAX_LEN);
 			strncat(msg, " ", DEBUG_MAX_LEN);
-			strncat(msg, ftoa(m_debug_buf_float[m_debug_index_read]), DEBUG_MAX_LEN);
+			strncat(msg, flow_ftoa(m_debug_buf_float[m_debug_index_read]), DEBUG_MAX_LEN);
 			break;
 
 		default:

diff --git a/src/modules/flow/main.c b/src/modules/flow/main.c
@@ -38,7 +38,6 @@
 #include <px4_config.h>
 #include <bsp/board.h>
 
-#include <stdlib.h>
 #include <stdio.h>
 #include <stdbool.h>
 #include <stdint.h>

diff --git a/src/modules/flow/mt9v034.c b/src/modules/flow/mt9v034.c
@@ -79,9 +79,12 @@ 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 = 350 + MINIMUM_HORIZONTAL_BLANKING;// 350 is minimum value without distortions
+	uint16_t new_hor_blanking_context_a = 709 + 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;
 
 
@@ -140,7 +143,7 @@ void mt9v034_context_configuration(void)
 	else
 	{
 		min_exposure = 0x0001;
-		max_exposure = 0x0040;
+		max_exposure = 0x0080;
 		desired_brightness = 16; // VALID RANGE: 8-64
 		resolution_ctrl = 0x0202;//10bit linear
 		hdr_enabled = 0x0000; // off
@@ -170,6 +173,16 @@ void mt9v034_context_configuration(void)
 	{
 		mt9v034_WriteReg16(MTV_CHIP_CONTROL_REG, new_control);
 
+		// Initialize frame control reg
+		mt9v034_WriteReg(0x72, 0x0000);
+
+		// Write reserved registers per Rev G datasheet table 8 recommendations
+		mt9v034_WriteReg16(0x13, 0x2D2E);
+		mt9v034_WriteReg16(0x20, 0x03C7);
+		mt9v034_WriteReg16(0x24, 0x001B);
+		mt9v034_WriteReg16(0x2B, 0x0003);
+		mt9v034_WriteReg16(0x2F, 0x0003);
+
 		/* Context A */
 		mt9v034_WriteReg16(MTV_WINDOW_WIDTH_REG_A, new_width_context_a);
 		mt9v034_WriteReg16(MTV_WINDOW_HEIGHT_REG_A, new_height_context_a);
@@ -181,7 +194,7 @@ void mt9v034_context_configuration(void)
 		mt9v034_WriteReg16(MTV_COARSE_SW_1_REG_A, coarse_sw1);
 		mt9v034_WriteReg16(MTV_COARSE_SW_2_REG_A, coarse_sw2);
 		mt9v034_WriteReg16(MTV_COARSE_SW_CTRL_REG_A, shutter_width_ctrl);
-		mt9v034_WriteReg16(MTV_V2_CTRL_REG_A, total_shutter_width);
+		mt9v034_WriteReg16(MTV_COARSE_SW_TOTAL_REG_A, total_shutter_width);
 
 
 		/* Context B */
@@ -195,7 +208,7 @@ void mt9v034_context_configuration(void)
 		mt9v034_WriteReg16(MTV_COARSE_SW_1_REG_B, coarse_sw1);
 		mt9v034_WriteReg16(MTV_COARSE_SW_2_REG_B, coarse_sw2);
 		mt9v034_WriteReg16(MTV_COARSE_SW_CTRL_REG_B, shutter_width_ctrl);
-		mt9v034_WriteReg16(MTV_V2_CTRL_REG_B, total_shutter_width);
+		mt9v034_WriteReg16(MTV_COARSE_SW_TOTAL_REG_B, total_shutter_width);
 
 		/* General Settings */
 		mt9v034_WriteReg16(MTV_ROW_NOISE_CORR_CTRL_REG, row_noise_correction);

diff --git a/src/modules/flow/sonar.c b/src/modules/flow/sonar.c
@@ -136,7 +136,10 @@ void UART4_IRQHandler(void)
 					dt = ((float)(measure_time - last_measure_time)) / 1000000.0f;
 
 					valid_data = temp;
-					sonar_mode = insert_sonar_value_and_get_mode_value(valid_data / SONAR_SCALE);
+					// the mode filter turned out to be more problematic
+					// than using the raw value of the sonar
+					//insert_sonar_value_and_get_mode_value(valid_data / SONAR_SCALE);
+					sonar_mode = valid_data / SONAR_SCALE;
 					new_value = 1;
 					sonar_valid = true;
 				} else {

diff --git a/src/modules/flow/utils.c b/src/modules/flow/utils.c
@@ -41,7 +41,7 @@ long	L;
 float	F;
 }	 LF_t;
 
-char *ftoa(float f) //, int *status)
+char *flow_ftoa(float f) //, int *status)
 {
 	long mantissa, int_part, frac_part;
 	short exp2;
@@ -97,7 +97,7 @@ char *ftoa(float f) //, int *status)
 		*p++ = '0';
 	else
 	{
-		ltoa(p, int_part, 10);
+		flow_ltoa(p, int_part, 10);
 		while (*p)
 		p++;
 	}
@@ -132,7 +132,7 @@ char *ftoa(float f) //, int *status)
 }
 
 
-void ltoa(char *buf, unsigned long i, int base)
+char* flow_ltoa(char *buf, unsigned long i, int base)
 {
 	char *s;
 	#define LEN	25
@@ -156,9 +156,11 @@ void ltoa(char *buf, unsigned long i, int base)
 			}
 		}
 	strcpy(buf, s);
+
+	return buf;
 }
 
-void itoa(char *buf, unsigned int i, int base)
+char* flow_itoa(char *buf, unsigned int i, int base)
 {
 	char *s;
 	const int len = 10;
@@ -182,4 +184,6 @@ void itoa(char *buf, unsigned int i, int base)
 			}
 		}
 	strcpy(buf, s);
+
+	return buf;
 }