diff options
| author | tnaegeli <naegelit@student.ethz.ch> | 2012-10-04 16:04:49 +0200 |
|---|---|---|
| committer | tnaegeli <naegelit@student.ethz.ch> | 2012-10-04 16:04:49 +0200 |
| commit | f292b03772ddf9a0ae72615248c65959a110d8e2 (patch) | |
| tree | 8c358c25e41c3710bf6421c10b5edbb8bab5cef6 /apps/mavlink | |
| parent | 8dfa66cb9710f1f5f8baddb6d0b542787af44f15 (diff) | |
| parent | 67a2c8a173819018ba2155688aa2a7bb682d8a77 (diff) | |
| download | px4-firmware-f292b03772ddf9a0ae72615248c65959a110d8e2.tar.gz px4-firmware-f292b03772ddf9a0ae72615248c65959a110d8e2.tar.bz2 px4-firmware-f292b03772ddf9a0ae72615248c65959a110d8e2.zip | |
Merge branch 'master' of https://github.com/PX4/Firmware
Diffstat (limited to 'apps/mavlink')
| -rw-r--r-- | apps/mavlink/mavlink.c | 28 |
1 files changed, 25 insertions, 3 deletions
diff --git a/apps/mavlink/mavlink.c b/apps/mavlink/mavlink.c index f36fb009d..c13f462ea 100644 --- a/apps/mavlink/mavlink.c +++ b/apps/mavlink/mavlink.c @@ -68,6 +68,7 @@ #include <uORB/topics/offboard_control_setpoint.h> #include <uORB/topics/vehicle_command.h> #include <uORB/topics/vehicle_local_position_setpoint.h> +#include <uORB/topics/vehicle_vicon_position.h> #include <uORB/topics/vehicle_global_position_setpoint.h> #include <uORB/topics/vehicle_attitude_setpoint.h> #include <uORB/topics/optical_flow.h> @@ -134,6 +135,8 @@ static struct vehicle_command_s vcmd; static struct actuator_armed_s armed; +static struct vehicle_vicon_position_s vicon_position; + static orb_advert_t pub_hil_global_pos = -1; static orb_advert_t cmd_pub = -1; static orb_advert_t flow_pub = -1; @@ -191,8 +194,10 @@ static struct mavlink_subscriptions { static struct mavlink_publications { orb_advert_t offboard_control_sp_pub; + orb_advert_t vicon_position_pub; } mavlink_pubs = { - .offboard_control_sp_pub = -1 + .offboard_control_sp_pub = -1, + .vicon_position_pub = -1 }; @@ -1240,9 +1245,26 @@ void handleMessage(mavlink_message_t *msg) /* check if topic is advertised */ if (cmd_pub <= 0) { cmd_pub = orb_advertise(ORB_ID(vehicle_command), &vcmd); + } else { + /* create command */ + orb_publish(ORB_ID(vehicle_command), cmd_pub, &vcmd); + } + } + + /* Handle Vicon position estimates */ + if (msg->msgid == MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE) { + mavlink_vicon_position_estimate_t pos; + mavlink_msg_vicon_position_estimate_decode(msg, &pos); + + vicon_position.x = pos.x; + vicon_position.y = pos.y; + vicon_position.z = pos.z; + + if (mavlink_pubs.vicon_position_pub <= 0) { + mavlink_pubs.vicon_position_pub = orb_advertise(ORB_ID(vehicle_vicon_position), &vicon_position); + } else { + orb_publish(ORB_ID(vehicle_vicon_position), mavlink_pubs.vicon_position_pub, &vicon_position); } - /* create command */ - orb_publish(ORB_ID(vehicle_command), cmd_pub, &vcmd); } /* Handle quadrotor motor setpoints */ |