diff options
author | Anton Babushkin <anton.babushkin@me.com> | 2014-02-17 16:50:00 +0400 |
---|---|---|
committer | Anton Babushkin <anton.babushkin@me.com> | 2014-02-17 16:50:00 +0400 |
commit | e8bee868e2386b5ce9fc2bc4b8ca6aa63f240232 (patch) | |
tree | 531330f9dc38e2d8c794f5ea79fdcdf4af07797b | |
parent | 08055e5d52b8c522ca86e86bc161343dc396e5d9 (diff) | |
download | px4-firmware-e8bee868e2386b5ce9fc2bc4b8ca6aa63f240232.tar.gz px4-firmware-e8bee868e2386b5ce9fc2bc4b8ca6aa63f240232.tar.bz2 px4-firmware-e8bee868e2386b5ce9fc2bc4b8ca6aa63f240232.zip |
commander: publish vehicle_control_mode for OFFBOARD state
-rw-r--r-- | src/modules/commander/commander.cpp | 32 | ||||
-rw-r--r-- | src/modules/mavlink/mavlink_receiver.cpp | 1 | ||||
-rw-r--r-- | src/modules/uORB/topics/vehicle_control_mode.h | 1 |
3 files changed, 30 insertions, 4 deletions
diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 1b9ca6988..61ef63921 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -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; } diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 0ca64853a..735bc50da 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -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) { diff --git a/src/modules/uORB/topics/vehicle_control_mode.h b/src/modules/uORB/topics/vehicle_control_mode.h index 7cbb37cd8..5444c4ebf 100644 --- a/src/modules/uORB/topics/vehicle_control_mode.h +++ b/src/modules/uORB/topics/vehicle_control_mode.h @@ -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 */ |