Skip to content

Commit

Permalink
mavlink message receive. Tested OK!
Browse files Browse the repository at this point in the history
  • Loading branch information
bing0037 committed Jul 13, 2017
1 parent 9a716d1 commit 724b200
Show file tree
Hide file tree
Showing 3 changed files with 29 additions and 11 deletions.
1 change: 1 addition & 0 deletions .gitignore
Original file line number Diff line number Diff line change
@@ -1,3 +1,4 @@
.vscode/
*.d
!ROMFS/*/*.d
!ROMFS/*/*/*.d
Expand Down
4 changes: 4 additions & 0 deletions ROMFS/px4fmu_common/init.d/rc.mc_apps
Original file line number Diff line number Diff line change
Expand Up @@ -44,3 +44,7 @@ mc_pos_control start
# Start Land Detector
#
land_detector start multicopter

mavlink_msg_receive_test start
mavlink stop-all -d /dev/ttyS2
mavlink start -d /dev/ttyS2 -m custom -b 57600
35 changes: 24 additions & 11 deletions src/modules/mavlink_msg_receive_test/mavlink_msg_receive_test.c
Original file line number Diff line number Diff line change
Expand Up @@ -144,16 +144,20 @@ int mavlink_msg_receive_thread_main(int argc, char *argv[])

thread_running = true;

/* subscribe to sensor_combined topic */
int topic_sub_fd = orb_subscribe(ORB_ID(parafoil_att));
/* subscribe to parafoil_att/attrate topic */
int parafoil_att_sub_fd = orb_subscribe(ORB_ID(parafoil_att));
int parafoil_attrate_sub_fd = orb_subscribe(ORB_ID(parafoil_attrate));

//orb_set_interval(motor_sub_fd, 1000);

px4_pollfd_struct_t fds[1];
fds[0].fd = topic_sub_fd;
px4_pollfd_struct_t fds[2];
fds[0].fd = parafoil_att_sub_fd;
fds[0].events = POLLIN;
fds[1].fd = parafoil_attrate_sub_fd;
fds[1].events = POLLIN;

while (!thread_should_exit) {
int poll_ret = px4_poll(fds, 1, 500);
int poll_ret = px4_poll(fds, 2, 500);
if(poll_ret < 0)
{
continue;
Expand All @@ -163,12 +167,21 @@ int mavlink_msg_receive_thread_main(int argc, char *argv[])
continue;
}
if (fds[0].revents & POLLIN) {
struct parafoil_att_s topic_data;
orb_copy(ORB_ID(parafoil_att), topic_sub_fd, &topic_data);
PX4_WARN("\t%8.4f\t%8.4f\t%8.4f",
(double)topic_data.roll_angle,
(double)topic_data.pitch_angle,
(double)topic_data.yaw_angle);
struct parafoil_att_s parafoil_att_data;
orb_copy(ORB_ID(parafoil_att), parafoil_att_sub_fd, &parafoil_att_data);
PX4_WARN("roll_angle: %8.4f\tpitch_angle: %8.4f\tyaw_angle: %8.4f",
(double)((parafoil_att_data.roll_angle)*180.0f/3.14f),
(double)((parafoil_att_data.pitch_angle)*180.0f/3.14f),
(double)((parafoil_att_data.yaw_angle)*180.0f/3.14f));
}
if (fds[1].revents & POLLIN) {
struct parafoil_attrate_s parafoil_attrate_data;
orb_copy(ORB_ID(parafoil_attrate), parafoil_attrate_sub_fd, &parafoil_attrate_data);
PX4_INFO("");
PX4_WARN("roll_rate: %8.4f\tpitch_rate: %8.4f\tyaw_rate: %8.4f",
(double)((parafoil_attrate_data.roll_rate)*180.0f/3.14f),
(double)((parafoil_attrate_data.pitch_rate)*180.0f/3.14f),
(double)((parafoil_attrate_data.yaw_rate)*180.0f/3.14f));
}
}

Expand Down

0 comments on commit 724b200

Please sign in to comment.