AP_MotorsMulticopter.cpp 28 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497498499500501502503504505506507508509510511512513514515516517518519520521522523524525526527528529530531532533534535536537538539540541542543544545546547548549550551552553554555556557558559560561562563564565566567568569570571572573574575576577578579580581582583584585586587588589590591592593594595596597598599600601602603604605606607608609610611612613614615616617618619620621622623624625626627628629630631632633634635636637638639640641642643644645646647648649650651652653654655656657658659660661662663664665666667668669670671672673674675676677678679680681682683684685686687688689690691692693694695696697698699700701702703704705706707708709710711712713714715716717718719720721722723724725726727728729730731732733734735736737738739740741742
  1. /*
  2. This program is free software: you can redistribute it and/or modify
  3. it under the terms of the GNU General Public License as published by
  4. the Free Software Foundation, either version 3 of the License, or
  5. (at your option) any later version.
  6. This program is distributed in the hope that it will be useful,
  7. but WITHOUT ANY WARRANTY; without even the implied warranty of
  8. MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
  9. GNU General Public License for more details.
  10. You should have received a copy of the GNU General Public License
  11. along with this program. If not, see <http://www.gnu.org/licenses/>.
  12. */
  13. /*
  14. * AP_MotorsMulticopter.cpp - ArduCopter multicopter motors library
  15. * Code by Randy Mackay and Robert Lefebvre. DIYDrones.com
  16. *
  17. */
  18. #include "AP_MotorsMulticopter.h"
  19. #include <AP_HAL/AP_HAL.h>
  20. #include <AP_BattMonitor/AP_BattMonitor.h>
  21. extern const AP_HAL::HAL& hal;
  22. // parameters for the motor class
  23. const AP_Param::GroupInfo AP_MotorsMulticopter::var_info[] = {
  24. // 0 was used by TB_RATIO
  25. // 1,2,3 were used by throttle curve
  26. // 5 was SPIN_ARMED
  27. // @Param: YAW_HEADROOM
  28. // @DisplayName: Matrix Yaw Min
  29. // @Description: Yaw control is given at least this pwm in microseconds range
  30. // @Range: 0 500
  31. // @Units: PWM
  32. // @User: Advanced
  33. AP_GROUPINFO("YAW_HEADROOM", 6, AP_MotorsMulticopter, _yaw_headroom, AP_MOTORS_YAW_HEADROOM_DEFAULT),
  34. // 7 was THR_LOW_CMP
  35. // @Param: THST_EXPO
  36. // @DisplayName: Thrust Curve Expo
  37. // @Description: Motor thrust curve exponent (from 0 for linear to 1.0 for second order curve)
  38. // @Range: 0.25 0.8
  39. // @User: Advanced
  40. AP_GROUPINFO("THST_EXPO", 8, AP_MotorsMulticopter, _thrust_curve_expo, AP_MOTORS_THST_EXPO_DEFAULT),
  41. // @Param: SPIN_MAX
  42. // @DisplayName: Motor Spin maximum
  43. // @Description: Point at which the thrust saturates expressed as a number from 0 to 1 in the entire output range
  44. // @Values: 0.9:Low, 0.95:Default, 1.0:High
  45. // @User: Advanced
  46. AP_GROUPINFO("SPIN_MAX", 9, AP_MotorsMulticopter, _spin_max, AP_MOTORS_SPIN_MAX_DEFAULT),
  47. // @Param: BAT_VOLT_MAX
  48. // @DisplayName: Battery voltage compensation maximum voltage
  49. // @Description: Battery voltage compensation maximum voltage (voltage above this will have no additional scaling effect on thrust). Recommend 4.4 * cell count, 0 = Disabled
  50. // @Range: 6 35
  51. // @Units: V
  52. // @User: Advanced
  53. AP_GROUPINFO("BAT_VOLT_MAX", 10, AP_MotorsMulticopter, _batt_voltage_max, AP_MOTORS_BAT_VOLT_MAX_DEFAULT),
  54. // @Param: BAT_VOLT_MIN
  55. // @DisplayName: Battery voltage compensation minimum voltage
  56. // @Description: Battery voltage compensation minimum voltage (voltage below this will have no additional scaling effect on thrust). Recommend 3.5 * cell count, 0 = Disabled
  57. // @Range: 6 35
  58. // @Units: V
  59. // @User: Advanced
  60. AP_GROUPINFO("BAT_VOLT_MIN", 11, AP_MotorsMulticopter, _batt_voltage_min, AP_MOTORS_BAT_VOLT_MIN_DEFAULT),
  61. // @Param: BAT_CURR_MAX
  62. // @DisplayName: Motor Current Max
  63. // @Description: Maximum current over which maximum throttle is limited (0 = Disabled)
  64. // @Range: 0 200
  65. // @Units: A
  66. // @User: Advanced
  67. AP_GROUPINFO("BAT_CURR_MAX", 12, AP_MotorsMulticopter, _batt_current_max, AP_MOTORS_BAT_CURR_MAX_DEFAULT),
  68. // 13, 14 were used by THR_MIX_MIN, THR_MIX_MAX
  69. // @Param: PWM_TYPE
  70. // @DisplayName: Output PWM type
  71. // @Description: This selects the output PWM type, allowing for normal PWM continuous output, OneShot, brushed or DShot motor output
  72. // @Values: 0:Normal,1:OneShot,2:OneShot125,3:Brushed,4:DShot150,5:DShot300,6:DShot600,7:DShot1200
  73. // @User: Advanced
  74. // @RebootRequired: True
  75. AP_GROUPINFO("PWM_TYPE", 15, AP_MotorsMulticopter, _pwm_type, PWM_TYPE_NORMAL),
  76. // @Param: PWM_MIN
  77. // @DisplayName: PWM output miniumum
  78. // @Description: This sets the min PWM output value in microseconds that will ever be output to the motors, 0 = use input RC3_MIN
  79. // @Units: PWM
  80. // @Range: 0 2000
  81. // @User: Advanced
  82. AP_GROUPINFO("PWM_MIN", 16, AP_MotorsMulticopter, _pwm_min, 0),
  83. // @Param: PWM_MAX
  84. // @DisplayName: PWM output maximum
  85. // @Description: This sets the max PWM value in microseconds that will ever be output to the motors, 0 = use input RC3_MAX
  86. // @Units: PWM
  87. // @Range: 0 2000
  88. // @User: Advanced
  89. AP_GROUPINFO("PWM_MAX", 17, AP_MotorsMulticopter, _pwm_max, 0),
  90. // @Param: SPIN_MIN
  91. // @DisplayName: Motor Spin minimum
  92. // @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.
  93. // @Values: 0.0:Low, 0.15:Default, 0.3:High
  94. // @User: Advanced
  95. AP_GROUPINFO("SPIN_MIN", 18, AP_MotorsMulticopter, _spin_min, AP_MOTORS_SPIN_MIN_DEFAULT),
  96. // @Param: SPIN_ARM
  97. // @DisplayName: Motor Spin armed
  98. // @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.
  99. // @Values: 0.0:Low, 0.1:Default, 0.2:High
  100. // @User: Advanced
  101. AP_GROUPINFO("SPIN_ARM", 19, AP_MotorsMulticopter, _spin_arm, AP_MOTORS_SPIN_ARM_DEFAULT),
  102. // @Param: BAT_CURR_TC
  103. // @DisplayName: Motor Current Max Time Constant
  104. // @Description: Time constant used to limit the maximum current
  105. // @Range: 0 10
  106. // @Units: s
  107. // @User: Advanced
  108. AP_GROUPINFO("BAT_CURR_TC", 20, AP_MotorsMulticopter, _batt_current_time_constant, AP_MOTORS_BAT_CURR_TC_DEFAULT),
  109. // @Param: THST_HOVER
  110. // @DisplayName: Thrust Hover Value
  111. // @Description: Motor thrust needed to hover expressed as a number from 0 to 1
  112. // @Range: 0.2 0.8
  113. // @User: Advanced
  114. AP_GROUPINFO("THST_HOVER", 21, AP_MotorsMulticopter, _throttle_hover, AP_MOTORS_THST_HOVER_DEFAULT),
  115. // @Param: HOVER_LEARN
  116. // @DisplayName: Hover Value Learning
  117. // @Description: Enable/Disable automatic learning of hover throttle
  118. // @Values{Copter}: 0:Disabled, 1:Learn, 2:LearnAndSave
  119. // @Values{Sub}: 0:Disabled
  120. // @Values{Plane}: 0:Disabled
  121. // @User: Advanced
  122. AP_GROUPINFO("HOVER_LEARN", 22, AP_MotorsMulticopter, _throttle_hover_learn, HOVER_LEARN_AND_SAVE),
  123. // @Param: SAFE_DISARM
  124. // @DisplayName: Motor PWM output disabled when disarmed
  125. // @Description: Disables motor PWM output when disarmed
  126. // @Values: 0:PWM enabled while disarmed, 1:PWM disabled while disarmed
  127. // @User: Advanced
  128. AP_GROUPINFO("SAFE_DISARM", 23, AP_MotorsMulticopter, _disarm_disable_pwm, 0),
  129. // @Param: YAW_SV_ANGLE
  130. // @DisplayName: Yaw Servo Max Lean Angle
  131. // @Description: Yaw servo's maximum lean angle
  132. // @Range: 5 80
  133. // @Units: deg
  134. // @Increment: 1
  135. // @User: Standard
  136. AP_GROUPINFO_FRAME("YAW_SV_ANGLE", 35, AP_MotorsMulticopter, _yaw_servo_angle_max_deg, 30, AP_PARAM_FRAME_TRICOPTER),
  137. // @Param: SPOOL_TIME
  138. // @DisplayName: Spool up time
  139. // @Description: Time in seconds to spool up the motors from zero to min throttle.
  140. // @Range: 0 2
  141. // @Units: s
  142. // @Increment: 0.1
  143. // @User: Advanced
  144. AP_GROUPINFO("SPOOL_TIME", 36, AP_MotorsMulticopter, _spool_up_time, AP_MOTORS_SPOOL_UP_TIME_DEFAULT),
  145. // @Param: BOOST_SCALE
  146. // @DisplayName: Motor boost scale
  147. // @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.
  148. // @Range: 0 5
  149. // @Increment: 0.1
  150. // @User: Advanced
  151. AP_GROUPINFO("BOOST_SCALE", 37, AP_MotorsMulticopter, _boost_scale, 0),
  152. // 38 RESERVED for BAT_POW_MAX
  153. // @Param: BAT_IDX
  154. // @DisplayName: Battery compensation index
  155. // @Description: Which battery monitor should be used for doing compensation
  156. // @Values: 0:First battery, 1:Second battery
  157. // @User: Advanced
  158. AP_GROUPINFO("BAT_IDX", 39, AP_MotorsMulticopter, _batt_idx, 0),
  159. // @Param: SLEW_UP_TIME
  160. // @DisplayName: Output slew time for increasing throttle
  161. // @Description: Time in seconds to slew output from zero to full. For medium size copter such as a Solo, a value of 0.25 is a good starting point. This is used to limit the rate at which output can change. Range is constrained between 0 and 0.5.
  162. // @Range: 0 .5
  163. // @Units: s
  164. // @Increment: 0.001
  165. // @User: Advanced
  166. AP_GROUPINFO("SLEW_UP_TIME", 40, AP_MotorsMulticopter, _slew_up_time, AP_MOTORS_SLEW_TIME_DEFAULT),
  167. // @Param: SLEW_DN_TIME
  168. // @DisplayName: Output slew time for decreasing throttle
  169. // @Description: Time in seconds to slew output from full to zero. For medium size copter such as a Solo, a value of 0.275 is a good starting point. This is used to limit the rate at which output can change. Range is constrained between 0 and 0.5.
  170. // @Range: 0 .5
  171. // @Units: s
  172. // @Increment: 0.001
  173. // @User: Advanced
  174. AP_GROUPINFO("SLEW_DN_TIME", 41, AP_MotorsMulticopter, _slew_dn_time, AP_MOTORS_SLEW_TIME_DEFAULT),
  175. AP_GROUPEND
  176. };
  177. // Constructor
  178. AP_MotorsMulticopter::AP_MotorsMulticopter(uint16_t loop_rate, uint16_t speed_hz) :
  179. AP_Motors(loop_rate, speed_hz),
  180. _lift_max(1.0f),
  181. _throttle_limit(1.0f)
  182. {
  183. AP_Param::setup_object_defaults(this, var_info);
  184. // setup battery voltage filtering
  185. _batt_voltage_filt.set_cutoff_frequency(AP_MOTORS_BATT_VOLT_FILT_HZ);
  186. _batt_voltage_filt.reset(1.0f);
  187. // default throttle range
  188. _throttle_radio_min = 1100;
  189. _throttle_radio_max = 1900;
  190. };
  191. // output - sends commands to the motors
  192. void AP_MotorsMulticopter::output()
  193. {
  194. // update throttle filter
  195. update_throttle_filter();
  196. // calc filtered battery voltage and lift_max
  197. update_lift_max_from_batt_voltage();
  198. // run spool logic
  199. output_logic();
  200. // calculate thrust
  201. output_armed_stabilizing();
  202. // apply any thrust compensation for the frame
  203. thrust_compensation();
  204. // convert rpy_thrust values to pwm
  205. //output_to_motors();
  206. output_to_Dshot();
  207. // output any booster throttle
  208. output_boost_throttle();
  209. };
  210. // output booster throttle, if any
  211. void AP_MotorsMulticopter::output_boost_throttle(void)
  212. {
  213. if (_boost_scale > 0) {
  214. float throttle = constrain_float(get_throttle() * _boost_scale, 0, 1);
  215. SRV_Channels::set_output_scaled(SRV_Channel::k_boost_throttle, throttle * 1000);
  216. }
  217. }
  218. // sends minimum values out to the motors
  219. void AP_MotorsMulticopter::output_min()
  220. {
  221. set_desired_spool_state(DesiredSpoolState::SHUT_DOWN);
  222. _spool_state = SpoolState::SHUT_DOWN;
  223. output();
  224. }
  225. // update the throttle input filter
  226. void AP_MotorsMulticopter::update_throttle_filter()
  227. {
  228. if (armed()) {
  229. _throttle_filter.apply(_throttle_in, 1.0f / _loop_rate);
  230. // constrain filtered throttle
  231. if (_throttle_filter.get() < 0.0f) {
  232. _throttle_filter.reset(0.0f);
  233. }
  234. if (_throttle_filter.get() > 1.0f) {
  235. _throttle_filter.reset(1.0f);
  236. }
  237. } else {
  238. _throttle_filter.reset(0.0f);
  239. }
  240. }
  241. // return current_limit as a number from 0 ~ 1 in the range throttle_min to throttle_max
  242. float AP_MotorsMulticopter::get_current_limit_max_throttle()
  243. {
  244. AP_BattMonitor &battery = AP::battery();
  245. float _batt_current;
  246. if (_batt_current_max <= 0 || // return maximum if current limiting is disabled
  247. !_flags.armed || // remove throttle limit if disarmed
  248. !battery.current_amps(_batt_current, _batt_idx)) { // no current monitoring is available
  249. _throttle_limit = 1.0f;
  250. return 1.0f;
  251. }
  252. float _batt_resistance = battery.get_resistance(_batt_idx);
  253. if (is_zero(_batt_resistance)) {
  254. _throttle_limit = 1.0f;
  255. return 1.0f;
  256. }
  257. // calculate the maximum current to prevent voltage sag below _batt_voltage_min
  258. float batt_current_max = MIN(_batt_current_max, _batt_current + (battery.voltage(_batt_idx) - _batt_voltage_min) / _batt_resistance);
  259. float batt_current_ratio = _batt_current / batt_current_max;
  260. float loop_interval = 1.0f / _loop_rate;
  261. _throttle_limit += (loop_interval / (loop_interval + _batt_current_time_constant)) * (1.0f - batt_current_ratio);
  262. // throttle limit drops to 20% between hover and full throttle
  263. _throttle_limit = constrain_float(_throttle_limit, 0.2f, 1.0f);
  264. // limit max throttle
  265. return get_throttle_hover() + ((1.0 - get_throttle_hover()) * _throttle_limit);
  266. }
  267. // apply_thrust_curve_and_volt_scaling - returns throttle in the range 0 ~ 1
  268. float AP_MotorsMulticopter::apply_thrust_curve_and_volt_scaling(float thrust) const
  269. {
  270. float throttle_ratio = thrust;
  271. // apply thrust curve - domain 0.0 to 1.0, range 0.0 to 1.0
  272. float thrust_curve_expo = constrain_float(_thrust_curve_expo, -1.0f, 1.0f);
  273. if (fabsf(thrust_curve_expo) < 0.001) {
  274. // zero expo means linear, avoid floating point exception for small values
  275. return thrust;
  276. }
  277. if (!is_zero(_batt_voltage_filt.get())) {
  278. 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());
  279. } else {
  280. 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);
  281. }
  282. return constrain_float(throttle_ratio, 0.0f, 1.0f);
  283. }
  284. // update_lift_max from battery voltage - used for voltage compensation
  285. void AP_MotorsMulticopter::update_lift_max_from_batt_voltage()
  286. {
  287. // sanity check battery_voltage_min is not too small
  288. // if disabled or misconfigured exit immediately
  289. float _batt_voltage_resting_estimate = AP::battery().voltage_resting_estimate(_batt_idx);
  290. if ((_batt_voltage_max <= 0) || (_batt_voltage_min >= _batt_voltage_max) || (_batt_voltage_resting_estimate < 0.25f * _batt_voltage_min)) {
  291. _batt_voltage_filt.reset(1.0f);
  292. _lift_max = 1.0f;
  293. return;
  294. }
  295. _batt_voltage_min = MAX(_batt_voltage_min, _batt_voltage_max * 0.6f);
  296. // contrain resting voltage estimate (resting voltage is actual voltage with sag removed based on current draw and resistance)
  297. _batt_voltage_resting_estimate = constrain_float(_batt_voltage_resting_estimate, _batt_voltage_min, _batt_voltage_max);
  298. // filter at 0.5 Hz
  299. float batt_voltage_filt = _batt_voltage_filt.apply(_batt_voltage_resting_estimate / _batt_voltage_max, 1.0f / _loop_rate);
  300. // calculate lift max
  301. float thrust_curve_expo = constrain_float(_thrust_curve_expo, -1.0f, 1.0f);
  302. _lift_max = batt_voltage_filt * (1 - thrust_curve_expo) + thrust_curve_expo * batt_voltage_filt * batt_voltage_filt;
  303. }
  304. float AP_MotorsMulticopter::get_compensation_gain() const
  305. {
  306. // avoid divide by zero
  307. if (_lift_max <= 0.0f) {
  308. return 1.0f;
  309. }
  310. float ret = 1.0f / _lift_max;
  311. #if AP_MOTORS_DENSITY_COMP == 1
  312. // air density ratio is increasing in density / decreasing in altitude
  313. if (_air_density_ratio > 0.3f && _air_density_ratio < 1.5f) {
  314. ret *= 1.0f / constrain_float(_air_density_ratio, 0.5f, 1.25f);
  315. }
  316. #endif
  317. return ret;
  318. }
  319. // convert actuator output (0~1) range to pwm range
  320. int16_t AP_MotorsMulticopter::output_to_pwm(float actuator)
  321. {
  322. float pwm_output;
  323. if (_spool_state == SpoolState::SHUT_DOWN) {
  324. // in shutdown mode, use PWM 0 or minimum PWM
  325. if (_disarm_disable_pwm && _disarm_safety_timer == 0 && !armed()) {
  326. pwm_output = 0;
  327. } else {
  328. pwm_output = get_pwm_output_min();
  329. }
  330. } else {
  331. // in all other spool modes, covert to desired PWM
  332. pwm_output = get_pwm_output_min() + (get_pwm_output_max() - get_pwm_output_min()) * actuator;
  333. }
  334. return pwm_output;
  335. }
  336. // converts desired thrust to linearized actuator output in a range of 0~1
  337. float AP_MotorsMulticopter::thrust_to_actuator(float thrust_in)
  338. {
  339. thrust_in = constrain_float(thrust_in, 0.0f, 1.0f);
  340. return _spin_min + (_spin_max - _spin_min) * apply_thrust_curve_and_volt_scaling(thrust_in);
  341. }
  342. // adds slew rate limiting to actuator output
  343. void AP_MotorsMulticopter::set_actuator_with_slew(float& actuator_output, float input)
  344. {
  345. /*
  346. If MOT_SLEW_UP_TIME is 0 (default), no slew limit is applied to increasing output.
  347. If MOT_SLEW_DN_TIME is 0 (default), no slew limit is applied to decreasing output.
  348. MOT_SLEW_UP_TIME and MOT_SLEW_DN_TIME are constrained to 0.0~0.5 for sanity.
  349. If spool mode is shutdown, no slew limit is applied to allow immediate disarming of motors.
  350. */
  351. // Output limits with no slew time applied
  352. float output_slew_limit_up = 1.0f;
  353. float output_slew_limit_dn = 0.0f;
  354. // If MOT_SLEW_UP_TIME is set, calculate the highest allowed new output value, constrained 0.0~1.0
  355. if (is_positive(_slew_up_time)) {
  356. float output_delta_up_max = 1.0f / (constrain_float(_slew_up_time, 0.0f, 0.5f) * _loop_rate);
  357. output_slew_limit_up = constrain_float(actuator_output + output_delta_up_max, 0.0f, 1.0f);
  358. }
  359. // If MOT_SLEW_DN_TIME is set, calculate the lowest allowed new output value, constrained 0.0~1.0
  360. if (is_positive(_slew_dn_time)) {
  361. float output_delta_dn_max = 1.0f / (constrain_float(_slew_dn_time, 0.0f, 0.5f) * _loop_rate);
  362. output_slew_limit_dn = constrain_float(actuator_output - output_delta_dn_max, 0.0f, 1.0f);
  363. }
  364. // Constrain change in output to within the above limits
  365. actuator_output = constrain_float(input, output_slew_limit_dn, output_slew_limit_up);
  366. }
  367. // gradually increase actuator output to spin_min
  368. float AP_MotorsMulticopter::actuator_spin_up_to_ground_idle() const
  369. {
  370. return constrain_float(_spin_up_ratio, 0.0f, 1.0f) * _spin_min;
  371. }
  372. // get minimum pwm value that can be output to motors
  373. int16_t AP_MotorsMulticopter::get_pwm_output_min() const
  374. {
  375. // return _pwm_min if both PWM_MIN and PWM_MAX parameters are defined and valid
  376. if ((_pwm_min > 0) && (_pwm_max > 0) && (_pwm_max > _pwm_min)) {
  377. return _pwm_min;
  378. }
  379. return _throttle_radio_min;
  380. }
  381. // get maximum pwm value that can be output to motors
  382. int16_t AP_MotorsMulticopter::get_pwm_output_max() const
  383. {
  384. // return _pwm_max if both PWM_MIN and PWM_MAX parameters are defined and valid
  385. if ((_pwm_min > 0) && (_pwm_max > 0) && (_pwm_max > _pwm_min)) {
  386. return _pwm_max;
  387. }
  388. return _throttle_radio_max;
  389. }
  390. // 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)
  391. // also sets throttle channel minimum and maximum pwm
  392. void AP_MotorsMulticopter::set_throttle_range(int16_t radio_min, int16_t radio_max)
  393. {
  394. // sanity check
  395. if (radio_max <= radio_min) {
  396. return;
  397. }
  398. _throttle_radio_min = radio_min;
  399. _throttle_radio_max = radio_max;
  400. if (_pwm_type >= PWM_TYPE_DSHOT150 && _pwm_type <= PWM_TYPE_DSHOT1200) {
  401. // force PWM range for DShot ESCs
  402. _pwm_min.set(1000);
  403. _pwm_max.set(2000);
  404. }
  405. hal.rcout->set_esc_scaling(get_pwm_output_min(), get_pwm_output_max());
  406. }
  407. // update the throttle input filter. should be called at 100hz
  408. void AP_MotorsMulticopter::update_throttle_hover(float dt)
  409. {
  410. if (_throttle_hover_learn != HOVER_LEARN_DISABLED) {
  411. // we have chosen to constrain the hover throttle to be within the range reachable by the third order expo polynomial.
  412. _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);
  413. }
  414. }
  415. // run spool logic
  416. void AP_MotorsMulticopter::output_logic()
  417. {
  418. if (_flags.armed) {
  419. _disarm_safety_timer = 100;
  420. } else if (_disarm_safety_timer != 0) {
  421. _disarm_safety_timer--;
  422. }
  423. // force desired and current spool mode if disarmed or not interlocked
  424. if (!_flags.armed || !_flags.interlock) {
  425. _spool_desired = DesiredSpoolState::SHUT_DOWN;
  426. _spool_state = SpoolState::SHUT_DOWN;
  427. }
  428. if (_spool_up_time < 0.05) {
  429. // prevent float exception
  430. _spool_up_time.set(0.05);
  431. }
  432. switch (_spool_state) {
  433. case SpoolState::SHUT_DOWN:
  434. // Motors should be stationary.
  435. // Servos set to their trim values or in a test condition.
  436. // set limits flags
  437. limit.roll = true;
  438. limit.pitch = true;
  439. limit.yaw = true;
  440. limit.throttle_lower = true;
  441. limit.throttle_upper = true;
  442. // make sure the motors are spooling in the correct direction
  443. if (_spool_desired != DesiredSpoolState::SHUT_DOWN) {
  444. _spool_state = SpoolState::GROUND_IDLE;
  445. break;
  446. }
  447. // set and increment ramp variables
  448. _spin_up_ratio = 0.0f;
  449. _throttle_thrust_max = 0.0f;
  450. // initialise motor failure variables
  451. _thrust_boost = false;
  452. _thrust_boost_ratio = 0.0f;
  453. break;
  454. case SpoolState::GROUND_IDLE: {
  455. // Motors should be stationary or at ground idle.
  456. // Servos should be moving to correct the current attitude.
  457. // set limits flags
  458. limit.roll = true;
  459. limit.pitch = true;
  460. limit.yaw = true;
  461. limit.throttle_lower = true;
  462. limit.throttle_upper = true;
  463. // set and increment ramp variables
  464. float spool_step = 1.0f / (_spool_up_time * _loop_rate);
  465. switch (_spool_desired) {
  466. case DesiredSpoolState::SHUT_DOWN:
  467. _spin_up_ratio -= spool_step;
  468. // constrain ramp value and update mode
  469. if (_spin_up_ratio <= 0.0f) {
  470. _spin_up_ratio = 0.0f;
  471. _spool_state = SpoolState::SHUT_DOWN;
  472. }
  473. break;
  474. case DesiredSpoolState::THROTTLE_UNLIMITED:
  475. _spin_up_ratio += spool_step;
  476. // constrain ramp value and update mode
  477. if (_spin_up_ratio >= 1.0f) {
  478. _spin_up_ratio = 1.0f;
  479. _spool_state = SpoolState::SPOOLING_UP;
  480. }
  481. break;
  482. case DesiredSpoolState::GROUND_IDLE:
  483. float spin_up_armed_ratio = 0.0f;
  484. if (_spin_min > 0.0f) {
  485. spin_up_armed_ratio = _spin_arm / _spin_min;
  486. }
  487. _spin_up_ratio += constrain_float(spin_up_armed_ratio - _spin_up_ratio, -spool_step, spool_step);
  488. break;
  489. }
  490. _throttle_thrust_max = 0.0f;
  491. // initialise motor failure variables
  492. _thrust_boost = false;
  493. _thrust_boost_ratio = 0.0f;
  494. break;
  495. }
  496. case SpoolState::SPOOLING_UP:
  497. // Maximum throttle should move from minimum to maximum.
  498. // Servos should exhibit normal flight behavior.
  499. // initialize limits flags
  500. limit.roll = false;
  501. limit.pitch = false;
  502. limit.yaw = false;
  503. limit.throttle_lower = false;
  504. limit.throttle_upper = false;
  505. // make sure the motors are spooling in the correct direction
  506. if (_spool_desired != DesiredSpoolState::THROTTLE_UNLIMITED) {
  507. _spool_state = SpoolState::SPOOLING_DOWN;
  508. break;
  509. }
  510. // set and increment ramp variables
  511. _spin_up_ratio = 1.0f;
  512. _throttle_thrust_max += 1.0f / (_spool_up_time * _loop_rate);
  513. // constrain ramp value and update mode
  514. if (_throttle_thrust_max >= MIN(get_throttle(), get_current_limit_max_throttle())) {
  515. _throttle_thrust_max = get_current_limit_max_throttle();
  516. _spool_state = SpoolState::THROTTLE_UNLIMITED;
  517. } else if (_throttle_thrust_max < 0.0f) {
  518. _throttle_thrust_max = 0.0f;
  519. }
  520. // initialise motor failure variables
  521. _thrust_boost = false;
  522. _thrust_boost_ratio = MAX(0.0, _thrust_boost_ratio - 1.0 / (_spool_up_time * _loop_rate));
  523. break;
  524. case SpoolState::THROTTLE_UNLIMITED:
  525. // Throttle should exhibit normal flight behavior.
  526. // Servos should exhibit normal flight behavior.
  527. // initialize limits flags
  528. limit.roll = false;
  529. limit.pitch = false;
  530. limit.yaw = false;
  531. limit.throttle_lower = false;
  532. limit.throttle_upper = false;
  533. // make sure the motors are spooling in the correct direction
  534. if (_spool_desired != DesiredSpoolState::THROTTLE_UNLIMITED) {
  535. _spool_state = SpoolState::SPOOLING_DOWN;
  536. break;
  537. }
  538. // set and increment ramp variables
  539. _spin_up_ratio = 1.0f;
  540. _throttle_thrust_max = get_current_limit_max_throttle();
  541. if (_thrust_boost && !_thrust_balanced) {
  542. _thrust_boost_ratio = MIN(1.0, _thrust_boost_ratio + 1.0f / (_spool_up_time * _loop_rate));
  543. } else {
  544. _thrust_boost_ratio = MAX(0.0, _thrust_boost_ratio - 1.0f / (_spool_up_time * _loop_rate));
  545. }
  546. break;
  547. case SpoolState::SPOOLING_DOWN:
  548. // Maximum throttle should move from maximum to minimum.
  549. // Servos should exhibit normal flight behavior.
  550. // initialize limits flags
  551. limit.roll = false;
  552. limit.pitch = false;
  553. limit.yaw = false;
  554. limit.throttle_lower = false;
  555. limit.throttle_upper = false;
  556. // make sure the motors are spooling in the correct direction
  557. if (_spool_desired == DesiredSpoolState::THROTTLE_UNLIMITED) {
  558. _spool_state = SpoolState::SPOOLING_UP;
  559. break;
  560. }
  561. // set and increment ramp variables
  562. _spin_up_ratio = 1.0f;
  563. _throttle_thrust_max -= 1.0f / (_spool_up_time * _loop_rate);
  564. // constrain ramp value and update mode
  565. if (_throttle_thrust_max <= 0.0f) {
  566. _throttle_thrust_max = 0.0f;
  567. }
  568. if (_throttle_thrust_max >= get_current_limit_max_throttle()) {
  569. _throttle_thrust_max = get_current_limit_max_throttle();
  570. } else if (is_zero(_throttle_thrust_max)) {
  571. _spool_state = SpoolState::GROUND_IDLE;
  572. }
  573. _thrust_boost_ratio = MAX(0.0, _thrust_boost_ratio - 1.0f / (_spool_up_time * _loop_rate));
  574. break;
  575. }
  576. }
  577. // passes throttle directly to all motors for ESC calibration.
  578. // 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()
  579. void AP_MotorsMulticopter::set_throttle_passthrough_for_esc_calibration(float throttle_input)
  580. {
  581. if (armed()) {
  582. uint16_t pwm_out = get_pwm_output_min() + constrain_float(throttle_input, 0.0f, 1.0f) * (get_pwm_output_max() - get_pwm_output_min());
  583. // send the pilot's input directly to each enabled motor
  584. for (uint16_t i = 0; i < AP_MOTORS_MAX_NUM_MOTORS; i++) {
  585. if (motor_enabled[i]) {
  586. rc_write(i, pwm_out);
  587. }
  588. }
  589. // send pwm output to channels used by bicopter
  590. SRV_Channels::set_output_pwm(SRV_Channel::k_throttleRight, pwm_out);
  591. SRV_Channels::set_output_pwm(SRV_Channel::k_throttleLeft, pwm_out);
  592. }
  593. }
  594. // output a thrust to all motors that match a given motor mask. This
  595. // is used to control tiltrotor motors in forward flight. Thrust is in
  596. // the range 0 to 1
  597. void AP_MotorsMulticopter::output_motor_mask(float thrust, uint8_t mask, float rudder_dt)
  598. {
  599. for (uint8_t i = 0; i < AP_MOTORS_MAX_NUM_MOTORS; i++) {
  600. if (motor_enabled[i]) {
  601. if (mask & (1U << i)) {
  602. /*
  603. apply rudder mixing differential thrust
  604. copter frame roll is plane frame yaw as this only
  605. apples to either tilted motors or tailsitters
  606. */
  607. float diff_thrust = get_roll_factor(i) * rudder_dt * 0.5f;
  608. set_actuator_with_slew(_actuator[i], thrust_to_actuator(thrust + diff_thrust));
  609. int16_t pwm_output = get_pwm_output_min() + (get_pwm_output_max() - get_pwm_output_min()) * _actuator[i];
  610. rc_write(i, pwm_output);
  611. } else {
  612. rc_write(i, get_pwm_output_min());
  613. }
  614. }
  615. }
  616. }
  617. // get_motor_mask - returns a bitmask of which outputs are being used for motors (1 means being used)
  618. // this can be used to ensure other pwm outputs (i.e. for servos) do not conflict
  619. uint16_t AP_MotorsMulticopter::get_motor_mask()
  620. {
  621. return SRV_Channels::get_output_channel_mask(SRV_Channel::k_boost_throttle);
  622. }
  623. // save parameters as part of disarming
  624. void AP_MotorsMulticopter::save_params_on_disarm()
  625. {
  626. // save hover throttle
  627. if (_throttle_hover_learn == HOVER_LEARN_AND_SAVE) {
  628. _throttle_hover.save();
  629. }
  630. }