zrzk
/
px4_autopilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

Moved parameter command handling to mavlink app

sbg
Lorenz Meier 13 years ago
parent
2c8fafd12a
commit
41172f24d5
3 changed files with 90 additions and 64 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 40
      apps/commander/commander.c
  2. 84
      apps/mavlink/mavlink_parameters.c
  3. 30
      apps/mavlink/mavlink_parameters.h

40
apps/commander/commander.c
Unescape View File

@ -581,46 +581,6 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta @@ -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;

84
apps/mavlink/mavlink_parameters.c
Unescape View File

@ -148,13 +148,13 @@ int mavlink_pm_send_param(param_t param) @@ -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)
{
switch (msg->msgid) {
case MAVLINK_MSG_ID_PARAM_REQUEST_LIST: {
case MAVLINK_MSG_ID_PARAM_REQUEST_LIST: {
/* Start sending parameters */
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 */
@ -184,26 +184,76 @@ void mavlink_pm_message_handler(const mavlink_channel_t chan, const mavlink_mess @@ -184,26 +184,76 @@ void mavlink_pm_message_handler(const mavlink_channel_t chan, const mavlink_mess
}
} 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) {
/* 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);
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))) {
/* 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;
}
}

30
apps/mavlink/mavlink_parameters.h
Unescape View File

@ -56,24 +56,40 @@ void mavlink_pm_message_handler(const mavlink_channel_t chan, const mavlink_mess @@ -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.
* 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);
/**
* Send one parameter.
*
* @param param The parameter id to send
* @return zero on success, nonzero on failure
* @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
* @return zero on success, nonzero on failure
*/
int mavlink_pm_queued_send();
int mavlink_pm_queued_send(void);
/**
* Start sending the parameter queue.
@ -81,6 +97,6 @@ int mavlink_pm_send_param(param_t param); @@ -81,6 +97,6 @@ int mavlink_pm_send_param(param_t param);
* 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()
* @see mavlink_pm_queued_send()
*/
void mavlink_pm_start_queued_send();
void mavlink_pm_start_queued_send(void);

Write Preview
Loading…
Cancel
Save