simple_flight controller code complete

AirLib/AirLib.vcxproj

@@ -87,6 +87,7 @@
     <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\ConstantOutputController.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" />
@@ -105,6 +106,7 @@
     <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\PassthroughController.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" />

AirLib/AirLib.vcxproj.filters

@@ -435,6 +435,12 @@
     <ClInclude Include="include\firmwares\simple_flight\firmware\OffboardApi.hpp">
       <Filter>Header Files</Filter>
     </ClInclude>
+    <ClInclude Include="include\firmwares\simple_flight\firmware\PassthroughController.hpp">
+      <Filter>Header Files</Filter>
+    </ClInclude>
+    <ClInclude Include="include\firmwares\simple_flight\firmware\ConstantOutputController.hpp">
+      <Filter>Header Files</Filter>
+    </ClInclude>
   </ItemGroup>
   <ItemGroup>
     <ClCompile Include="src\safety\ObstacleMap.cpp">

AirLib/include/controllers/DroneControllerBase.hpp

@@ -99,7 +99,7 @@ class DroneControllerBase : public VehicleControllerBase {
     /// above the home position.  Once the drone is safely in the air you can use other commands to fly from there.
     /// If the drone is already flying takeoff will be ignored.  Pass non-zer max_wait_seconds if you want the
     /// method to also wait until the takeoff altitude is achieved.
-    virtual bool takeoff(float max_wait_seconds, CancelableBase& cancelable_action) = 0;
+    virtual bool takeoff(float max_wait_seconds, CancelableBase& cancelable_action);
 
     /// At any point this command will disable offboard control and land the drone at the current GPS location.
     /// How quickly the drone descends is up to the drone.  Some models will descend slowly if they have no 
@@ -109,12 +109,12 @@ class DroneControllerBase : public VehicleControllerBase {
     /// method to also wait until the drone reports it has landed, the timeout here is a bit tricky, depends
     /// on how high you are and what the drone's configured descent velocity is.  If you don't want to wait
     /// pass zero.  You can also periodically check getLandedState to see if it has landed.
-    virtual bool land(float max_wait_seconds, CancelableBase& cancelable_action) = 0;
+    virtual bool land(float max_wait_seconds, CancelableBase& cancelable_action);
 
     /// This command is a safety measure, at any point this command will cancel offboard control and send the
     /// drone back to the launch point (or home position).  Most drones are also configured to climb to a safe
     /// altitude before doing that so they don't run into a tree on the way home.
-    virtual bool goHome(CancelableBase& cancelable_action) = 0;
+    virtual bool goHome(CancelableBase& cancelable_action);
 
     /// Move the drone by controlling the angles (or attitude) of the drone, if you set pitch, roll to zero
     /// and z to the current z value then it is equivalent to a hover command.  A little bit of pitch can

AirLib/include/firmwares/simple_flight/AirSimSimpleFlightCommon.hpp

@@ -30,17 +30,17 @@ class AirSimSimpleFlightCommon {
     static simple_flight::Axis4r toAxis4r(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();
+        conv.x() = q.x(); conv.y() = q.y(); conv.z() = q.z();
+        conv.val4() = q.w();
 
         return conv;
     }
 
     static Quaternionr toQuaternion(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;
+        conv.x() = q.x(); conv.y() = q.y(); conv.z() = q.z();
+        conv.w() = q.val4();
         return conv;
     }
 
@@ -63,6 +63,15 @@ class AirSimSimpleFlightCommon {
 
         return conv;
     }
+
+    template<typename T> const T& makeConstant(T& _) 
+    { 
+        return const_cast<const T&>(_); 
+    } 
+    template<typename T> T& makeVariable(const T& _) 
+    { 
+        return const_cast<T&>(_); 
+    }
 };
 
 

AirLib/include/firmwares/simple_flight/SimpleFlightDroneController.hpp

@@ -185,32 +185,6 @@ class SimpleFlightDroneController : public DroneControllerBase {
             return firmware_->offboardApi().disarm(message);
     }
 
-    bool takeoff(float max_wait_seconds, CancelableBase& cancelable_action) override
-    {
-        unused(max_wait_seconds);
-        unused(cancelable_action);
-        return true;
-    }
-
-    bool land(float max_wait_seconds, CancelableBase& cancelable_action) override
-    {
-        unused(max_wait_seconds);
-        unused(cancelable_action);
-        return true;
-    }
-
-    bool goHome(CancelableBase& cancelable_action) override
-    {
-        unused(cancelable_action);
-        return true;
-    }
-
-    bool hover(CancelableBase& cancelable_action) override
-    {
-        unused(cancelable_action);
-        return true;
-    }
-
     GeoPoint getHomeGeoPoint() override
     {
         return AirSimSimpleFlightCommon::toGeoPoint(firmware_->offboardApi().getHomeGeoPoint());
@@ -247,42 +221,52 @@ class SimpleFlightDroneController : public DroneControllerBase {
 protected: 
     void commandRollPitchZ(float pitch, float roll, float z, float yaw) override
     {
-        unused(pitch);
-        unused(roll);
-        unused(z);
-        unused(yaw);
+        typedef simple_flight::GoalModeType GoalModeType;
+        static simple_flight::GoalMode mode(GoalModeType::AngleLevel, GoalModeType::AngleLevel, GoalModeType::AngleLevel, GoalModeType::PositionWorld);
 
-        //TODO: implement this
+        simple_flight::Axis4r goal(roll, pitch, yaw, z);
+
+        std::string message;
+        firmware_->offboardApi().setGoalAndMode(&goal, &mode, message);
     }
 
     void commandVelocity(float vx, float vy, float vz, const YawMode& yaw_mode) override
     {
-        unused(vx);
-        unused(vy);
-        unused(vz);
-        unused(yaw_mode);
+        typedef simple_flight::GoalModeType GoalModeType;
+        static simple_flight::GoalMode mode(GoalModeType::VelocityWorld, GoalModeType::VelocityWorld, 
+            yaw_mode.is_rate ? GoalModeType::AngleRate : GoalModeType::AngleLevel, 
+            GoalModeType::VelocityWorld);
 
-        //TODO: implement this
+        simple_flight::Axis4r goal(vx, vy, yaw_mode.yaw_or_rate, vz);
+
+        std::string message;
+        firmware_->offboardApi().setGoalAndMode(&goal, &mode, message);
     }
 
     void commandVelocityZ(float vx, float vy, float z, const YawMode& yaw_mode) override
     {
-        unused(vx);
-        unused(vy);
-        unused(z);
-        unused(yaw_mode);
+        typedef simple_flight::GoalModeType GoalModeType;
+        static simple_flight::GoalMode mode(GoalModeType::VelocityWorld, GoalModeType::VelocityWorld, 
+            yaw_mode.is_rate ? GoalModeType::AngleRate : GoalModeType::AngleLevel, 
+            GoalModeType::PositionWorld);
 
-        //TODO: implement this
+        simple_flight::Axis4r goal(vx, vy, yaw_mode.yaw_or_rate, z);
+
+        std::string message;
+        firmware_->offboardApi().setGoalAndMode(&goal, &mode, message);
     }
 
     void commandPosition(float x, float y, float z, const YawMode& yaw_mode) override
     {
-        unused(x);
-        unused(y);
-        unused(z);
-        unused(yaw_mode);
+        typedef simple_flight::GoalModeType GoalModeType;
+        static simple_flight::GoalMode mode(GoalModeType::PositionWorld, GoalModeType::PositionWorld, 
+            yaw_mode.is_rate ? GoalModeType::AngleRate : GoalModeType::AngleLevel, 
+            GoalModeType::PositionWorld);
 
-        //TODO: implement this
+        simple_flight::Axis4r goal(x, y, yaw_mode.yaw_or_rate, z);
+
+        std::string message;
+        firmware_->offboardApi().setGoalAndMode(&goal, &mode, message);
     }
 
     const VehicleParams& getVehicleParams() override

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

@@ -10,6 +10,9 @@
 #include "Params.hpp"
 #include "PidController.hpp"
 #include "common/common_utils/Utils.hpp"
+#include <string>
+#include <exception>
+
 
 namespace simple_flight {
 
@@ -25,6 +28,9 @@ class AngleLevelController :
 
     virtual void initialize(unsigned int axis, const IGoal* goal, const IStateEstimator* state_estimator) override
     {
+        if (axis > 2)
+            throw std::invalid_argument("AngleLevelController only supports axis 0-2 but it was " + std::to_string(axis));
+
         axis_ = axis;
         goal_ = goal;
         state_estimator_ = state_estimator;
@@ -58,13 +64,12 @@ class AngleLevelController :
 
         //get response of level PID
         const auto& level_goal = goal_->getGoalValue();
-        pid_->setGoal(level_goal.axis3[axis_]);
+        pid_->setGoal(level_goal[axis_]);
         pid_->setMeasured(state_estimator_->getAngles()[axis_]);
         pid_->update();
 
         //use this to drive rate controller
-        rate_goal_.throttle() = level_goal.throttle();
-        rate_goal_.axis3[axis_] = pid_->getOutput() * params_->angle_rate_pid.max_limit[axis_];
+        rate_goal_[axis_] = pid_->getOutput() * params_->angle_rate_pid.max_limit[axis_];
         rate_controller_->update();
 
         //rate controller's output is final output

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

@@ -8,6 +8,9 @@
 #include "PidController.hpp"
 #include "common/common_utils/Utils.hpp"
 #include <memory>
+#include <string>
+#include <exception>
+
 
 namespace simple_flight {
 
@@ -20,6 +23,9 @@ class AngleRateController : public IAxisController {
 
     virtual void initialize(unsigned int axis, const IGoal* goal, const IStateEstimator* state_estimator) override
     {
+        if (axis > 2)
+            throw std::invalid_argument("AngleRateController only supports axis 0-2 but it was " + std::to_string(axis));
+
         axis_ = axis;
         goal_ = goal;
         state_estimator_ = state_estimator;
@@ -40,7 +46,7 @@ class AngleRateController : public IAxisController {
     {
         IAxisController::update();
 
-        pid_->setGoal(goal_->getGoalValue().axis3[axis_]);
+        pid_->setGoal(goal_->getGoalValue()[axis_]);
         pid_->setMeasured(state_estimator_->getAngulerVelocity()[axis_]);
         pid_->update();
 

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

@@ -8,6 +8,8 @@
 #include "interfaces/IGoal.hpp"
 #include "AngleRateController.hpp"
 #include "AngleLevelController.hpp"
+#include "PassthroughController.hpp"
+#include "ConstantOutputController.hpp"
 #include "VelocityController.hpp"
 #include "PositionController.hpp"
 
@@ -35,7 +37,7 @@ class CascadeController : public IController {
         last_goal_mode_ = GoalMode::getUnknown();
         output_ = Axis4r();
 
-        for (unsigned int axis = 0; axis < 3; ++axis) {
+        for (unsigned int axis = 0; axis < Axis4r::AxisCount(); ++axis) {
             if (axis_controllers_[axis] != nullptr)
                 axis_controllers_[axis]->reset();
         }
@@ -48,10 +50,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();
-
-        for (unsigned int axis = 0; axis < 3; ++axis) {
+        for (unsigned int axis = 0; axis < Axis4r::AxisCount(); ++axis) {
             //re-create axis controllers if goal mode was changed since last time
             if (goal_mode[axis] != last_goal_mode_[axis]) {
                 switch (goal_mode[axis]) {
@@ -67,6 +66,12 @@ class CascadeController : public IController {
                 case GoalModeType::PositionWorld:
                     axis_controllers_[axis].reset(new PositionController(params_, clock_));
                     break;
+                case GoalModeType::Passthrough:
+                    axis_controllers_[axis].reset(new PassthroughController());
+                    break;
+                case GoalModeType::ConstantOutput:
+                    axis_controllers_[axis].reset(new ConstantOutputController());
+                    break;
                 default:
                     throw std::invalid_argument("Axis controller type is not yet implemented for axis " 
                         + std::to_string(axis));
@@ -80,7 +85,7 @@ class CascadeController : public IController {
             //update axis controller
             if (axis_controllers_[axis] != nullptr) {
                 axis_controllers_[axis]->update();
-                output_.axis3[axis] = axis_controllers_[axis]->getOutput();
+                output_[axis] = axis_controllers_[axis]->getOutput();
             }
             else
                 comm_link_->log(std::string("Axis controller type is not set for axis ").append(std::to_string(axis)), ICommLink::kLogLevelError);
@@ -105,7 +110,7 @@ class CascadeController : public IController {
 
     GoalMode last_goal_mode_;
 
-    std::unique_ptr<IAxisController> axis_controllers_[kAxisCount];
+    std::unique_ptr<IAxisController> axis_controllers_[Axis4r::AxisCount()];
 };
 
 }

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

@@ -0,0 +1,52 @@
+
+#pragma once
+
+#include "interfaces/CommonStructs.hpp"
+#include "interfaces/IBoardClock.hpp"
+#include "interfaces/IAxisController.hpp"
+#include "common/common_utils//Utils.hpp"
+#include "Params.hpp"
+#include <memory>
+
+namespace simple_flight {
+
+class ConstantOutputController : public IAxisController {
+public:
+    ConstantOutputController(TReal update_output = TReal(), TReal reset_output = TReal())
+        : update_output_(update_output), reset_output_(reset_output)
+    {
+    }
+
+    virtual void initialize(unsigned int axis, const IGoal* goal, const IStateEstimator* state_estimator) override
+    {
+        axis_ = axis;
+        unused(goal);
+        unused(state_estimator);
+    }
+
+    virtual void reset() override
+    {
+        IAxisController::reset();
+        output_ = reset_output_;
+    }
+
+    virtual void update() override
+    {
+        IAxisController::update();
+        output_ = update_output_;
+    }
+
+    virtual TReal getOutput() override
+    {
+        return output_;
+    }
+
+private:
+    unsigned int axis_;
+    TReal update_output_;
+    TReal reset_output_;
+    TReal output_;
+};
+
+
+} //namespace

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

@@ -24,9 +24,9 @@ class Mixer {
         for (int motor_index = 0; motor_index < kMotorCount; ++motor_index) {
             motor_outputs[motor_index] =
                 controls.throttle() * mixerQuadX[motor_index].throttle
-                + controls.axis3.pitch() * mixerQuadX[motor_index].pitch
-                + controls.axis3.roll() * mixerQuadX[motor_index].roll
-                + controls.axis3.yaw() * mixerQuadX[motor_index].yaw
+                + controls.pitch() * mixerQuadX[motor_index].pitch
+                + controls.roll() * mixerQuadX[motor_index].roll
+                + controls.yaw() * mixerQuadX[motor_index].yaw
                 ;
         }