예제 #1
0
int mavlink_logbuffer_read(struct mavlink_logbuffer *lb, struct mavlink_logmessage *elem)
{
	if (!mavlink_logbuffer_is_empty(lb)) {
		memcpy(elem, &(lb->elems[lb->start]), sizeof(struct mavlink_logmessage));
		lb->start = (lb->start + 1) % lb->size;
		--lb->count;
		return 0;

	} else {
		return 1;
	}
}
예제 #2
0
/**
 * MAVLink Protocol main function.
 */
int mavlink_thread_main(int argc, char *argv[])
{
	/* initialize mavlink text message buffering */
	mavlink_logbuffer_init(&lb, 5);

	int ch;
	char *device_name = "/dev/ttyS1";
	baudrate = 57600;

	/* work around some stupidity in task_create's argv handling */
	argc -= 2;
	argv += 2;

	while ((ch = getopt(argc, argv, "b:d:eo")) != EOF) {
		switch (ch) {
		case 'b':
			baudrate = strtoul(optarg, NULL, 10);

			if (baudrate == 0)
				errx(1, "invalid baud rate '%s'", optarg);

			break;

		case 'd':
			device_name = optarg;
			break;

		case 'e':
			mavlink_link_termination_allowed = true;
			break;

		case 'o':
			mavlink_link_mode = MAVLINK_INTERFACE_MODE_ONBOARD;
			break;

		default:
			usage();
		}
	}

	struct termios uart_config_original;

	bool usb_uart;

	/* print welcome text */
	warnx("MAVLink v1.0 serial interface starting...");

	/* inform about mode */
	warnx((mavlink_link_mode == MAVLINK_INTERFACE_MODE_ONBOARD) ? "ONBOARD MODE" : "DOWNLINK MODE");

	/* Flush stdout in case MAVLink is about to take it over */
	fflush(stdout);

	/* default values for arguments */
	uart = mavlink_open_uart(baudrate, device_name, &uart_config_original, &usb_uart);

	if (uart < 0)
		err(1, "could not open %s", device_name);

	/* create the device node that's used for sending text log messages, etc. */
	register_driver(MAVLINK_LOG_DEVICE, &mavlink_fops, 0666, NULL);

	/* Initialize system properties */
	mavlink_update_system();

	/* start the MAVLink receiver */
	receive_thread = receive_start(uart);

	/* start the ORB receiver */
	uorb_receive_thread = uorb_receive_start();

	/* initialize waypoint manager */
	mavlink_wpm_init(wpm);

	/* all subscriptions are now active, set up initial guess about rate limits */
	if (baudrate >= 230400) {
		/* 200 Hz / 5 ms */
		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_HIGHRES_IMU, 20);
		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_RAW_IMU, 20);
		/* 50 Hz / 20 ms */
		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_ATTITUDE, 30);
		/* 20 Hz / 50 ms */
		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, 10);
		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 50);
		/* 10 Hz */
		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_GPS_RAW_INT, 100);
		/* 10 Hz */
		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_MANUAL_CONTROL, 100);

	} else if (baudrate >= 115200) {
		/* 20 Hz / 50 ms */
		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_HIGHRES_IMU, 50);
		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_RAW_IMU, 50);
		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_ATTITUDE, 50);
		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, 50);
		/* 5 Hz / 200 ms */
		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 200);
		/* 5 Hz / 200 ms */
		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_GPS_RAW_INT, 200);
		/* 2 Hz */
		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_MANUAL_CONTROL, 500);

	} else if (baudrate >= 57600) {
		/* 10 Hz / 100 ms */
		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_RAW_IMU, 300);
		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_HIGHRES_IMU, 300);
		/* 10 Hz / 100 ms ATTITUDE */
		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_ATTITUDE, 200);
		/* 5 Hz / 200 ms */
		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, 200);
		/* 5 Hz / 200 ms */
		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 500);
		/* 2 Hz */
		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_MANUAL_CONTROL, 500);
		/* 2 Hz */
		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_GPS_RAW_INT, 500);

	} else {
		/* very low baud rate, limit to 1 Hz / 1000 ms */
		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_RAW_IMU, 1000);
		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_ATTITUDE, 1000);
		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_HIGHRES_IMU, 1000);
		/* 1 Hz / 1000 ms */
		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, 1000);
		/* 0.5 Hz */
		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 2000);
		/* 0.1 Hz */
		set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_MANUAL_CONTROL, 10000);
	}

	thread_running = true;

	/* arm counter to go off immediately */
	unsigned lowspeed_counter = 10;

	while (!thread_should_exit) {

		/* 1 Hz */
		if (lowspeed_counter == 10) {
			mavlink_update_system();

			/* translate the current system state to mavlink state and mode */
			uint8_t mavlink_state = 0;
			uint8_t mavlink_mode = 0;
			get_mavlink_mode_and_state(&mavlink_state, &mavlink_mode);

			/* send heartbeat */
			mavlink_msg_heartbeat_send(chan, mavlink_system.type, MAV_AUTOPILOT_PX4, mavlink_mode, v_status.state_machine, mavlink_state);

			/* switch HIL mode if required */
			set_hil_on_off(v_status.flag_hil_enabled);

			/* send status (values already copied in the section above) */
			mavlink_msg_sys_status_send(chan,
						    v_status.onboard_control_sensors_present,
						    v_status.onboard_control_sensors_enabled,
						    v_status.onboard_control_sensors_health,
						    v_status.load,
						    v_status.voltage_battery * 1000.0f,
						    v_status.current_battery * 1000.0f,
						    v_status.battery_remaining,
						    v_status.drop_rate_comm,
						    v_status.errors_comm,
						    v_status.errors_count1,
						    v_status.errors_count2,
						    v_status.errors_count3,
						    v_status.errors_count4);
			lowspeed_counter = 0;
		}

		lowspeed_counter++;

		/* sleep quarter the time */
		usleep(25000);

		/* check if waypoint has been reached against the last positions */
		mavlink_waypoint_eventloop(mavlink_missionlib_get_system_timestamp(), &global_pos, &local_pos);

		/* sleep quarter the time */
		usleep(25000);

		/* send parameters at 20 Hz (if queued for sending) */
		mavlink_pm_queued_send();

		/* sleep quarter the time */
		usleep(25000);

		if (baudrate > 57600) {
			mavlink_pm_queued_send();
		}

		/* sleep 10 ms */
		usleep(10000);

		/* send one string at 10 Hz */
		if (!mavlink_logbuffer_is_empty(&lb)) {
			struct mavlink_logmessage msg;
			int lb_ret = mavlink_logbuffer_read(&lb, &msg);

			if (lb_ret == OK) {
				mavlink_missionlib_send_gcs_string(msg.text);
			}
		}

		/* sleep 15 ms */
		usleep(15000);
	}

	/* wait for threads to complete */
	pthread_join(receive_thread, NULL);
	pthread_join(uorb_receive_thread, NULL);

	/* Reset the UART flags to original state */
	if (!usb_uart)
		tcsetattr(uart, TCSANOW, &uart_config_original);

	thread_running = false;

	exit(0);
}
예제 #3
0
int
Mavlink::task_main(int argc, char *argv[])
{
	int ch;
	_baudrate = 57600;
	_datarate = 0;
	_mode = MAVLINK_MODE_NORMAL;

	/* work around some stupidity in task_create's argv handling */
	argc -= 2;
	argv += 2;

	/* don't exit from getopt loop to leave getopt global variables in consistent state,
	 * set error flag instead */
	bool err_flag = false;

	while ((ch = getopt(argc, argv, "b:r:d:m:fpvwx")) != EOF) {
		switch (ch) {
		case 'b':
			_baudrate = strtoul(optarg, NULL, 10);

			if (_baudrate < 9600 || _baudrate > 921600) {
				warnx("invalid baud rate '%s'", optarg);
				err_flag = true;
			}

			break;

		case 'r':
			_datarate = strtoul(optarg, NULL, 10);

			if (_datarate < 10 || _datarate > MAX_DATA_RATE) {
				warnx("invalid data rate '%s'", optarg);
				err_flag = true;
			}

			break;

		case 'd':
			_device_name = optarg;
			break;

//		case 'e':
//			mavlink_link_termination_allowed = true;
//			break;

		case 'm':
			if (strcmp(optarg, "custom") == 0) {
				_mode = MAVLINK_MODE_CUSTOM;

			} else if (strcmp(optarg, "camera") == 0) {
				_mode = MAVLINK_MODE_CAMERA;
			}

			break;

		case 'f':
			_forwarding_on = true;
			break;

		case 'p':
			_passing_on = true;
			break;

		case 'v':
			_verbose = true;
			break;

		case 'w':
			_wait_to_transmit = true;
			break;

		case 'x':
			_ftp_on = true;
			break;

		default:
			err_flag = true;
			break;
		}
	}

	if (err_flag) {
		usage();
		return ERROR;
	}

	if (_datarate == 0) {
		/* convert bits to bytes and use 1/2 of bandwidth by default */
		_datarate = _baudrate / 20;
	}

	if (_datarate > MAX_DATA_RATE) {
		_datarate = MAX_DATA_RATE;
	}

	if (Mavlink::instance_exists(_device_name, this)) {
		warnx("mavlink instance for %s already running", _device_name);
		return ERROR;
	}

	/* inform about mode */
	switch (_mode) {
	case MAVLINK_MODE_NORMAL:
		warnx("mode: NORMAL");
		break;

	case MAVLINK_MODE_CUSTOM:
		warnx("mode: CUSTOM");
		break;

	case MAVLINK_MODE_CAMERA:
		warnx("mode: CAMERA");
		break;

	default:
		warnx("ERROR: Unknown mode");
		break;
	}

	warnx("data rate: %d Bytes/s, port: %s, baud: %d", _datarate, _device_name, _baudrate);

	/* flush stdout in case MAVLink is about to take it over */
	fflush(stdout);

	struct termios uart_config_original;

	/* default values for arguments */
	_uart_fd = mavlink_open_uart(_baudrate, _device_name, &uart_config_original, &_is_usb_uart);

	if (_uart_fd < 0) {
		warn("could not open %s", _device_name);
		return ERROR;
	}

	/* initialize mavlink text message buffering */
	mavlink_logbuffer_init(&_logbuffer, 5);

	/* if we are passing on mavlink messages, we need to prepare a buffer for this instance */
	if (_passing_on || _ftp_on) {
		/* initialize message buffer if multiplexing is on or its needed for FTP.
		 * make space for two messages plus off-by-one space as we use the empty element
		 * marker ring buffer approach.
		 */
		if (OK != message_buffer_init(2 * MAVLINK_MAX_PACKET_LEN + 2)) {
			errx(1, "can't allocate message buffer, exiting");
		}

		/* initialize message buffer mutex */
		pthread_mutex_init(&_message_buffer_mutex, NULL);
	}

	/* create the device node that's used for sending text log messages, etc. */
	register_driver(MAVLINK_LOG_DEVICE, &fops, 0666, NULL);

	/* initialize logging device */
	_mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);

	/* Initialize system properties */
	mavlink_update_system();

	/* start the MAVLink receiver */
	_receive_thread = MavlinkReceiver::receive_start(this);

	_mission_result_sub = orb_subscribe(ORB_ID(mission_result));

	/* create mission manager */
	_mission_manager = new MavlinkMissionManager(this);
	_mission_manager->set_verbose(_verbose);

	_task_running = true;

	MavlinkOrbSubscription *param_sub = add_orb_subscription(ORB_ID(parameter_update));
	uint64_t param_time = 0;
	MavlinkOrbSubscription *status_sub = add_orb_subscription(ORB_ID(vehicle_status));
	uint64_t status_time = 0;

	struct vehicle_status_s status;
	status_sub->update(&status_time, &status);

	MavlinkCommandsStream commands_stream(this, _channel);

	/* add default streams depending on mode and intervals depending on datarate */
	float rate_mult = get_rate_mult();

	configure_stream("HEARTBEAT", 1.0f);

	switch (_mode) {
	case MAVLINK_MODE_NORMAL:
		configure_stream("SYS_STATUS", 1.0f);
		configure_stream("GPS_GLOBAL_ORIGIN", 0.5f);
		configure_stream("HIGHRES_IMU", 1.0f * rate_mult);
		configure_stream("ATTITUDE", 10.0f * rate_mult);
		configure_stream("VFR_HUD", 8.0f * rate_mult);
		configure_stream("GPS_RAW_INT", 1.0f * rate_mult);
		configure_stream("GLOBAL_POSITION_INT", 3.0f * rate_mult);
		configure_stream("LOCAL_POSITION_NED", 3.0f * rate_mult);
		configure_stream("RC_CHANNELS_RAW", 1.0f * rate_mult);
		configure_stream("NAMED_VALUE_FLOAT", 1.0f * rate_mult);
		configure_stream("GLOBAL_POSITION_SETPOINT_INT", 3.0f * rate_mult);
		configure_stream("ROLL_PITCH_YAW_THRUST_SETPOINT", 3.0f * rate_mult);
		configure_stream("DISTANCE_SENSOR", 0.5f);
		break;

	case MAVLINK_MODE_CAMERA:
		configure_stream("SYS_STATUS", 1.0f);
		configure_stream("ATTITUDE", 15.0f * rate_mult);
		configure_stream("GLOBAL_POSITION_INT", 15.0f * rate_mult);
		configure_stream("CAMERA_CAPTURE", 1.0f);
		break;

	default:
		break;
	}

	/* don't send parameters on startup without request */
	_mavlink_param_queue_index = param_count();

	MavlinkRateLimiter fast_rate_limiter(30000.0f / rate_mult);

	/* set main loop delay depending on data rate to minimize CPU overhead */
	_main_loop_delay = MAIN_LOOP_DELAY / rate_mult;

	/* now the instance is fully initialized and we can bump the instance count */
	LL_APPEND(_mavlink_instances, this);

	while (!_task_should_exit) {
		/* main loop */
		usleep(_main_loop_delay);

		perf_begin(_loop_perf);

		hrt_abstime t = hrt_absolute_time();

		if (param_sub->update(&param_time, nullptr)) {
			/* parameters updated */
			mavlink_update_system();
		}

		if (status_sub->update(&status_time, &status)) {
			/* switch HIL mode if required */
			set_hil_enabled(status.hil_state == HIL_STATE_ON);
		}

		/* update commands stream */
		commands_stream.update(t);

		/* check for requested subscriptions */
		if (_subscribe_to_stream != nullptr) {
			if (OK == configure_stream(_subscribe_to_stream, _subscribe_to_stream_rate)) {
				if (_subscribe_to_stream_rate > 0.0f) {
					warnx("stream %s on device %s enabled with rate %.1f Hz", _subscribe_to_stream, _device_name,
					      (double)_subscribe_to_stream_rate);

				} else {
					warnx("stream %s on device %s disabled", _subscribe_to_stream, _device_name);
				}

			} else {
				warnx("stream %s on device %s not found", _subscribe_to_stream, _device_name);
			}

			delete _subscribe_to_stream;
			_subscribe_to_stream = nullptr;
		}

		/* update streams */
		MavlinkStream *stream;
		LL_FOREACH(_streams, stream) {
			stream->update(t);
		}

		if (fast_rate_limiter.check(t)) {
			mavlink_pm_queued_send();
			_mission_manager->eventloop();

			if (!mavlink_logbuffer_is_empty(&_logbuffer)) {
				struct mavlink_logmessage msg;
				int lb_ret = mavlink_logbuffer_read(&_logbuffer, &msg);

				if (lb_ret == OK) {
					send_statustext(msg.severity, msg.text);
				}
			}
		}

		/* pass messages from other UARTs or FTP worker */
		if (_passing_on || _ftp_on) {

			bool is_part;
			uint8_t *read_ptr;
			uint8_t *write_ptr;

			pthread_mutex_lock(&_message_buffer_mutex);
			int available = message_buffer_get_ptr((void **)&read_ptr, &is_part);
			pthread_mutex_unlock(&_message_buffer_mutex);

			if (available > 0) {
				// Reconstruct message from buffer

				mavlink_message_t msg;
				write_ptr = (uint8_t *)&msg;

				// Pull a single message from the buffer
				size_t read_count = available;

				if (read_count > sizeof(mavlink_message_t)) {
					read_count = sizeof(mavlink_message_t);
				}

				memcpy(write_ptr, read_ptr, read_count);

				// We hold the mutex until after we complete the second part of the buffer. If we don't
				// we may end up breaking the empty slot overflow detection semantics when we mark the
				// possibly partial read below.
				pthread_mutex_lock(&_message_buffer_mutex);

				message_buffer_mark_read(read_count);

				/* write second part of buffer if there is some */
				if (is_part && read_count < sizeof(mavlink_message_t)) {
					write_ptr += read_count;
					available = message_buffer_get_ptr((void **)&read_ptr, &is_part);
					read_count = sizeof(mavlink_message_t) - read_count;
					memcpy(write_ptr, read_ptr, read_count);
					message_buffer_mark_read(available);
				}

				pthread_mutex_unlock(&_message_buffer_mutex);

				_mavlink_resend_uart(_channel, &msg);
			}
		}

		/* update TX/RX rates*/
		if (t > _bytes_timestamp + 1000000) {
			if (_bytes_timestamp != 0) {
				float dt = (t - _bytes_timestamp) / 1000.0f;
				_rate_tx = _bytes_tx / dt;
				_rate_txerr = _bytes_txerr / dt;
				_rate_rx = _bytes_rx / dt;
				_bytes_tx = 0;
				_bytes_txerr = 0;
				_bytes_rx = 0;
			}
			_bytes_timestamp = t;
		}

		perf_end(_loop_perf);
	}