moved rotor boost from base to derived class

SriniVest · May 2, 2017 · 677ac85 · 677ac85
1 parent d1874df
commit 677ac85
Show file tree

Hide file tree

Showing 4 changed files with 17 additions and 24 deletions.
diff --git a/AirLib/include/vehicles/MultiRotor.hpp b/AirLib/include/vehicles/MultiRotor.hpp
@@ -25,21 +25,21 @@ class MultiRotor : public PhysicsBody {
     {
         initialize(params, initial_kinematic_state, environment);
     }
-	void initialize(MultiRotorParams* params, const Kinematics::State& initial_kinematic_state, Environment* environment)
-	{
-		params_ = params;
+    void initialize(MultiRotorParams* params, const Kinematics::State& initial_kinematic_state, Environment* environment)
+    {
+        params_ = params;
 
         PhysicsBody::initialize(params_->getParams().mass, params_->getParams().inertia, initial_kinematic_state, environment);
 
         createRotors(*params_, rotors_, environment);
 
         initSensors(*params_, getKinematics(), getEnvironment());
 
-		//setup drag factors (must come after createRotors).
-		setupDragFactors();
+        //setup drag factors (must come after createRotors).
+        setupDragFactors();
 
         MultiRotor::reset();
-	}
+    }
 
     DroneControllerBase* getController()
     {
@@ -99,24 +99,13 @@ class MultiRotor : public PhysicsBody {
 
         getController()->update();
 
-		float throttle_boost = params_->getParams().rotor_params.throttle_boost;
-
         //transfer new input values from controller to rotors
         for (uint rotor_index = 0; rotor_index < rotors_.size(); ++rotor_index) {
-            rotors_.at(rotor_index).setControlSignal(boost(
-                getController()->getVertexControlSignal(rotor_index), throttle_boost));
+            rotors_.at(rotor_index).setControlSignal(
+                getController()->getVertexControlSignal(rotor_index));
         }
     }
 
-	float boost(float signal, float amount) {
-		if (amount > 0 && amount < 0.7) {
-			float top = 1 - amount;
-			return Utils::clip(top * signal + amount, 0.0f, 1.0f);
-		}
-		return signal;
-	}
-
-
     //sensor getter
     const SensorCollection& getSensors() const
     {

diff --git a/AirLib/include/vehicles/RotorParams.hpp b/AirLib/include/vehicles/RotorParams.hpp
@@ -46,9 +46,6 @@ namespace msr {
             real_T max_thrust = 4.179446268f; //computed from above formula for the given constants
             real_T max_torque = 0.055562f; //computed from above formula
 
-			// set this to zero for more accurate simulation to match playback from real drone.
-			real_T throttle_boost = 0.2f; // boost the normal throttle control signal to make it more fun to fly 
-
             // call this method to recaculate thrust if you want to use different numbers for C_T, C_P, max_rpm, etc.
             void calculateMaxThrust() {
                 revolutions_per_second = max_rpm / 60;

diff --git a/AirLib/include/vehicles/configs/Px4QuadX.hpp b/AirLib/include/vehicles/configs/Px4QuadX.hpp
@@ -79,7 +79,6 @@ class Px4QuadX : public MultiRotorParams {
         params.rotor_params.max_rpm = 9500;
         params.rotor_params.calculateMaxThrust();
         params.linear_drag_coefficient *= 4; // make top speed more real.
-        params.rotor_params.throttle_boost = 0.01f;
 
         //set up dimensions of core body box or abdomen (not including arms).
         params.body_box.x = 0.16f; params.body_box.y = 0.10f; params.body_box.z = 0.14f;
@@ -148,7 +147,6 @@ class Px4QuadX : public MultiRotorParams {
         params.rotor_params.C_P = 0.047f;
         params.rotor_params.max_rpm = 9500;
         params.rotor_params.calculateMaxThrust();
-        params.rotor_params.throttle_boost = 0.01f;
 
         //set up dimensions of core body box or abdomen (not including arms).
         params.body_box.x = 0.20f; params.body_box.y = 0.12f; params.body_box.z = 0.04f;

diff --git a/AirLib/src/controllers/MavLinkDroneController.cpp b/AirLib/src/controllers/MavLinkDroneController.cpp
@@ -119,12 +119,21 @@ struct MavLinkDroneController::impl {
 
     void normalizeRotorControls()
     {
+        //if rotor controls are in not in 0-1 range then they are in -1 to 1 range in which case
+        //we normalize them to 0 to 1 for PX4
         if (!is_controls_0_1_) {
             // change -1 to 1 to 0 to 1.
             for (size_t i = 0; i < Utils::length(rotor_controls_); ++i) {
                 rotor_controls_[i] = (rotor_controls_[i] + 1.0f) / 2.0f;
             }
         }
+        else {
+            //this applies to PX4 and may work differently on other firmwares. 
+            //We use 0.2 as idle rotors which leaves out range of 0.8
+            for (size_t i = 0; i < Utils::length(rotor_controls_); ++i) {
+                rotor_controls_[i] = Utils::clip(0.8f * rotor_controls_[i] + 0.20f, 0.0f, 1.0f);
+            }
+        }
     }
 
     void initializeMavSubscriptions()