From 2c8fafd12af505f0f6dbcce521c99f7cd76109ca Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 20 Aug 2012 09:07:33 +0200 Subject: [PATCH] Reworked MAVLink parameter interface to support new parameter storage, tested. --- apps/mavlink/mavlink.c | 19 ++- apps/mavlink/mavlink_parameters.c | 210 ++++++++++++++++-------------- apps/mavlink/mavlink_parameters.h | 44 ++++++- apps/systemlib/param/param.c | 9 ++ apps/systemlib/param/param.h | 8 ++ 5 files changed, 186 insertions(+), 104 deletions(-) diff --git a/apps/mavlink/mavlink.c b/apps/mavlink/mavlink.c index 47eab6be87..1ac3f907ae 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 a064fc1675..6ad811ad3b 100644 --- a/apps/mavlink/mavlink_parameters.c +++ b/apps/mavlink/mavlink_parameters.c @@ -45,49 +45,113 @@ #include #include #include +#include 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; + mavlink_pm_send_param(param); + usleep(*(unsigned int*)arg); +} - 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, - 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); +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))) { + /* 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); } } } @@ -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++; -} diff --git a/apps/mavlink/mavlink_parameters.h b/apps/mavlink/mavlink_parameters.h index e32732f581..e136d69bc2 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 + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Author: @author Lorenz Meier * * 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 +#include +/** + * 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(); diff --git a/apps/systemlib/param/param.c b/apps/systemlib/param/param.c index 78f3f74689..d296fc748e 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 b848f7f789..643d0ef7b7 100644 --- a/apps/systemlib/param/param.h +++ b/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. *