px4_simple_app

最新推荐文章于 2023-12-21 23:00:51 发布

ssr_acty

最新推荐文章于 2023-12-21 23:00:51 发布

阅读量426

点赞数

文章标签：无人机

本文链接：https://blog.csdn.net/ssr_acty/article/details/127263093

版权

在这里插入代码片

#include <px4_platform_common/px4_config.h>
#include <px4_platform_common/tasks.h>
#include <px4_platform_common/posix.h>
#include <unistd.h>
#include <stdio.h>
#include <poll.h>
#include <string.h>
#include <math.h>

#include <uORB/uORB.h>
#include <uORB/topics/vehicle_acceleration.h>
#include <uORB/topics/vehicle_attitude.h>

__EXPORT int px4_simple_app_main(int argc, char *argv[]);

int uart_write(const char* str);

int px4_simple_app_main(int argc, char *argv[])
{
	PX4_INFO("Hello Sky!");

	/* subscribe to vehicle_acceleration topic */
	int sensor_sub_fd = orb_subscribe(ORB_ID(vehicle_acceleration));
	/* limit the update rate to 5 Hz */
	orb_set_interval(sensor_sub_fd, 200);

	/* advertise attitude topic */
	struct vehicle_attitude_s att;
	memset(&att, 0, sizeof(att));
	orb_advert_t att_pub = orb_advertise(ORB_ID(vehicle_attitude), &att);

	/* one could wait for multiple topics with this technique, just using one here */
	px4_pollfd_struct_t fds[] = {
		{ .fd = sensor_sub_fd,   .events = POLLIN },
		/* there could be more file descriptors here, in the form like:
		 * { .fd = other_sub_fd,   .events = POLLIN },
		 */
	};

	int error_counter = 0;

	for (int i = 0; i < 5; i++) {
		/* wait for sensor update of 1 file descriptor for 1000 ms (1 second) */
		int poll_ret = px4_poll(fds, 1, 1000);

		/* handle the poll result */
		if (poll_ret == 0) {
			/* this means none of our providers is giving us data */
			PX4_ERR("Got no data within a second");

		} else if (poll_ret < 0) {
			/* this is seriously bad - should be an emergency */
			if (error_counter < 10 || error_counter % 50 == 0) {
				/* use a counter to prevent flooding (and slowing us down) */
				PX4_ERR("ERROR return value from poll(): %d", poll_ret);
			}

			error_counter++;

		} else {

			if (fds[0].revents & POLLIN) {
				/* obtained data for the first file descriptor */
				struct vehicle_acceleration_s accel;
				/* copy sensors raw data into local buffer */
				orb_copy(ORB_ID(vehicle_acceleration), sensor_sub_fd, &accel);
				PX4_INFO("Accelerometer:\t%8.4f\t%8.4f\t%8.4f",
					 (double)accel.xyz[0],
					 (double)accel.xyz[1],
					 (double)accel.xyz[2]);
				uart_write("Accelerometer:\n");
				char tmp[21] = {"\0"};
				sprintf(tmp,"%lf",(double)accel.xyz[0]);
				uart_write(tmp);uart_write("\n");
				char tmp1[21] = {"\0"};
				sprintf(tmp1,"%lf",(double)accel.xyz[1]);
				uart_write(tmp1);uart_write("\n");
				char tmp2[21] = {"\0"};
				sprintf(tmp2,"%lf",(double)accel.xyz[2]);
				uart_write(tmp2);uart_write("\n");


				/* set att and publish this information for other apps
				 the following does not have any meaning, it's just an example
				*/
				att.q[0] = accel.xyz[0];
				att.q[1] = accel.xyz[1];
				att.q[2] = accel.xyz[2];

				orb_publish(ORB_ID(vehicle_attitude), att_pub, &att);
			}

			/* there could be more file descriptors here, in the form like:
			 * if (fds[1..n].revents & POLLIN) {}
			 */
		}
	}

	PX4_INFO("exiting");

	return 0;
}

int uart_write(const char* str){
    //dev/ttyS3  == UART4  57600
    const char* char_uart_name = "/dev/ttyS3";
    int test_uart = -1 ;
    test_uart = open(char_uart_name , O_RDWR | O_NONBLOCK | O_NOCTTY);
    if(test_uart<0){
        printf("open UART4 ERROR\n");
    }else {
        int size_of_str = 0; while(str!=NULL&&str[size_of_str]!='\0')size_of_str++;
        int res = write(test_uart,str,size_of_str);
        return res;
    }
    return -1;
}