diff options
author | px4dev <px4@purgatory.org> | 2012-08-20 01:53:37 -0700 |
---|---|---|
committer | px4dev <px4@purgatory.org> | 2012-08-20 01:53:37 -0700 |
commit | 56bba7816f179f3743853866343946fd9e303c69 (patch) | |
tree | bb0ac8db9f8829982c3d85b884ed0cbae3678598 | |
parent | cc7a9c9b668c4575c2b3b50e004b2a9c1137908c (diff) | |
parent | 41172f24d5146575baba685f37631baee22ce0ef (diff) | |
download | px4-firmware-56bba7816f179f3743853866343946fd9e303c69.tar.gz px4-firmware-56bba7816f179f3743853866343946fd9e303c69.tar.bz2 px4-firmware-56bba7816f179f3743853866343946fd9e303c69.zip |
Merge branch 'px4dev_new_param' of https://github.com/PX4/Firmware into px4dev_new_param
-rw-r--r-- | apps/commander/commander.c | 40 | ||||
-rw-r--r-- | apps/mavlink/mavlink.c | 19 | ||||
-rw-r--r-- | apps/mavlink/mavlink_parameters.c | 266 | ||||
-rw-r--r-- | apps/mavlink/mavlink_parameters.h | 60 | ||||
-rw-r--r-- | apps/systemlib/param/param.c | 9 | ||||
-rw-r--r-- | apps/systemlib/param/param.h | 14 |
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. |