Blogs

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