Code cleanup and ifdefs required for qurt build

Code that was previously out of tree that was #if 0, is now #ifdef __PX4_QURT.
These changes were required for flight using the qurt build.

Changes include code cleanup for shmem_posix.c.

Signed-off-by: Mark Charlebois <charlebm@gmail.com>

src/modules/commander/accelerometer_calibration.cpp

@@ -175,7 +175,9 @@ typedef struct  {
 
 int do_accel_calibration(int mavlink_fd)
 {
+#ifndef __PX4_QURT
 	int fd;
+#endif
 
 	mavlink_and_console_log_info(mavlink_fd, CAL_QGC_STARTED_MSG, sensor_name);
 
@@ -192,6 +194,7 @@ int do_accel_calibration(int mavlink_fd)
 
 	char str[30];
 
+#ifndef __PX4_QURT
 	/* reset all sensors */
 	for (unsigned s = 0; s < max_accel_sens; s++) {
 		sprintf(str, "%s%u", ACCEL_BASE_DEVICE_PATH, s);
@@ -211,6 +214,7 @@ int do_accel_calibration(int mavlink_fd)
 			mavlink_and_console_log_critical(mavlink_fd, CAL_ERROR_RESET_CAL_MSG, s);
 		}
 	}
+#endif
 
 	float accel_offs[max_accel_sens][3];
 	float accel_T[max_accel_sens][3][3];
@@ -277,14 +281,17 @@ int do_accel_calibration(int mavlink_fd)
 		failed |= (OK != param_set_no_notification(param_find(str), &(accel_scale.y_scale)));
 		(void)sprintf(str, "CAL_ACC%u_ZSCALE", i);
 		failed |= (OK != param_set_no_notification(param_find(str), &(accel_scale.z_scale)));
+#ifndef __PX4_QURT
 		(void)sprintf(str, "CAL_ACC%u_ID", i);
 		failed |= (OK != param_set_no_notification(param_find(str), &(device_id[i])));
+#endif
 		
 		if (failed) {
 			mavlink_and_console_log_critical(mavlink_fd, CAL_ERROR_SET_PARAMS_MSG, i);
 			return ERROR;
 		}
 
+#ifndef __PX4_QURT
 		sprintf(str, "%s%u", ACCEL_BASE_DEVICE_PATH, i);
 		fd = px4_open(str, 0);
 
@@ -299,6 +306,7 @@ int do_accel_calibration(int mavlink_fd)
 		if (res != OK) {
 			mavlink_and_console_log_critical(mavlink_fd, CAL_ERROR_APPLY_CAL_MSG, i);
 		}
+#endif
 	}
 
 	if (res == OK) {

src/modules/commander/gyro_calibration.cpp

@@ -173,7 +173,7 @@ int do_gyro_calibration(int mavlink_fd)
 
 	for (unsigned s = 0; s < max_gyros; s++) {
 		char str[30];
-		
+#ifndef __PX4_QURT
 		// Reset gyro ids to unavailable
 		worker_data.device_id[s] = 0;
 		(void)sprintf(str, "CAL_GYRO%u_ID", s);
@@ -182,9 +182,18 @@ int do_gyro_calibration(int mavlink_fd)
 			mavlink_and_console_log_critical(mavlink_fd, "[cal] Unable to reset CAL_GYRO%u_ID", s);
 			return ERROR;
 		}
+#else
+		(void)sprintf(str, "CAL_GYRO%u_ID", s);
+		res = param_get(param_find(str), &(worker_data.device_id[s]));
+		if (res != OK) {
+			mavlink_log_critical(mavlink_fd, "[cal] Unable to get CAL_GYRO%u_ID", s);
+			return ERROR;
+		}
+#endif
 		
 		// Reset all offsets to 0 and scales to 1
 		(void)memcpy(&worker_data.gyro_scale[s], &gyro_scale_zero, sizeof(gyro_scale));
+#ifndef __PX4_QURT
 		sprintf(str, "%s%u", GYRO_BASE_DEVICE_PATH, s);
 		int fd = px4_open(str, 0);
 		if (fd >= 0) {
@@ -197,6 +206,7 @@ int do_gyro_calibration(int mavlink_fd)
 				return ERROR;
 			}
 		}
+#endif
 		
 	}
 	
@@ -286,6 +296,8 @@ int do_gyro_calibration(int mavlink_fd)
 				failed |= (OK != param_set_no_notification(param_find(str), &(worker_data.gyro_scale[s].y_offset)));
 				(void)sprintf(str, "CAL_GYRO%u_ZOFF", s);
 				failed |= (OK != param_set_no_notification(param_find(str), &(worker_data.gyro_scale[s].z_offset)));
+
+#ifndef __PX4_QURT
 				(void)sprintf(str, "CAL_GYRO%u_ID", s);
 				failed |= (OK != param_set_no_notification(param_find(str), &(worker_data.device_id[s])));
 
@@ -304,6 +316,7 @@ int do_gyro_calibration(int mavlink_fd)
 				if (res != OK) {
 					mavlink_and_console_log_critical(mavlink_fd, CAL_ERROR_APPLY_CAL_MSG, 1);
 				}
+#endif
 			}
 		}
 

src/modules/commander/mag_calibration.cpp

@@ -101,6 +101,7 @@ int do_mag_calibration(int mavlink_fd)
 {
 	mavlink_and_console_log_info(mavlink_fd, CAL_QGC_STARTED_MSG, sensor_name);
 
+#ifndef __PX4_QURT
 	struct mag_scale mscale_null = {
 		0.0f,
 		1.0f,
@@ -109,6 +110,7 @@ int do_mag_calibration(int mavlink_fd)
 		0.0f,
 		1.0f,
 	};
+#endif
 
 	int result = OK;
 	
@@ -121,6 +123,7 @@ int do_mag_calibration(int mavlink_fd)
 	}
 	
 	for (unsigned cur_mag = 0; cur_mag < max_mags; cur_mag++) {
+#ifndef __PX4_QURT
 		// Reset mag id to mag not available
 		(void)sprintf(str, "CAL_MAG%u_ID", cur_mag);
 		result = param_set_no_notification(param_find(str), &(device_ids[cur_mag]));
@@ -128,7 +131,17 @@ int do_mag_calibration(int mavlink_fd)
 			mavlink_and_console_log_info(mavlink_fd, "[cal] Unable to reset CAL_MAG%u_ID", cur_mag);
 			break;
 		}
+#else
+		(void)sprintf(str, "CAL_MAG%u_ID", cur_mag);
+		result = param_get(param_find(str), &(device_ids[cur_mag]));
+		if (result != OK) {
+			mavlink_and_console_log_info(mavlink_fd, "[cal] Unable to reset CAL_MAG%u_ID", cur_mag);
+			break;
+		}
+#endif
 
+/* for calibration, commander will run on apps, so orb messages are used to get info from dsp */
+#ifndef __PX4_QURT
 		// Attempt to open mag
 		(void)sprintf(str, "%s%u", MAG_BASE_DEVICE_PATH, cur_mag);
 		int fd = px4_open(str, O_RDONLY);
@@ -158,6 +171,7 @@ int do_mag_calibration(int mavlink_fd)
 		}
 
 		px4_close(fd);
+#endif
 	}
 
 	// Calibrate all mags at the same time
@@ -615,8 +629,11 @@ calibrate_return mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mag
 					bool failed = false;
 					
 					/* set parameters */
+
+#ifndef __PX4_QURT
 					(void)sprintf(str, "CAL_MAG%u_ID", cur_mag);
 					failed |= (OK != param_set_no_notification(param_find(str), &(device_ids[cur_mag])));
+#endif
 					(void)sprintf(str, "CAL_MAG%u_XOFF", cur_mag);
 					failed |= (OK != param_set_no_notification(param_find(str), &(mscale.x_offset)));
 					(void)sprintf(str, "CAL_MAG%u_YOFF", cur_mag);

src/modules/systemlib/param/param_shmem.c

@@ -72,11 +72,7 @@
 
 #include "shmem.h"
 
-#if 0
+#define debug(fmt, args...)		do { } while(0)
-# define debug(fmt, args...)		do { warnx(fmt, ##args); } while(0)
-#else
-# define debug(fmt, args...)		do { } while(0)
-#endif
 
 #define PARAM_OPEN	open
 #define PARAM_CLOSE	close
@@ -89,7 +85,7 @@ extern struct param_info_s	param_array[];
 extern struct param_info_s	*param_info_base;
 extern struct param_info_s	*param_info_limit;
 #else
-// FIXME - start and end are reversed
+// TODO: start and end are reversed
 static struct param_info_s *param_info_base = (struct param_info_s *) &px4_parameters;
 #endif
 
@@ -187,7 +183,7 @@ param_unlock(void)
 static void
 param_assert_locked(void)
 {
-	/* XXX */
+	/* TODO */
 }
 
 /**
@@ -196,7 +192,6 @@ param_assert_locked(void)
  * @param param			The parameter handle to test.
  * @return			True if the handle is valid.
  */
-//static bool
 bool
 handle_in_range(param_t param)
 {
@@ -241,19 +236,11 @@ param_find_changed(param_t param)
 	param_assert_locked();
 
 	if (param_values != NULL) {
-#if 0	/* utarray_find requires bsearch, not available */
-		struct param_wbuf_s key;
-		key.param = param;
-		s = utarray_find(param_values, &key, param_compare_values);
-#else
-
 		while ((s = (struct param_wbuf_s *)utarray_next(param_values, s)) != NULL) {
 			if (s->param == param) {
 				break;
 			}
 		}
-
-#endif
 	}
 
 	return s;
@@ -282,7 +269,6 @@ param_find_internal(const char *name, bool notification)
 	param_t param;
 
 	/* perform a linear search of the known parameters */
-
 	for (param = 0; handle_in_range(param); param++) {
 		if (!strcmp(param_info_base[param].name, name)) {
 			if (notification) {

src/platforms/posix/px4_layer/shmem_posix.c

@@ -1,7 +1,6 @@
-
 /****************************************************************************
  *
- * Copyright (c) 2015 Ramakrishna Kintada. All rights reserved.
+ * Copyright (c) 2015 Vijay Venkatraman. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -32,7 +31,6 @@
  *
  ****************************************************************************/
 
-
 #include <px4_defines.h>
 #include <px4_posix.h>
 #include <string.h>
@@ -71,9 +69,9 @@ uint64_t update_from_shmem_prev_time = 0, update_from_shmem_current_time = 0;
 static unsigned char adsp_changed_index[MAX_SHMEM_PARAMS / 8 + 1];
 
 struct param_wbuf_s {
-	param_t			param;
+	param_t param;
-	union param_value_u	val;
+	union param_value_u val;
-	bool			unsaved;
+	bool unsaved;
 };
 extern struct param_wbuf_s *param_find_changed(param_t param);
 
@@ -88,7 +86,8 @@ static void *map_memory(off_t target)
 	}
 
 	/* Map one page */
-	map_base = (unsigned char *) mmap(0, MAP_SIZE, PROT_READ | PROT_WRITE, MAP_SHARED, mem_fd, target & ~MAP_MASK);
+	map_base = (unsigned char *) mmap(0, MAP_SIZE, PROT_READ | PROT_WRITE,
+					  MAP_SHARED, mem_fd, target & ~MAP_MASK);
 
 	if (map_base == (void *) - 1) {
 		PX4_ERR("Cannot mmap /dev/atl_mem\n");
@@ -109,10 +108,14 @@ int get_shmem_lock(void)
 		usleep(100000); //sleep for 100 msec
 		i++;
 
-		if (i > 100) { break; }
+		if (i > 100) {
+			break;
+		}
 	}
 
-	if (i > 100) { return -1; }
+	if (i > 100) {
+		return -1;
+	}
 
 	return 0; //got the lock
 }
@@ -126,14 +129,14 @@ void init_shared_memory(void)
 {
 
 	virt_addr = map_memory(MAP_ADDRESS); //16K space
-	shmem_info_p = (struct shmem_info *)virt_addr;
+	shmem_info_p = (struct shmem_info *) virt_addr;
 
 	//PX4_INFO("linux memory mapped\n");
 }
 
 void copy_params_to_shmem(struct param_info_s *param_info_base)
 {
-	param_t	param;
+	param_t param;
 	unsigned int i;
 
 	if (get_shmem_lock() != 0) {
@@ -145,17 +148,26 @@ void copy_params_to_shmem(struct param_info_s *param_info_base)
 	for (param = 0; param < param_count(); param++) {
 		struct param_wbuf_s *s = param_find_changed(param);
 
-		if (s == NULL) { shmem_info_p->params_val[param] = param_info_base[param].val; }
+		if (s == NULL) {
+			shmem_info_p->params_val[param] = param_info_base[param].val;
 
-		else { shmem_info_p->params_val[param] = s->val; }
+		} else {
+			shmem_info_p->params_val[param] = s->val;
+		}
 
 #ifdef SHMEM_DEBUG
 
 		if (param_type(param) == PARAM_TYPE_INT32) {
-			{PX4_INFO("%d: written %d for param %s to shared mem", param, shmem_info_p->params_val[param].i, param_name(param));}
+			{
+				PX4_INFO("%d: written %d for param %s to shared mem",
+					 param, shmem_info_p->params_val[param].i, param_name(param));
+			}
 
 		} else if (param_type(param) == PARAM_TYPE_FLOAT) {
-			{PX4_INFO("%d: written %f for param %s to shared mem", param, (double)shmem_info_p->params_val[param].f, param_name(param));}
+			{
+				PX4_INFO("%d: written %f for param %s to shared mem",
+					 param, (double)shmem_info_p->params_val[param].f, param_name(param));
+			}
 		}
 
 #endif
@@ -191,11 +203,14 @@ void update_to_shmem(param_t param, union param_value_u value)
 
 #ifdef SHMEM_DEBUG
 
-	if (param_type(param) == PARAM_TYPE_INT32)
+	if (param_type(param) == PARAM_TYPE_INT32) {
-	{PX4_INFO("Set value %d for param %s to shmem, set krait index %d:%d\n", value.i, param_name(param), byte_changed, bit_changed);}
+		PX4_INFO("Set value %d for param %s to shmem, set krait index %d:%d\n",
+			 value.i, param_name(param), byte_changed, bit_changed);
 
-	else if (param_type(param) == PARAM_TYPE_FLOAT)
+	} else if (param_type(param) == PARAM_TYPE_FLOAT) {
-	{PX4_INFO("Set value %f for param %s to shmem, set krait index %d:%d\n", (double)value.f, param_name(param), byte_changed, bit_changed);}
+		PX4_INFO("Set value %f for param %s to shmem, set krait index %d:%d\n",
+			 (double)value.f, param_name(param), byte_changed, bit_changed);
+	}
 
 #endif
 
@@ -221,7 +236,6 @@ static void update_index_from_shmem(void)
 	release_shmem_lock();
 }
 
-
 static void update_value_from_shmem(param_t param, union param_value_u *value)
 {
 	unsigned int byte_changed, bit_changed;
@@ -242,11 +256,16 @@ static void update_value_from_shmem(param_t param, union param_value_u *value)
 
 #ifdef SHMEM_DEBUG
 
-	if (param_type(param) == PARAM_TYPE_INT32)
+	if (param_type(param) == PARAM_TYPE_INT32) {
-	{PX4_INFO("Got value %d for param %s from shmem, cleared adsp index %d:%d\n", value->i, param_name(param), byte_changed, bit_changed);}
+		PX4_INFO(
+			"Got value %d for param %s from shmem, cleared adsp index %d:%d\n",
+			value->i, param_name(param), byte_changed, bit_changed);
 
-	else if (param_type(param) == PARAM_TYPE_FLOAT)
+	} else if (param_type(param) == PARAM_TYPE_FLOAT) {
-	{PX4_INFO("Got value %f for param %s from shmem, cleared adsp index %d:%d\n", (double)value->f, param_name(param), byte_changed, bit_changed);}
+		PX4_INFO(
+			"Got value %f for param %s from shmem, cleared adsp index %d:%d\n",
+			(double)value->f, param_name(param), byte_changed, bit_changed);
+	}
 
 #endif
 }
@@ -258,7 +277,8 @@ int update_from_shmem(param_t param, union param_value_u *value)
 
 	update_from_shmem_current_time = hrt_absolute_time();
 
-	if ((update_from_shmem_current_time - update_from_shmem_prev_time) > 1000000) { //update every 1 second
+	if ((update_from_shmem_current_time - update_from_shmem_prev_time)
+	    > 1000000) { //update every 1 second
 		update_from_shmem_prev_time = update_from_shmem_current_time;
 		update_index_from_shmem();
 	}
@@ -279,4 +299,3 @@ int update_from_shmem(param_t param, union param_value_u *value)
 	return retval;
 }
 
-