2015-07-02 18:15:09 -03:00
/*
This program is free software : you can redistribute it and / or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation , either version 3 of the License , or
( at your option ) any later version .
This program is distributed in the hope that it will be useful ,
but WITHOUT ANY WARRANTY ; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE . See the
GNU General Public License for more details .
You should have received a copy of the GNU General Public License
along with this program . If not , see < http : //www.gnu.org/licenses/>.
*/
/*
2015-07-15 08:21:13 -03:00
* AP_MotorsMulticopter . cpp - ArduCopter multicopter motors library
2015-07-02 18:15:09 -03:00
* Code by Randy Mackay and Robert Lefebvre . DIYDrones . com
*
*/
2015-07-15 08:21:13 -03:00
# include "AP_MotorsMulticopter.h"
2015-08-11 03:28:44 -03:00
# include <AP_HAL/AP_HAL.h>
2016-05-01 19:00:45 -03:00
2015-07-02 18:15:09 -03:00
extern const AP_HAL : : HAL & hal ;
// parameters for the motor class
2015-10-25 14:03:46 -03:00
const AP_Param : : GroupInfo AP_MotorsMulticopter : : var_info [ ] = {
2015-07-02 18:15:09 -03:00
// 0 was used by TB_RATIO
// 1,2,3 were used by throttle curve
2016-06-09 00:42:38 -03:00
// 5 was SPIN_ARMED
2015-07-02 18:15:09 -03:00
// @Param: YAW_HEADROOM
// @DisplayName: Matrix Yaw Min
// @Description: Yaw control is given at least this pwm range
// @Range: 0 500
// @Units: pwm
// @User: Advanced
2015-07-15 08:21:13 -03:00
AP_GROUPINFO ( " YAW_HEADROOM " , 6 , AP_MotorsMulticopter , _yaw_headroom , AP_MOTORS_YAW_HEADROOM_DEFAULT ) ,
2015-07-02 18:15:09 -03:00
// 7 was THR_LOW_CMP
// @Param: THST_EXPO
// @DisplayName: Thrust Curve Expo
// @Description: Motor thrust curve exponent (from 0 for linear to 1.0 for second order curve)
// @Range: 0.25 0.8
// @User: Advanced
2015-07-15 08:21:13 -03:00
AP_GROUPINFO ( " THST_EXPO " , 8 , AP_MotorsMulticopter , _thrust_curve_expo , AP_MOTORS_THST_EXPO_DEFAULT ) ,
2015-07-02 18:15:09 -03:00
2016-05-21 03:51:36 -03:00
// @Param: SPIN_MAX
// @DisplayName: Motor Spin maximum
// @Description: Point at which the thrust saturates expressed as a number from 0 to 1 in the entire output range
// @Values: 0.9:Low, 0.95:Default, 1.0:High
2015-07-02 18:15:09 -03:00
// @User: Advanced
2016-05-22 06:02:19 -03:00
AP_GROUPINFO ( " SPIN_MAX " , 9 , AP_MotorsMulticopter , _spin_max , AP_MOTORS_SPIN_MAX_DEFAULT ) ,
2015-07-02 18:15:09 -03:00
2016-05-21 03:51:36 -03:00
// @Param: BAT_VOLT_MAX
2015-07-02 18:15:09 -03:00
// @DisplayName: Battery voltage compensation maximum voltage
// @Description: Battery voltage compensation maximum voltage (voltage above this will have no additional scaling effect on thrust). Recommend 4.4 * cell count, 0 = Disabled
// @Range: 6 35
// @Units: Volts
// @User: Advanced
2016-05-21 03:51:36 -03:00
AP_GROUPINFO ( " BAT_VOLT_MAX " , 10 , AP_MotorsMulticopter , _batt_voltage_max , AP_MOTORS_BAT_VOLT_MAX_DEFAULT ) ,
2015-07-02 18:15:09 -03:00
2016-05-21 03:51:36 -03:00
// @Param: BAT_VOLT_MIN
2015-07-02 18:15:09 -03:00
// @DisplayName: Battery voltage compensation minimum voltage
// @Description: Battery voltage compensation minimum voltage (voltage below this will have no additional scaling effect on thrust). Recommend 3.5 * cell count, 0 = Disabled
// @Range: 6 35
// @Units: Volts
// @User: Advanced
2016-05-21 03:51:36 -03:00
AP_GROUPINFO ( " BAT_VOLT_MIN " , 11 , AP_MotorsMulticopter , _batt_voltage_min , AP_MOTORS_BAT_VOLT_MIN_DEFAULT ) ,
2015-07-02 18:15:09 -03:00
2016-05-21 03:51:36 -03:00
// @Param: BAT_CURR_MAX
2015-07-02 18:15:09 -03:00
// @DisplayName: Motor Current Max
// @Description: Maximum current over which maximum throttle is limited (0 = Disabled)
// @Range: 0 200
// @Units: Amps
// @User: Advanced
2016-05-21 03:51:36 -03:00
AP_GROUPINFO ( " BAT_CURR_MAX " , 12 , AP_MotorsMulticopter , _batt_current_max , AP_MOTORS_BAT_CURR_MAX_DEFAULT ) ,
2015-07-02 18:15:09 -03:00
2016-03-21 11:21:19 -03:00
// 13, 14 were used by THR_MIX_MIN, THR_MIX_MAX
2015-08-06 08:39:49 -03:00
2016-04-13 18:48:54 -03:00
// @Param: PWM_TYPE
// @DisplayName: Output PWM type
2016-11-29 06:10:41 -04:00
// @Description: This selects the output PWM type, allowing for normal PWM continuous output, OneShot or brushed motor output
// @Values: 0:Normal,1:OneShot,2:OneShot125,3:Brushed16kHz
2016-04-13 03:33:20 -03:00
// @User: Advanced
2016-11-29 19:49:17 -04:00
// @RebootRequired: True
2016-04-13 18:48:54 -03:00
AP_GROUPINFO ( " PWM_TYPE " , 15 , AP_MotorsMulticopter , _pwm_type , PWM_TYPE_NORMAL ) ,
2016-05-21 03:47:46 -03:00
// @Param: PWM_MIN
// @DisplayName: PWM output miniumum
// @Description: This sets the min PWM output value that will ever be output to the motors, 0 = use input RC3_MIN
// @Range: 0 2000
// @User: Advanced
AP_GROUPINFO ( " PWM_MIN " , 16 , AP_MotorsMulticopter , _pwm_min , 0 ) ,
// @Param: PWM_MAX
// @DisplayName: PWM output maximum
// @Description: This sets the max PWM value that will ever be output to the motors, 0 = use input RC3_MAX
// @Range: 0 2000
// @User: Advanced
AP_GROUPINFO ( " PWM_MAX " , 17 , AP_MotorsMulticopter , _pwm_max , 0 ) ,
2016-06-09 03:34:47 -03:00
// @Param: SPIN_MIN
// @DisplayName: Motor Spin minimum
2016-09-02 23:12:31 -03:00
// @Description: Point at which the thrust starts expressed as a number from 0 to 1 in the entire output range. Should be higher than MOT_SPIN_ARM.
2016-06-09 03:34:47 -03:00
// @Values: 0.0:Low, 0.15:Default, 0.3:High
// @User: Advanced
2016-05-22 06:02:19 -03:00
AP_GROUPINFO ( " SPIN_MIN " , 18 , AP_MotorsMulticopter , _spin_min , AP_MOTORS_SPIN_MIN_DEFAULT ) ,
2016-06-09 03:34:47 -03:00
2016-06-09 00:42:38 -03:00
// @Param: SPIN_ARM
// @DisplayName: Motor Spin armed
2016-09-02 23:12:31 -03:00
// @Description: Point at which the motors start to spin expressed as a number from 0 to 1 in the entire output range. Should be lower than MOT_SPIN_MIN.
2016-06-09 00:42:38 -03:00
// @Values: 0.0:Low, 0.1:Default, 0.2:High
// @User: Advanced
2016-05-22 06:02:19 -03:00
AP_GROUPINFO ( " SPIN_ARM " , 19 , AP_MotorsMulticopter , _spin_arm , AP_MOTORS_SPIN_ARM_DEFAULT ) ,
2016-06-09 00:42:38 -03:00
2016-06-08 08:54:32 -03:00
// @Param: BAT_CURR_TC
// @DisplayName: Motor Current Max Time Constant
// @Description: Time constant used to limit the maximum current
// @Range: 0 10
// @Units: Seconds
// @User: Advanced
AP_GROUPINFO ( " BAT_CURR_TC " , 20 , AP_MotorsMulticopter , _batt_current_time_constant , AP_MOTORS_BAT_CURR_TC_DEFAULT ) ,
2016-03-22 10:57:26 -03:00
// @Param: THST_HOVER
// @DisplayName: Thrust Hover Value
// @Description: Motor thrust needed to hover expressed as a number from 0 to 1
2016-09-07 03:27:44 -03:00
// @Range: 0.2 0.8
2016-03-22 10:57:26 -03:00
// @User: Advanced
AP_GROUPINFO ( " THST_HOVER " , 21 , AP_MotorsMulticopter , _throttle_hover , AP_MOTORS_THST_HOVER_DEFAULT ) ,
2016-06-09 02:21:04 -03:00
// @Param: HOVER_LEARN
// @DisplayName: Hover Value Learning
// @Description: Enable/Disable automatic learning of hover throttle
2016-06-16 02:28:49 -03:00
// @Values: 0:Disabled, 1:Learn, 2:LearnAndSave
2016-06-09 02:21:04 -03:00
// @User: Advanced
2016-06-16 02:28:49 -03:00
AP_GROUPINFO ( " HOVER_LEARN " , 22 , AP_MotorsMulticopter , _throttle_hover_learn , HOVER_LEARN_AND_SAVE ) ,
2016-06-09 02:21:04 -03:00
2016-11-18 21:48:22 -04:00
// @Param: SAFE_DISARM
// @DisplayName: Motor PWM output disabled when disarmed
// @Description: Disables motor PWM output when disarmed
// @Values: 0:PWM enabled while disarmed, 1:PWM disabled while disarmed
// @User: Advanced
AP_GROUPINFO ( " SAFE_DISARM " , 23 , AP_MotorsMulticopter , _disarm_disable_pwm , 0 ) ,
2015-07-02 18:15:09 -03:00
AP_GROUPEND
} ;
// Constructor
2015-07-15 08:21:13 -03:00
AP_MotorsMulticopter : : AP_MotorsMulticopter ( uint16_t loop_rate , uint16_t speed_hz ) :
2015-07-02 18:15:09 -03:00
AP_Motors ( loop_rate , speed_hz ) ,
2016-08-08 01:28:28 -03:00
_spool_mode ( SHUT_DOWN ) ,
_spin_up_ratio ( 0.0f ) ,
2015-07-15 08:21:13 -03:00
_batt_voltage_resting ( 0.0f ) ,
_batt_current_resting ( 0.0f ) ,
_batt_resistance ( 0.0f ) ,
_batt_timer ( 0 ) ,
_lift_max ( 1.0f ) ,
2016-08-08 01:28:28 -03:00
_throttle_limit ( 1.0f ) ,
2016-11-18 21:48:22 -04:00
_throttle_thrust_max ( 0.0f ) ,
_disarm_safety_timer ( 0 )
2015-07-02 18:15:09 -03:00
{
AP_Param : : setup_object_defaults ( this , var_info ) ;
2015-07-15 08:21:13 -03:00
// disable all motors by default
memset ( motor_enabled , false , sizeof ( motor_enabled ) ) ;
2015-07-02 18:15:09 -03:00
// setup battery voltage filtering
_batt_voltage_filt . set_cutoff_frequency ( AP_MOTORS_BATT_VOLT_FILT_HZ ) ;
_batt_voltage_filt . reset ( 1.0f ) ;
2016-05-21 03:47:46 -03:00
2016-06-09 03:34:47 -03:00
// default throttle ranges (i.e. _throttle_radio_min, _throttle_radio_max)
set_throttle_range ( 1100 , 1900 ) ;
2015-07-02 18:15:09 -03:00
} ;
// output - sends commands to the motors
2015-07-15 08:21:13 -03:00
void AP_MotorsMulticopter : : output ( )
2015-07-02 18:15:09 -03:00
{
// update throttle filter
update_throttle_filter ( ) ;
// update battery resistance
update_battery_resistance ( ) ;
// calc filtered battery voltage and lift_max
update_lift_max_from_batt_voltage ( ) ;
2016-01-19 01:49:46 -04:00
// run spool logic
output_logic ( ) ;
// calculate thrust
output_armed_stabilizing ( ) ;
2016-05-01 19:00:45 -03:00
// apply any thrust compensation for the frame
thrust_compensation ( ) ;
2016-01-19 01:49:46 -04:00
// convert rpy_thrust values to pwm
output_to_motors ( ) ;
2015-07-02 18:15:09 -03:00
} ;
2016-02-17 23:04:35 -04:00
// sends minimum values out to the motors
void AP_MotorsMulticopter : : output_min ( )
{
set_desired_spool_state ( DESIRED_SHUT_DOWN ) ;
2016-06-04 02:54:00 -03:00
_spool_mode = SHUT_DOWN ;
2016-02-17 23:04:35 -04:00
output ( ) ;
}
2015-07-02 18:15:09 -03:00
// update the throttle input filter
2015-07-15 08:21:13 -03:00
void AP_MotorsMulticopter : : update_throttle_filter ( )
2015-07-02 18:15:09 -03:00
{
if ( armed ( ) ) {
_throttle_filter . apply ( _throttle_in , 1.0f / _loop_rate ) ;
2016-01-21 21:23:12 -04:00
// constrain filtered throttle
if ( _throttle_filter . get ( ) < 0.0f ) {
_throttle_filter . reset ( 0.0f ) ;
}
if ( _throttle_filter . get ( ) > 1.0f ) {
_throttle_filter . reset ( 1.0f ) ;
}
2015-07-02 18:15:09 -03:00
} else {
_throttle_filter . reset ( 0.0f ) ;
}
}
2015-12-18 23:40:10 -04:00
// return current_limit as a number from 0 ~ 1 in the range throttle_min to throttle_max
float AP_MotorsMulticopter : : get_current_limit_max_throttle ( )
{
// return maximum if current limiting is disabled
if ( _batt_current_max < = 0 ) {
_throttle_limit = 1.0f ;
return 1.0f ;
}
2016-01-21 02:17:13 -04:00
// remove throttle limit if disarmed
if ( ! _flags . armed ) {
2015-12-18 23:40:10 -04:00
_throttle_limit = 1.0f ;
return 1.0f ;
}
2016-06-08 09:00:49 -03:00
// calculate the maximum current to prevent voltage sag below _batt_voltage_min
float batt_current_max = MIN ( _batt_current_max , _batt_current + ( _batt_voltage - _batt_voltage_min ) / _batt_resistance ) ;
float batt_current_ratio = _batt_current / batt_current_max ;
2015-12-18 23:40:10 -04:00
2016-06-08 08:54:32 -03:00
float loop_interval = 1.0f / _loop_rate ;
_throttle_limit + = ( loop_interval / ( loop_interval + _batt_current_time_constant ) ) * ( 1.0f - batt_current_ratio ) ;
2015-12-18 23:40:10 -04:00
// throttle limit drops to 20% between hover and full throttle
_throttle_limit = constrain_float ( _throttle_limit , 0.2f , 1.0f ) ;
// limit max throttle
2016-03-22 10:57:26 -03:00
return get_throttle_hover ( ) + ( ( 1.0 - get_throttle_hover ( ) ) * _throttle_limit ) ;
2015-12-18 23:40:10 -04:00
}
2015-12-03 02:49:19 -04:00
// apply_thrust_curve_and_volt_scaling - returns throttle in the range 0 ~ 1
float AP_MotorsMulticopter : : apply_thrust_curve_and_volt_scaling ( float thrust ) const
{
float throttle_ratio = thrust ;
// apply thrust curve - domain 0.0 to 1.0, range 0.0 to 1.0
2016-05-22 06:53:22 -03:00
if ( _thrust_curve_expo > 0.0f ) {
if ( ! is_zero ( _batt_voltage_filt . get ( ) ) ) {
throttle_ratio = ( ( _thrust_curve_expo - 1.0f ) + safe_sqrt ( ( 1.0f - _thrust_curve_expo ) * ( 1.0f - _thrust_curve_expo ) + 4.0f * _thrust_curve_expo * _lift_max * thrust ) ) / ( 2.0f * _thrust_curve_expo * _batt_voltage_filt . get ( ) ) ;
} else {
throttle_ratio = ( ( _thrust_curve_expo - 1.0f ) + safe_sqrt ( ( 1.0f - _thrust_curve_expo ) * ( 1.0f - _thrust_curve_expo ) + 4.0f * _thrust_curve_expo * _lift_max * thrust ) ) / ( 2.0f * _thrust_curve_expo ) ;
}
2015-12-03 02:49:19 -04:00
}
2016-05-22 06:53:22 -03:00
return constrain_float ( throttle_ratio , 0.0f , 1.0f ) ;
2015-12-03 02:49:19 -04:00
}
2015-07-02 18:15:09 -03:00
// update_lift_max from battery voltage - used for voltage compensation
2015-07-15 08:21:13 -03:00
void AP_MotorsMulticopter : : update_lift_max_from_batt_voltage ( )
2015-07-02 18:15:09 -03:00
{
// sanity check battery_voltage_min is not too small
// if disabled or misconfigured exit immediately
if ( ( _batt_voltage_max < = 0 ) | | ( _batt_voltage_min > = _batt_voltage_max ) | | ( _batt_voltage < 0.25f * _batt_voltage_min ) ) {
_batt_voltage_filt . reset ( 1.0f ) ;
_lift_max = 1.0f ;
return ;
}
2015-11-27 13:11:58 -04:00
_batt_voltage_min = MAX ( _batt_voltage_min , _batt_voltage_max * 0.6f ) ;
2015-07-02 18:15:09 -03:00
// add current based voltage sag to battery voltage
2016-05-22 06:53:22 -03:00
float batt_voltage = _batt_voltage + ( _batt_current - _batt_current_resting ) * _batt_resistance ;
2015-07-02 18:15:09 -03:00
batt_voltage = constrain_float ( batt_voltage , _batt_voltage_min , _batt_voltage_max ) ;
// filter at 0.5 Hz
2016-05-22 06:53:22 -03:00
float batt_voltage_filt = _batt_voltage_filt . apply ( batt_voltage / _batt_voltage_max , 1.0f / _loop_rate ) ;
2015-07-02 18:15:09 -03:00
// calculate lift max
2016-05-22 06:53:22 -03:00
_lift_max = batt_voltage_filt * ( 1 - _thrust_curve_expo ) + _thrust_curve_expo * batt_voltage_filt * batt_voltage_filt ;
2015-07-02 18:15:09 -03:00
}
// update_battery_resistance - calculate battery resistance when throttle is above hover_out
2015-07-15 08:21:13 -03:00
void AP_MotorsMulticopter : : update_battery_resistance ( )
2015-07-02 18:15:09 -03:00
{
2016-01-21 21:26:21 -04:00
// if disarmed reset resting voltage and current
if ( ! _flags . armed ) {
2015-07-02 18:15:09 -03:00
_batt_voltage_resting = _batt_voltage ;
_batt_current_resting = _batt_current ;
_batt_timer = 0 ;
2016-12-13 22:07:11 -04:00
} else if ( _batt_voltage_resting > _batt_voltage & & _batt_current_resting < _batt_current ) {
2015-07-02 18:15:09 -03:00
// update battery resistance when throttle is over hover throttle
2016-05-22 06:53:22 -03:00
float batt_resistance = ( _batt_voltage_resting - _batt_voltage ) / ( _batt_current - _batt_current_resting ) ;
2015-07-02 18:15:09 -03:00
if ( ( _batt_timer < 400 ) & & ( ( _batt_current_resting * 2.0f ) < _batt_current ) ) {
2016-03-22 10:57:26 -03:00
if ( get_throttle ( ) > = get_throttle_hover ( ) ) {
2015-07-02 18:15:09 -03:00
// filter reaches 90% in 1/4 the test time
2016-05-22 06:53:22 -03:00
_batt_resistance + = 0.05f * ( batt_resistance - _batt_resistance ) ;
2015-07-02 18:15:09 -03:00
_batt_timer + = 1 ;
} else {
// initialize battery resistance to prevent change in resting voltage estimate
2016-05-22 06:53:22 -03:00
_batt_resistance = batt_resistance ;
2015-07-02 18:15:09 -03:00
}
}
2016-05-22 06:53:22 -03:00
// make sure battery resistance value doesn't result in the predicted battery voltage exceeding the resting voltage
if ( batt_resistance < _batt_resistance ) {
_batt_resistance = batt_resistance ;
}
2015-07-02 18:15:09 -03:00
}
}
2015-07-15 08:21:13 -03:00
float AP_MotorsMulticopter : : get_compensation_gain ( ) const
2015-07-02 18:15:09 -03:00
{
// avoid divide by zero
if ( _lift_max < = 0.0f ) {
return 1.0f ;
}
float ret = 1.0f / _lift_max ;
2016-01-22 14:41:19 -04:00
# if AP_MOTORS_DENSITY_COMP == 1
2015-07-02 18:15:09 -03:00
// air density ratio is increasing in density / decreasing in altitude
if ( _air_density_ratio > 0.3f & & _air_density_ratio < 1.5f ) {
ret * = 1.0f / constrain_float ( _air_density_ratio , 0.5f , 1.25f ) ;
}
2016-01-22 14:41:19 -04:00
# endif
2015-07-02 18:15:09 -03:00
return ret ;
}
2015-12-03 02:47:51 -04:00
int16_t AP_MotorsMulticopter : : calc_thrust_to_pwm ( float thrust_in ) const
{
2016-05-26 10:13:49 -03:00
thrust_in = constrain_float ( thrust_in , 0.0f , 1.0f ) ;
2016-05-22 06:02:19 -03:00
return get_pwm_output_min ( ) + ( get_pwm_output_max ( ) - get_pwm_output_min ( ) ) * ( _spin_min + ( _spin_max - _spin_min ) * apply_thrust_curve_and_volt_scaling ( thrust_in ) ) ;
2016-06-09 03:34:47 -03:00
}
2016-06-08 06:10:54 -03:00
int16_t AP_MotorsMulticopter : : calc_spin_up_to_pwm ( ) const
{
2016-05-22 06:02:19 -03:00
return get_pwm_output_min ( ) + constrain_float ( _spin_up_ratio , 0.0f , 1.0f ) * _spin_min * ( get_pwm_output_max ( ) - get_pwm_output_min ( ) ) ;
}
2016-05-21 03:47:46 -03:00
// get minimum or maximum pwm value that can be output to motors
2016-05-21 23:02:42 -03:00
int16_t AP_MotorsMulticopter : : get_pwm_output_min ( ) const
2016-05-21 03:47:46 -03:00
{
2016-05-21 23:01:04 -03:00
// return _pwm_min if both PWM_MIN and PWM_MAX parameters are defined and valid
2016-05-21 03:47:46 -03:00
if ( ( _pwm_min > 0 ) & & ( _pwm_max > 0 ) & & ( _pwm_max > _pwm_min ) ) {
return _pwm_min ;
}
return _throttle_radio_min ;
}
// get maximum pwm value that can be output to motors
2016-05-21 23:02:42 -03:00
int16_t AP_MotorsMulticopter : : get_pwm_output_max ( ) const
2016-05-21 03:47:46 -03:00
{
2016-05-21 23:01:04 -03:00
// return _pwm_max if both PWM_MIN and PWM_MAX parameters are defined and valid
2016-05-21 03:47:46 -03:00
if ( ( _pwm_min > 0 ) & & ( _pwm_max > 0 ) & & ( _pwm_max > _pwm_min ) ) {
return _pwm_max ;
}
return _throttle_radio_max ;
2015-12-03 02:47:51 -04:00
}
2015-07-02 18:15:09 -03:00
// set_throttle_range - sets the minimum throttle that will be sent to the engines when they're not off (i.e. to prevents issues with some motors spinning and some not at very low throttle)
// also sets throttle channel minimum and maximum pwm
2016-06-09 03:34:47 -03:00
void AP_MotorsMulticopter : : set_throttle_range ( int16_t radio_min , int16_t radio_max )
2015-07-02 18:15:09 -03:00
{
2016-05-21 03:47:46 -03:00
// sanity check
2016-06-09 03:34:47 -03:00
if ( ( radio_max > radio_min ) ) {
2016-05-21 03:47:46 -03:00
_throttle_radio_min = radio_min ;
_throttle_radio_max = radio_max ;
}
2015-07-02 18:15:09 -03:00
}
2016-03-22 10:57:26 -03:00
// update the throttle input filter. should be called at 100hz
void AP_MotorsMulticopter : : update_throttle_hover ( float dt )
{
2016-06-16 02:28:49 -03:00
if ( _throttle_hover_learn ! = HOVER_LEARN_DISABLED ) {
2016-06-25 01:34:26 -03:00
// we have chosen to constrain the hover throttle to be within the range reachable by the third order expo polynomial.
_throttle_hover = constrain_float ( _throttle_hover + ( dt / ( dt + AP_MOTORS_THST_HOVER_TC ) ) * ( get_throttle ( ) - _throttle_hover ) , AP_MOTORS_THST_HOVER_MIN , AP_MOTORS_THST_HOVER_MAX ) ;
2016-06-09 02:21:04 -03:00
}
2016-03-22 10:57:26 -03:00
}
2016-06-04 02:51:00 -03:00
// run spool logic
2016-01-19 01:49:46 -04:00
void AP_MotorsMulticopter : : output_logic ( )
{
2016-11-18 21:48:22 -04:00
if ( _flags . armed ) {
_disarm_safety_timer = 100 ;
} else if ( _disarm_safety_timer ! = 0 ) {
_disarm_safety_timer - - ;
}
2016-01-19 01:49:46 -04:00
// force desired and current spool mode if disarmed or not interlocked
if ( ! _flags . armed | | ! _flags . interlock ) {
2016-02-02 08:13:43 -04:00
_spool_desired = DESIRED_SHUT_DOWN ;
2016-06-04 02:54:00 -03:00
_spool_mode = SHUT_DOWN ;
2016-01-19 01:49:46 -04:00
}
2016-06-04 02:54:00 -03:00
switch ( _spool_mode ) {
2016-01-19 01:49:46 -04:00
case SHUT_DOWN :
// Motors should be stationary.
// Servos set to their trim values or in a test condition.
// set limits flags
limit . roll_pitch = true ;
limit . yaw = true ;
limit . throttle_lower = true ;
limit . throttle_upper = true ;
// make sure the motors are spooling in the correct direction
2016-03-25 06:06:02 -03:00
if ( _spool_desired ! = DESIRED_SHUT_DOWN ) {
2016-06-04 02:54:00 -03:00
_spool_mode = SPIN_WHEN_ARMED ;
2016-01-19 01:49:46 -04:00
break ;
}
// set and increment ramp variables
2016-06-08 06:10:54 -03:00
_spin_up_ratio = 0.0f ;
2016-01-19 01:49:46 -04:00
_throttle_thrust_max = 0.0f ;
break ;
2016-03-25 06:06:02 -03:00
case SPIN_WHEN_ARMED : {
2016-01-19 01:49:46 -04:00
// Motors should be stationary or at spin when armed.
2016-03-25 06:06:02 -03:00
// Servos should be moving to correct the current attitude.
2016-01-19 01:49:46 -04:00
// set limits flags
limit . roll_pitch = true ;
limit . yaw = true ;
limit . throttle_lower = true ;
limit . throttle_upper = true ;
// set and increment ramp variables
float spool_step = 1.0f / ( AP_MOTORS_SPOOL_UP_TIME * _loop_rate ) ;
2016-03-25 06:04:52 -03:00
if ( _spool_desired = = DESIRED_SHUT_DOWN ) {
2016-06-08 06:10:54 -03:00
_spin_up_ratio - = spool_step ;
2016-03-25 06:04:52 -03:00
// constrain ramp value and update mode
2016-06-08 06:10:54 -03:00
if ( _spin_up_ratio < = 0.0f ) {
_spin_up_ratio = 0.0f ;
2016-06-04 02:54:00 -03:00
_spool_mode = SHUT_DOWN ;
2016-03-25 06:04:52 -03:00
}
} else if ( _spool_desired = = DESIRED_THROTTLE_UNLIMITED ) {
2016-06-08 06:10:54 -03:00
_spin_up_ratio + = spool_step ;
2016-03-25 06:04:52 -03:00
// constrain ramp value and update mode
2016-06-08 06:10:54 -03:00
if ( _spin_up_ratio > = 1.0f ) {
_spin_up_ratio = 1.0f ;
2016-06-04 02:54:00 -03:00
_spool_mode = SPOOL_UP ;
2016-03-25 06:04:52 -03:00
}
} else { // _spool_desired == SPIN_WHEN_ARMED
2016-06-08 06:10:54 -03:00
float spin_up_armed_ratio = 0.0f ;
2016-05-22 06:02:19 -03:00
if ( _spin_min > 0.0f ) {
spin_up_armed_ratio = _spin_arm / _spin_min ;
2016-03-25 06:04:52 -03:00
}
2016-06-08 06:10:54 -03:00
_spin_up_ratio + = constrain_float ( spin_up_armed_ratio - _spin_up_ratio , - spool_step , spool_step ) ;
2016-01-19 01:49:46 -04:00
}
_throttle_thrust_max = 0.0f ;
break ;
}
case SPOOL_UP :
// Maximum throttle should move from minimum to maximum.
2016-04-13 17:45:19 -03:00
// Servos should exhibit normal flight behavior.
2016-01-19 01:49:46 -04:00
// initialize limits flags
limit . roll_pitch = false ;
limit . yaw = false ;
limit . throttle_lower = false ;
limit . throttle_upper = false ;
// make sure the motors are spooling in the correct direction
2016-03-25 06:06:02 -03:00
if ( _spool_desired ! = DESIRED_THROTTLE_UNLIMITED ) {
2016-06-04 02:54:00 -03:00
_spool_mode = SPOOL_DOWN ;
2016-01-19 01:49:46 -04:00
break ;
}
// set and increment ramp variables
2016-06-08 06:10:54 -03:00
_spin_up_ratio = 1.0f ;
2016-01-19 01:49:46 -04:00
_throttle_thrust_max + = 1.0f / ( AP_MOTORS_SPOOL_UP_TIME * _loop_rate ) ;
// constrain ramp value and update mode
if ( _throttle_thrust_max > = MIN ( get_throttle ( ) , get_current_limit_max_throttle ( ) ) ) {
_throttle_thrust_max = get_current_limit_max_throttle ( ) ;
2016-06-04 02:54:00 -03:00
_spool_mode = THROTTLE_UNLIMITED ;
2016-01-19 01:49:46 -04:00
} else if ( _throttle_thrust_max < 0.0f ) {
_throttle_thrust_max = 0.0f ;
}
break ;
case THROTTLE_UNLIMITED :
// Throttle should exhibit normal flight behavior.
2016-04-13 17:45:19 -03:00
// Servos should exhibit normal flight behavior.
2016-01-19 01:49:46 -04:00
// initialize limits flags
limit . roll_pitch = false ;
limit . yaw = false ;
limit . throttle_lower = false ;
limit . throttle_upper = false ;
// make sure the motors are spooling in the correct direction
2016-03-25 06:06:02 -03:00
if ( _spool_desired ! = DESIRED_THROTTLE_UNLIMITED ) {
2016-06-04 02:54:00 -03:00
_spool_mode = SPOOL_DOWN ;
2016-01-19 01:49:46 -04:00
break ;
}
// set and increment ramp variables
2016-06-08 06:10:54 -03:00
_spin_up_ratio = 1.0f ;
2016-01-19 01:49:46 -04:00
_throttle_thrust_max = get_current_limit_max_throttle ( ) ;
break ;
case SPOOL_DOWN :
// Maximum throttle should move from maximum to minimum.
2016-04-13 17:45:19 -03:00
// Servos should exhibit normal flight behavior.
2016-01-19 01:49:46 -04:00
// initialize limits flags
limit . roll_pitch = false ;
limit . yaw = false ;
limit . throttle_lower = false ;
limit . throttle_upper = false ;
// make sure the motors are spooling in the correct direction
2016-03-25 06:06:02 -03:00
if ( _spool_desired = = DESIRED_THROTTLE_UNLIMITED ) {
2016-06-04 02:54:00 -03:00
_spool_mode = SPOOL_UP ;
2016-01-19 01:49:46 -04:00
break ;
}
// set and increment ramp variables
2016-06-08 06:10:54 -03:00
_spin_up_ratio = 1.0f ;
2016-01-19 01:49:46 -04:00
_throttle_thrust_max - = 1.0f / ( AP_MOTORS_SPOOL_UP_TIME * _loop_rate ) ;
// constrain ramp value and update mode
if ( _throttle_thrust_max < = 0.0f ) {
_throttle_thrust_max = 0.0f ;
}
if ( _throttle_thrust_max > = get_current_limit_max_throttle ( ) ) {
_throttle_thrust_max = get_current_limit_max_throttle ( ) ;
2016-03-21 11:21:19 -03:00
} else if ( is_zero ( _throttle_thrust_max ) ) {
2016-06-04 02:54:00 -03:00
_spool_mode = SPIN_WHEN_ARMED ;
2016-01-19 01:49:46 -04:00
}
break ;
}
}
2016-05-21 03:47:46 -03:00
// passes throttle directly to all motors for ESC calibration.
2016-05-21 23:02:42 -03:00
// throttle_input is in the range of 0 ~ 1 where 0 will send get_pwm_output_min() and 1 will send get_pwm_output_max()
2016-05-21 03:47:46 -03:00
void AP_MotorsMulticopter : : set_throttle_passthrough_for_esc_calibration ( float throttle_input )
2015-07-02 18:15:09 -03:00
{
if ( armed ( ) ) {
2016-05-21 23:02:42 -03:00
uint16_t pwm_out = get_pwm_output_min ( ) + constrain_float ( throttle_input , 0.0f , 1.0f ) * ( get_pwm_output_max ( ) - get_pwm_output_min ( ) ) ;
2015-07-02 18:15:09 -03:00
// send the pilot's input directly to each enabled motor
2015-09-28 19:03:32 -03:00
hal . rcout - > cork ( ) ;
2015-09-28 23:59:46 -03:00
for ( uint16_t i = 0 ; i < AP_MOTORS_MAX_NUM_MOTORS ; i + + ) {
2015-07-02 18:15:09 -03:00
if ( motor_enabled [ i ] ) {
2016-05-21 03:47:46 -03:00
rc_write ( i , pwm_out ) ;
2015-07-02 18:15:09 -03:00
}
}
2015-09-28 19:03:32 -03:00
hal . rcout - > push ( ) ;
2015-07-02 18:15:09 -03:00
}
2015-07-15 08:21:13 -03:00
}
2016-05-01 00:07:00 -03:00
// output a thrust to all motors that match a given motor mask. This
// is used to control tiltrotor motors in forward flight. Thrust is in
// the range 0 to 1
void AP_MotorsMulticopter : : output_motor_mask ( float thrust , uint8_t mask )
{
hal . rcout - > cork ( ) ;
for ( uint8_t i = 0 ; i < AP_MOTORS_MAX_NUM_MOTORS ; i + + ) {
if ( motor_enabled [ i ] ) {
int16_t motor_out ;
if ( mask & ( 1U < < i ) ) {
motor_out = calc_thrust_to_pwm ( thrust ) ;
} else {
2016-05-21 23:02:42 -03:00
motor_out = get_pwm_output_min ( ) ;
2016-05-01 00:07:00 -03:00
}
rc_write ( i , motor_out ) ;
}
}
hal . rcout - > push ( ) ;
}
2016-06-09 02:21:04 -03:00
// save parameters as part of disarming
void AP_MotorsMulticopter : : save_params_on_disarm ( )
{
// save hover throttle
2016-06-16 02:28:49 -03:00
if ( _throttle_hover_learn = = HOVER_LEARN_AND_SAVE ) {
2016-06-09 02:21:04 -03:00
_throttle_hover . save ( ) ;
}
}