Exemplo n.º 1
0
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;
	}
}
Exemplo n.º 2
0
void mavlink_pm_callback(void *arg, param_t param)
{
	mavlink_pm_send_param(param);
	usleep(*(unsigned int*)arg);
}
Exemplo n.º 3
0
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;
	}
}
Exemplo n.º 4
0
int mavlink_pm_send_param_for_name(const char* name)
{
	return mavlink_pm_send_param(param_find(name));
}
Exemplo n.º 5
0
int mavlink_pm_send_param_for_index(uint16_t index)
{
	return mavlink_pm_send_param(param_for_index(index));
}
Exemplo n.º 6
0
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;
	}
}