Motors: fix thrust curve and add constraint

3drobotics · Mar 3, 2015 · cf8c211 · cf8c211
1 parent 3e960df
commit cf8c211
Showing 1 changed file with 2 additions and 1 deletion.
diff --git a/libraries/AP_Motors/AP_Motors_Class.cpp b/libraries/AP_Motors/AP_Motors_Class.cpp
@@ -268,7 +268,8 @@ int16_t AP_Motors::apply_thrust_curve_and_volt_scaling(int16_t pwm_out, int16_t
     if (_thrust_curve_expo > 0.0f){
         temp_out = ((_thrust_curve_expo-1.0f) + safe_sqrt((1.0f-_thrust_curve_expo)*(1.0f-_thrust_curve_expo) + 4.0f*_thrust_curve_expo*_lift_max*temp_out))/(2.0f*_thrust_curve_expo*_batt_voltage_filt.get());
     }
-    return (temp_out*(_thrust_curve_max*pwm_max-pwm_min)+pwm_min);
+    temp_out = constrain_float(temp_out*_thrust_curve_max*(pwm_max-pwm_min)+pwm_min, pwm_min, pwm_max);
+    return (int16_t)temp_out;
 }
 
 // update_lift_max from battery voltage - used for voltage compensation