Add check for offboard control in the wiggle command.

SriniVest · Mar 27, 2017 · 0d0a4e6 · 0d0a4e6
1 parent ee26c20
commit 0d0a4e6
Show file tree

Hide file tree

Showing 2 changed files with 11 additions and 11 deletions.
diff --git a/MavLinkCom/MavLinkTest/Commands.cpp b/MavLinkCom/MavLinkTest/Commands.cpp
@@ -1215,7 +1215,6 @@ bool GotoCommand::Parse(const std::vector<std::string>& args) {
 void GotoCommand::Execute(std::shared_ptr<MavLinkVehicle> com) {
     this->channel = com;
     this->requestedControl = false;
-    this->hasControl = false;
     this->targetReached = false;
     this->settled = false;
     this->hasLocalPosition = false;
@@ -1874,18 +1873,20 @@ void WiggleCommand::HandleMessage(const MavLinkMessage& message)
         if (!ready_) {
             return;
         }
-        MavLinkLocalPositionNed pos;
-        pos.decode(message);
 
-        if (xaxis_) {
-            wiggleX(pos);
-        }
-        else {
-            wiggleY(pos);
-        }
+		if (vehicle->hasOffboardControl()) {
+			MavLinkLocalPositionNed pos;
+			pos.decode(message);
 
-        meter_.reportMessageRate();
+			if (xaxis_) {
+				wiggleX(pos);
+			}
+			else {
+				wiggleY(pos);
+			}
 
+			meter_.reportMessageRate();
+		}
         break;
     }
     default:

diff --git a/MavLinkCom/MavLinkTest/Commands.h b/MavLinkCom/MavLinkTest/Commands.h
@@ -422,7 +422,6 @@ class GotoCommand : public Command
 {
 	bool hasLocalPosition;
 	bool requestedControl;
-	bool hasControl;
 	bool targetReached;
 	bool targetPosition;
 	bool targetVelocity;