enter hover mode if API activity timesout

AirLib/include/common/common_utils/WorkerThread.hpp

@@ -24,6 +24,13 @@ class CancelableBase {
 public:
     CancelableBase() : is_cancelled_(false), is_complete_(false) {
     }
+
+    void reset()
+    {
+        is_cancelled_ = false;
+        is_complete_ = false;
+    }
+
     bool isCancelled() {
         return is_cancelled_;
     }
@@ -34,7 +41,7 @@ class CancelableBase {
 
     virtual void execute() = 0;
 
-    bool sleep(double secs)
+    virtual bool sleep(double secs)
     {
         //We can pass duration directly to sleep_for however it is known that on 
         //some systems, sleep_for makes system call anyway even if passed duration 

AirLib/include/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)
+          state_estimator_(state_estimator), comm_link_(comm_link), clock_(clock)
     {
     }
 
@@ -27,18 +27,32 @@ class OffboardApi :
         IUpdatable::reset();
 
         vehicle_state_.setState(params_->default_vehicle_state, state_estimator_->getGeoPoint());
-        has_api_control_ = false;
         rc_.reset();
+        has_api_control_ = false;
+        goal_timestamp_ = 0;
         updateGoalFromRc();
     }
 
     virtual void update() override
     {
         IUpdatable::update();
 
-        if (! has_api_control_) {
-            rc_.update();
+        rc_.update();
+        if (! has_api_control_)
             updateGoalFromRc();
+        else {
+            if (clock_->millis() - goal_timestamp_ > params_->api_goal_timeout) {
+                if (!is_api_timedout_) {
+                    comm_link_->log("API call timed out, entering hover mode");
+                    goal_mode_ = GoalMode::getPositionMode();
+                    goal_ = Axis4r::xyzToAxis4(state_estimator_->getPosition());
+
+                    is_api_timedout_ = true;
+                }
+
+                //do not update goal_timestamp_
+            }
+
         }
         //else leave the goal set by IOffboardApi API
     }
@@ -99,6 +113,8 @@ class OffboardApi :
                 goal_ = *goal;
             if (goal_mode != nullptr)
                 goal_mode_ = *goal_mode;
+            goal_timestamp_ = clock_->millis();
+            is_api_timedout_ = false;
             return true;
         }
         else {
@@ -185,13 +201,16 @@ class OffboardApi :
     RemoteControl rc_;
     IStateEstimator* state_estimator_;
     ICommLink* comm_link_;
+    const IBoardClock* clock_;
 
     VehicleState vehicle_state_;
 
     Axis4r goal_;
     GoalMode goal_mode_;
+    uint64_t goal_timestamp_;
 
     bool has_api_control_;
+    bool is_api_timedout_;
 };
 
 

AirLib/include/firmwares/simple_flight/firmware/Params.hpp

@@ -91,6 +91,7 @@ struct Params {
 
     GoalMode default_goal_mode = GoalMode::getStandardAngleMode();
     VehicleStateType default_vehicle_state = VehicleStateType::Inactive;
+    uint64_t api_goal_timeout = 60; //milliseconds
 };
 
 

AirLib/include/firmwares/simple_flight/firmware/PidController.hpp

@@ -101,7 +101,7 @@ class PidController : public IUpdatable {
 
         float dt = clock_ == nullptr ? 1 :
             (static_cast<float>(clock_->millis()) - last_time_)
-                / config_.time_scale;
+            * config_.time_scale;
 
         float pterm = error * config_.kp;
         float dterm = 0;

AirLib/include/firmwares/simple_flight/firmware/PositionController.hpp

@@ -61,12 +61,12 @@ class PositionController :
 
         const Axis4r& goal_position_world = goal_->getGoalValue();
         pid_->setGoal(goal_position_world[axis_]);
-        const Axis3r& measured_position_world = state_estimator_->getPosition();
+        const Axis4r& measured_position_world = Axis4r::xyzToAxis4(
+            state_estimator_->getPosition());
         pid_->setMeasured(measured_position_world[axis_]);
         pid_->update();
 
         //use this to drive child controller
-        velocity_goal_.throttle() = goal_position_world.throttle();
         velocity_goal_[axis_] = pid_->getOutput() * params_->velocity_pid.max_limit[axis_];
         velocity_controller_->update();
 

AirLib/include/firmwares/simple_flight/firmware/VelocityController.hpp

@@ -94,16 +94,17 @@ class VelocityController :
         switch (axis_)
         {
         case 0: //+vx is -ve pitch
-            child_goal_.pitch() = -pid_->getOutput() * params_->angle_level_pid.max_limit.pitch();
+            child_goal_.pitch() = pid_->getOutput() * params_->angle_level_pid.max_limit.pitch();
             child_controller_->update();
             output_ = child_controller_->getOutput();
             break;
         case 1: //+vy is +ve roll
-            child_goal_.roll() = pid_->getOutput() * params_->angle_level_pid.max_limit.roll();
+            child_goal_.roll() = -pid_->getOutput() * params_->angle_level_pid.max_limit.roll();
+            child_controller_->update();
             output_ = child_controller_->getOutput();
             break;
         case 3: //+vz is -ve throttle (NED coordinates)
-            output_ = (- pid_->getOutput() + 1) / 2; //-1 to 1 --> 0 to 1
+            output_ = (-pid_->getOutput() + 1) / 2; //-1 to 1 --> 0 to 1
             output_ = std::max(output_, params_->velocity_pid.min_throttle);
             break;
         default:

AirLib/include/firmwares/simple_flight/firmware/interfaces/CommonStructs.hpp

@@ -223,15 +223,38 @@ class GoalMode : public Axis4<GoalModeType> {
         return mode;
     }
 
+    static const GoalMode& getVelocityXYPosZMode() 
+    {
+        static const GoalMode mode = GoalMode(GoalModeType::VelocityWorld, 
+            GoalModeType::VelocityWorld, GoalModeType::AngleRate, GoalModeType::PositionWorld);
+        return mode;
+    }
+
+    static const GoalMode& getVelocityMode() 
+    {
+        static const GoalMode mode = GoalMode(GoalModeType::VelocityWorld, 
+            GoalModeType::VelocityWorld, GoalModeType::AngleRate, GoalModeType::VelocityWorld);
+        return mode;
+    }
+
+    static const GoalMode& getPositionMode() 
+    {
+        static const GoalMode mode = GoalMode(GoalModeType::PositionWorld, 
+            GoalModeType::PositionWorld, GoalModeType::AngleRate, GoalModeType::PositionWorld);
+        return mode;
+    }
+
     static const GoalMode& getAllRateMode()
     {
-        static const GoalMode mode = GoalMode(GoalModeType::AngleRate, GoalModeType::AngleRate, GoalModeType::AngleRate, GoalModeType::Passthrough);
+        static const GoalMode mode = GoalMode(GoalModeType::AngleRate, 
+            GoalModeType::AngleRate, GoalModeType::AngleRate, GoalModeType::Passthrough);
         return mode;
     }
 
     static const GoalMode& getUnknown()
     {
-        static const GoalMode mode = GoalMode(GoalModeType::Unknown, GoalModeType::Unknown, GoalModeType::Unknown, GoalModeType::Unknown);
+        static const GoalMode mode = GoalMode(GoalModeType::Unknown, 
+            GoalModeType::Unknown, GoalModeType::Unknown, GoalModeType::Unknown);
         return mode;
     }
 };

AirLib/src/controllers/DroneControllerBase.cpp

@@ -324,7 +324,7 @@ bool DroneControllerBase::rotateByYawRate(float yaw_rate, float duration, Cancel
 bool DroneControllerBase::takeoff(float max_wait_seconds, CancelableBase& cancelable_action)
 {
     unused(max_wait_seconds);
-    return moveToPosition(0, 0, -6, 0.5f, DrivetrainType::MaxDegreeOfFreedom, YawMode::Zero(), -1, 1, cancelable_action);
+    return moveToPosition(0, 0, getTakeoffZ(), 0.5f, DrivetrainType::MaxDegreeOfFreedom, YawMode::Zero(), -1, 1, cancelable_action);
 }
 
 bool DroneControllerBase::goHome(CancelableBase& cancelable_action)

AirLibUnitTests/SimpleFlightTest.hpp

@@ -35,31 +35,56 @@ class SimpleFlightTest : public TestBase
         std::string message;
         testAssert(controller->isAvailable(message), message);
 
-        DirectCancelableBase cancellable;
+        clock->sleep_for(0.04f);
+
+        DirectCancelableBase cancellable(controller, &vehicle);
         controller->setOffboardMode(true);
         controller->armDisarm(true, cancellable);
         controller->takeoff(10, cancellable);
 
-        std::vector<std::string> messages;
-
-        for (int i = 0; i < 100; ++i) {
-            clock->sleep_for(0.02f);
-
-            controller->getStatusMessages(messages);
-            for (const auto& status_message : messages) {
+        while (true) {
+            clock->sleep_for(0.1f);
+            controller->getStatusMessages(messages_);
+            for (const auto& status_message : messages_) {
                 std::cout << status_message << std::endl;
             }
-            messages.clear();
+            messages_.clear();
 
             std::cout << VectorMath::toString(vehicle.getKinematics().pose.position) << std::endl;
         }
     }
 
+private:
+    std::vector<std::string> messages_;
+
 private:
     class DirectCancelableBase : public CancelableBase
     {
     public:
-        virtual void execute() override {};
+        DirectCancelableBase(DroneControllerBase* controller, const MultiRotor* vehicle)
+            : controller_(controller), vehicle_(vehicle)
+        {
+        }
+        virtual void execute() override
+        {}
+
+        virtual bool sleep(double secs) override 
+        {
+            controller_->getStatusMessages(messages_);
+            for (const auto& status_message : messages_) {
+                std::cout << status_message << std::endl;
+            }
+            messages_.clear();
+
+            std::cout << VectorMath::toString(vehicle_->getKinematics().pose.position) << std::endl;
+
+            return CancelableBase::sleep(secs);
+        };
+
+    private:
+        DroneControllerBase* controller_;
+        const MultiRotor* vehicle_;
+        std::vector<std::string> messages_;
     };
 
 };

Unreal/Plugins/AirSim/Source/SimMode/SimModeWorldBase.cpp

@@ -103,12 +103,17 @@ size_t ASimModeWorldBase::getVehicleCount() const
 
 void ASimModeWorldBase::Tick(float DeltaSeconds)
 {
-    physics_world_->lock();
+    { //keep this lock as short as possible
+        physics_world_->lock();
 
-    for (auto& vehicle : vehicles_)
-        vehicle->updateRenderedState();
+        physics_world_->enableStateReport(EnableReport);
+        physics_world_->updateStateReport();
+
+        for (auto& vehicle : vehicles_)
+            vehicle->updateRenderedState();
 
-    physics_world_->unlock();
+        physics_world_->unlock();
+    }
 
     //perfom any expensive rendering update outside of lock region
     for (auto& vehicle : vehicles_)