updated goAutonomous, goManual and toggleAutonomy

d7ae91c5 · Thomas Gubler · f50c15f1 · d7ae91c5
Commit d7ae91c5 authored Dec 06, 2013 by Thomas Gubler
Hide whitespace changes
Inline Side-by-side

Showing with 3 additions and 3 deletions

UAS.cc src/uas/UAS.cc +3 -3

No files found.
--- a/src/uas/UAS.cc
+++ b/src/uas/UAS.cc
@@ -2763,17 +2763,17 @@ void UAS::toggleArmedState()

 void UAS::goAutonomous()
 {
-    setMode(base_mode | MAV_MODE_FLAG_AUTO_ENABLED, custom_mode);
+    setMode((base_mode & ~MAV_MODE_FLAG_MANUAL_INPUT_ENABLED) | (MAV_MODE_FLAG_AUTO_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED), 0);
 }

 void UAS::goManual()
 {
-    setMode(base_mode | MAV_MODE_FLAG_MANUAL_INPUT_ENABLED, custom_mode);
+    setMode((base_mode & ~(MAV_MODE_FLAG_AUTO_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED))  | MAV_MODE_FLAG_MANUAL_INPUT_ENABLED, 0);
 }

 void UAS::toggleAutonomy()
 {
-    setMode(base_mode ^ MAV_MODE_FLAG_AUTO_ENABLED ^ MAV_MODE_FLAG_MANUAL_INPUT_ENABLED, custom_mode);
+    setMode(base_mode ^ MAV_MODE_FLAG_AUTO_ENABLED ^ MAV_MODE_FLAG_MANUAL_INPUT_ENABLED ^ MAV_MODE_FLAG_GUIDED_ENABLED ^ MAV_MODE_FLAG_STABILIZE_ENABLED, 0);
 }

 /**