MAVLink app: send out right mode flags for new stabilized mode

This commit is contained in:
Lorenz Meier 2015-05-22 21:07:53 +02:00
parent 5197be67a7
commit 0e11f1632c

View File

@ -130,6 +130,12 @@ void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_set
custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_ACRO;
break;
case vehicle_status_s::NAVIGATION_STATE_STAB:
*mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED
| MAV_MODE_FLAG_STABILIZE_ENABLED;
custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_STABILIZED;
break;
case vehicle_status_s::NAVIGATION_STATE_ALTCTL:
*mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED
| MAV_MODE_FLAG_STABILIZE_ENABLED;