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

1 files changed, 118 insertions, 98 deletions

diff --git a/apps/mavlink/mavlink_parameters.c b/apps/mavlink/mavlink_parameters.c
index a064fc167..6ad811ad3 100644
--- a/apps/mavlink/mavlink_parameters.c
+++ b/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 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)
 {
-	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);
+	mavlink_pm_send_param(param);
+	usleep(*(unsigned int*)arg);
+}
+
+void mavlink_pm_send_all_params(unsigned int delay)
+{
+	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: {
 			/* 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: {
@@ -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))) {
-
-					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);
-						}
+					/* 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);
+
+					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);
 					}
 				}
 			}
@@ -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) {
-
-					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_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);
-						}
-					}
-
+					/* 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_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++;
-}