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

Merge remote-tracking branch 'px4/pr/1058' into navigator_rewrite_offboard

Browse Source 
Conflicts:
	src/modules/commander/commander.cpp
	src/modules/commander/state_machine_helper.cpp
	src/modules/mavlink/mavlink_receiver.cpp
	src/modules/mavlink/mavlink_receiver.h
	src/modules/uORB/topics/vehicle_status.h
This commit is contained in:
Julian Oes
2014-06-20 18:18:39 +02:00
parent e0c78e51e3 9d18da4433
commit 4e08457afe
12 changed files with 294 additions and 11 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -124,6 +124,7 @@ extern struct system_load_s system_load;
#define FAILSAFE_DEFAULT_TIMEOUT (3 * 1000 * 1000) /**< hysteresis time - the failsafe will trigger after 3 seconds in this state */
#define RC_TIMEOUT 500000
#define DL_TIMEOUT 5 * 1000* 1000
#define OFFBOARD_TIMEOUT 500000
#define DIFFPRESS_TIMEOUT 2000000
#define PRINT_INTERVAL 5000000
@@ -164,6 +165,7 @@ static struct vehicle_status_s status;
static struct actuator_armed_s armed;
static struct safety_s safety;
static struct vehicle_control_mode_s control_mode;
static struct offboard_control_setpoint_s sp_offboard;
/* tasks waiting for low prio thread */
typedef enum {
@@ -437,6 +439,10 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe
} else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_ACRO) {
/* ACRO */
main_ret = main_state_transition(status, MAIN_STATE_ACRO);
} else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_OFFBOARD) {
/* OFFBOARD */
main_ret = main_state_transition(status, MAIN_STATE_OFFBOARD);
}
} else {
@@ -632,6 +638,7 @@ int commander_thread_main(int argc, char *argv[])
main_states_str[MAIN_STATE_AUTO_LOITER] = "AUTO_LOITER";
main_states_str[MAIN_STATE_AUTO_RTL] = "AUTO_RTL";
main_states_str[MAIN_STATE_ACRO] = "ACRO";
main_states_str[MAIN_STATE_OFFBOARD] = "OFFBOARD";
char *arming_states_str[ARMING_STATE_MAX];
arming_states_str[ARMING_STATE_INIT] = "INIT";
@@ -778,7 +785,6 @@ int commander_thread_main(int argc, char *argv[])
/* Subscribe to offboard control data */
int sp_offboard_sub = orb_subscribe(ORB_ID(offboard_control_setpoint));
struct offboard_control_setpoint_s sp_offboard;
memset(&sp_offboard, 0, sizeof(sp_offboard));
/* Subscribe to telemetry status */
@@ -1589,6 +1595,18 @@ set_main_state_rc(struct vehicle_status_s *status, struct manual_control_setpoin
/* set main state according to RC switches */
transition_result_t res = TRANSITION_DENIED;
/* offboard switch overrides main switch */
if (sp_man->offboard_switch == SWITCH_POS_ON) {
res = main_state_transition(status, MAIN_STATE_OFFBOARD);
if (res == TRANSITION_DENIED) {
print_reject_mode(status, "OFFBOARD");
} else {
return res;
}
}
/* offboard switched off or denied, check main mode switch */
switch (sp_man->mode_switch) {
case SWITCH_POS_NONE:
res = TRANSITION_NOT_CHANGED;
@@ -1680,6 +1698,7 @@ set_control_mode()
/* TODO: check this */
control_mode.flag_external_manual_override_ok = !status.is_rotary_wing;
control_mode.flag_system_hil_enabled = status.hil_state == HIL_STATE_ON;
control_mode.flag_control_offboard_enabled = false;
switch (status.nav_state) {
case NAVIGATION_STATE_MANUAL:
@@ -1718,6 +1737,54 @@ set_control_mode()
control_mode.flag_control_termination_enabled = false;
break;
case NAVIGATION_STATE_OFFBOARD:
control_mode.flag_control_manual_enabled = false;
control_mode.flag_control_auto_enabled = false;
control_mode.flag_control_offboard_enabled = true;
switch (sp_offboard.mode) {
case OFFBOARD_CONTROL_MODE_DIRECT_RATES:
control_mode.flag_control_rates_enabled = true;
control_mode.flag_control_attitude_enabled = false;
control_mode.flag_control_altitude_enabled = false;
control_mode.flag_control_climb_rate_enabled = false;
control_mode.flag_control_position_enabled = false;
control_mode.flag_control_velocity_enabled = false;
break;
case OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE:
control_mode.flag_control_rates_enabled = true;
control_mode.flag_control_attitude_enabled = true;
control_mode.flag_control_altitude_enabled = false;
control_mode.flag_control_climb_rate_enabled = false;
control_mode.flag_control_position_enabled = false;
control_mode.flag_control_velocity_enabled = false;
break;
case OFFBOARD_CONTROL_MODE_DIRECT_VELOCITY:
control_mode.flag_control_rates_enabled = true;
control_mode.flag_control_attitude_enabled = true;
control_mode.flag_control_altitude_enabled = false;
control_mode.flag_control_climb_rate_enabled = true;
control_mode.flag_control_position_enabled = false;
control_mode.flag_control_velocity_enabled = true;
break;
case OFFBOARD_CONTROL_MODE_DIRECT_POSITION:
control_mode.flag_control_rates_enabled = true;
control_mode.flag_control_attitude_enabled = true;
control_mode.flag_control_altitude_enabled = true;
control_mode.flag_control_climb_rate_enabled = true;
control_mode.flag_control_position_enabled = true;
control_mode.flag_control_velocity_enabled = true;
break;
default:
control_mode.flag_control_rates_enabled = false;
control_mode.flag_control_attitude_enabled = false;
control_mode.flag_control_altitude_enabled = false;
control_mode.flag_control_climb_rate_enabled = false;
control_mode.flag_control_position_enabled = false;
control_mode.flag_control_velocity_enabled = false;
}
break;
case NAVIGATION_STATE_POSCTL:
control_mode.flag_control_manual_enabled = true;
control_mode.flag_control_auto_enabled = false;