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 */ mavlink_pm_start_queued_send(); mavlink_missionlib_send_gcs_string("[mavlink pm] sending list"); } break; case MAVLINK_MSG_ID_PARAM_SET: { /* Handle parameter setting */ if (msg->msgid == MAVLINK_MSG_ID_PARAM_SET) { mavlink_param_set_t mavlink_param_set; 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); 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: { 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); } } } break; case MAVLINK_MSG_ID_COMMAND_LONG: { mavlink_command_long_t cmd_mavlink; mavlink_msg_command_long_decode(msg, &cmd_mavlink); uint8_t result = MAV_RESULT_UNSUPPORTED; 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) { /* read all parameters from EEPROM to RAM */ int read_ret = mavlink_pm_load_eeprom(); if (read_ret == OK) { //printf("[mavlink pm] Loaded EEPROM params in RAM\n"); mavlink_missionlib_send_gcs_string("[mavlink pm] OK loaded EEPROM params"); result = MAV_RESULT_ACCEPTED; } else if (read_ret == 1) { mavlink_missionlib_send_gcs_string("[mavlink pm] No stored parameters to load"); result = MAV_RESULT_ACCEPTED; } else { if (read_ret < -1) { mavlink_missionlib_send_gcs_string("[mavlink pm] ERR loading params from EEPROM"); } else { mavlink_missionlib_send_gcs_string("[mavlink pm] ERR loading params, no EEPROM found"); } result = MAV_RESULT_FAILED; } } else if (((int)(cmd_mavlink.param1)) == 1) { /* write all parameters from RAM to EEPROM */ int write_ret = mavlink_pm_save_eeprom(); if (write_ret == OK) { mavlink_missionlib_send_gcs_string("[mavlink pm] OK params written to EEPROM"); result = MAV_RESULT_ACCEPTED; } else { if (write_ret < -1) { mavlink_missionlib_send_gcs_string("[mavlink pm] ERR writing params to EEPROM"); } else { mavlink_missionlib_send_gcs_string("[mavlink pm] ERR writing params, no EEPROM found"); } result = MAV_RESULT_FAILED; } } else { //fprintf(stderr, "[mavlink pm] refusing unsupported storage request\n"); mavlink_missionlib_send_gcs_string("[mavlink pm] refusing unsupported STOR request"); result = MAV_RESULT_UNSUPPORTED; } } } /* send back command result */ //mavlink_msg_command_ack_send(chan, cmd.command, result); } break; } }
void Mavlink::mavlink_pm_message_handler(const mavlink_channel_t chan, const mavlink_message_t *msg) { switch (msg->msgid) { case MAVLINK_MSG_ID_PARAM_REQUEST_LIST: { mavlink_param_request_list_t req; mavlink_msg_param_request_list_decode(msg, &req); if (req.target_system == mavlink_system.sysid && (req.target_component == mavlink_system.compid || req.target_component == MAV_COMP_ID_ALL)) { /* Start sending parameters */ mavlink_pm_start_queued_send(); send_statustext_info("[pm] sending list"); } } break; case MAVLINK_MSG_ID_PARAM_SET: { /* Handle parameter setting */ if (msg->msgid == MAVLINK_MSG_ID_PARAM_SET) { mavlink_param_set_t mavlink_param_set; 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); if (param == PARAM_INVALID) { char buf[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN]; sprintf(buf, "[pm] unknown: %s", name); send_statustext_info(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: { 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); } } } break; } }