Fix survey.py and related problems.

modi0012 · Dec 12, 2018 · 380372f · 380372f
1 parent 4f8c9d1
commit 380372f
Show file tree

Hide file tree

Showing 4 changed files with 16 additions and 4 deletions.
diff --git a/AirLib/src/vehicles/multirotor/api/MultirotorApiBase.cpp b/AirLib/src/vehicles/multirotor/api/MultirotorApiBase.cpp
@@ -664,7 +664,7 @@ float MultirotorApiBase::setNextPathPosition(const vector<Vector3r>& path, const
         offset = 0;
 
         if (&cur_path_loc == &next_path_loc)
-            Utils::log(Utils::stringf("segment %d done: x=%f, y=%f, z=%f", i, path.at(i).x(), path.at(i).z(), path.at(i).z()));
+            Utils::log(Utils::stringf("segment %d done: x=%f, y=%f, z=%f", i, path.at(i).x(), path.at(i).y(), path.at(i).z()));
 
         ++i;
     }

diff --git a/MavLinkCom/src/impl/MavLinkNodeImpl.cpp b/MavLinkCom/src/impl/MavLinkNodeImpl.cpp
@@ -52,7 +52,7 @@ void MavLinkNodeImpl::sendHeartbeat()
         MavLinkHeartbeat heartbeat;
         // send a heart beat so that the remote node knows we are still alive
         // (otherwise drone will trigger a failsafe operation).	
-        heartbeat.autopilot = static_cast<uint8_t>(MAV_AUTOPILOT::MAV_AUTOPILOT_INVALID);
+        heartbeat.autopilot = static_cast<uint8_t>(MAV_AUTOPILOT::MAV_AUTOPILOT_GENERIC);
         heartbeat.type = static_cast<uint8_t>(MAV_TYPE::MAV_TYPE_GCS);
         heartbeat.mavlink_version = 3;
         heartbeat.base_mode = 0; // ignored by PX4

diff --git a/MavLinkCom/src/impl/MavLinkVehicleImpl.cpp b/MavLinkCom/src/impl/MavLinkVehicleImpl.cpp
@@ -178,7 +178,11 @@ void MavLinkVehicleImpl::handleMessage(std::shared_ptr<MavLinkConnection> connec
             int submode = (custom >> 8);
 
             bool isOffboard = (mode == PX4_CUSTOM_MAIN_MODE_OFFBOARD);
-            if (vehicle_state_.controls.offboard != isOffboard) {
+            if (isOffboard) {
+                vehicle_state_.controls.offboard = isOffboard;
+                Utils::log("MavLinkVehicle: confirmed offboard mode\n");
+            }
+            else if (vehicle_state_.controls.offboard != isOffboard) {
                 vehicle_state_.controls.offboard = isOffboard;
                 Utils::log("MavLinkVehicle: is no longer in offboard mode\n");
             }

diff --git a/PythonClient/multirotor/survey.py b/PythonClient/multirotor/survey.py
@@ -23,6 +23,11 @@ def start(self):
         if landed == airsim.LandedState.Landed:
             print("taking off...")
             self.client.takeoffAsync().join()
+
+        landed = self.client.getMultirotorState().landed_state
+        if landed == airsim.LandedState.Landed:
+            print("takeoff failed - check Unreal message log for details")
+            return
 
         # AirSim uses NED coordinates so negative axis is up.
         x = -self.boxsize
@@ -38,6 +43,9 @@ def start(self):
         self.client.hoverAsync().join()
         time.sleep(2)
 
+        # after hovering we need to re-enabled api control for next leg of the trip
+        self.client.enableApiControl(True)
+
         # now compute the survey path required to fill the box 
         path = []
         distance = 0
@@ -59,7 +67,7 @@ def start(self):
         print("estimated survey time is " + str(trip_time))
         try:
             result = self.client.moveOnPathAsync(path, self.velocity, trip_time, airsim.DrivetrainType.ForwardOnly, 
-                airsim.YawMode(True,0), self.velocity + (self.velocity/2), 1).join()
+                airsim.YawMode(False,0), self.velocity + (self.velocity/2), 1).join()
         except:
             errorType, value, traceback = sys.exc_info()
             print("moveOnPath threw exception: " + str(value))