yemai/bizhang_-obav
1
0
Fork 0
mirror of https://gitee.com/xiaohuolufeihua/bizhang_-obav.git synced 2026-05-22 09:28:58 +00:00
Code Issues Packages Projects Releases Wiki Activity

let commander know if VTOL is in fw or in mc mode (important because of external_override)

Browse Source
This commit is contained in:
Roman Bapst
2014-12-02 10:47:07 +01:00
parent 15ad9e441e
commit 9a64004d7f
1 changed files with 38 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files

41
src/modules/commander/commander.cpp
View File

@@ -82,6 +82,7 @@
#include <uORB/topics/mission.h>
#include <uORB/topics/mission_result.h>
#include <uORB/topics/telemetry_status.h>
#include <uORB/topics/vtol_vehicle_status.h>
#include <drivers/drv_led.h>
#include <drivers/drv_hrt.h>
@@ -149,6 +150,9 @@ enum MAV_MODE_FLAG {
/* Mavlink file descriptors */
static int mavlink_fd = 0;
/* Syste autostart ID */
static int autostart_id;
/* flags */
static bool commander_initialized = false;
static volatile bool thread_should_exit = false; /**< daemon exit flag */
@@ -732,6 +736,7 @@ int commander_thread_main(int argc, char *argv[])
param_t _param_ef_throttle_thres = param_find("COM_EF_THROT");
param_t _param_ef_current2throttle_thres = param_find("COM_EF_C2T");
param_t _param_ef_time_thres = param_find("COM_EF_TIME");
param_t _param_autostart_id = param_find("SYS_AUTOSTART");
/* welcome user */
warnx("starting");
@@ -1014,6 +1019,13 @@ int commander_thread_main(int argc, char *argv[])
struct actuator_controls_s actuator_controls;
memset(&actuator_controls, 0, sizeof(actuator_controls));
/* Subscribe to vtol vehicle status topic */
int vtol_vehicle_status_sub = orb_subscribe(ORB_ID(vtol_vehicle_status));
struct vtol_vehicle_status_s vtol_status;
memset(&vtol_status, 0, sizeof(vtol_status));
vtol_status.vtol_in_rw_mode = true; //default for vtol is rotary wing
control_status_leds(&status, &armed, true);
/* now initialized */
@@ -1070,7 +1082,10 @@ int commander_thread_main(int argc, char *argv[])
status.system_type == VEHICLE_TYPE_TRICOPTER ||
status.system_type == VEHICLE_TYPE_QUADROTOR ||
status.system_type == VEHICLE_TYPE_HEXAROTOR ||
status.system_type == VEHICLE_TYPE_OCTOROTOR) {
status.system_type == VEHICLE_TYPE_OCTOROTOR ||
(status.system_type == VEHICLE_TYPE_VTOL_DUOROTOR && vtol_status.vtol_in_rw_mode) ||
(status.system_type == VEHICLE_TYPE_VTOL_QUADROTOR && vtol_status.vtol_in_rw_mode)) {
status.is_rotary_wing = true;
} else {
@@ -1106,6 +1121,7 @@ int commander_thread_main(int argc, char *argv[])
param_get(_param_ef_throttle_thres, &ef_throttle_thres);
param_get(_param_ef_current2throttle_thres, &ef_current2throttle_thres);
param_get(_param_ef_time_thres, &ef_time_thres);
param_get(_param_autostart_id, &autostart_id);
}
orb_check(sp_man_sub, &updated);
@@ -1231,6 +1247,19 @@ int commander_thread_main(int argc, char *argv[])
}
}
/* update vtol vehicle status*/
orb_check(vtol_vehicle_status_sub, &updated);
if (updated) {
/* vtol status changed */
orb_copy(ORB_ID(vtol_vehicle_status), vtol_vehicle_status_sub, &vtol_status);
/* Make sure that this is only adjusted if vehicle realy is of type vtol*/
if (status.system_type == VEHICLE_TYPE_VTOL_DUOROTOR || VEHICLE_TYPE_VTOL_QUADROTOR) {
status.is_rotary_wing = vtol_status.vtol_in_rw_mode;
}
}
/* update global position estimate */
orb_check(global_position_sub, &updated);
@@ -2189,7 +2218,13 @@ set_control_mode()
/* set vehicle_control_mode according to set_navigation_state */
control_mode.flag_armed = armed.armed;
/* TODO: check this */
control_mode.flag_external_manual_override_ok = !status.is_rotary_wing;
if (autostart_id < 13000 || autostart_id >= 14000) {
control_mode.flag_external_manual_override_ok = !status.is_rotary_wing;
} else {
control_mode.flag_external_manual_override_ok = false;
}
control_mode.flag_system_hil_enabled = status.hil_state == HIL_STATE_ON;
control_mode.flag_control_offboard_enabled = false;
@@ -2205,7 +2240,7 @@ set_control_mode()
control_mode.flag_control_velocity_enabled = false;
control_mode.flag_control_termination_enabled = false;
break;
case NAVIGATION_STATE_ALTCTL:
control_mode.flag_control_manual_enabled = true;
control_mode.flag_control_auto_enabled = false;