Reworked MAVLink parameter interface to support new parameter storage, tested.

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);
+int mavlink_missionlib_send_message(mavlink_message_t *msg);
-void mavlink_missionlib_send_gcs_string(const char *string);
+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;
 	}
 }
 

apps/mavlink/mavlink_parameters.c

@@ -45,49 +45,113 @@
 #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 int mavlink_missionlib_send_message(mavlink_message_t *msg);
-extern void mavlink_missionlib_send_gcs_string(const char *string);
+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)
 {
-	if (next_param < global_data_parameter_storage->pm.size) {
+	mavlink_pm_send_param(param);
-		static char name_buf[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN];
+	usleep(*(unsigned int*)arg);
-		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);
+void mavlink_pm_send_all_params(unsigned int delay)
+{
+	unsigned int dbuf = delay;
+	param_foreach(&mavlink_pm_callback, &dbuf, false);
+}
 
-		mavlink_msg_param_value_pack_chan(mavlink_system.sysid,
+int mavlink_pm_queued_send()
-						  mavlink_system.compid,
+{
-						  MAVLINK_COMM_0,
+	if (mavlink_param_queue_index < param_count()) {
-						  &tx_msg,
+		mavlink_pm_send_param(param_for_index(mavlink_param_queue_index));
-						  name_buf,
+		mavlink_param_queue_index++;
-						  global_data_parameter_storage->pm.param_values[next_param],
+		return 0;
-						  MAVLINK_TYPE_FLOAT,
+	} else {
-						  global_data_parameter_storage->pm.size,
+		return 1;
-						  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);
 	}
 }
 
+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: {
 			/* Start sending parameters */
-			global_data_parameter_storage->pm.next_param = 0;
+			mavlink_pm_start_queued_send();
-			mavlink_missionlib_send_gcs_string("[pm] sending list");
+			mavlink_missionlib_send_gcs_string("[mavlink pm] sending list");
 		} break;
 
 	case MAVLINK_MSG_ID_PARAM_SET: {
@@ -99,32 +163,22 @@ 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
+					if (param == PARAM_INVALID) {
-					uint16_t j; //chars
+						char buf[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN];
-					bool match;
+						sprintf(buf, "[mavlink pm] unknown: %s", name);
-
+						mavlink_missionlib_send_gcs_string(buf);
-					for (i = 0; i < PARAM_MAX_COUNT; i++) {
+					} else {
-						match = true;
+						/* set and send parameter */
-
+						param_set(param, &(mavlink_param_set.param_value));
-						for (j = 0; j < MAX_PARAM_NAME_LEN; j++) {
+						mavlink_pm_send_param(param);
-							/* 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);
-						}
 					}
 				}
 			}
@@ -137,53 +191,19 @@ void mavlink_pm_message_handler(const mavlink_channel_t chan, const mavlink_mess
 			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 */
-					uint16_t i; //parameters
+					char name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN+1];
-					uint16_t j; //chars
+					strncpy(name, mavlink_param_request_read.param_id, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN);
-					bool match;
+					/* enforce null termination */
-
+					name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN] = '\0';
-					for (i = 0; i < PARAM_MAX_COUNT; i++) {
+					/* attempt to find parameter and send it */
-						match = true;
+					mavlink_pm_send_param_for_name(name);
-
-						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;
-							}
-
-							/* End matching if null termination is reached */
-							if (global_data_parameter_storage->pm.param_names[i][j] == '\0') {
-								break;
-							}
-						}
-
-						/* Check if matched */
-						if (match) {
-							mavlink_pm_send_one_parameter(i);
-						}
-					}
-
 				} else {
 					/* when index is >= 0, send this parameter again */
-					mavlink_pm_send_one_parameter(mavlink_param_request_read.param_index);
+					mavlink_pm_send_param_for_index(mavlink_param_request_read.param_index);
 				}
 			}
 
 		} 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++;
-}

apps/mavlink/mavlink_parameters.h

@@ -1,7 +1,7 @@
 /****************************************************************************
  *
- *   Copyright (C) 2008-2012 PX4 Development Team. All rights reserved.
+ *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
- *   Author: Lorenz Meier <lm@inf.ethz.ch>
+ *   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,44 @@
 
 #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 a queue of parameters, one parameter per function call.
+ *
+ * @return zero on success, nonzero on failure
+ */
+ int mavlink_pm_queued_send();
+
+/**
+ * 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();

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)
 {

apps/systemlib/param/param.h

@@ -102,6 +102,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.
  *