Merge pull request PX4#50 from 3drobotics/cleanup

Cleanup

src/include/camera.h

@@ -72,7 +72,7 @@
  * If there is a certain amount of change in the image brightness the tolerance band will decide after
  * how much brightness delta the exposure algorithm will start to re-adjust the exposure time.
  */
-#define CONFIG_CAMERA_DESIRED_EXPOSURE_TOL_8B (24u)
+#define CONFIG_CAMERA_DESIRED_EXPOSURE_TOL_8B (24)
 /**
  * The smoothing factor of the exposure time. This is the constant of an exponential filter.
  * 
@@ -84,7 +84,7 @@
 /**
  * The interval between updating the exposure value in number of frames. Snapshot and invalid frames are skipped.
  */
-#define CONFIG_CAMERA_EXPOSURE_UPDATE_INTERVAL (4u)
+#define CONFIG_CAMERA_EXPOSURE_UPDATE_INTERVAL (4)
 
 struct _camera_sensor_interface;
 typedef struct _camera_sensor_interface camera_sensor_interface;
@@ -189,14 +189,14 @@ bool camera_img_stream_schedule_param_change(camera_ctx *ctx, const camera_img_p
  *					It is guaranteed to retrieve consecutive images.
  * @param count		Number of most recent images to retrieve. Must be lower than buffer_count - 1 which was 
  *					passed to camera_init.
- * @param wait_for_new When true the function will wait until the the requested images are available.
- * @return			 0 when a set of count consecutive most recent images have been retrieved.
- *					 1 it is not possible to return count consecutive frames. you need to wait until the frames are captured.
- *					-1 on error.
+ * @param check_for_new When true the function will return true only if a new frame is pending. It will then clear the
+                        pending flag.
+ * @return	true when a set of count consecutive most recent images have been retrieved.
+            false when two consecutive images are not ready
  * @note			When this function is successful (return value 0) the buffers need to be returned to the camera driver before 
  *					requesting new buffers. (use camera_img_stream_return_buffers)
  */
-int camera_img_stream_get_buffers(camera_ctx *ctx, camera_image_buffer *buffers[], size_t count, bool wait_for_new);
+bool camera_img_stream_get_buffers(camera_ctx *ctx, camera_image_buffer *buffers[], size_t count, bool wait_for_new);
 
 /**
  * Returns the buffers that have been retrieved by camera_img_stream_get_buffers back to the camera driver.
@@ -393,7 +393,7 @@ struct _camera_ctx {
 	camera_img_param_ex img_stream_param;					///< The parameters of the image streaming mode.
 	camera_img_param img_stream_param_pend;					///< The pending image streaming mode parameters.
 	camera_image_buffer buffers[CONFIG_CAMERA_MAX_BUFFER_COUNT];///< The image buffers for image stream mode.
-	int buffer_count;										///< Total number of buffers.
+	size_t buffer_count;										///< Total number of buffers.
 	volatile uint8_t buf_avail[CONFIG_CAMERA_MAX_BUFFER_COUNT];///< Indexes to the buffers that are available. Ordered in the MRU order.
 	volatile uint8_t buf_avail_count;						///< Number of buffer indexes in the avail_bufs array.
 	volatile uint8_t buf_put_back_pos;						///< Position where to put back the reserved buffers.

src/modules/flow/camera.c

@@ -83,8 +83,7 @@ bool camera_init(camera_ctx *ctx, const camera_sensor_interface *sensor, const c
 	ctx->img_stream_param.analog_gain = ctx->analog_gain;
 	ctx->img_stream_param.exposure = ctx->exposure;
 	ctx->snapshot_param = ctx->img_stream_param;
-	int i;
-	for (i = 0; i < buffer_count; ++i) {
+	for (unsigned i = 0; i < buffer_count; ++i) {
 		// check the buffer:
 		if (buffers[i].buffer_size < img_size || buffers[i].buffer == NULL) {
 			return false;
@@ -128,8 +127,7 @@ bool camera_init(camera_ctx *ctx, const camera_sensor_interface *sensor, const c
 }
 
 static void uint32_t_memcpy(uint32_t *dst, const uint32_t *src, size_t count) {
-	int i;
-	for (i = 0; i < count; ++i) {
+	for (size_t i = 0; i < count; ++i) {
 		dst[i] = src[i];
 	}
 }
@@ -180,7 +178,7 @@ static void camera_buffer_fifo_push_at(camera_ctx *ctx, size_t pos, const int *i
 
 static void camera_update_exposure_hist(camera_ctx *ctx, const uint8_t *buffer, size_t size) {
 	if (ctx->exposure_sampling_stride > 0) {
-		int i;
+		size_t i;
 		int c = 0;
 		for (i = ctx->exposure_sampling_stride / 2; i < size; i += ctx->exposure_sampling_stride) {
 			int idx = (buffer[i] >> (8u - CONFIG_CAMERA_EXPOSURE_BIN_BITS));
@@ -451,7 +449,7 @@ bool camera_img_stream_schedule_param_change(camera_ctx *ctx, const camera_img_p
 		// invalid size parameter!
 		return false;
 	}
-	int i;
+	size_t i;
 	for (i = 0; i < ctx->buffer_count; ++i) {
 		// check image size against each buffer:
 		if (img_size > ctx->buffers[i].buffer_size) {
@@ -504,7 +502,7 @@ void camera_snapshot_acknowledge(camera_ctx *ctx) {
 }
 
 static bool camera_img_stream_get_buffers_idx(camera_ctx *ctx, int bidx[], size_t count, bool reset_new_frm) {
-	int i;
+	size_t i;
 	__disable_irq();
 	if (!camera_buffer_fifo_remove_front(ctx, bidx, count)) {
 		if (reset_new_frm) {
@@ -537,43 +535,39 @@ static bool camera_img_stream_get_buffers_idx(camera_ctx *ctx, int bidx[], size_
 	return consecutive;
 }
 
-int camera_img_stream_get_buffers(camera_ctx *ctx, camera_image_buffer *buffers[], size_t count, bool wait_for_new) {
+bool camera_img_stream_get_buffers(camera_ctx *ctx, camera_image_buffer *buffers[], size_t count, bool wait_for_new) {
 	if (ctx->buffers_are_reserved) return -1;
 	if (count > ctx->buffer_count - 1 || count <= 0) return -1;
 	/* buffer management needs to be performed atomically: */
 	int bidx[count];
-	int i;
-	while (1) {
-		if (wait_for_new) {
-			/* wait until a new frame is available: */
-			while(!ctx->new_frame_arrived) {
-				//TODO: camera_img_stream_get_buffers is calling fmu_comm_run()
-				fmu_comm_run();
-			}				
+	size_t i;
+
+	if (wait_for_new) {
+		/* wait until a new frame is available: */
+		if (!ctx->new_frame_arrived) {
+			return false;
 		}
-		if (camera_img_stream_get_buffers_idx(ctx, bidx, count, wait_for_new)) {
-			/* update the pointers: */
-			for (i = 0; i < count; ++i) {
-				buffers[i] = &ctx->buffers[bidx[i]];
-			}
-			return 0;
-		} else {
-			/* not possible! check if we want to wait for the new frame: */
-			if (!wait_for_new) {
-				return 1;
-			}
+	}
+	if (camera_img_stream_get_buffers_idx(ctx, bidx, count, wait_for_new)) {
+		/* update the pointers: */
+		for (i = 0; i < count; ++i) {
+			buffers[i] = &ctx->buffers[bidx[i]];
 		}
+		return true;
+	} else {
+		/* not possible! */
+		return false;
 	}
+
 }
 
 void camera_img_stream_return_buffers(camera_ctx *ctx, camera_image_buffer *buffers[], size_t count) {
 	if (!ctx->buffers_are_reserved) return;
 	/* get the buffer indexes: */
 	int bidx[count];
-	int i;
-	for (i = 0; i < count; ++i) {
+	for (size_t i = 0; i < count; ++i) {
 		int idx = buffers[i] - &ctx->buffers[0];
-		if (idx < 0 || idx >= ctx->buffer_count) return;
+		if (idx < 0 || idx >= (int) ctx->buffer_count) return;
 		bidx[i] = idx;
 	}
 	/* buffer management needs to be performed atomically: */

src/modules/flow/communication.c

@@ -50,7 +50,7 @@
 
 extern void systemreset(bool to_bootloader);
 
-extern void notify_changed_camera_parameters();
+extern void notify_changed_camera_parameters(void);
 
 mavlink_system_t mavlink_system;
 

src/modules/flow/distance_mode_filter.c

@@ -37,24 +37,22 @@
 /**
  * insert-only ring buffer of distance data, needs to be of uneven size
  * the initialization to zero will make the filter respond zero for the
- * first N inserted readinds, which is a decent startup-logic.
+ * first N inserted readings, which is a decent startup-logic.
  */
 static float distance_values[5] = { 0.0f };
 static unsigned insert_index = 0;
 
-static void distance_bubble_sort(float distance_values[], unsigned n);
-
-void distance_bubble_sort(float distance_values[], unsigned n)
+static void distance_bubble_sort(float distances[], unsigned n)
 {
 	float t;
 
 	for (unsigned i = 0; i < (n - 1); i++) {
 		for (unsigned j = 0; j < (n - i - 1); j++) {
-			if (distance_values[j] > distance_values[j+1]) {
+			if (distances[j] > distances[j+1]) {
 				/* swap two values */
-				t = distance_values[j];
-				distance_values[j] = distance_values[j + 1];
-				distance_values[j + 1] = t;
+				t = distances[j];
+				distances[j] = distances[j + 1];
+				distances[j + 1] = t;
 			}
 		}
 	}

src/modules/flow/main.c

@@ -97,6 +97,9 @@ static volatile bool snap_capture_success = false;
 static bool snap_ready = true;
 volatile bool usb_image_transfer_active = false;
 
+static void check_for_frame(void);
+static float latest_ground_distance(void);
+
 /* timer constants */
 #define DISTANCE_POLL_MS	 	100	/* steps in milliseconds ticks */
 #define SYSTEM_STATE_MS		1000/* steps in milliseconds ticks */
@@ -220,7 +223,8 @@ static void send_image_step(void) {
 	}
 }
 
-static void start_send_image(float ground_distance_m) {
+static void start_send_image(void) {
+	float ground_distance_m = latest_ground_distance();
 	usb_image_transfer_active = true;
 	usb_image_pos = 0;
 #if defined(CONFIG_ARCH_BOARD_PX4FLOW_V2)
@@ -260,6 +264,17 @@ static uint8_t image_buffer_8bit_5[FLOW_IMAGE_SIZE * FLOW_IMAGE_SIZE] __attribut
 
 static flow_klt_image flow_klt_images[2] __attribute__((section(".ccm")));
 
+/* variables */
+uint32_t counter = 0;
+
+result_accumulator_ctx mavlink_accumulator;
+uint32_t fps_timing_start;
+uint16_t fps_counter = 0;
+uint16_t fps_skipped_counter = 0;
+
+uint32_t last_frame_index = 0;
+uint32_t last_processed_frame_timestamp;
+
 /**
   * @brief  Main function.
   */
@@ -313,12 +328,7 @@ int main(void)
 
 	/* usart config*/
 	usart_init();
-
   fmu_comm_init();
-
-	float distance_filtered = 0.0f; // distance in meter
-	float distance_raw = 0.0f; // distance in meter
-	bool distance_valid = false;
 	distance_init();
 
 	/* reset/start timers */
@@ -330,57 +340,45 @@ int main(void)
 	timer_register(take_snapshot_fn, 500);
 	//timer_register(switch_params_fn, 2000);
 
-	/* variables */
-	uint32_t counter = 0;
-
-	result_accumulator_ctx mavlink_accumulator;
-
 	result_accumulator_init(&mavlink_accumulator);
-
-	uint32_t fps_timing_start = get_boot_time_us();
-	uint16_t fps_counter = 0;
-	uint16_t fps_skipped_counter = 0;
-
-	uint32_t last_frame_index = 0;
-
-	uint32_t last_processed_frame_timestamp = get_boot_time_us();
+	fps_timing_start = last_processed_frame_timestamp = get_boot_time_us();
 
 	/* main loop */
 	while (1)
 	{
 		/* check timers */
 		timer_check();
-
 		fmu_comm_run();
-
-		distance_valid = distance_read(&distance_filtered, &distance_raw);
-		/* reset to zero for invalid distances */
-		if (!distance_valid) {
-			distance_filtered = -1.0f;
-			distance_raw = -1.0f;
-		}
-
-		/* decide which distance to use */
-		float ground_distance;
-		if(FLOAT_AS_BOOL(global_data.param[PARAM_SONAR_FILTERED]))
-		{
-			ground_distance = distance_filtered;
-		}
-		else
-		{
-			ground_distance = distance_raw;
-		}
+		check_for_frame();
 
 		if (snap_capture_done) {
 			snap_capture_done = false;
 			camera_snapshot_acknowledge(&cam_ctx);
 			snap_ready = true;
 			if (snap_capture_success) {
 				/* send the snapshot! */
-				start_send_image(ground_distance);
+				start_send_image();
 			}
 		}
+	}
+}
+
+static float latest_ground_distance(void) {
+	float distance_filtered, distance_raw;
+	bool distance_valid = distance_read(&distance_filtered, &distance_raw);
+	if (!distance_valid) {
+		return -1;
+	} else if(FLOAT_AS_BOOL(global_data.param[PARAM_SONAR_FILTERED])) {
+		return distance_filtered;
+	}	else {
+		return distance_raw;
+	}
+}
 
+static void check_for_frame(void) {
+	/* get recent images */
+	camera_image_buffer *frames[2];
+	if (camera_img_stream_get_buffers(&cam_ctx, frames, 2, true)) {
 		/* new gyroscope data */
 		float x_rate_sensor, y_rate_sensor, z_rate_sensor;
 		int16_t gyro_temp;
@@ -392,14 +390,8 @@ int main(void)
 		float z_rate =   z_rate_sensor; // z is correct
 
 		bool use_klt = !FLOAT_EQ_INT(global_data.param[PARAM_ALGORITHM_CHOICE], 0);
-
-		uint32_t start_computations = 0;
-
-		/* get recent images */
-		camera_image_buffer *frames[2];
-		camera_img_stream_get_buffers(&cam_ctx, frames, 2, true);
 
-		start_computations = get_boot_time_us();
+		uint32_t start_computations = get_boot_time_us();
 
 		int frame_delta = ((int32_t)frames[0]->frame_number - (int32_t)last_frame_index);
 		last_frame_index = frames[0]->frame_number;
@@ -536,7 +528,7 @@ int main(void)
 			.pixel_flow_x = pixel_flow_x,
 			.pixel_flow_y = pixel_flow_y,
 			.rad_per_pixel = 1.0f / focal_length_px,
-			.ground_distance = ground_distance,
+			.ground_distance = latest_ground_distance(),
 			.distance_age = get_time_delta_us(get_distance_measure_time()),
 			.max_px_frame = flow_mv_cap,
 		};
@@ -545,7 +537,7 @@ int main(void)
 		fmu_comm_update(&frame_data);
 		result_accumulator_feed(&mavlink_accumulator, &frame_data);
 
-		uint32_t computaiton_time_us = get_time_delta_us(start_computations);
+		uint32_t computation_time_us = get_time_delta_us(start_computations);
 
 		counter++;
 		fps_counter++;
@@ -563,7 +555,7 @@ int main(void)
 				fps_counter = 0;
 				fps_skipped_counter = 0;
 
-				mavlink_msg_debug_vect_send(MAVLINK_COMM_2, "TIMING", get_boot_time_us(), computaiton_time_us, fps, fps_skip);
+				mavlink_msg_debug_vect_send(MAVLINK_COMM_2, "TIMING", get_boot_time_us(), computation_time_us, fps, fps_skip);
 			}
 			mavlink_msg_debug_vect_send(MAVLINK_COMM_2, "EXPOSURE", get_boot_time_us(), 
 					frames[0]->param.exposure, frames[0]->param.analog_gain, cam_ctx.last_brightness);

src/modules/flow/result_accumulator.c

@@ -41,6 +41,7 @@
 #include "result_accumulator.h"
 #include "i2c_frame.h"
 #include "settings.h"
+#include "gyro.h"
 
 void result_accumulator_init(result_accumulator_ctx *ctx)
 {

src/modules/i2c_slave/i2c_slave.c

@@ -32,6 +32,7 @@
  ****************************************************************************/
 
 #include <stdbool.h>
+#include <string.h>
 #include "fmu_comm.h"
 
 #include "px4_config.h"