fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-06-28 17:30:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

added subscriber to vehicle_control_mode

Browse Source
This commit is contained in:
Diogo Machado
2014-02-26 11:24:32 +00:00
parent 170ede1ea9
commit 92e15b83cc
1 changed files with 15 additions and 8 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mavlink/mavlink_receiver.cpp
+15 -8
View File
@@ -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)) {