implement getLandedState.

AirLib/include/vehicles/multirotor/firmwares/simple_flight/SimpleFlightDroneController.hpp

@@ -143,8 +143,7 @@ class SimpleFlightDroneController : public DroneControllerBase {
 
     virtual LandedState getLandedState() const override
     {
-        //TODO: implement this
-        return LandedState::Landed;
+        return firmware_->offboardApi().getLandedState() ? LandedState::Landed : LandedState::Flying;
     }
 
     virtual int getRemoteControlID() const override

AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/OffboardApi.hpp

@@ -18,7 +18,7 @@ class OffboardApi :
     OffboardApi(const Params* params, const IBoardClock* clock, const IBoardInputPins* board_inputs, 
         IStateEstimator* state_estimator, ICommLink* comm_link)
         : params_(params), rc_(params, clock, board_inputs, &vehicle_state_, state_estimator, comm_link), 
-          state_estimator_(state_estimator), comm_link_(comm_link), clock_(clock)
+          state_estimator_(state_estimator), comm_link_(comm_link), clock_(clock), landed_(true)
     {
     }
 
@@ -28,7 +28,8 @@ class OffboardApi :
 
         vehicle_state_.setState(params_->default_vehicle_state, state_estimator_->getGeoPoint());
         rc_.reset();
-        has_api_control_ = false;
+        has_api_control_ = false; 
+        landed_ = true;
         goal_timestamp_ = 0;
         updateGoalFromRc();
     }
@@ -46,7 +47,6 @@ class OffboardApi :
                     comm_link_->log("API call timed out, entering hover mode");
                     goal_mode_ = GoalMode::getPositionMode();
                     goal_ = Axis4r::xyzToAxis4(state_estimator_->getPosition(), true);
-
                     is_api_timedout_ = true;
                 }
 
@@ -55,6 +55,8 @@ class OffboardApi :
 
         }
         //else leave the goal set by IOffboardApi API
+
+        detectLanding();
     }
 
     /**************** IOffboardApi ********************/
@@ -199,6 +201,10 @@ class OffboardApi :
         return state_estimator_->getGeoPoint();
     }
 
+    virtual bool getLandedState() const override
+    {
+        return landed_;
+    }
 
 private:
     void updateGoalFromRc()
@@ -207,7 +213,34 @@ class OffboardApi :
         goal_mode_ = rc_.getGoalMode();
     }
 
+    void detectLanding() {
+
+        // if we are not trying to move by setting motor outputs
+        if (isAlmostZero(goal_.roll()) && isAlmostZero(goal_.pitch()) && isAlmostZero(goal_.yaw()) && isGreaterThanMinThrottle(goal_.throttle()))
+        {
+            // and we are not currently moving (based on current velocities)
+            auto angular = state_estimator_->getAngulerVelocity();
+            auto velocity = state_estimator_->getLinearVelocity();
+            if (isAlmostZero(angular.roll()) && isAlmostZero(angular.pitch()) && isAlmostZero(angular.yaw()) &&
+                isAlmostZero(velocity.roll()) && isAlmostZero(velocity.pitch()) && isAlmostZero(velocity.yaw())) {
+                // then we must be landed...
+                landed_ = true;
+                return;
+            }
+        }
+
+        landed_ = false;
+    }
+
+    bool isAlmostZero(float v) {
+        return std::abs(v) < kMovementTolerance;
+    }
+    bool isGreaterThanMinThrottle(float throttle) {
+        return std::abs(throttle) <= std::abs(params_->rc.min_angling_throttle);
+    }
+
 private:
+    const TReal kMovementTolerance = (TReal)0.08;
     const Params* params_;
     RemoteControl rc_;
     IStateEstimator* state_estimator_;
@@ -222,6 +255,7 @@ class OffboardApi :
 
     bool has_api_control_;
     bool is_api_timedout_;
+    bool landed_;
 };
 
 

AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/interfaces/IOffboardApi.hpp

@@ -18,6 +18,8 @@ class IOffboardApi : public IGoal {
     virtual bool disarm(std::string& message) = 0;
     virtual VehicleStateType getVehicleState() const = 0;
 
+    virtual bool getLandedState() const = 0;
+
     virtual const IStateEstimator& getStateEstimator() = 0;
     virtual GeoPoint getHomeGeoPoint() const = 0;
     virtual GeoPoint getGeoPoint() const = 0;

PythonClient/orbit.py

@@ -12,7 +12,7 @@ def __init__(self, pos):
 
 # Make the drone fly in a circle.
 class OrbitNavigator:
-    def __init__(self, takeoff = True, radius = 2, altitude = 10, speed = 2, iterations = 1, center = [1,0], snapshots = None):
+    def __init__(self, radius = 2, altitude = 10, speed = 2, iterations = 1, center = [1,0], snapshots = None):
         self.radius = radius
         self.altitude = altitude
         self.speed = speed
@@ -22,7 +22,7 @@ def __init__(self, takeoff = True, radius = 2, altitude = 10, speed = 2, iterati
         self.next_snapshot = None
         self.z = None
         self.snapshot_index = 0
-        self.takeoff = takeoff # whether to do the take off and landing.
+        self.takeoff = False # whether we did a take off
 
         if self.snapshots > 0:
             self.snapshot_delta = 360 / self.snapshots
@@ -77,7 +77,8 @@ def start(self):
         # AirSim uses NED coordinates so negative axis is up.
         start = self.getPosition()
         landed = self.client.getLandedState()
-        if self.takeoff and landed == LandedState.Landed: 
+        if landed == LandedState.Landed: 
+            self.takeoff = True
             print("taking off...")
             self.client.takeoff()
             start = self.getPosition()
@@ -225,9 +226,6 @@ def sign(self, s):
             return -1
         return 1
 
-def str2bool(v):
-    return v.lower() in [ "true", "1", "yes"]
-
 if __name__ == "__main__":
     args = sys.argv
     args.pop(0)
@@ -238,7 +236,6 @@ def str2bool(v):
     arg_parser.add_argument("--center", help="x,y direction vector pointing to center of orbit from current starting position (default 1,0)", default="1,0")
     arg_parser.add_argument("--iterations", type=float, help="number of 360 degree orbits (default 3)", default=3)
     arg_parser.add_argument("--snapshots", type=float, help="number of FPV snapshots to take during orbit (default 0)", default=0)    
-    arg_parser.add_argument("--takeoff", help="Whether to perform takeoff or not", default="True")
     args = arg_parser.parse_args(args)    
-    nav = OrbitNavigator(str2bool(args.takeoff), args.radius, args.altitude, args.speed, args.iterations, args.center.split(','), args.snapshots)
+    nav = OrbitNavigator(args.radius, args.altitude, args.speed, args.iterations, args.center.split(','), args.snapshots)
     nav.start()