af: Add basic autofocus support.

main.c

@@ -46,6 +46,10 @@ static int exposure;
 static int exposure_max;
 static int exposure_min;
 
+static bool focus_is_manual = true;
+static int focus;
+static int focus_phase;
+
 static bool wb_is_manual = true;
 
 static const bool sw_auto = true;
@@ -125,7 +129,7 @@ update_io_pipeline(void)
 
 #define FIXUP(x, res) if (x < x##_min) { x = x##_min; res = 1; } if (x > x##_max) { x = x##_max; res = 1; }
 
-int auto_exposure(const struct image_stats *stats)
+static int auto_exposure(const struct image_stats *stats)
 {
 	int step, res = 0;
 	step = exposure / 16;
@@ -136,7 +140,7 @@ int auto_exposure(const struct image_stats *stats)
 	return res;
 }
 
-int auto_gain(const struct image_stats *stats)
+static int auto_gain(const struct image_stats *stats)
 {
 	int step, res = 0;
 
@@ -148,11 +152,64 @@ int auto_gain(const struct image_stats *stats)
 	return res;
 }
 
+static void auto_focus_start(void)
+{
+	focus_phase = 0;
+	focus = 0;
+}
 
-void auto_adjust(const struct image_stats *stats)
+#define PH_SWEEP 5
+#define PH_DONE 6
+
+static void auto_focus_step(const struct image_stats *stats)
 {
-	int step;
+	static uint64_t best_sharpness, best_focus;
+	static const bool debug;
+
+	if (focus_phase >= PH_DONE) {
+		focus_phase++;
+		if (stats->sharpness < best_sharpness * 0.6) {
+			if (debug) printf("Focus lost, restart.\n");
+			auto_focus_start();
+		}
+		return;
+	}
+	if (debug) printf("Phase %d, sharp %d best %d ", focus_phase, (int)stats->sharpness / 10000, (int)best_sharpness/ 10000);
+	if (focus_phase < PH_SWEEP) {
+		best_sharpness = 0;
+		focus = 200;
+		focus_phase ++;
+		best_focus = 0;
+		if (debug) printf("...prepare\n");
+		goto set;
+	}
+	if (stats->sharpness > best_sharpness) {
+		if (debug) printf("Still improving, focus %d\n", focus);
+		best_focus = focus;
+		best_sharpness = stats->sharpness;
+		focus += 10;
+		goto set;
+	}
+	if (stats->sharpness < best_sharpness * 0.8) {
+		if (debug) printf("AF done?\n");
+		focus = best_focus - 10;
+		focus_phase = PH_DONE;
+		goto set;
+	}
+	if (focus > 1023) {
+		if (debug) printf("Finished range\n");
+		focus = best_focus - 10;
+		focus_phase = PH_DONE;
+		goto set;
+	}
+	if (debug) printf("Not improving\n");
+	focus += 10;
+ set:
+	set_focus(focus);
+}
 
+void auto_adjust(const struct image_stats *stats)
+{
 	if (sw_auto) {
 		if (!exposure_is_manual && gain_is_manual)
 			auto_exposure(stats);
@@ -188,9 +245,14 @@ void auto_adjust(const struct image_stats *stats)
 		wb += stats->balance_adjust;
 		FIXUP(wb, res);
 		if (stats->balance_adjust)
-		  mp_calculate_matrices(wb);
+			mp_calculate_matrices(wb);
+		if (res)
+			printf("White balance beyond limits\n");
 	}
 
+	if (!focus_is_manual)
+		auto_focus_step(stats);
+
 	update_io_pipeline();
 }
 
@@ -368,14 +430,17 @@ draw_controls(void)
 		sprintf(iso, "auto");
 	}
 
-	sprintf(f, "%d", focus);
+	if (focus_is_manual) {
+		sprintf(f, "%d", focus);
+	} else {
+		sprintf(f, "a %d", focus);
+	}
 
 	if (wb_is_manual) {
 		sprintf(wb_text, "%s", WB_ILLUMINANTS[wb]);
 	} else {
 		sprintf(wb_text, "a %s", WB_ILLUMINANTS[wb]);
 	}
-	
 
 	if (status_surface)
 		cairo_surface_destroy(status_surface);
@@ -469,7 +534,6 @@ draw_controls(void)
 
 	cairo_move_to(cr, 200, 16);
 	cairo_text_path(cr, debug_message);
-	printf("Paint: %s\n", debug_message);
 	cairo_stroke(cr);
 
 	cairo_destroy(cr);
@@ -814,9 +878,17 @@ on_control_auto_toggled(GtkToggleButton *widget, gpointer user_data)
 			has_changed = true;
 		}
 		break;
+	case USER_CONTROL_FOCUS:
+		focus_is_manual = is_manual;
+		if (is_manual)
+			has_changed = true;
+		else
+			auto_focus_start();
+		break;
 	case USER_CONTROL_WHITE_BALANCE:
 		wb_is_manual = is_manual;
-		has_changed = true;
+		if (is_manual)
+			has_changed = true;
 		break;
 	}
 
@@ -827,12 +899,19 @@ on_control_auto_toggled(GtkToggleButton *widget, gpointer user_data)
 
 		switch (current_control) {
 		case USER_CONTROL_ISO:
-			value = (double)gain;
+			value = (double) gain;
 			break;
 		case USER_CONTROL_SHUTTER:
 			value = (double) exposure;
 			break;
+		case USER_CONTROL_FOCUS:
+			value = (double) (1.0 - sqrt(focus / (double)DW9714_FOCUS_VAL_MAX)) * DW9714_FOCUS_VAL_MAX;
+			break;
+		case USER_CONTROL_WHITE_BALANCE:
+			value = (double) wb;
+			break;
 		default:
+			printf("Unexpected control\n");
 			return;
 		}
 
@@ -903,14 +982,16 @@ on_control_slider_changed(GtkAdjustment *widget, gpointer user_data)
 		}
 		break;
 	}
-	case USER_CONTROL_FOCUS:
-		if (value != focus) {
-			focus = round(DW9714_FOCUS_VAL_MAX * pow(1.0 - value / (double)DW9714_FOCUS_VAL_MAX, 2));
+	case USER_CONTROL_FOCUS: {
+		int new = round(DW9714_FOCUS_VAL_MAX * pow(1.0 - value / (double)DW9714_FOCUS_VAL_MAX, 2));
+		if (new != focus) {
+			focus = new;
 			has_changed = true;
 			set_focus(focus);
 		}
 		break;
 	}
+	}
 
 	if (has_changed) {
 		update_io_pipeline();

process_pipeline.c

@@ -189,9 +189,10 @@ compute_statistics(uint32_t *dst, const uint32_t width, const uint32_t height, s
 {
 	uint32_t y, x, p;
 	uint32_t total = 0, too_bright = 0, bright = 0;
-	uint64_t sum_g = 0, sum_r = 0, sum_b = 0;
-  
-	for (y=0; y<height; y++)
+	uint64_t sum_g = 0, sum_r = 0, sum_b = 0, sharp = 0;
+
+	for (y=0; y<height; y++) {
+		uint8_t last_m;
 		for (x=0; x<width; x++) {
 			uint8_t r, g, b, m;
 			p = y*width + x;
@@ -205,28 +206,36 @@ compute_statistics(uint32_t *dst, const uint32_t width, const uint32_t height, s
 				too_bright++;
 			if (m > 200)
 				bright++;
+			if (x > width / 3 && x < 2*width / 3)
+				if (y > height / 3 && x < 2*height / 3)
+					sharp += (m-last_m) * (m-last_m);
 			total++;
 			sum_r += r;
 			sum_g += g;
 			sum_b += b;
+			last_m = m;
 		}
+	}
+
+	stats->sharpness = sharp;
 
 	uint32_t p_bright = (bright*100)/total,
 		p_too_bright = (too_bright*100)/total;
-	//printf("%d bright, %d too bright, %d total -- ", bright, too_bright, total);
-	printf("%d %% bright, %d %% too bright -- ", p_bright, p_too_bright);
+	static const bool debug;
+
+	if (debug) printf("%d %% bright, %d %% too bright -- ", p_bright, p_too_bright);
 
 	stats->exposure_adjust = 0;
 	if (p_bright < 1)
 		stats->exposure_adjust = +1;
 	if (p_too_bright > 8)
 		stats->exposure_adjust = -1;
-	printf("ae %d", stats->exposure_adjust);
+	if (debug) printf("ae %d", stats->exposure_adjust);
 
 	float r = (float) sum_r/sum_g, b = (float) sum_b/sum_g;
 	stats->balance_adjust = 0;
 
-	printf(", r %.2f b %.2f", r, b);
+	if (debug) printf(", r %.2f b %.2f", r, b);
 
 	/* With 1.1 and 1.2, I got oscillations in some cases */
 	if (r > b * 1.3)
@@ -234,8 +243,9 @@ compute_statistics(uint32_t *dst, const uint32_t width, const uint32_t height, s
 	if (b > r * 1.3)
 		stats->balance_adjust = 1;
 	
-	printf(" awb %d", stats->balance_adjust);
-	printf("\n");
+	if (debug) printf(" awb %d", stats->balance_adjust);
+	if (debug) printf(" sharp %d", (int) sharp / 10000);
+	if (debug) printf("\n");
 }
 
 static cairo_surface_t *
@@ -258,12 +268,12 @@ process_image_for_preview(const MPImage *image)
 			  module.camera->blacklevel, skip);
 
 	{
-		static char msg[10240];
+		static char msg[1024];
 		struct image_stats stats;
 	  
 		compute_statistics((uint32_t *) pixels, surface_width, surface_height, &stats);
 
-		sprintf(msg, "%d", stats.exposure_adjust);
+		sprintf(msg, "");
 		debug_message = msg;
 		auto_adjust(&stats);
 	}

process_pipeline.h

@@ -31,6 +31,7 @@ struct mp_process_pipeline_state {
 
 struct image_stats {
 	int exposure_adjust, balance_adjust;
+	uint64_t sharpness;
 };
 
 void mp_process_pipeline_start();