px4용 App 만들기
px4에서 App을 어떻게 개발해야 하는지 그리고 실행 방법에 대해서 알아보자.
- 참고: http://dev.px4.io/tutorial-hello-sky.html 여기 튜토리얼을 따라해보았다!
- 준비 사항 : Pixhawk를 빌드할 수 있는 toolchain 설치
단계 1 : px4 개발환경 설치
- 우선 Firmware 디렉토리에서 Pixhawk를 일단 빌드하여 toolchain이 제대로 설치되었는지 확인한다. 제대로 빌드가 된다면 다음 단계로 넘어가자.
- 빌드하기 참고: http://dev.px4.io/starting-building.html
단계 2 : px4_simple_app 만들기
"Hello Sky!" 찍어보기.
기존의 Firmware/src/examples/px4_simple_app 디렉토리를 삭제하고 px4_simple_app 디렉토리에 px4_simple_app.c와 CMakeLists.txt파일을 만들어 보자.
px4_simple_app.c
#include <nuttx/config.h>
#include <stdio.h>
#include <errno.h>
__EXPORT int px4_simple_app_main(int argc, char *argv[]);
int px4_simple_app_main(int argc, char *argv[])
{
printf("Hello Sky!\n");
return OK;
}
CMakeLists.txt
px4_add_module(
MODULE examples__px4_simple_app
MAIN px4_simple_app
STACK_MAIN 2000
SRCS
px4_simple_app.c
DEPENDS
platforms__common
)
단계 3: nuttshell에 px4_simple_app추가하기
Pixhawk v1/2: Firmware/cmake/configs/nuttx_px4fmu-v2_default.cmake
examples/px4_simple_app
빌드하기
make px4fmu-v2_default
단계 4: 앱 업로드
make px4fmu-v2_default upload
USB나 시리얼포트를 이용하여 px4의 nsh에 접속한다. nsh에 접속하여 px4_simple_app 을 실행해보자.
nsh> px4_simple_app
Hello Sky!
mavlink nsh
./Tools/mavlink_shell.py /dev/tty.usbmodem1
단계 5: 센서데이터 구독하기
/Firmware/build_px4fmu-v2_default/src/modules/uORB/topics/sensor_combined.h
sensor_combined.h 토픽을 구독하여 PX4_INFO 로그를 남겨보자.
#include <px4_config.h>
#include <px4_tasks.h>
#include <px4_posix.h>
#include <unistd.h>
#include <stdio.h>
#include <poll.h>
#include <string.h>
#include <uORB/uORB.h>
#include <uORB/topics/sensor_combined.h>
__EXPORT int px4_simple_app_main(int argc, char *argv[]);
int px4_simple_app_main(int argc, char *argv[])
{
PX4_INFO("Hello Sky!");
int sensor_sub_fd = orb_subscribe(ORB_ID(sensor_combined));
/* one could wait for multiple topics with this technique, just using one here */
px4_pollfd_struct_t fds[] = {
{ .fd = sensor_sub_fd, .events = POLLIN },
};
while (true) {
/* 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 (fds[0].revents & POLLIN) {
/* obtained data for the first file descriptor */
struct sensor_combined_s raw;
/* copy sensors raw data into local buffer */
orb_copy(ORB_ID(sensor_combined), sensor_sub_fd, &raw);
PX4_INFO("Accelerometer:\t%8.4f\t%8.4f\t%8.4f",
(double)raw.accelerometer_m_s2[0],
(double)raw.accelerometer_m_s2[1],
(double)raw.accelerometer_m_s2[2]);
}
}
}
PX4_INFO("exiting");
return OK;
}
컴파일하고 다시 nsh에서 px4_simple_app을 실행한다.
make
nsh > px4_simple_app &
[px4_simple_app] Accelerometer: 0.0483 0.0821 0.0332
[px4_simple_app] Accelerometer: 0.0486 0.0820 0.0336
[px4_simple_app] Accelerometer: 0.0487 0.0819 0.0327
[px4_simple_app] Accelerometer: 0.0482 0.0818 0.0323
[px4_simple_app] Accelerometer: 0.0482 0.0827 0.0331
[px4_simple_app] Accelerometer: 0.0489 0.0804 0.0328
단계 7: publising data
sensor 데이터를 ground control station으로 보내보자. vehicle_attitude 토픽으로 자세정보를 publish 하면, mavlink 로 ground control station 에서 비행체의 자세정보를 받을 수 있다.
https://github.com/PX4/Firmware/blob/master/msg/vehicle_attitude.msg
#include <px4_config.h>
#include <px4_tasks.h>
#include <px4_posix.h>
#include <unistd.h>
#include <stdio.h>
#include <poll.h>
#include <string.h>
#include <uORB/uORB.h>
#include <uORB/topics/sensor_combined.h>
#include <uORB/topics/vehicle_attitude.h>
__EXPORT int px4_simple_app_main(int argc, char *argv[]);
int px4_simple_app_main(int argc, char *argv[])
{
PX4_INFO("Hello Sky!");
/* subscribe to sensor_combined topic */
int sensor_sub_fd = orb_subscribe(ORB_ID(sensor_combined));
/* 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 sensor_combined_s raw;
/* copy sensors raw data into local buffer */
orb_copy(ORB_ID(sensor_combined), sensor_sub_fd, &raw);
PX4_INFO("Accelerometer:\t%8.4f\t%8.4f\t%8.4f",
(double)raw.accelerometer_m_s2[0],
(double)raw.accelerometer_m_s2[1],
(double)raw.accelerometer_m_s2[2]);
/* set att and publish this information for other apps */
att.roll = raw.accelerometer_m_s2[0];
att.pitch = raw.accelerometer_m_s2[1];
att.yaw = raw.accelerometer_m_s2[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;
}
make
nsh > px4_simple_app
QGroundControl 에서 Tools -> Analyze 에서 그래프를 볼 수 있다.