From 92e15b83cc565cab149b7a8103794672dc82dcb5 Mon Sep 17 00:00:00 2001 From: Diogo Machado Date: Wed, 26 Feb 2014 11:24:32 +0000 Subject: [PATCH] added subscriber to vehicle_control_mode --- src/modules/mavlink/mavlink_receiver.cpp | 23 +++++++++++++++-------- 1 file changed, 15 insertions(+), 8 deletions(-) diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index b724447c5b..8ea0f9ea37 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -98,6 +98,7 @@ static struct vehicle_vicon_position_s vicon_position; static struct vehicle_command_s vcmd; static struct offboard_control_setpoint_s offboard_control_sp; static struct vehicle_attitude_setpoint_s att_sp; +static struct vehicle_control_mode_s _v_control_mode; struct vehicle_global_position_s hil_global_pos; struct vehicle_local_position_s hil_local_pos; @@ -117,6 +118,8 @@ static orb_advert_t pub_hil_baro = -1; static orb_advert_t pub_hil_airspeed = -1; static orb_advert_t pub_hil_battery = -1; +static int _v_control_mode_sub = -1; + /* packet counter */ static int hil_counter = 0; static int hil_frames = 0; @@ -330,14 +333,14 @@ handle_message(mavlink_message_t *msg) orb_publish(ORB_ID(offboard_control_setpoint), offboard_control_sp_pub, &offboard_control_sp); } - static struct vehicle_control_mode_s _v_control_mode; - memset(&_v_control_mode, 0, sizeof(_v_control_mode)); - - /**< vehicle control mode subscription */ - static int _v_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); - - // TODO use vehicle_control_mode.flag_control_offboard_enabled - if (v_status.main_state == MAIN_STATE_OFFBOARD) { + // TODO use vehicle_control_mode.flag_control_offboard_enabled + bool updated; + orb_check(_v_control_mode_sub, &updated); + if (updated) { + orb_copy(ORB_ID(vehicle_control_mode), _v_control_mode_sub, &_v_control_mode); + } + if(_v_control_mode.flag_control_offboard_enabled) { +// if (v_status.main_state == MAIN_STATE_OFFBOARD) { /* in offboard mode also publish setpoint directly */ switch (offboard_control_sp.mode) { case OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE: @@ -863,6 +866,10 @@ receive_thread(void *arg) ssize_t nread = 0; + // vehicle control mode subscription + memset(&_v_control_mode, 0, sizeof(_v_control_mode)); + _v_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); + while (!thread_should_exit) { if (poll(fds, 1, timeout) > 0) { if (nread < sizeof(buf)) {