fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-11-01 08:31:36 +08:00
Code Issues Packages Projects Releases Wiki Activity

Moved parameter command handling to mavlink app

Browse Source
This commit is contained in:
Lorenz Meier
2012-08-20 09:32:42 +02:00
parent 2c8fafd12a
commit 41172f24d5
3 changed files with 90 additions and 64 deletions
Download Patch File Download Diff File Expand all files Collapse all files
apps/commander/commander.c
-40
View File
@@ -581,46 +581,6 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta
} }
break; 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: { default: {
mavlink_log_critical(mavlink_fd, "[commander] refusing unsupported command"); mavlink_log_critical(mavlink_fd, "[commander] refusing unsupported command");
result = MAV_RESULT_UNSUPPORTED; result = MAV_RESULT_UNSUPPORTED;
apps/mavlink/mavlink_parameters.c
+67 -17
View File
@@ -148,13 +148,13 @@ int mavlink_pm_send_param(param_t param)
void mavlink_pm_message_handler(const mavlink_channel_t chan, const mavlink_message_t *msg) void mavlink_pm_message_handler(const mavlink_channel_t chan, const mavlink_message_t *msg)
{ {
switch (msg->msgid) { switch (msg->msgid) {
case MAVLINK_MSG_ID_PARAM_REQUEST_LIST: { case MAVLINK_MSG_ID_PARAM_REQUEST_LIST: {
/* Start sending parameters */ /* Start sending parameters */
mavlink_pm_start_queued_send(); mavlink_pm_start_queued_send();
mavlink_missionlib_send_gcs_string("[mavlink pm] sending list"); mavlink_missionlib_send_gcs_string("[mavlink pm] sending list");
} break; } break;
case MAVLINK_MSG_ID_PARAM_SET: { case MAVLINK_MSG_ID_PARAM_SET: {
/* Handle parameter setting */ /* Handle parameter setting */
@@ -184,26 +184,76 @@ void mavlink_pm_message_handler(const mavlink_channel_t chan, const mavlink_mess
} }
} break; } 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_param_request_read_t mavlink_param_request_read;
mavlink_msg_param_request_read_decode(msg, &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))) { 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 */ /* 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.param_index == -1) {
/* local name buffer to enforce null-terminated string */ /* local name buffer to enforce null-terminated string */
char name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN+1]; 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); strncpy(name, mavlink_param_request_read.param_id, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN);
/* enforce null termination */ /* enforce null termination */
name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN] = '\0'; name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN] = '\0';
/* attempt to find parameter and send it */ /* attempt to find parameter and send it */
mavlink_pm_send_param_for_name(name); mavlink_pm_send_param_for_name(name);
} else { } else {
/* when index is >= 0, send this parameter again */ /* when index is >= 0, send this parameter again */
mavlink_pm_send_param_for_index(mavlink_param_request_read.param_index); 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))) {
/* preflight parameter load / store */
if (cmd_mavlink.command == MAV_CMD_PREFLIGHT_STORAGE) {
/* Read all parameters from EEPROM to RAM */
if (((int)(cmd_mavlink.param1)) == 0) {
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;
} 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;
}
/* 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 {
//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; } break;
} }
} }
apps/mavlink/mavlink_parameters.h
+23 -7
View File
@@ -56,24 +56,40 @@ void mavlink_pm_message_handler(const mavlink_channel_t chan, const mavlink_mess
* This function blocks until all parameters have been sent. * This function blocks until all parameters have been sent.
* it delays each parameter by the passed amount of microseconds. * it delays each parameter by the passed amount of microseconds.
* *
* @param delay The delay in us between sending all parameters. * @param delay The delay in us between sending all parameters.
*/ */
void mavlink_pm_send_all_params(unsigned int delay); void mavlink_pm_send_all_params(unsigned int delay);
/** /**
* Send one parameter. * Send one parameter.
* *
* @param param The parameter id to send * @param param The parameter id to send.
* @return zero on success, nonzero on failure * @return zero on success, nonzero on failure.
*/ */
int mavlink_pm_send_param(param_t param); 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. * Send a queue of parameters, one parameter per function call.
* *
* @return zero on success, nonzero on failure * @return zero on success, nonzero on failure
*/ */
int mavlink_pm_queued_send(); int mavlink_pm_queued_send(void);
/** /**
* Start sending the parameter queue. * Start sending the parameter queue.
@@ -81,6 +97,6 @@ int mavlink_pm_send_param(param_t param);
* This function will not directly send parameters, but instead * This function will not directly send parameters, but instead
* activate the sending of one parameter on each call of * activate the sending of one parameter on each call of
* mavlink_pm_queued_send(). * mavlink_pm_queued_send().
* @see mavlink_pm_queued_send() * @see mavlink_pm_queued_send()
*/ */
void mavlink_pm_start_queued_send(); void mavlink_pm_start_queued_send(void);