Arducopter: Motor Response Compensation


I see a lot of work being done to increase the update rate getting to the motors of a quad for stability. The APM only updates the PWM value at 100hz (not ideal), so I got to thinking about how one could improve the performance without too much headache... (TODO: see if the 100hz loop could be increased to 200 or more...)

In this graph I modeled a couple of step responses being imposed on the motor - blue line is requested throttle, purple line is an idealized response from the motor ignoring load conditions. Basically, the motor responds to the step input with a certain time constant. 

The red and the green lines represent a calculated output pwm based on the requested "throttle in" and the modeled motor response. The goal being to shorten the step response time constant by boosting the pwm output during the initial rise. 

KA = constant for motor response estimate. 0.1 = slooooow response, 1.0 = instant. 

KB = Boost factor. Multiplied by the difference between the request and the response and added to the request. 

LIM = caps the amount added or subtracted from the request.

I coded this idea up, and have to say I'm please with the results. On a new quad I was playing with, I noticed that if I let it freefall for a while and then punched the throttle to hover, I'd get some wobbles. With this in place, you can hear the motors responding much quicker and get much more stability. Unfortunately, the SimonK firmware I have in the HK30A escs doesn't actively slow the props down. If it did, I think this mod would be even more effective. The next steps are to play with the Stability / Rate gains to see if there are improvements to be made there. 

Patch against 2.9.1b (I'm sure it looks like hell just pasted in, sorry)

diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde
index 945b187..26009bf 100644
--- a/ArduCopter/ArduCopter.pde
+++ b/ArduCopter/ArduCopter.pde
@@ -1,6 +1,6 @@
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-

-#define THISFIRMWARE "ArduCopter V2.9.1b-Bob 2014-Mar-12"
+#define THISFIRMWARE "ArduCopter V2.9.1b-Bob 2014-Mar-14"
* ArduCopter Version 2.9
* Lead author: Jason Short
diff --git a/libraries/AP_Motors/AP_Motors.cpp b/libraries/AP_Motors/AP_Motors.cpp
index 5a435ac..3c4876a 100644
--- a/libraries/AP_Motors/AP_Motors.cpp
+++ b/libraries/AP_Motors/AP_Motors.cpp
@@ -35,6 +35,24 @@ const AP_Param::GroupInfo AP_Motors::var_info[] PROGMEM = {
// @Range: 20 80

+ // @Param: TBST_KA
+ // @DisplayName:
+ // @Description:
+ // @Range: 0 - 1.0
+ AP_GROUPINFO("TBST_KA", 4, AP_Motors, _throttle_boost_ka, THROTTLE_BOOST_KA),
+ // @Param: TBST_KB
+ // @DisplayName:
+ // @Description:
+ // @Range: 0 - 1.0
+ AP_GROUPINFO("TBST_KB", 5, AP_Motors, _throttle_boost_kb, THROTTLE_BOOST_KB),
+ // @Param: TBST_KB
+ // @DisplayName:
+ // @Description:
+ // @Range: 0 - 1000
+ AP_GROUPINFO("TBST_LIM", 6, AP_Motors, _throttle_boost_limit, THROTTLE_BOOST_LIM),

diff --git a/libraries/AP_Motors/AP_Motors.h b/libraries/AP_Motors/AP_Motors.h
index 18e7754..671f80f 100644
--- a/libraries/AP_Motors/AP_Motors.h
+++ b/libraries/AP_Motors/AP_Motors.h
@@ -50,6 +50,11 @@
#define THROTTLE_CURVE_MID_THRUST 52 // throttle which produces 1/2 the maximum thrust. expressed as a percentage of the full throttle range (i.e 0 ~ 100)
#define THROTTLE_CURVE_MAX_THRUST 93 // throttle which produces the maximum thrust. expressed as a percentage of the full throttle range (i.e 0 ~ 100)

+#define THROTTLE_BOOST_KA 1.0
+#define THROTTLE_BOOST_KB 0.0
// bit mask for recording which limits we have reached when outputting to motors
@@ -182,6 +187,10 @@ protected:
AP_Int8 _throttle_curve_mid; // throttle which produces 1/2 the maximum thrust. expressed as a percentage (i.e. 0 ~ 100 ) of the full throttle range
AP_Int8 _throttle_curve_max; // throttle which produces the maximum thrust. expressed as a percentage (i.e. 0 ~ 100 ) of the full throttle range
uint8_t _reached_limit; // bit mask to record which motor limits we hit (if any) during most recent output. Used to provide feedback to attitude controllers
+ AP_Float _throttle_boost_ka; // motor time constant (0.01 = very slow, 1 = instant response)
+ AP_Float _throttle_boost_kb; // boost factor, multiplied by the difference between the normalized motor speed estimate (based on ka) and the requested throttle
+ AP_Int16 _throttle_boost_limit; // max boost/cut to apply to the commanded throttle
+ int16_t _motor_v_estimate[AP_MOTORS_MAX_NUM_MOTORS];

#endif // AP_MOTORS
\ No newline at end of file
diff --git a/libraries/AP_Motors/AP_MotorsMatrix.cpp b/libraries/AP_Motors/AP_MotorsMatrix.cpp
index fff0a74..e6149f7 100644
--- a/libraries/AP_Motors/AP_MotorsMatrix.cpp
+++ b/libraries/AP_Motors/AP_MotorsMatrix.cpp
@@ -85,6 +85,8 @@ void AP_MotorsMatrix::output_min()
if( motor_enabled[i] ) {
motor_out[i] = _rc_throttle->radio_min;
_rc->OutputCh(_motor_to_channel_map[i], motor_out[i]);
+ _motor_v_estimate[i] = _rc_throttle->radio_min;
@@ -253,6 +255,17 @@ void AP_MotorsMatrix::output_armed()

+ // throttle booster, experimental. Try to get motors to the desired rpm faster
+ if(_throttle_boost_kb != 0)
+ {
+ for( i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++ ) {
+ if( motor_enabled[i] ) {
+ motor_out[i] = motor_out[i] + constrain( ((motor_out[i] - _motor_v_estimate[i])*_throttle_boost_kb), -_throttle_boost_limit, _throttle_boost_limit);
+ _motor_v_estimate[i] = (_motor_v_estimate[i]*(1.0-_throttle_boost_ka)) + (motor_out[i] * _throttle_boost_ka);
+ }
+ }
+ }
// adjust for throttle curve
if( _throttle_curve_enabled ) {
for( i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++ ) {

E-mail me when people leave their comments –

You need to be a member of diydrones to add comments!

Join diydrones


  • Randy is correct in that the best possible solution is to close the loop in the ESCs.  However, if that is not possible, then it is possible to model the motor's setpoint change performance and create an open loop controller that is tuned to compensate for their slow response.  For best results, each prop/motor combination will need to be characterized.

  • Hi Bob!

    Thanks for sharing your idea!

    Your estimate is a LPF that might be faster this way:

            _motor_v_estimate[i] += _throttle_boost_ka * (motor_out[i] - _motor_v_estimate[i]);

    Maybe it is reasonable to initialize that lpf to reduce runup time. (like _motor_v_estimate[i] = idlethrottle or something. Maybe having a floatvariable to gather the "estimate" could improve things (I used it).

    I tried it out on stock GAUI 330 ESC/Motor combo (small,light 8Inch props) because I think if your proposal is an improvement it should be most obvious on stock, unflashed ESC. Besides that I use a 32Bit mwii port running a 500Hz loop for the test.

    Well it's too early to say something definitive but I didn't see a big difference - in case I was able (with too much gain and too much filtering) to degrade stability but that is what I expected. Other values lead to a little shaking but the over all stability seemed better. Hmm.... will investigate that further *sometime* but you will see the most noticable difference when changing the cycle time - and 100Hz is the major drawback of your hardware/code combination IMHO.

    Best Regards


  • Got some Afros on the way so I can explore just that :)

  • Developer

    The rate controller updates were specifically separated from the rest of the control algorithms so that they could be run at a different (i.e. higher) rate from everything else.  We've never actually done that but that was the intention.

    I'm not an expert on ESCs but I would have thought that if one were trying to get the fastest response out of the motors it would be better to focus on the firmware that's in the ESC rather than the controller.  Of course SimonK is open source but there's also the AutoQuad ESC32s which are more sophisticated and also have more spare CPU cycles to do interesting things with.  The ESCs are the closest thing to the motor and they can see how much current is being fed to the motors which is likely the limitation on the response of the motor.

  • Ha! Yeah I'd like to get in to a pixhawk. For now tough, I don't see why the rate controller couldn't be increased... if it meant slowing down they way outer control loops. But - Does this idea make sense within the current design? I see 'one shot' pwm ideas to get at the motor faster, but that really doesn't change physics of the motor - this idea is trying to use the maximum motor torque available to get the job done. 

  • Developer


         Great that you're looking at this level of detail.

         The update rate to the motors is currently 490hz but as you say, the main loop only runs at 100hz on the APM2.x.  To increase the output rate to 200hz you'd probably only want to run the bottom level of the control code (the rate controllers) at the higher rate.  I'm pretty darn sure this isn't possible in AC3.1.2 at least in autopilot modes (loiter, rtl, auto, etc) but it might be possible in manual modes (stabilize, acro).  The main development code (aka "master" or "trunk") which will go out eventually as AC3.2 runs the main loop at 400hz on the Pixhawk where we have sufficient CPU.

         Anyway, best of luck.

This reply was deleted.