Drone controller implementation for simple_flight part 1

AirLib/AirLib.vcxproj

@@ -83,10 +83,11 @@
     <ClInclude Include="include\firmwares\simple_flight\AirSimSimpleFlightBoard.hpp" />
     <ClInclude Include="include\firmwares\simple_flight\AirSimSimpleFlightCommLink.hpp" />
     <ClInclude Include="include\firmwares\simple_flight\AirSimSimpleFlightEstimator.hpp" />
+    <ClInclude Include="include\firmwares\simple_flight\AirSimSimpleFlightCommon.hpp" />
     <ClInclude Include="include\firmwares\simple_flight\firmware\AngleLevelController.hpp" />
     <ClInclude Include="include\firmwares\simple_flight\firmware\AngleRateController.hpp" />
     <ClInclude Include="include\firmwares\simple_flight\firmware\CascadeController.hpp" />
-    <ClInclude Include="include\firmwares\simple_flight\firmware\CombinedGoalInput.hpp" />
+    <ClInclude Include="include\firmwares\simple_flight\firmware\OffboardApi.hpp" />
     <ClInclude Include="include\firmwares\simple_flight\firmware\Firmware.hpp" />
     <ClInclude Include="include\firmwares\simple_flight\firmware\interfaces\CommonStructs.hpp" />
     <ClInclude Include="include\firmwares\simple_flight\firmware\interfaces\IAxisController.hpp" />
@@ -99,13 +100,15 @@
     <ClInclude Include="include\firmwares\simple_flight\firmware\interfaces\IController.hpp" />
     <ClInclude Include="include\firmwares\simple_flight\firmware\interfaces\IFirmware.hpp" />
     <ClInclude Include="include\firmwares\simple_flight\firmware\interfaces\IGoal.hpp" />
-    <ClInclude Include="include\firmwares\simple_flight\firmware\interfaces\IGoalInput.hpp" />
+    <ClInclude Include="include\firmwares\simple_flight\firmware\interfaces\IOffboardApi.hpp" />
     <ClInclude Include="include\firmwares\simple_flight\firmware\interfaces\IStateEstimator.hpp" />
     <ClInclude Include="include\firmwares\simple_flight\firmware\interfaces\IUpdatable.hpp" />
     <ClInclude Include="include\firmwares\simple_flight\firmware\Mixer.hpp" />
     <ClInclude Include="include\firmwares\simple_flight\firmware\Params.hpp" />
     <ClInclude Include="include\firmwares\simple_flight\firmware\PidController.hpp" />
+    <ClInclude Include="include\firmwares\simple_flight\firmware\PositionController.hpp" />
     <ClInclude Include="include\firmwares\simple_flight\firmware\RemoteControl.hpp" />
+    <ClInclude Include="include\firmwares\simple_flight\firmware\VelocityController.hpp" />
     <ClInclude Include="include\firmwares\simple_flight\SimpleFlightDroneController.hpp" />
     <ClInclude Include="include\physics\DebugPhysicsBody.hpp" />
     <ClInclude Include="include\api\ControlServerBase.hpp" />

AirLib/AirLib.vcxproj.filters

@@ -387,9 +387,6 @@
     <ClInclude Include="include\firmwares\simple_flight\firmware\CascadeController.hpp">
       <Filter>Header Files</Filter>
     </ClInclude>
-    <ClInclude Include="include\firmwares\simple_flight\firmware\CombinedGoalInput.hpp">
-      <Filter>Header Files</Filter>
-    </ClInclude>
     <ClInclude Include="include\firmwares\simple_flight\firmware\Firmware.hpp">
       <Filter>Header Files</Filter>
     </ClInclude>
@@ -423,7 +420,19 @@
     <ClInclude Include="include\firmwares\simple_flight\firmware\interfaces\IGoal.hpp">
       <Filter>Header Files</Filter>
     </ClInclude>
-    <ClInclude Include="include\firmwares\simple_flight\firmware\interfaces\IGoalInput.hpp">
+    <ClInclude Include="include\firmwares\simple_flight\firmware\PositionController.hpp">
+      <Filter>Header Files</Filter>
+    </ClInclude>
+    <ClInclude Include="include\firmwares\simple_flight\firmware\VelocityController.hpp">
+      <Filter>Header Files</Filter>
+    </ClInclude>
+    <ClInclude Include="include\firmwares\simple_flight\AirSimSimpleFlightCommon.hpp">
+      <Filter>Header Files</Filter>
+    </ClInclude>
+    <ClInclude Include="include\firmwares\simple_flight\firmware\interfaces\IOffboardApi.hpp">
+      <Filter>Header Files</Filter>
+    </ClInclude>
+    <ClInclude Include="include\firmwares\simple_flight\firmware\OffboardApi.hpp">
       <Filter>Header Files</Filter>
     </ClInclude>
   </ItemGroup>

AirLib/include/firmwares/simple_flight/AirSimSimpleFlightCommon.hpp

@@ -0,0 +1,50 @@
+// Copyright (c) Microsoft Corporation. All rights reserved.
+// Licensed under the MIT License.
+
+#ifndef msr_airlib_AirSimSimpleFlightCommon_hpp
+#define msr_airlib_AirSimSimpleFlightCommon_hpp
+
+#include "physics/Kinematics.hpp"
+#include "common/Common.hpp"
+
+namespace msr { namespace airlib {
+
+
+class AirSimSimpleFlightCommon {
+public:
+    static simple_flight::Axis3r vector3rToAxis3r(const Vector3r& vec)
+    {
+        simple_flight::Axis3r conv;
+        conv.x() = vec.x(); conv.y() = vec.y(); conv.z() = vec.z();
+
+        return conv;
+    }
+
+    static Vector3r axis3rToVector3r(const simple_flight::Axis3r& vec)
+    {
+        Vector3r conv;
+        conv.x() = vec.x(); conv.y() = vec.y(); conv.z() = vec.z();
+        return conv;
+    }
+
+    static simple_flight::Axis4r quaternion3rToAxis4r(const Quaternionr& q)
+    {
+        simple_flight::Axis4r conv;
+        conv.axis3.x() = q.x(); conv.axis3.y() = q.y(); conv.axis3.z() = q.z();
+        conv.val4 = q.w();
+
+        return conv;
+    }
+
+    static Quaternionr axis4rToQuaternionr(const simple_flight::Axis4r& q)
+    {
+        Quaternionr conv;
+        conv.x() = q.axis3.x(); conv.y() = q.axis3.y(); conv.z() = q.axis3.z();
+        conv.w() = q.val4;
+        return conv;
+    }
+};
+
+
+}} //namespace
+#endif

AirLib/include/firmwares/simple_flight/AirSimSimpleFlightEstimator.hpp

@@ -5,7 +5,9 @@
 #define msr_airlib_AirSimSimpleFlightEstimator_hpp
 
 #include "firmware/interfaces/CommonStructs.hpp"
+#include "AirSimSimpleFlightCommon.hpp"
 #include "physics/Kinematics.hpp"
+#include "physics/Environment.hpp"
 #include "common/Common.hpp"
 
 namespace msr { namespace airlib {
@@ -14,9 +16,10 @@ namespace msr { namespace airlib {
 class AirSimSimpleFlightEstimator : public simple_flight::IStateEstimator {
 public:
     //for now we don't do any state estimation and use ground truth (i.e. assume perfect sensors)
-    void setKinematics(const Kinematics::State* kinematics)
+    void setKinematics(const Kinematics::State* kinematics, const Environment* environment)
     {
         kinematics_ = kinematics;
+        environment_ = environment;
     }
 
     virtual simple_flight::Axis3r getAngles() const override
@@ -34,15 +37,48 @@ class AirSimSimpleFlightEstimator : public simple_flight::IStateEstimator {
     {
         const auto& anguler = kinematics_->twist.angular;
 
-        simple_flight::Axis3r angular_vel;
-        angular_vel.x() = anguler.x(); angular_vel.y() = anguler.y(); angular_vel.z() = anguler.z();
+        simple_flight::Axis3r conv;
+        conv.x() = anguler.x(); conv.y() = anguler.y(); conv.z() = anguler.z();
 
-        return angular_vel;
+        return conv;
     }
 
+    virtual simple_flight::Axis3r getPosition() const
+    {
+        return AirSimSimpleFlightCommon::vector3rToAxis3r(kinematics_->pose.position);
+    }
+
+    virtual simple_flight::Axis3r transformToBodyFrame(const simple_flight::Axis3r& world_frame_val) const
+    {
+        const Vector3r& vec = AirSimSimpleFlightCommon::axis3rToVector3r(world_frame_val);
+        const Vector3r& trans = VectorMath::transformToBodyFrame(vec, kinematics_->pose.orientation);
+        return AirSimSimpleFlightCommon::vector3rToAxis3r(trans);
+    }
+
+    virtual simple_flight::Axis3r getLinearVelocity() const
+    {
+        return AirSimSimpleFlightCommon::vector3rToAxis3r(kinematics_->twist.linear);
+    }
+
+    virtual simple_flight::Axis4r getOrientation() const
+    {
+        return AirSimSimpleFlightCommon::quaternion3rToAxis4r(kinematics_->pose.orientation);
+    }
+
+    virtual simple_flight::GeoPoint getGeoPoint() const
+    {
+        const auto& geo_point = environment_->getState().geo_point;
+        simple_flight::GeoPoint conv;
+        conv.latitude = geo_point.latitude;
+        conv.longitude = geo_point.longitude;
+        conv.altiude = geo_point.altitude;
+
+        return conv;
+    }
 
 private:
     const Kinematics::State* kinematics_;
+    const Environment* environment_;
 };
 
 

AirLib/include/firmwares/simple_flight/SimpleFlightDroneController.hpp

@@ -15,6 +15,7 @@
 #include "AirSimSimpleFlightBoard.hpp"
 #include "AirSimSimpleFlightCommLink.hpp"
 #include "AirSimSimpleFlightEstimator.hpp"
+#include "AirSimSimpleFlightCommon.hpp"
 
 
 namespace msr { namespace airlib {
@@ -44,7 +45,7 @@ class SimpleFlightDroneController : public DroneControllerBase {
         physics_body_ = physics_body;
 
         board_->setKinematics(& physics_body_->getKinematics());
-        estimator_->setKinematics(& physics_body_->getKinematics());
+        estimator_->setKinematics(& physics_body_->getKinematics(), & physics_body_->getEnvironment());
     }
 
 public:
@@ -91,19 +92,24 @@ class SimpleFlightDroneController : public DroneControllerBase {
 
     virtual bool isOffboardMode() override
     {
-        //TODO: support offboard mode
-        return false;
+        return firmware_->offboardApi().hasApiControl();
     }
 
     virtual bool isSimulationMode() override
     {
+        //TODO: after we get real board implementation, change this
         return true;
     }
 
     virtual void setOffboardMode(bool is_set) override
     {
-        unused(is_set);
-        //TODO: implement this
+        if (is_set) {
+            //comm_link should print message so no extra handling for errors
+            std::string message;
+            firmware_->offboardApi().requestApiControl(message);
+        }
+        else
+            firmware_->offboardApi().releaseApiControl();
     }
 
     virtual void setSimulationMode(bool is_set) override
@@ -117,17 +123,20 @@ class SimpleFlightDroneController : public DroneControllerBase {
 public:
     Vector3r getPosition() override
     {
-        return physics_body_->getKinematics().pose.position;
+        const auto& val = firmware_->offboardApi().getStateEstimator().getPosition();
+        return AirSimSimpleFlightCommon::axis3rToVector3r(val);
     }
 
     Vector3r getVelocity() override
     {
-        return physics_body_->getKinematics().twist.linear;
+        const auto& val = firmware_->offboardApi().getStateEstimator().getLinearVelocity();
+        return AirSimSimpleFlightCommon::axis3rToVector3r(val);
     }
 
     Quaternionr getOrientation() override
     {
-        return physics_body_->getKinematics().pose.orientation;
+        const auto& val = firmware_->offboardApi().getStateEstimator().getOrientation();
+        return AirSimSimpleFlightCommon::axis4rToQuaternionr(val);    
     }
 
     LandedState getLandedState() override
@@ -167,9 +176,13 @@ class SimpleFlightDroneController : public DroneControllerBase {
 
     bool armDisarm(bool arm, CancelableBase& cancelable_action) override
     {
-        unused(arm);
         unused(cancelable_action);
-        return true;
+
+        std::string message;
+        if (arm)
+            return firmware_->offboardApi().arm(message);
+        else
+            return firmware_->offboardApi().disarm(message);
     }
 
     bool takeoff(float max_wait_seconds, CancelableBase& cancelable_action) override

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

@@ -63,7 +63,7 @@ class AngleLevelController :
         pid_->update();
 
         //use this to drive rate controller
-        rate_goal_.throttle = level_goal.throttle;
+        rate_goal_.throttle() = level_goal.throttle();
         rate_goal_.axis3[axis_] = pid_->getOutput() * params_->angle_rate_pid.max_limit[axis_];
         rate_controller_->update();
 

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

@@ -8,6 +8,10 @@
 #include "interfaces/IGoal.hpp"
 #include "AngleRateController.hpp"
 #include "AngleLevelController.hpp"
+#include "VelocityController.hpp"
+#include "PositionController.hpp"
+
+
 
 namespace simple_flight {
 
@@ -45,7 +49,7 @@ class CascadeController : public IController {
         const auto& goal_mode = goal_->getGoalMode();
 
         //for now we set throttle to same as goal
-        output_.throttle = goal_->getGoalValue().throttle;
+        output_.throttle() = goal_->getGoalValue().throttle();
 
         for (unsigned int axis = 0; axis < 3; ++axis) {
             //re-create axis controllers if goal mode was changed since last time
@@ -57,6 +61,12 @@ class CascadeController : public IController {
                 case GoalModeType::AngleLevel:
                     axis_controllers_[axis].reset(new AngleLevelController(params_, clock_));
                     break;
+                case GoalModeType::VelocityWorld:
+                    axis_controllers_[axis].reset(new VelocityController(params_, clock_));
+                    break;
+                case GoalModeType::PositionWorld:
+                    axis_controllers_[axis].reset(new PositionController(params_, clock_));
+                    break;
                 default:
                     throw std::invalid_argument("Axis controller type is not yet implemented for axis " 
                         + std::to_string(axis));