コード例 #1
0
ファイル: orb_listener.c プロジェクト: wuyou33/Firmware-1
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
ファイル: orb_listener.c プロジェクト: PX4CAR/Firmware
void
l_vehicle_attitude_controls(struct listener *l)
{
	struct actuator_controls_s actuators;

	orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, mavlink_subs.actuators_sub, &actuators);

	if (gcs_link) {
		/* send, add spaces so that string buffer is at least 10 chars long */
		mavlink_msg_named_value_float_send(MAVLINK_COMM_0,
						   last_sensor_timestamp / 1000,
						   "ctrl0       ",
						   actuators.control[0]);
		mavlink_msg_named_value_float_send(MAVLINK_COMM_0,
						   last_sensor_timestamp / 1000,
						   "ctrl1       ",
						   actuators.control[1]);
		mavlink_msg_named_value_float_send(MAVLINK_COMM_0,
						   last_sensor_timestamp / 1000,
						   "ctrl2       ",
						   actuators.control[2]);
		mavlink_msg_named_value_float_send(MAVLINK_COMM_0,
						   last_sensor_timestamp / 1000,
						   "ctrl3       ",
						   actuators.control[3]);
	}

	/* Only send in HIL mode */
	if (mavlink_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);

		/* HIL message as per MAVLink spec */
		mavlink_msg_hil_controls_send(chan,
			hrt_absolute_time(),
			actuators.control[0],
			actuators.control[1],
			actuators.control[2],
			actuators.control[3],
			0,
			0,
			0,
			0,
			mavlink_mode,
			0);
	}
}
コード例 #3
0
ファイル: orb_listener.c プロジェクト: youngklee/Firmware
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);
}
コード例 #4
0
ファイル: orb_listener.c プロジェクト: EricTHTseng/Firmware
void
l_actuator_outputs(const struct listener *l)
{
	struct actuator_outputs_s act_outputs;

	orb_id_t ids[] = {
		ORB_ID(actuator_outputs_0),
		ORB_ID(actuator_outputs_1),
		ORB_ID(actuator_outputs_2),
		ORB_ID(actuator_outputs_3)
	};

	/* copy actuator data into local buffer */
	orb_copy(ids[l->arg], *l->subp, &act_outputs);

	if (gcs_link) {
		mavlink_msg_servo_output_raw_send(MAVLINK_COMM_0, last_sensor_timestamp / 1000,
						  l->arg /* port number */,
						  act_outputs.output[0],
						  act_outputs.output[1],
						  act_outputs.output[2],
						  act_outputs.output[3],
						  act_outputs.output[4],
						  act_outputs.output[5],
						  act_outputs.output[6],
						  act_outputs.output[7]);

		/* only send in HIL mode */
		if (mavlink_hil_enabled && armed.armed) {

			/* 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);

			/* HIL message as per MAVLink spec */

			/* scale / assign outputs depending on system type */

			if (mavlink_system.type == MAV_TYPE_QUADROTOR) {
				mavlink_msg_hil_controls_send(chan,
							      hrt_absolute_time(),
							      ((act_outputs.output[0] - 900.0f) / 600.0f) / 2.0f,
							      ((act_outputs.output[1] - 900.0f) / 600.0f) / 2.0f,
							      ((act_outputs.output[2] - 900.0f) / 600.0f) / 2.0f,
							      ((act_outputs.output[3] - 900.0f) / 600.0f) / 2.0f,
							      -1,
							      -1,
							      -1,
							      -1,
							      mavlink_mode,
							      0);

			} else if (mavlink_system.type == MAV_TYPE_HEXAROTOR) {
				mavlink_msg_hil_controls_send(chan,
							      hrt_absolute_time(),
							      ((act_outputs.output[0] - 900.0f) / 600.0f) / 2.0f,
							      ((act_outputs.output[1] - 900.0f) / 600.0f) / 2.0f,
							      ((act_outputs.output[2] - 900.0f) / 600.0f) / 2.0f,
							      ((act_outputs.output[3] - 900.0f) / 600.0f) / 2.0f,
							      ((act_outputs.output[4] - 900.0f) / 600.0f) / 2.0f,
							      ((act_outputs.output[5] - 900.0f) / 600.0f) / 2.0f,
							      -1,
							      -1,
							      mavlink_mode,
							      0);

			} else if (mavlink_system.type == MAV_TYPE_OCTOROTOR) {
				mavlink_msg_hil_controls_send(chan,
							      hrt_absolute_time(),
							      ((act_outputs.output[0] - 900.0f) / 600.0f) / 2.0f,
							      ((act_outputs.output[1] - 900.0f) / 600.0f) / 2.0f,
							      ((act_outputs.output[2] - 900.0f) / 600.0f) / 2.0f,
							      ((act_outputs.output[3] - 900.0f) / 600.0f) / 2.0f,
							      ((act_outputs.output[4] - 900.0f) / 600.0f) / 2.0f,
							      ((act_outputs.output[5] - 900.0f) / 600.0f) / 2.0f,
							      ((act_outputs.output[6] - 900.0f) / 600.0f) / 2.0f,
							      ((act_outputs.output[7] - 900.0f) / 600.0f) / 2.0f,
							      mavlink_mode,
							      0);

			} else {
				mavlink_msg_hil_controls_send(chan,
							      hrt_absolute_time(),
							      (act_outputs.output[0] - 1500.0f) / 500.0f,
							      (act_outputs.output[1] - 1500.0f) / 500.0f,
							      (act_outputs.output[2] - 1500.0f) / 500.0f,
							      (act_outputs.output[3] - 1000.0f) / 1000.0f,
							      (act_outputs.output[4] - 1500.0f) / 500.0f,
							      (act_outputs.output[5] - 1500.0f) / 500.0f,
							      (act_outputs.output[6] - 1500.0f) / 500.0f,
							      (act_outputs.output[7] - 1500.0f) / 500.0f,
							      mavlink_mode,
							      0);
			}
		}
	}
}
コード例 #5
0
ファイル: mavlink.c プロジェクト: HimanshuKamat/Firmware
/**
 * MAVLink Protocol main function.
 */
int mavlink_thread_main(int argc, char *argv[])
{
	int ch;
	char *device_name = "/dev/ttyS1";
	baudrate = 57600;

	struct vehicle_status_s v_status;
	struct actuator_armed_s armed;

	/* 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);

	/* Initialize system properties */
	mavlink_update_system();

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

	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(&v_status, &armed, &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);

			/* 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 1000 ms */
		usleep(1000000);
	}

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

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

	thread_running = false;

	exit(0);
}
コード例 #6
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);
}
コード例 #7
0
ファイル: orb_listener.c プロジェクト: youngklee/Firmware
void
l_actuator_outputs(struct listener *l)
{
    struct actuator_outputs_s act_outputs;

    orb_id_t ids[] = {
        ORB_ID(actuator_outputs_0),
        ORB_ID(actuator_outputs_1),
        ORB_ID(actuator_outputs_2),
        ORB_ID(actuator_outputs_3)
    };

    /* copy actuator data into local buffer */
    orb_copy(ids[l->arg], *l->subp, &act_outputs);

    if (gcs_link) {
        mavlink_msg_servo_output_raw_send(MAVLINK_COMM_0, last_sensor_timestamp / 1000,
                                          l->arg /* port number */,
                                          act_outputs.output[0],
                                          act_outputs.output[1],
                                          act_outputs.output[2],
                                          act_outputs.output[3],
                                          act_outputs.output[4],
                                          act_outputs.output[5],
                                          act_outputs.output[6],
                                          act_outputs.output[7]);

        /* only send in HIL mode */
        if (mavlink_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);

            float rudder, throttle;

            // XXX very ugly, needs rework
            if (isfinite(act_outputs.output[3])
                    && act_outputs.output[3] > 800 && act_outputs.output[3] < 2200) {
                /* throttle is fourth output */
                rudder = (act_outputs.output[2] - 1500.0f) / 1000.0f;
                throttle = (act_outputs.output[3] - 1000.0f) / 1000.0f;
            } else {
                /* only three outputs, put throttle on position 4 / index 3 */
                rudder = 0;
                throttle = (act_outputs.output[2] - 1000.0f) / 1000.0f;
            }

            /* HIL message as per MAVLink spec */
            mavlink_msg_hil_controls_send(chan,
                                          hrt_absolute_time(),
                                          (act_outputs.output[0] - 1500.0f) / 1000.0f,
                                          (act_outputs.output[1] - 1500.0f) / 1000.0f,
                                          rudder,
                                          throttle,
                                          (act_outputs.output[4] - 1500.0f) / 1000.0f,
                                          (act_outputs.output[5] - 1500.0f) / 1000.0f,
                                          (act_outputs.output[6] - 1500.0f) / 1000.0f,
                                          (act_outputs.output[7] - 1500.0f) / 1000.0f,
                                          mavlink_mode,
                                          0);
        }
    }
}