Arducopter: Motor Response Compensation

3689582032?profile=original

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
AP_GROUPINFO("TCRV_MAXPCT", 3, AP_Motors, _throttle_curve_max, THROTTLE_CURVE_MAX_THRUST),

+ // @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),
+
AP_GROUPEND
};

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
+#define THROTTLE_BOOST_LIM 0
+
+
// bit mask for recording which limits we have reached when outputting to motors
#define AP_MOTOR_NO_LIMITS_REACHED 0x00
#define AP_MOTOR_ROLLPITCH_LIMIT 0x01
@@ -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

Comments

  • "KA is rate dependent" yes, I had a rate of >300Hz during testing. I played around a little further but can not get BLheli ESC/Motor/CrabonProp combi to work any better that with an unaltered signal. Maybe the motors are already quick enough and have enough torque to brake/acclerate the light carbon props. Maybe heavier APC props could reveal a benefit with my combo. Since I have no equipment to do measurements on acceleration/motor etc - it's just guesswork on my side. Changing the LPF to frequency dependency (cut off) would be the next step but for pure initial testing of your idea the KA rate dependency is good enough. I read you have Afro esc on the way, well be warned they may produce problems depending on ambient temperature http://www.multiwii.com/forum/viewtopic.php?f=13&t=4494. I am a little suspicious about their quality.

    I will go for some Autoquad esc for my next copter :).

    MultiWii
    MultiWii project related stuffs
  • The parameters I ended up using for my SimonK flashed HK30A ESCs with D3530 1100Kv motors and 10x4.5 props are KA=0.25, KB=0.7, LIM=500. KA is rate dependent - these values are running at 100hz and generate a compensation pulse that decays in 6 or 7 samples (60-70ms).

  • Interesting. Maybe it needs a scale factor and/or a separate limit for braking? - the motor probably has better braking performance since it's generating the stopping current and not the battery.

  • @Bob: I did some further testing and simplified it with bitshifts etc. and have very good results with my cheapo ESC - the reaction is more crisp and stability is increased. Still have to flash my bigger copter to see what those BL Heli ESC do (active braking). I tested around and took the motorestimate as a constant because from my findings a short pwm spike is all those esc need (think they resemble turnigy plush) so it's more "gain" dependent. I also loosened up the limits (-300 -> + 300) to +-500. When reverting my bitshift stuff I end up with something that would resemble your parameters:

    THROTTLE_BOOST_KB 0.5

    THROTTLE_BOOST_KA 0.25

    THROTTLE_BOOST_LIM 500

    Quiet happy with the results! I am pretty sure those intelligent ESC with adjustable internal PID controllers (closed loop) will hate those spikes... so that should be an optional feature...

    Will do more testing with other copter and report.

    Cheers Rob

    Update:
    Tested on Skywalker 20A with BL heli @ Robbe 2827-26 @ 10 Inch carbon props on DJI450 Frame.

    With THROTTLE_BOOST_KB of "1" audible stutter of motors (braking) degraded flight experience.
    With THROTTLE_BOOST_KB of "0.5" ok, not really better than without Motor response compensation.
    No further tests possible - starting rain :( .
    Maybe I just see what I wanted to see but BLheli with enabled active braking don't seem to profit.
    Your idea works for me best on unflashed stock ESC.

    Cheers Rob

  • It would be overly complex to try and model ~everything~ - especially real world disturbances like wind gusts, propeller deflection, etc. Since the ESCs can take updates at 490Hz, it seems fitting to take try and take full advantage of that for the most responsive reaction to a non-modeled disturbance. The APM code also runs cascaded PID controllers, position->rate and sometimes position->rate->acceleration. The rule of thumb I'd love to use is to have inner loop runs at 10 times the outer loop frequency. This simply isn't practical in this case. Closed loop rpm or thrust controllers in the ESCs would be ideal!

  • @Bob - I would posit that a better plant model would be significantly more beneficial than trying to go beyond 100 Hz.... clearly there are some gains that will be crap even if you are running your controller at infinite speed (like in any of the classical control models that don't take into account discrete time steps). Provided your sampling frequency is some number of times greater than your natural frequency of your system (or bandwidth, possibly?), I think it doesn't really make a performance impact. Theoretically, the gains in your system should be compensating for the built-in motor lag. There are lots of ways I have seen to do flips, the oldest one I remember seeing was essentially open-loop where a huge roll rate was commanded, the controller was turned off while the vehicle was rolling and then snapped back on to stabilize it when it came out of the roll. IIRC, this was pretty sloppy. I think the next step was trajectory following which closed the loop on the whole maneuver and was a lot crisper. I guess what I am trying to say is if you ask for a messy maneuver, and you don't have your plant modeled well, things could be sloppy no matter how fast you are running your controller!

  • @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. 

  • T3

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

  • @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.

  • 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).

This reply was deleted.