diff options
author | Roman Bapst <romanbapst@yahoo.de> | 2014-12-02 10:47:07 +0100 |
---|---|---|
committer | Roman Bapst <romanbapst@yahoo.de> | 2014-12-02 10:47:07 +0100 |
commit | 9a64004d7ff60d0c4ca25877b34183631636112a (patch) | |
tree | 815a0582e01072cafcc42d2e05168886f78c1f24 /src | |
parent | 15ad9e441ef87b3e917a057e2f4b24d4bf3d0cee (diff) | |
download | px4-firmware-9a64004d7ff60d0c4ca25877b34183631636112a.tar.gz px4-firmware-9a64004d7ff60d0c4ca25877b34183631636112a.tar.bz2 px4-firmware-9a64004d7ff60d0c4ca25877b34183631636112a.zip |
let commander know if VTOL is in fw or in mc mode (important because of external_override)
Diffstat (limited to 'src')
-rw-r--r-- | src/modules/commander/commander.cpp | 41 |
1 files changed, 38 insertions, 3 deletions
diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index c9c8d16b5..bed467698 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -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; |