diff options
| author | Lorenz Meier <lm@inf.ethz.ch> | 2012-10-04 15:33:39 +0200 |
|---|---|---|
| committer | Lorenz Meier <lm@inf.ethz.ch> | 2012-10-04 15:33:39 +0200 |
| commit | 67a2c8a173819018ba2155688aa2a7bb682d8a77 (patch) | |
| tree | b3cf26e1ac002c3eb144ee50254b874bc5f0b506 /apps/mavlink | |
| parent | 607e902b886b39b5e9b58999dee97c2ea8938151 (diff) | |
| download | px4-firmware-67a2c8a173819018ba2155688aa2a7bb682d8a77.tar.gz px4-firmware-67a2c8a173819018ba2155688aa2a7bb682d8a77.tar.bz2 px4-firmware-67a2c8a173819018ba2155688aa2a7bb682d8a77.zip | |
Added controller parameters, added vicon position reading
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 cec2593c1..580287805 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 */ |