Merge branch 'px4dev_new_param' of https://github.com/PX4/Firmware into px4dev_new_param

6 files changed, 259 insertions, 149 deletions

diff --git a/apps/commander/commander.c b/apps/commander/commander.c
index 149a662fd..a81e102e9 100644
--- a/apps/commander/commander.c
+++ b/apps/commander/commander.c
@@ -581,46 +581,6 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta
 		}
 		break;
 
-		/* preflight parameter load / store */
-		case MAV_CMD_PREFLIGHT_STORAGE: {
-			/* Read all parameters from EEPROM to RAM */
-
-			if (((int)cmd->param1) == 0)	{
-
-				if (OK == get_params_from_eeprom(global_data_parameter_storage)) {
-					//printf("[commander] Loaded EEPROM params in RAM\n");
-					mavlink_log_info(mavlink_fd, "[commander] CMD Loaded EEPROM params in RAM");
-					result = MAV_RESULT_ACCEPTED;
-
-				} else {
-					//fprintf(stderr, "[commander] ERROR loading EEPROM params in RAM\n");
-					mavlink_log_critical(mavlink_fd, "[commander] ERROR loading EEPROM params in RAM");
-					result = MAV_RESULT_FAILED;
-				}
-
-				/* Write all parameters from RAM to EEPROM */
-
-			} else if (((int)cmd->param1) == 1)	{
-
-				if (OK == store_params_in_eeprom(global_data_parameter_storage)) {
-					//printf("[commander] RAM params written to EEPROM\n");
-					mavlink_log_info(mavlink_fd, "[commander] RAM params written to EEPROM");
-					result = MAV_RESULT_ACCEPTED;
-
-				} else {
-					//fprintf(stderr, "[commander] ERROR writing RAM params to EEPROM\n");
-					mavlink_log_critical(mavlink_fd, "[commander] ERROR writing RAM params to EEPROM");
-					result = MAV_RESULT_FAILED;
-				}
-
-			} else {
-				//fprintf(stderr, "[commander] refusing unsupported storage request\n");
-				mavlink_log_critical(mavlink_fd, "[commander] refusing unsupported storage request");
-				result = MAV_RESULT_UNSUPPORTED;
-			}
-		}
-		break;
-
 		default: {
 			mavlink_log_critical(mavlink_fd, "[commander] refusing unsupported command");
 			result = MAV_RESULT_UNSUPPORTED;
diff --git a/apps/mavlink/mavlink.c b/apps/mavlink/mavlink.c
index 47eab6be8..1ac3f907a 100644
--- a/apps/mavlink/mavlink.c
+++ b/apps/mavlink/mavlink.c
@@ -151,8 +151,8 @@ static enum {
 void mavlink_wpm_send_message(mavlink_message_t *msg);
 void mavlink_wpm_send_gcs_string(const char *string);
 uint64_t mavlink_wpm_get_system_timestamp(void);
-void mavlink_missionlib_send_message(mavlink_message_t *msg);
-void mavlink_missionlib_send_gcs_string(const char *string);
+int mavlink_missionlib_send_message(mavlink_message_t *msg);
+int mavlink_missionlib_send_gcs_string(const char *string);
 uint64_t mavlink_missionlib_get_system_timestamp(void);
 
 void handleMessage(mavlink_message_t *msg);
@@ -183,13 +183,18 @@ static void usage(const char *reason);
 
 static uint8_t missionlib_msg_buf[MAVLINK_MAX_PACKET_LEN];
 
-void mavlink_missionlib_send_message(mavlink_message_t *msg)
+int mavlink_missionlib_send_message(mavlink_message_t *msg)
 {
 	uint16_t len = mavlink_msg_to_send_buffer(missionlib_msg_buf, msg);
-	write(uart, missionlib_msg_buf, len);
+	int writelen = write(uart, missionlib_msg_buf, len);
+	if (writelen != len) {
+		return 1;
+	} else {
+		return 0;
+	}
 }
 
-void mavlink_missionlib_send_gcs_string(const char *string)
+int mavlink_missionlib_send_gcs_string(const char *string)
 {
 	const int len = MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN;
 	mavlink_statustext_t statustext;
@@ -210,7 +215,9 @@ void mavlink_missionlib_send_gcs_string(const char *string)
 		mavlink_message_t msg;
 
 		mavlink_msg_statustext_encode(mavlink_system.sysid, mavlink_system.compid, &msg, &statustext);
-		mavlink_missionlib_send_message(&msg);
+		return mavlink_missionlib_send_message(&msg);
+	} else {
+		return 1;
 	}
 }
 
diff --git a/apps/mavlink/mavlink_parameters.c b/apps/mavlink/mavlink_parameters.c
index a064fc167..103b8afd0 100644
--- a/apps/mavlink/mavlink_parameters.c
+++ b/apps/mavlink/mavlink_parameters.c
@@ -45,52 +45,116 @@
 #include <fcntl.h>
 #include <stdbool.h>
 #include <string.h>
+#include <systemlib/param/param.h>
 
 extern mavlink_system_t mavlink_system;
 
-extern void mavlink_missionlib_send_message(mavlink_message_t *msg);
-extern void mavlink_missionlib_send_gcs_string(const char *string);
+extern int mavlink_missionlib_send_message(mavlink_message_t *msg);
+extern int mavlink_missionlib_send_gcs_string(const char *string);
 
-/* send one parameter, assume lock on global_data_parameter_storage */
-void mavlink_pm_send_one_parameter(uint16_t next_param)
+/**
+ * If the queue index is not at 0, the queue sending
+ * logic will send parameters from the current index
+ * to len - 1, the end of the param list.
+ */
+static unsigned int mavlink_param_queue_index = 0;
+
+/**
+ * Callback for param interface.
+ */
+void mavlink_pm_callback(void *arg, param_t param);
+
+void mavlink_pm_callback(void *arg, param_t param)
+{
+	mavlink_pm_send_param(param);
+	usleep(*(unsigned int*)arg);
+}
+
+void mavlink_pm_send_all_params(unsigned int delay)
 {
-	if (next_param < global_data_parameter_storage->pm.size) {
-		static char name_buf[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN];
-		mavlink_message_t tx_msg;
-
-		strncpy((char *)name_buf, global_data_parameter_storage->pm.param_names[next_param], MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN);
-
-		mavlink_msg_param_value_pack_chan(mavlink_system.sysid,
-						  mavlink_system.compid,
-						  MAVLINK_COMM_0,
-						  &tx_msg,
-						  name_buf,
-						  global_data_parameter_storage->pm.param_values[next_param],
-						  MAVLINK_TYPE_FLOAT,
-						  global_data_parameter_storage->pm.size,
-						  next_param);
-		mavlink_missionlib_send_message(&tx_msg);
-
-
-		// mavlink_msg_param_value_send(MAVLINK_COMM_0,
-		// 			     name_buf,
-		// 			     global_data_parameter_storage->pm.param_values[next_param],
-		// 			     MAVLINK_TYPE_FLOAT,
-		// 			     global_data_parameter_storage->pm.size,
-		// 			     next_param);
+	unsigned int dbuf = delay;
+	param_foreach(&mavlink_pm_callback, &dbuf, false);
+}
+
+int mavlink_pm_queued_send()
+{
+	if (mavlink_param_queue_index < param_count()) {
+		mavlink_pm_send_param(param_for_index(mavlink_param_queue_index));
+		mavlink_param_queue_index++;
+		return 0;
+	} else {
+		return 1;
 	}
 }
 
+void mavlink_pm_start_queued_send()
+{
+	mavlink_param_queue_index = 0;
+}
+
+int mavlink_pm_send_param_for_index(uint16_t index)
+{
+	return mavlink_pm_send_param(param_for_index(index));
+}
+
+int mavlink_pm_send_param_for_name(const char* name)
+{
+	return mavlink_pm_send_param(param_find(name));
+}
+
+int mavlink_pm_send_param(param_t param)
+{
+	if (param == PARAM_INVALID) return 1;
+
+	/* buffers for param transmission */
+	static char name_buf[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN];
+	float val_buf;
+	static mavlink_message_t tx_msg;
+
+	/* query parameter type */
+	param_type_t type = param_type(param);
+	/* copy parameter name */
+	strncpy((char *)name_buf, param_name(param), MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN);
+	
+	/*
+	 * Map onboard parameter type to MAVLink type,
+	 * endianess matches (both little endian)
+	 */
+	uint8_t mavlink_type;
+	if (type == PARAM_TYPE_INT32) {
+		mavlink_type = MAVLINK_TYPE_INT32_T;
+	} else if (type == PARAM_TYPE_FLOAT) {
+		mavlink_type = MAVLINK_TYPE_FLOAT;
+	}
+
+	/*
+	 * get param value, since MAVLink encodes float and int params in the same
+	 * space during transmission, copy param onto float val_buf
+	 */
+	if (param_get(param, &val_buf) != OK) return;
+
+	mavlink_msg_param_value_pack_chan(mavlink_system.sysid,
+					  mavlink_system.compid,
+					  MAVLINK_COMM_0,
+					  &tx_msg,
+					  name_buf,
+					  val_buf,
+					  mavlink_type,
+					  param_count(),
+					  param_get_index(param));
+	return mavlink_missionlib_send_message(&tx_msg);
+}
+
 void mavlink_pm_message_handler(const mavlink_channel_t chan, const mavlink_message_t *msg)
 {
 	switch (msg->msgid) {
-	case MAVLINK_MSG_ID_PARAM_REQUEST_LIST: {
+		case MAVLINK_MSG_ID_PARAM_REQUEST_LIST: {
 			/* Start sending parameters */
-			global_data_parameter_storage->pm.next_param = 0;
-			mavlink_missionlib_send_gcs_string("[pm] sending list");
+			mavlink_pm_start_queued_send();
+			mavlink_missionlib_send_gcs_string("[mavlink pm] sending list");
 		} break;
 
-	case MAVLINK_MSG_ID_PARAM_SET: {
+		case MAVLINK_MSG_ID_PARAM_SET: {
 
 			/* Handle parameter setting */
 
@@ -99,91 +163,97 @@ void mavlink_pm_message_handler(const mavlink_channel_t chan, const mavlink_mess
 				mavlink_msg_param_set_decode(msg, &mavlink_param_set);
 
 				if (mavlink_param_set.target_system == mavlink_system.sysid && ((mavlink_param_set.target_component == mavlink_system.compid) || (mavlink_param_set.target_component == MAV_COMP_ID_ALL))) {
+					/* local name buffer to enforce null-terminated string */
+					char name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN+1];
+					strncpy(name, mavlink_param_set.param_id, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN);
+					/* enforce null termination */
+					name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN] = '\0';
+					/* attempt to find parameter, set and send it */
+					param_t param = param_find(name);
 
-					uint16_t i; //parameters
-					uint16_t j; //chars
-					bool match;
-
-					for (i = 0; i < PARAM_MAX_COUNT; i++) {
-						match = true;
-
-						for (j = 0; j < MAX_PARAM_NAME_LEN; j++) {
-							/* Compare char by char */
-							if (global_data_parameter_storage->pm.param_names[i][j] != mavlink_param_set.param_id[j]) {
-								match = false;
-							}
-
-							/* End matching if null termination is reached */
-							if (global_data_parameter_storage->pm.param_names[i][j] == '\0') {
-								break;
-							}
-						}
-
-						/* Check if matched */
-						if (match) {
-							// XXX handle param type as well, assuming float here
-							global_data_parameter_storage->pm.param_values[i] = mavlink_param_set.param_value;
-							mavlink_pm_send_one_parameter(i);
-						}
+					if (param == PARAM_INVALID) {
+						char buf[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN];
+						sprintf(buf, "[mavlink pm] unknown: %s", name);
+						mavlink_missionlib_send_gcs_string(buf);
+					} else {
+						/* set and send parameter */
+						param_set(param, &(mavlink_param_set.param_value));
+						mavlink_pm_send_param(param);
 					}
 				}
 			}
 		} break;
 
-	case MAVLINK_MSG_ID_PARAM_REQUEST_READ: {
+		case MAVLINK_MSG_ID_PARAM_REQUEST_READ: {
 			mavlink_param_request_read_t mavlink_param_request_read;
 			mavlink_msg_param_request_read_decode(msg, &mavlink_param_request_read);
 
-			if (mavlink_param_request_read.target_system == mavlink_system.sysid && ((mavlink_param_request_read.target_component == mavlink_system.compid) || (mavlink_param_request_read.target_component == MAV_COMP_ID_ALL))) {
-				/* when no index is given, loop through string ids and compare them */
-				if (mavlink_param_request_read.param_index == -1) {
+				if (mavlink_param_request_read.target_system == mavlink_system.sysid && ((mavlink_param_request_read.target_component == mavlink_system.compid) || (mavlink_param_request_read.target_component == MAV_COMP_ID_ALL))) {
+					/* when no index is given, loop through string ids and compare them */
+					if (mavlink_param_request_read.param_index == -1) {
+						/* local name buffer to enforce null-terminated string */
+						char name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN+1];
+						strncpy(name, mavlink_param_request_read.param_id, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN);
+						/* enforce null termination */
+						name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN] = '\0';
+						/* attempt to find parameter and send it */
+						mavlink_pm_send_param_for_name(name);
+					} else {
+						/* when index is >= 0, send this parameter again */
+						mavlink_pm_send_param_for_index(mavlink_param_request_read.param_index);
+					}
+				}
+
+		} break;
+
+		case MAVLINK_MSG_ID_COMMAND_LONG: {
+			mavlink_command_long_t cmd_mavlink;
+			mavlink_msg_command_long_decode(msg, &cmd_mavlink);
+
+			uint8_t result;
+
+			if (cmd_mavlink.target_system == mavlink_system.sysid &&
+				((cmd_mavlink.target_component == mavlink_system.compid) ||(cmd_mavlink.target_component == MAV_COMP_ID_ALL))) {
 
-					uint16_t i; //parameters
-					uint16_t j; //chars
-					bool match;
+				/* preflight parameter load / store */
+				if (cmd_mavlink.command == MAV_CMD_PREFLIGHT_STORAGE) {
+					/* Read all parameters from EEPROM to RAM */
 
-					for (i = 0; i < PARAM_MAX_COUNT; i++) {
-						match = true;
+					if (((int)(cmd_mavlink.param1)) == 0)	{
 
-						for (j = 0; j < MAX_PARAM_NAME_LEN; j++) {
-							/* Compare char by char */
-							if (global_data_parameter_storage->pm.param_names[i][j] != mavlink_param_request_read.param_id[j]) {
-								match = false;
-							}
+						if (OK == get_params_from_eeprom(global_data_parameter_storage)) {
+							//printf("[mavlink pm] Loaded EEPROM params in RAM\n");
+							mavlink_missionlib_send_gcs_string("[mavlink pm] CMD Loaded EEPROM params in RAM");
+							result = MAV_RESULT_ACCEPTED;
 
-							/* End matching if null termination is reached */
-							if (global_data_parameter_storage->pm.param_names[i][j] == '\0') {
-								break;
-							}
+						} else {
+							//fprintf(stderr, "[mavlink pm] ERROR loading EEPROM params in RAM\n");
+							mavlink_missionlib_send_gcs_string("[mavlink pm] ERROR loading EEPROM params in RAM");
+							result = MAV_RESULT_FAILED;
 						}
 
-						/* Check if matched */
-						if (match) {
-							mavlink_pm_send_one_parameter(i);
+						/* Write all parameters from RAM to EEPROM */
+
+					} else if (((int)(cmd_mavlink.param1)) == 1)	{
+
+						if (OK == store_params_in_eeprom(global_data_parameter_storage)) {
+							//printf("[mavlink pm] RAM params written to EEPROM\n");
+							mavlink_missionlib_send_gcs_string("[mavlink pm] RAM params written to EEPROM");
+							result = MAV_RESULT_ACCEPTED;
+
+						} else {
+							//fprintf(stderr, "[mavlink pm] ERROR writing RAM params to EEPROM\n");
+							mavlink_missionlib_send_gcs_string("[mavlink pm] ERROR writing RAM params to EEPROM");
+							result = MAV_RESULT_FAILED;
 						}
-					}
 
-				} else {
-					/* when index is >= 0, send this parameter again */
-					mavlink_pm_send_one_parameter(mavlink_param_request_read.param_index);
+					} else {
+						//fprintf(stderr, "[mavlink pm] refusing unsupported storage request\n");
+						mavlink_missionlib_send_gcs_string("[mavlink pm] refusing unsupported storage request");
+						result = MAV_RESULT_UNSUPPORTED;
+					}
 				}
 			}
-
 		} break;
 	}
 }
-
-
-/**
- * Send low-priority messages at a maximum rate of xx Hertz.
- *
- * This function sends messages at a lower rate to not exceed the wireless
- * bandwidth. It sends one message each time it is called until the buffer is empty.
- * Call this function with xx Hertz to increase/decrease the bandwidth.
- */
-void mavlink_pm_queued_send(void)
-{
-	//send parameters one by one:
-	mavlink_pm_send_one_parameter(global_data_parameter_storage->pm.next_param);
-	global_data_parameter_storage->pm.next_param++;
-}
diff --git a/apps/mavlink/mavlink_parameters.h b/apps/mavlink/mavlink_parameters.h
index e32732f58..950f82d2d 100644
--- a/apps/mavlink/mavlink_parameters.h
+++ b/apps/mavlink/mavlink_parameters.h
@@ -1,7 +1,7 @@
 /****************************************************************************
  *
- *   Copyright (C) 2008-2012 PX4 Development Team. All rights reserved.
- *   Author: Lorenz Meier <lm@inf.ethz.ch>
+ *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *   Author: @author Lorenz Meier <lm@inf.ethz.ch>
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -43,6 +43,60 @@
 
 #include "v1.0/common/mavlink.h"
 #include <stdbool.h>
+#include <systemlib/param/param.h>
 
+/**
+ * Handle parameter related messages. 
+ */
 void mavlink_pm_message_handler(const mavlink_channel_t chan, const mavlink_message_t *msg);
-void mavlink_pm_queued_send(void);
+
+/**
+ * Send all parameters at once.
+ *
+ * This function blocks until all parameters have been sent.
+ * it delays each parameter by the passed amount of microseconds.
+ *
+ * @param delay		The delay in us between sending all parameters.
+ */
+void mavlink_pm_send_all_params(unsigned int delay);
+
+/**
+ * Send one parameter.
+ *
+ * @param param		The parameter id to send.
+ * @return		zero on success, nonzero on failure.
+ */
+int mavlink_pm_send_param(param_t param);
+
+/**
+ * Send one parameter identified by index.
+ *
+ * @param index		The index of the parameter to send.
+ * @return		zero on success, nonzero else.
+ */
+int mavlink_pm_send_param_for_index(uint16_t index);
+
+/**
+ * Send one parameter identified by name.
+ *
+ * @param name		The index of the parameter to send.
+ * @return		zero on success, nonzero else.
+ */
+int mavlink_pm_send_param_for_name(const char* name);
+
+/**
+ * Send a queue of parameters, one parameter per function call.
+ *
+ * @return		zero on success, nonzero on failure
+ */
+ int mavlink_pm_queued_send(void);
+
+/**
+ * Start sending the parameter queue.
+ *
+ * This function will not directly send parameters, but instead
+ * activate the sending of one parameter on each call of
+ * mavlink_pm_queued_send().
+ * @see 		mavlink_pm_queued_send()
+ */
+void mavlink_pm_start_queued_send(void);
diff --git a/apps/systemlib/param/param.c b/apps/systemlib/param/param.c
index 93a71b735..692489cd4 100644
--- a/apps/systemlib/param/param.c
+++ b/apps/systemlib/param/param.c
@@ -193,6 +193,15 @@ param_for_index(unsigned index)
 	return PARAM_INVALID;
 }
 
+int
+param_get_index(param_t param)
+{
+	if (handle_in_range(param))
+		return (unsigned)param;
+
+	return -1;
+}
+
 const char *
 param_name(param_t param)
 {
diff --git a/apps/systemlib/param/param.h b/apps/systemlib/param/param.h
index 29857b46b..643d0ef7b 100644
--- a/apps/systemlib/param/param.h
+++ b/apps/systemlib/param/param.h
@@ -103,6 +103,14 @@ __EXPORT unsigned	param_count(void) __attribute__((const));
 __EXPORT param_t	param_for_index(unsigned index) __attribute__((const));
 
 /**
+ * Look up the index of a parameter.
+ *
+ * @param param		The parameter to obtain the index for.
+ * @return		The index, or -1 if the parameter does not exist.
+ */
+__EXPORT int		param_get_index(param_t param) __attribute__((const));
+
+/**
  * Obtain the name of a parameter.
  *
  * @param param		A handle returned by param_find or passed by param_foreach.
@@ -156,7 +164,7 @@ __EXPORT int		param_set(param_t param, const void *val);
 __EXPORT int		param_export(int fd, bool only_unsaved);
 
 /**
- * Import parameters from a file, discarding any unrecognised parameters.
+ * Import parameters from a file, discarding any unrecognized parameters.
  *
  * @param fd		File descriptor to import from.  (Currently expected to be a file.)
  * @return		Zero on success, nonzero if an error occurred during import.
@@ -167,7 +175,9 @@ __EXPORT int		param_import(int fd);
 /**
  * Apply a function to each parameter.
  *
- * Note that the parameter set is not locked during the traversal.
+ * Note that the parameter set is not locked during the traversal. It also does
+ * not hold an internal state, so the callback function can block or sleep between
+ * parameter callbacks.
  *
  * @param func		The function to invoke for each parameter.
  * @param arg		Argument passed to the function.