示例#1
0
void
l_vehicle_status(const struct listener *l)
{
    /* immediately communicate state changes back to user */
    orb_copy(ORB_ID(vehicle_status), status_sub, &v_status);
    orb_copy(ORB_ID(actuator_armed), mavlink_subs.armed_sub, &armed);

    /* enable or disable HIL */
    if (v_status.hil_state == HIL_STATE_ON)
        set_hil_on_off(true);
    else if (v_status.hil_state == HIL_STATE_OFF)
        set_hil_on_off(false);

    /* translate the current syste state to mavlink state and mode */
    uint8_t mavlink_state = 0;
    uint8_t mavlink_base_mode = 0;
    uint32_t mavlink_custom_mode = 0;
    get_mavlink_mode_and_state(&mavlink_state, &mavlink_base_mode, &mavlink_custom_mode);

    /* send heartbeat */
    mavlink_msg_heartbeat_send(chan,
                               mavlink_system.type,
                               MAV_AUTOPILOT_PX4,
                               mavlink_base_mode,
                               mavlink_custom_mode,
                               mavlink_state);
}
示例#2
0
void
l_vehicle_status(struct listener *l)
{
    /* immediately communicate state changes back to user */
    orb_copy(ORB_ID(vehicle_status), status_sub, &v_status);
    orb_copy(ORB_ID(actuator_armed), mavlink_subs.armed_sub, &armed);

    /* enable or disable HIL */
    set_hil_on_off(v_status.flag_hil_enabled);

    /* translate the current syste 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);
}
/**
 * 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);
}