diff options
author | Lorenz Meier <lm@inf.ethz.ch> | 2012-08-20 09:07:33 +0200 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2012-08-20 09:07:33 +0200 |
commit | 2c8fafd12af505f0f6dbcce521c99f7cd76109ca (patch) | |
tree | a4fbfbb6ce3662c1e7df37240e281f05bbe13d19 /apps/mavlink/mavlink_parameters.c | |
parent | 4a7f92fad0f5f7170038690e3e35af889bddae78 (diff) | |
download | px4-firmware-2c8fafd12af505f0f6dbcce521c99f7cd76109ca.tar.gz px4-firmware-2c8fafd12af505f0f6dbcce521c99f7cd76109ca.tar.bz2 px4-firmware-2c8fafd12af505f0f6dbcce521c99f7cd76109ca.zip |
Reworked MAVLink parameter interface to support new parameter storage, tested.
Diffstat (limited to 'apps/mavlink/mavlink_parameters.c')
-rw-r--r-- | apps/mavlink/mavlink_parameters.c | 216 |
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++; -} |