change reset() model for simple_flight

Kjell-K · Jul 31, 2017 · 0397979 · 0397979
1 parent 3da637b
commit 0397979
Show file tree

Hide file tree

Showing 5 changed files with 10 additions and 9 deletions.
diff --git a/AirLib/include/controllers/simple_flight/firmware/AngleLevelController.hpp b/AirLib/include/controllers/simple_flight/firmware/AngleLevelController.hpp
@@ -40,8 +40,6 @@ class AngleLevelController :
         //we will be setting goal for rate controller so we need these two things
         rate_mode_  = GoalMode::getUnknown();
         rate_mode_[axis] = GoalModeType::AngleRate;
-
-        AngleLevelController::reset();
     }
 
     virtual void reset() override

diff --git a/AirLib/include/controllers/simple_flight/firmware/AngleRateController.hpp b/AirLib/include/controllers/simple_flight/firmware/AngleRateController.hpp
@@ -26,8 +26,6 @@ class AngleRateController : public IAxisController {
 
         pid_.reset(new PidController<float>(clock_,
             PidController<float>::Config(params_->pid_p_angle_rate[axis], 0, 0)));
-
-        AngleRateController::reset();
     }
 
     virtual void reset() override

diff --git a/AirLib/include/controllers/simple_flight/firmware/CascadeController.hpp b/AirLib/include/controllers/simple_flight/firmware/CascadeController.hpp
@@ -23,8 +23,6 @@ class CascadeController : public IController {
         goal_input_ = goal_input;
         state_estimator_ = state_estimator;
         last_goal_mode_ = GoalMode::getUnknown();
-
-        CascadeController::reset();
     }
 
     virtual void reset() override

diff --git a/AirLib/include/controllers/simple_flight/firmware/Mixer.hpp b/AirLib/include/controllers/simple_flight/firmware/Mixer.hpp
@@ -16,11 +16,18 @@ class Mixer {
 
     void getMotorOutput(const Axis4r& controls, std::vector<float>& motor_outputs) const
     {
-        float throttle = std::max(params_->min_armed_output, controls.throttle);
+        //if throttle is too low then set all motors to same value as throttle because
+        //otherwise values in pitch/roll/yaw would get clipped randomly and can produce random results
+        //in other words: we can't do angling if throttle is too low
+        if (controls.throttle < params_->min_armed_output) {
+            motor_outputs.assign(params_->motor_count, controls.throttle);
+            return;
+        }
+
 
         for (int motor_index = 0; motor_index < kMotorCount; ++motor_index) {
             motor_outputs[motor_index] =
-                throttle * mixerQuadX[motor_index].throttle
+                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

diff --git a/AirLib/include/controllers/simple_flight/firmware/Params.hpp b/AirLib/include/controllers/simple_flight/firmware/Params.hpp
@@ -22,7 +22,7 @@ struct Params {
     float min_motor_output = 0;
     float max_motor_output = 1;
     //if min_armed_output too low then noise in pitch/roll can destabilize quad copter when throttle is zero
-    float min_armed_output = 0.2f;
+    float min_armed_output = 0.1f;
 
     const float pi = 3.14159265359f; //180-degrees