RC disconnected state handling

SleepyDummy · Aug 12, 2017 · f929195 · f929195
1 parent d484d50
commit f929195
Show file tree

Hide file tree

Showing 6 changed files with 43 additions and 9 deletions.
diff --git a/AirLib/include/firmwares/simple_flight/AirSimSimpleFlightBoard.hpp b/AirLib/include/firmwares/simple_flight/AirSimSimpleFlightBoard.hpp
@@ -42,6 +42,11 @@ class AirSimSimpleFlightBoard : public simple_flight::IBoard {
         input_channels_[index] = static_cast<float>(val);
     }
 
+    void setIsRcConnected(bool is_connected)
+    {
+        is_connected_ = is_connected;
+    }
+
 public:
     //Board interface implementation --------------------------------------------------------------------------
 
@@ -60,6 +65,11 @@ class AirSimSimpleFlightBoard : public simple_flight::IBoard {
         return input_channels_[index];
     }
 
+    virtual bool isRcConnected() const override
+    {
+        return is_connected_;
+    }
+
     virtual void writeOutput(uint16_t index, float value) override 
     {
         motor_output_[index] = value;
@@ -94,6 +104,7 @@ class AirSimSimpleFlightBoard : public simple_flight::IBoard {
 
         motor_output_.assign(params_->motor.motor_count, 0);
         input_channels_.assign(params_->rc.channel_count, 0);
+        is_connected_ = false;
     }
 
     virtual void update() override
@@ -123,6 +134,7 @@ class AirSimSimpleFlightBoard : public simple_flight::IBoard {
     //motor outputs
     std::vector<float> motor_output_;
     std::vector<float> input_channels_;
+    bool is_connected_;
 
     const simple_flight::Params* params_;
     const Kinematics::State* kinematics_;

diff --git a/AirLib/include/firmwares/simple_flight/SimpleFlightDroneController.hpp b/AirLib/include/firmwares/simple_flight/SimpleFlightDroneController.hpp
@@ -154,6 +154,7 @@ class SimpleFlightDroneController : public DroneControllerBase {
     void setRCData(const RCData& rcData) override
     {
         if (rcData.is_connected) {
+            board_->setIsRcConnected(true);
             board_->setInputChannel(0, rcData.roll); //X
             board_->setInputChannel(1, rcData.yaw); //Y
             board_->setInputChannel(2, rcData.throttle); //F
@@ -167,7 +168,9 @@ class SimpleFlightDroneController : public DroneControllerBase {
             board_->setInputChannel(10, static_cast<float>(rcData.switch7)); 
             board_->setInputChannel(11, static_cast<float>(rcData.switch8)); 
         }
-        //else we don't have RC data
+        else { //else we don't have RC data
+            board_->setIsRcConnected(false);
+        }
     }
 
     bool armDisarm(bool arm, CancelableBase& cancelable_action) override

diff --git a/AirLib/include/firmwares/simple_flight/firmware/CascadeController.hpp b/AirLib/include/firmwares/simple_flight/firmware/CascadeController.hpp
@@ -69,6 +69,9 @@ class CascadeController : public IController {
                 case GoalModeType::Passthrough:
                     axis_controllers_[axis].reset(new PassthroughController());
                     break;
+                case GoalModeType::Unknown:
+                    axis_controllers_[axis].reset(nullptr);
+                    break;
                 case GoalModeType::ConstantOutput:
                     axis_controllers_[axis].reset(new ConstantOutputController());
                     break;
@@ -79,8 +82,10 @@ class CascadeController : public IController {
                 last_goal_mode_[axis] = goal_mode[axis];
 
                 //initialize axis controller
-                axis_controllers_[axis]->initialize(axis, goal_, state_estimator_);
-                axis_controllers_[axis]->reset();
+                if (axis_controllers_[axis] != nullptr) {
+                    axis_controllers_[axis]->initialize(axis, goal_, state_estimator_);
+                    axis_controllers_[axis]->reset();
+                }
             }
 
             //update axis controller
@@ -89,7 +94,7 @@ class CascadeController : public IController {
                 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);
+                comm_link_->log(std::string("Axis controller type is not set for axis ").append(std::to_string(axis)), ICommLink::kLogLevelInfo);
         }
     }
 

diff --git a/AirLib/include/firmwares/simple_flight/firmware/Params.hpp b/AirLib/include/firmwares/simple_flight/firmware/Params.hpp
@@ -42,6 +42,8 @@ struct Params {
 
         //should be >= motor.min_angling_throttle
         float min_angling_throttle = Params::min_armed_throttle() / 1.5f;
+
+        bool allow_api_when_disconnected = true;
     } rc;
 
     struct AngleRatePid {
@@ -88,7 +90,6 @@ struct Params {
     } velocity_pid;
 
     GoalMode default_goal_mode = GoalMode::getStandardAngleMode();
-    bool default_allow_api_control = true;
     VehicleStateType default_vehicle_state = VehicleStateType::Inactive;
 };
 

diff --git a/AirLib/include/firmwares/simple_flight/firmware/RemoteControl.hpp b/AirLib/include/firmwares/simple_flight/firmware/RemoteControl.hpp
@@ -26,7 +26,7 @@ class RemoteControl :
 
         goal_ = Axis4r::zero();
         goal_mode_ = params_->default_goal_mode;
-        allow_api_control_ = params_->default_allow_api_control;
+        allow_api_control_ = false;
         last_rec_read_ = 0;
         last_angle_mode_ = std::numeric_limits<TReal>::min();
         request_duration_ = 0;
@@ -46,8 +46,11 @@ class RemoteControl :
 
         //read channel values
         Axis4r channels;
-        for (unsigned int axis = 0; axis < Axis4r::AxisCount(); ++axis)
-            channels[axis] = board_inputs_->readChannel(params_->rc.channels[axis]);
+        for (unsigned int axis = 0; axis < Axis4r::AxisCount(); ++axis) {
+            channels[axis] = board_inputs_->isRcConnected() ?
+                board_inputs_->readChannel(params_->rc.channels[axis])
+                : 0;
+        }
 
         //set goal mode as per the switch position on RC
         updateGoalMode();
@@ -151,6 +154,13 @@ class RemoteControl :
 
     void updateGoalMode()
     {
+        if (!board_inputs_->isRcConnected()) {
+            if (!goal_mode_.equals4(GoalMode::getUnknown()))
+                goal_mode_ = GoalMode::getUnknown();
+
+            return;
+        }
+
         //set up RC mode as level or rate
         angle_mode_ = board_inputs_->readChannel(params_->rc.rate_level_mode_channel);
         if (last_angle_mode_ != angle_mode_) {
@@ -166,7 +176,9 @@ class RemoteControl :
 
     void updateAllowApiControl()
     {
-        bool allow = board_inputs_->readChannel(params_->rc.allow_api_control_channel) > 0.1f;
+        bool allow = board_inputs_->isRcConnected() ?
+            board_inputs_->readChannel(params_->rc.allow_api_control_channel) > 0.1f
+            : params_->rc.allow_api_when_disconnected;
 
         if (allow_api_control_ != allow)
             comm_link_->log(std::string("API control enabled:\t").append(std::to_string(allow_api_control_)));

diff --git a/AirLib/include/firmwares/simple_flight/firmware/interfaces/IBoardInputPins.hpp b/AirLib/include/firmwares/simple_flight/firmware/interfaces/IBoardInputPins.hpp
@@ -7,6 +7,7 @@ namespace simple_flight {
 class IBoardInputPins {
 public:
     virtual float readChannel(uint16_t index) const = 0; //output -1 to 1
+    virtual bool isRcConnected() const = 0;
 };
 
 }