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>
2018-03-20 22:42:17 -03:00
# include <AP_BattMonitor/AP_BattMonitor.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
2017-05-15 20:21:53 -03:00
// @Description: Yaw control is given at least this pwm in microseconds range
2015-07-02 18:15:09 -03:00
// @Range: 0 500
2017-05-02 10:45:39 -03:00
// @Units: PWM
2015-07-02 18:15:09 -03:00
// @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
2017-05-02 10:45:39 -03:00
// @Units: V
2015-07-02 18:15:09 -03:00
// @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
2017-05-02 10:45:39 -03:00
// @Units: V
2015-07-02 18:15:09 -03:00
// @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
2017-05-02 10:45:39 -03:00
// @Units: A
2015-07-02 18:15:09 -03:00
// @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
2018-03-14 06:47:33 -03:00
// @Description: This selects the output PWM type, allowing for normal PWM continuous output, OneShot, brushed or DShot motor output
// @Values: 0:Normal,1:OneShot,2:OneShot125,3:Brushed,4:DShot150,5:DShot300,6:DShot600,7:DShot1200
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
2017-05-15 20:21:53 -03:00
// @Description: This sets the min PWM output value in microseconds that will ever be output to the motors, 0 = use input RC3_MIN
2017-05-02 10:45:39 -03:00
// @Units: PWM
2016-05-21 03:47:46 -03:00
// @Range: 0 2000
// @User: Advanced
AP_GROUPINFO ( " PWM_MIN " , 16 , AP_MotorsMulticopter , _pwm_min , 0 ) ,
// @Param: PWM_MAX
// @DisplayName: PWM output maximum
2017-05-15 20:21:53 -03:00
// @Description: This sets the max PWM value in microseconds that will ever be output to the motors, 0 = use input RC3_MAX
2017-05-02 10:45:39 -03:00
// @Units: PWM
2016-05-21 03:47:46 -03:00
// @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
2017-05-02 10:45:39 -03:00
// @Units: s
2016-06-08 08:54:32 -03:00
// @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
2019-10-06 03:10:22 -03:00
// @Values{Copter}: 0:Disabled, 1:Learn, 2:Learn and Save
2018-08-16 09:03:06 -03:00
// @Values{Sub}: 0:Disabled
2019-10-06 03:10:22 -03:00
// @Values{Plane}: 0:Disabled, 1:Learn, 2:Learn and Save
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 ) ,
2017-01-09 03:31:56 -04:00
// @Param: YAW_SV_ANGLE
// @DisplayName: Yaw Servo Max Lean Angle
// @Description: Yaw servo's maximum lean angle
// @Range: 5 80
2017-05-02 10:45:39 -03:00
// @Units: deg
2017-01-09 03:31:56 -04:00
// @Increment: 1
// @User: Standard
2018-10-04 06:58:13 -03:00
AP_GROUPINFO_FRAME ( " YAW_SV_ANGLE " , 35 , AP_MotorsMulticopter , _yaw_servo_angle_max_deg , 30 , AP_PARAM_FRAME_TRICOPTER ) ,
2017-02-13 06:35:30 -04:00
// @Param: SPOOL_TIME
// @DisplayName: Spool up time
// @Description: Time in seconds to spool up the motors from zero to min throttle.
// @Range: 0 2
2017-05-02 10:45:39 -03:00
// @Units: s
2017-02-13 06:35:30 -04:00
// @Increment: 0.1
// @User: Advanced
2018-10-04 06:58:13 -03:00
AP_GROUPINFO ( " SPOOL_TIME " , 36 , AP_MotorsMulticopter , _spool_up_time , AP_MOTORS_SPOOL_UP_TIME_DEFAULT ) ,
2017-04-17 07:05:18 -03:00
// @Param: BOOST_SCALE
// @DisplayName: Motor boost scale
2018-12-26 21:06:59 -04:00
// @Description: Booster motor output scaling factor vs main throttle. The output to the BoostThrottle servo will be the main throttle times this scaling factor. A higher scaling factor will put more of the load on the booster motor. A value of 1 will set the BoostThrottle equal to the main throttle.
2017-04-17 07:05:18 -03:00
// @Range: 0 5
// @Increment: 0.1
// @User: Advanced
2018-10-04 06:58:13 -03:00
AP_GROUPINFO ( " BOOST_SCALE " , 37 , AP_MotorsMulticopter , _boost_scale , 0 ) ,
2017-06-02 16:56:37 -03:00
// 38 RESERVED for BAT_POW_MAX
2017-01-09 03:31:56 -04:00
2018-03-21 18:14:33 -03:00
// @Param: BAT_IDX
// @DisplayName: Battery compensation index
// @Description: Which battery monitor should be used for doing compensation
// @Values: 0:First battery, 1:Second battery
// @User: Advanced
2018-10-04 06:58:13 -03:00
AP_GROUPINFO ( " BAT_IDX " , 39 , AP_MotorsMulticopter , _batt_idx , 0 ) ,
2018-03-21 18:14:33 -03:00
2018-11-30 10:34:14 -04:00
// @Param: SLEW_UP_TIME
// @DisplayName: Output slew time for increasing throttle
2018-10-04 06:58:13 -03:00
// @Description: Time in seconds to slew output from zero to full. This is used to limit the rate at which output can change. Range is constrained between 0 and 0.5.
2018-11-30 10:34:14 -04:00
// @Range: 0 .5
// @Units: s
// @Increment: 0.001
// @User: Advanced
2018-10-04 06:58:13 -03:00
AP_GROUPINFO ( " SLEW_UP_TIME " , 40 , AP_MotorsMulticopter , _slew_up_time , AP_MOTORS_SLEW_TIME_DEFAULT ) ,
2019-01-21 07:07:21 -04:00
2018-11-30 10:34:14 -04:00
// @Param: SLEW_DN_TIME
// @DisplayName: Output slew time for decreasing throttle
2018-10-04 06:58:13 -03:00
// @Description: Time in seconds to slew output from full to zero. This is used to limit the rate at which output can change. Range is constrained between 0 and 0.5.
2018-11-30 10:34:14 -04:00
// @Range: 0 .5
// @Units: s
// @Increment: 0.001
// @User: Advanced
2018-10-04 06:58:13 -03:00
AP_GROUPINFO ( " SLEW_DN_TIME " , 41 , AP_MotorsMulticopter , _slew_dn_time , AP_MOTORS_SLEW_TIME_DEFAULT ) ,
// @Param: SAFE_TIME
// @DisplayName: Time taken to disable and enable the motor PWM output when disarmed and armed.
// @Description: Time taken to disable and enable the motor PWM output when disarmed and armed.
// @Range: 0 5
// @Units: s
// @Increment: 0.001
// @User: Advanced
AP_GROUPINFO ( " SAFE_TIME " , 42 , AP_MotorsMulticopter , _safe_time , AP_MOTORS_SAFE_TIME_DEFAULT ) ,
2018-11-30 10:34:14 -04:00
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 ) :
2019-04-19 21:59:40 -03:00
AP_Motors ( loop_rate , speed_hz ) ,
_lift_max ( 1.0f ) ,
_throttle_limit ( 1.0f )
2015-07-02 18:15:09 -03:00
{
AP_Param : : setup_object_defaults ( this , var_info ) ;
// 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
2017-02-28 12:56:07 -04:00
// default throttle range
_throttle_radio_min = 1100 ;
_throttle_radio_max = 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 ( ) ;
// 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 ( ) ;
2019-04-19 21:59:40 -03:00
2016-01-19 01:49:46 -04:00
// convert rpy_thrust values to pwm
output_to_motors ( ) ;
2017-04-17 07:05:18 -03:00
// output any booster throttle
output_boost_throttle ( ) ;
2019-10-14 20:25:51 -03:00
// output raw roll/pitch/yaw/thrust
output_rpyt ( ) ;
2015-07-02 18:15:09 -03:00
} ;
2017-04-17 07:05:18 -03:00
// output booster throttle, if any
void AP_MotorsMulticopter : : output_boost_throttle ( void )
{
if ( _boost_scale > 0 ) {
float throttle = constrain_float ( get_throttle ( ) * _boost_scale , 0 , 1 ) ;
2019-04-19 21:59:40 -03:00
SRV_Channels : : set_output_scaled ( SRV_Channel : : k_boost_throttle , throttle * 1000 ) ;
2017-04-17 07:05:18 -03:00
}
}
2019-10-14 20:25:51 -03:00
// output roll/pitch/yaw/thrust
void AP_MotorsMulticopter : : output_rpyt ( void )
{
SRV_Channels : : set_output_scaled ( SRV_Channel : : k_roll_out , _roll_in_ff * 4500 ) ;
SRV_Channels : : set_output_scaled ( SRV_Channel : : k_pitch_out , _pitch_in_ff * 4500 ) ;
SRV_Channels : : set_output_scaled ( SRV_Channel : : k_yaw_out , _yaw_in_ff * 4500 ) ;
SRV_Channels : : set_output_scaled ( SRV_Channel : : k_thrust_out , get_throttle ( ) * 1000 ) ;
}
2016-02-17 23:04:35 -04:00
// sends minimum values out to the motors
void AP_MotorsMulticopter : : output_min ( )
{
2019-04-09 09:15:45 -03:00
set_desired_spool_state ( DesiredSpoolState : : SHUT_DOWN ) ;
_spool_state = SpoolState : : 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 ( ) ) {
2019-04-19 21:59:40 -03:00
_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 ( )
{
2018-03-20 22:42:17 -03:00
AP_BattMonitor & battery = AP : : battery ( ) ;
2019-07-07 11:38:45 -03:00
float _batt_current ;
2018-03-20 22:42:17 -03:00
if ( _batt_current_max < = 0 | | // return maximum if current limiting is disabled
! _flags . armed | | // remove throttle limit if disarmed
2019-07-07 11:38:45 -03:00
! battery . current_amps ( _batt_current , _batt_idx ) ) { // no current monitoring is available
2015-12-18 23:40:10 -04:00
_throttle_limit = 1.0f ;
return 1.0f ;
}
2018-03-21 18:14:33 -03:00
float _batt_resistance = battery . get_resistance ( _batt_idx ) ;
2018-03-20 22:42:17 -03:00
if ( is_zero ( _batt_resistance ) ) {
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
2019-04-19 21:59:40 -03:00
float batt_current_max = MIN ( _batt_current_max , _batt_current + ( battery . voltage ( _batt_idx ) - _batt_voltage_min ) / _batt_resistance ) ;
2016-06-08 09:00:49 -03:00
2019-04-19 21:59:40 -03:00
float batt_current_ratio = _batt_current / batt_current_max ;
2015-12-18 23:40:10 -04:00
2019-04-19 21:59:40 -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
2019-04-19 21:59:40 -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
2017-06-05 08:39:29 -03:00
float thrust_curve_expo = constrain_float ( _thrust_curve_expo , - 1.0f , 1.0f ) ;
2017-10-25 00:01:26 -03:00
if ( fabsf ( thrust_curve_expo ) < 0.001 ) {
// zero expo means linear, avoid floating point exception for small values
return thrust ;
}
2019-04-19 21:59:40 -03:00
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 ( ) ) ;
2017-06-05 08:39:29 -03:00
} else {
2019-04-19 21:59:40 -03:00
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
2018-03-21 18:14:33 -03:00
float _batt_voltage_resting_estimate = AP : : battery ( ) . voltage_resting_estimate ( _batt_idx ) ;
2019-04-19 21:59:40 -03:00
if ( ( _batt_voltage_max < = 0 ) | | ( _batt_voltage_min > = _batt_voltage_max ) | | ( _batt_voltage_resting_estimate < 0.25f * _batt_voltage_min ) ) {
2015-07-02 18:15:09 -03:00
_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
2017-05-23 01:08:36 -03:00
// contrain resting voltage estimate (resting voltage is actual voltage with sag removed based on current draw and resistance)
2018-03-21 18:14:33 -03:00
_batt_voltage_resting_estimate = constrain_float ( _batt_voltage_resting_estimate , _batt_voltage_min , _batt_voltage_max ) ;
2015-07-02 18:15:09 -03:00
// filter at 0.5 Hz
2019-04-19 21:59:40 -03:00
float batt_voltage_filt = _batt_voltage_filt . apply ( _batt_voltage_resting_estimate / _batt_voltage_max , 1.0f / _loop_rate ) ;
2015-07-02 18:15:09 -03:00
// calculate lift max
2017-06-05 08:39:29 -03:00
float thrust_curve_expo = constrain_float ( _thrust_curve_expo , - 1.0f , 1.0f ) ;
2019-04-19 21:59:40 -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
}
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 ) {
2019-04-19 21:59:40 -03:00
ret * = 1.0f / constrain_float ( _air_density_ratio , 0.5f , 1.25f ) ;
2015-07-02 18:15:09 -03:00
}
2016-01-22 14:41:19 -04:00
# endif
2015-07-02 18:15:09 -03:00
return ret ;
}
2018-11-30 10:34:14 -04:00
// convert actuator output (0~1) range to pwm range
int16_t AP_MotorsMulticopter : : output_to_pwm ( float actuator )
{
float pwm_output ;
2019-04-09 09:15:45 -03:00
if ( _spool_state = = SpoolState : : SHUT_DOWN ) {
2018-11-30 10:34:14 -04:00
// in shutdown mode, use PWM 0 or minimum PWM
2019-09-10 18:43:11 -03:00
if ( _disarm_disable_pwm & & ! armed ( ) ) {
2018-11-30 10:34:14 -04:00
pwm_output = 0 ;
} else {
pwm_output = get_pwm_output_min ( ) ;
}
} else {
// in all other spool modes, covert to desired PWM
2019-04-19 21:59:40 -03:00
pwm_output = get_pwm_output_min ( ) + ( get_pwm_output_max ( ) - get_pwm_output_min ( ) ) * actuator ;
2018-11-30 10:34:14 -04:00
}
return pwm_output ;
}
// converts desired thrust to linearized actuator output in a range of 0~1
float AP_MotorsMulticopter : : thrust_to_actuator ( float thrust_in )
2015-12-03 02:47:51 -04:00
{
2016-05-26 10:13:49 -03:00
thrust_in = constrain_float ( thrust_in , 0.0f , 1.0f ) ;
2019-04-19 21:59:40 -03:00
return _spin_min + ( _spin_max - _spin_min ) * apply_thrust_curve_and_volt_scaling ( thrust_in ) ;
2018-11-30 10:34:14 -04:00
}
// adds slew rate limiting to actuator output
void AP_MotorsMulticopter : : set_actuator_with_slew ( float & actuator_output , float input )
{
/*
If MOT_SLEW_UP_TIME is 0 ( default ) , no slew limit is applied to increasing output .
If MOT_SLEW_DN_TIME is 0 ( default ) , no slew limit is applied to decreasing output .
MOT_SLEW_UP_TIME and MOT_SLEW_DN_TIME are constrained to 0.0 ~ 0.5 for sanity .
If spool mode is shutdown , no slew limit is applied to allow immediate disarming of motors .
*/
2019-01-21 07:07:21 -04:00
2018-11-30 10:34:14 -04:00
// Output limits with no slew time applied
float output_slew_limit_up = 1.0f ;
float output_slew_limit_dn = 0.0f ;
2019-01-21 07:07:21 -04:00
2018-11-30 10:34:14 -04:00
// If MOT_SLEW_UP_TIME is set, calculate the highest allowed new output value, constrained 0.0~1.0
if ( is_positive ( _slew_up_time ) ) {
2019-04-19 21:59:40 -03:00
float output_delta_up_max = 1.0f / ( constrain_float ( _slew_up_time , 0.0f , 0.5f ) * _loop_rate ) ;
2018-11-30 10:34:14 -04:00
output_slew_limit_up = constrain_float ( actuator_output + output_delta_up_max , 0.0f , 1.0f ) ;
}
2019-01-21 07:07:21 -04:00
2018-11-30 10:34:14 -04:00
// If MOT_SLEW_DN_TIME is set, calculate the lowest allowed new output value, constrained 0.0~1.0
if ( is_positive ( _slew_dn_time ) ) {
2019-04-19 21:59:40 -03:00
float output_delta_dn_max = 1.0f / ( constrain_float ( _slew_dn_time , 0.0f , 0.5f ) * _loop_rate ) ;
2018-11-30 10:34:14 -04:00
output_slew_limit_dn = constrain_float ( actuator_output - output_delta_dn_max , 0.0f , 1.0f ) ;
}
// Constrain change in output to within the above limits
actuator_output = constrain_float ( input , output_slew_limit_dn , output_slew_limit_up ) ;
2016-06-09 03:34:47 -03:00
}
2016-06-08 06:10:54 -03:00
2019-01-21 07:13:00 -04:00
// gradually increase actuator output to spin_min
float AP_MotorsMulticopter : : actuator_spin_up_to_ground_idle ( ) const
2016-06-08 06:10:54 -03:00
{
2018-11-30 10:34:14 -04:00
return constrain_float ( _spin_up_ratio , 0.0f , 1.0f ) * _spin_min ;
2016-05-22 06:02:19 -03:00
}
2018-11-30 10:34:14 -04:00
// get minimum 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
2017-02-09 23:14:28 -04:00
if ( radio_max < = radio_min ) {
return ;
2016-05-21 03:47:46 -03:00
}
2017-02-09 23:14:28 -04:00
_throttle_radio_min = radio_min ;
_throttle_radio_max = radio_max ;
2018-11-09 06:26:59 -04:00
if ( _pwm_type > = PWM_TYPE_DSHOT150 & & _pwm_type < = PWM_TYPE_DSHOT1200 ) {
// force PWM range for DShot ESCs
_pwm_min . set ( 1000 ) ;
_pwm_max . set ( 2000 ) ;
}
2017-02-09 23:14:28 -04:00
hal . rcout - > set_esc_scaling ( get_pwm_output_min ( ) , get_pwm_output_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.
2019-04-19 21:59:40 -03:00
_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 ) {
2018-10-04 06:58:13 -03:00
if ( _disarm_disable_pwm & & ( _disarm_safe_timer < _safe_time ) ) {
_disarm_safe_timer + = 1.0f / _loop_rate ;
} else {
_disarm_safe_timer = _safe_time ;
}
2019-09-10 18:43:11 -03:00
} else {
_disarm_safe_timer = 0.0f ;
2016-11-18 21:48:22 -04:00
}
2016-01-19 01:49:46 -04:00
// force desired and current spool mode if disarmed or not interlocked
if ( ! _flags . armed | | ! _flags . interlock ) {
2019-04-09 09:15:45 -03:00
_spool_desired = DesiredSpoolState : : SHUT_DOWN ;
_spool_state = SpoolState : : SHUT_DOWN ;
2016-01-19 01:49:46 -04:00
}
2017-02-13 06:35:30 -04:00
if ( _spool_up_time < 0.05 ) {
// prevent float exception
_spool_up_time . set ( 0.05 ) ;
}
2019-04-09 09:15:45 -03:00
switch ( _spool_state ) {
2019-04-19 21:59:40 -03:00
case SpoolState : : SHUT_DOWN :
// Motors should be stationary.
// Servos set to their trim values or in a test condition.
// set limits flags
2019-07-27 02:37:31 -03:00
limit . roll = true ;
limit . pitch = true ;
2019-04-19 21:59:40 -03:00
limit . yaw = true ;
limit . throttle_lower = true ;
limit . throttle_upper = true ;
// make sure the motors are spooling in the correct direction
2018-10-04 06:58:13 -03:00
if ( _spool_desired ! = DesiredSpoolState : : SHUT_DOWN & & _disarm_safe_timer > = _safe_time . get ( ) ) {
2019-04-19 21:59:40 -03:00
_spool_state = SpoolState : : GROUND_IDLE ;
2016-01-19 01:49:46 -04:00
break ;
2019-04-19 21:59:40 -03:00
}
2016-01-19 01:49:46 -04:00
2019-04-19 21:59:40 -03:00
// set and increment ramp variables
_spin_up_ratio = 0.0f ;
_throttle_thrust_max = 0.0f ;
// initialise motor failure variables
_thrust_boost = false ;
_thrust_boost_ratio = 0.0f ;
break ;
case SpoolState : : GROUND_IDLE : {
// Motors should be stationary or at ground idle.
// Servos should be moving to correct the current attitude.
// set limits flags
2019-07-27 02:37:31 -03:00
limit . roll = true ;
limit . pitch = true ;
2019-04-19 21:59:40 -03:00
limit . yaw = true ;
limit . throttle_lower = true ;
limit . throttle_upper = true ;
// set and increment ramp variables
float spool_step = 1.0f / ( _spool_up_time * _loop_rate ) ;
switch ( _spool_desired ) {
case DesiredSpoolState : : SHUT_DOWN :
_spin_up_ratio - = spool_step ;
// constrain ramp value and update mode
if ( _spin_up_ratio < = 0.0f ) {
_spin_up_ratio = 0.0f ;
_spool_state = SpoolState : : SHUT_DOWN ;
2016-01-19 01:49:46 -04:00
}
break ;
2018-10-04 06:58:13 -03:00
2019-04-19 21:59:40 -03:00
case DesiredSpoolState : : THROTTLE_UNLIMITED :
_spin_up_ratio + = spool_step ;
2016-01-19 01:49:46 -04:00
// constrain ramp value and update mode
2019-04-19 21:59:40 -03:00
if ( _spin_up_ratio > = 1.0f ) {
_spin_up_ratio = 1.0f ;
_spool_state = SpoolState : : SPOOLING_UP ;
}
break ;
2018-10-04 06:58:13 -03:00
2019-04-19 21:59:40 -03:00
case DesiredSpoolState : : GROUND_IDLE :
float spin_up_armed_ratio = 0.0f ;
if ( _spin_min > 0.0f ) {
spin_up_armed_ratio = _spin_arm / _spin_min ;
2016-01-19 01:49:46 -04:00
}
2019-04-19 21:59:40 -03:00
_spin_up_ratio + = constrain_float ( spin_up_armed_ratio - _spin_up_ratio , - spool_step , spool_step ) ;
break ;
}
_throttle_thrust_max = 0.0f ;
2018-08-12 11:19:20 -03:00
2019-04-19 21:59:40 -03:00
// initialise motor failure variables
_thrust_boost = false ;
_thrust_boost_ratio = 0.0f ;
break ;
}
case SpoolState : : SPOOLING_UP :
// Maximum throttle should move from minimum to maximum.
// Servos should exhibit normal flight behavior.
// initialize limits flags
2019-07-27 02:37:31 -03:00
limit . roll = false ;
limit . pitch = false ;
2019-04-19 21:59:40 -03:00
limit . yaw = false ;
limit . throttle_lower = false ;
limit . throttle_upper = false ;
// make sure the motors are spooling in the correct direction
if ( _spool_desired ! = DesiredSpoolState : : THROTTLE_UNLIMITED ) {
_spool_state = SpoolState : : SPOOLING_DOWN ;
2016-01-19 01:49:46 -04:00
break ;
2019-04-19 21:59:40 -03:00
}
2016-01-19 01:49:46 -04:00
2019-04-19 21:59:40 -03:00
// set and increment ramp variables
_spin_up_ratio = 1.0f ;
_throttle_thrust_max + = 1.0f / ( _spool_up_time * _loop_rate ) ;
2016-01-19 01:49:46 -04:00
2019-04-19 21:59:40 -03:00
// 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 ( ) ;
_spool_state = SpoolState : : THROTTLE_UNLIMITED ;
} else if ( _throttle_thrust_max < 0.0f ) {
_throttle_thrust_max = 0.0f ;
}
2016-01-19 01:49:46 -04:00
2019-04-19 21:59:40 -03:00
// initialise motor failure variables
_thrust_boost = false ;
_thrust_boost_ratio = MAX ( 0.0 , _thrust_boost_ratio - 1.0 / ( _spool_up_time * _loop_rate ) ) ;
break ;
2016-01-19 01:49:46 -04:00
2019-04-19 21:59:40 -03:00
case SpoolState : : THROTTLE_UNLIMITED :
// Throttle should exhibit normal flight behavior.
// Servos should exhibit normal flight behavior.
2018-08-12 11:19:20 -03:00
2019-04-19 21:59:40 -03:00
// initialize limits flags
2019-07-27 02:37:31 -03:00
limit . roll = false ;
limit . pitch = false ;
2019-04-19 21:59:40 -03:00
limit . yaw = false ;
limit . throttle_lower = false ;
limit . throttle_upper = false ;
// make sure the motors are spooling in the correct direction
if ( _spool_desired ! = DesiredSpoolState : : THROTTLE_UNLIMITED ) {
_spool_state = SpoolState : : SPOOLING_DOWN ;
2016-01-19 01:49:46 -04:00
break ;
2019-04-19 21:59:40 -03:00
}
2016-01-19 01:49:46 -04:00
2019-04-19 21:59:40 -03:00
// set and increment ramp variables
_spin_up_ratio = 1.0f ;
_throttle_thrust_max = get_current_limit_max_throttle ( ) ;
2016-01-19 01:49:46 -04:00
2019-04-19 21:59:40 -03:00
if ( _thrust_boost & & ! _thrust_balanced ) {
_thrust_boost_ratio = MIN ( 1.0 , _thrust_boost_ratio + 1.0f / ( _spool_up_time * _loop_rate ) ) ;
} else {
_thrust_boost_ratio = MAX ( 0.0 , _thrust_boost_ratio - 1.0f / ( _spool_up_time * _loop_rate ) ) ;
}
break ;
2016-01-19 01:49:46 -04:00
2019-04-19 21:59:40 -03:00
case SpoolState : : SPOOLING_DOWN :
// Maximum throttle should move from maximum to minimum.
// Servos should exhibit normal flight behavior.
2016-01-19 01:49:46 -04:00
2019-04-19 21:59:40 -03:00
// initialize limits flags
2019-07-27 02:37:31 -03:00
limit . roll = false ;
limit . pitch = false ;
2019-04-19 21:59:40 -03:00
limit . yaw = false ;
limit . throttle_lower = false ;
limit . throttle_upper = false ;
2016-01-19 01:49:46 -04:00
2019-04-19 21:59:40 -03:00
// make sure the motors are spooling in the correct direction
if ( _spool_desired = = DesiredSpoolState : : THROTTLE_UNLIMITED ) {
_spool_state = SpoolState : : SPOOLING_UP ;
2016-01-19 01:49:46 -04:00
break ;
2019-04-19 21:59:40 -03:00
}
// set and increment ramp variables
_spin_up_ratio = 1.0f ;
_throttle_thrust_max - = 1.0f / ( _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 ( ) ;
} else if ( is_zero ( _throttle_thrust_max ) ) {
_spool_state = SpoolState : : GROUND_IDLE ;
}
_thrust_boost_ratio = MAX ( 0.0 , _thrust_boost_ratio - 1.0f / ( _spool_up_time * _loop_rate ) ) ;
break ;
2016-01-19 01:49:46 -04:00
}
}
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
2019-04-19 21:59:40 -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
}
}
2018-12-20 08:02:46 -04:00
// send pwm output to channels used by bicopter
SRV_Channels : : set_output_pwm ( SRV_Channel : : k_throttleRight , pwm_out ) ;
SRV_Channels : : set_output_pwm ( SRV_Channel : : k_throttleLeft , pwm_out ) ;
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
2018-12-29 11:44:33 -04:00
void AP_MotorsMulticopter : : output_motor_mask ( float thrust , uint8_t mask , float rudder_dt )
2016-05-01 00:07:00 -03:00
{
2020-01-02 20:12:59 -04:00
const int16_t pwm_min = get_pwm_output_min ( ) ;
const int16_t pwm_range = get_pwm_output_max ( ) - pwm_min ;
2019-04-19 21:59:40 -03:00
for ( uint8_t i = 0 ; i < AP_MOTORS_MAX_NUM_MOTORS ; i + + ) {
2016-05-01 00:07:00 -03:00
if ( motor_enabled [ i ] ) {
2019-04-19 21:59:40 -03:00
if ( mask & ( 1U < < i ) ) {
2018-12-29 11:44:33 -04:00
/*
2019-04-19 21:59:40 -03:00
apply rudder mixing differential thrust
copter frame roll is plane frame yaw as this only
apples to either tilted motors or tailsitters
*/
2018-12-29 11:44:33 -04:00
float diff_thrust = get_roll_factor ( i ) * rudder_dt * 0.5f ;
2018-11-30 10:34:14 -04:00
set_actuator_with_slew ( _actuator [ i ] , thrust_to_actuator ( thrust + diff_thrust ) ) ;
2020-01-02 20:12:59 -04:00
int16_t pwm_output = pwm_min + pwm_range * _actuator [ i ] ;
2019-01-28 16:42:11 -04:00
rc_write ( i , pwm_output ) ;
2016-05-01 00:07:00 -03:00
} else {
2020-01-02 20:12:59 -04:00
rc_write ( i , pwm_min ) ;
2016-05-01 00:07:00 -03:00
}
}
}
}
2016-06-09 02:21:04 -03:00
2018-08-24 02:39:19 -03:00
// get_motor_mask - returns a bitmask of which outputs are being used for motors (1 means being used)
// this can be used to ensure other pwm outputs (i.e. for servos) do not conflict
uint16_t AP_MotorsMulticopter : : get_motor_mask ( )
{
return SRV_Channels : : get_output_channel_mask ( SRV_Channel : : k_boost_throttle ) ;
}
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 ( ) ;
}
}