fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

commander: publish vehicle_control_mode for OFFBOARD state

Browse Source
This commit is contained in:
Anton Babushkin 2014-02-17 16:50:00 +04:00
parent 08055e5d52
commit e8bee868e2
3 changed files with 30 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -159,6 +159,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 {
@ -767,7 +768,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 global position */
@ -1691,6 +1691,8 @@ set_control_mode()
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_auto_enabled = false;
control_mode.flag_control_offboard_enabled = false;
control_mode.flag_control_termination_enabled = false;
/* set this flag when navigator should act */
@ -1701,7 +1703,6 @@ set_control_mode()
switch (status.main_state) {
case MAIN_STATE_MANUAL:
control_mode.flag_control_manual_enabled = true;
control_mode.flag_control_auto_enabled = false;
control_mode.flag_control_rates_enabled = status.is_rotary_wing;
control_mode.flag_control_attitude_enabled = status.is_rotary_wing;
control_mode.flag_control_altitude_enabled = false;
@ -1712,7 +1713,6 @@ set_control_mode()
case MAIN_STATE_SEATBELT:
control_mode.flag_control_manual_enabled = true;
control_mode.flag_control_auto_enabled = false;
control_mode.flag_control_rates_enabled = true;
control_mode.flag_control_attitude_enabled = true;
control_mode.flag_control_altitude_enabled = true;
@ -1723,7 +1723,6 @@ set_control_mode()
case MAIN_STATE_EASY:
control_mode.flag_control_manual_enabled = true;
control_mode.flag_control_auto_enabled = false;
control_mode.flag_control_rates_enabled = true;
control_mode.flag_control_attitude_enabled = true;
control_mode.flag_control_altitude_enabled = true;
@ -1735,6 +1734,31 @@ set_control_mode()
case MAIN_STATE_AUTO:
navigator_enabled = true;
case MAIN_STATE_OFFBOARD:
control_mode.flag_control_manual_enabled = false;
control_mode.flag_control_offboard_enabled = false;
switch (sp_offboard.mode) {
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;
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;
default:
break;
}

1
src/modules/mavlink/mavlink_receiver.cpp
View File

@ -330,6 +330,7 @@ handle_message(mavlink_message_t *msg)
orb_publish(ORB_ID(offboard_control_setpoint), offboard_control_sp_pub, &offboard_control_sp);
}
// TODO use vehicle_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) {

1
src/modules/uORB/topics/vehicle_control_mode.h
View File

@ -74,6 +74,7 @@ struct vehicle_control_mode_s
bool flag_control_manual_enabled; /**< true if manual input is mixed in */
bool flag_control_auto_enabled; /**< true if onboard autopilot should act */
bool flag_control_offboard_enabled; /**< true if offboard control should be used */
bool flag_control_rates_enabled; /**< true if rates are stabilized */
bool flag_control_attitude_enabled; /**< true if attitude stabilization is mixed in */
bool flag_control_velocity_enabled; /**< true if horizontal velocity (implies direction) is controlled */