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++ ) {

Views: 2159

Comment by Randy on March 14, 2014 at 8:22pm


     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.

Comment by Bob Doiron on March 14, 2014 at 10:36pm

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. 

Comment by Randy on March 14, 2014 at 11:05pm

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.

Comment by Bob Doiron on March 15, 2014 at 4:28am

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

Comment by Crashpilot1000 on March 15, 2014 at 5:19am

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


Comment by Bill Nesbitt on March 15, 2014 at 7:35am

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.

Comment by Eli Cohen on March 15, 2014 at 8:46am

Where are you finding that 100 Hz is too slow? Is that based on analysis or just a gut feeling? For all the quad work I've done, I've  been very happy keeping my loop at 100 Hz and have even gone down to 50 Hz without losing an aircraft (in a lab setting, though).

Comment by Crashpilot1000 on March 15, 2014 at 9:53am

@Bill of course ESC is the way to go! I see it more as an attempt to help the more cheesy ESC out there :) .

@Eli: Flying is always gut feeling since you can not measure real life experience as a hobbyist. And with my altered mwii code I clearly see a change with different control loop speed but I guess there are many other factors to take into account.

Comment by Thorsten on March 15, 2014 at 10:09am

@Randy, is there any estimated release date for AC3.2 (RC)? Thanks, Thorsten

Comment by Bob Doiron on March 15, 2014 at 11:06am

@Crashpilot1000 - You're right, that would save a float multiply. I coded it up pretty quick after work to try and test it out before the sun went down. The response time of that low pass is very short, so you don't have to worry about seeding it to reduce run up time. I do initialize it to radio min whenever the motors are disarmed though. 

@Eli - Higher control loop speeds allows for tighter control, which comes into play more with aggressive flying. I have my aco gain set to 9 (405deg/s) and it does a ~reasonably~ controlled flip at that rate but with some overshoot if you try and stop it too quickly. Tighter control of the motors (higher control loop rate) would improve that.   

I got to fly it a bit more today, and I noticed two improvements: Altitude hold is much improved (acc controller enabled), secondly I was ably to boost my stability gain from 5 (225deg/s max) to 6.7 (~300deg/s) before any overshoot issues showed (same rate gains for both). 

That reminds me, I made a bunch of changes to the 2.9.1b code to improve altitude hold. The stick scaling was way off making up work well, but down not so much. I should post those changes as well... or at least check it it was fixed by someone else in the 3.x code. 


You need to be a member of DIY Drones to add comments!

Join DIY Drones

© 2019   Created by Chris Anderson.   Powered by

Badges  |  Report an Issue  |  Terms of Service