From 3854f2eda7b8aa2ed9768349486d1c72c3aa3e77 Mon Sep 17 00:00:00 2001 From: Leonard Hall Date: Mon, 18 Jan 2016 13:13:44 +0900 Subject: [PATCH] AP_MotorsMulticopter: rename _throttle_thr_mix_desired to _throttle_rpy_mix_desired No functional change Also rename _throttle_thr_mix to _throttle_rpy_mix --- libraries/AP_Motors/AP_MotorsMulticopter.cpp | 22 ++++++++++---------- libraries/AP_Motors/AP_MotorsMulticopter.h | 20 +++++++++--------- 2 files changed, 21 insertions(+), 21 deletions(-) diff --git a/libraries/AP_Motors/AP_MotorsMulticopter.cpp b/libraries/AP_Motors/AP_MotorsMulticopter.cpp index b179e64b76..a2d57be12f 100644 --- a/libraries/AP_Motors/AP_MotorsMulticopter.cpp +++ b/libraries/AP_Motors/AP_MotorsMulticopter.cpp @@ -105,8 +105,8 @@ const AP_Param::GroupInfo AP_MotorsMulticopter::var_info[] = { AP_MotorsMulticopter::AP_MotorsMulticopter(uint16_t loop_rate, uint16_t speed_hz) : AP_Motors(loop_rate, speed_hz), _spin_when_armed_ramped(0), - _throttle_thr_mix_desired(AP_MOTORS_THR_LOW_CMP_DEFAULT), - _throttle_thr_mix(AP_MOTORS_THR_LOW_CMP_DEFAULT), + _throttle_rpy_mix_desired(AP_MOTORS_THR_LOW_CMP_DEFAULT), + _throttle_rpy_mix(AP_MOTORS_THR_LOW_CMP_DEFAULT), _min_throttle(AP_MOTORS_DEFAULT_MIN_THROTTLE), _max_throttle(AP_MOTORS_DEFAULT_MAX_THROTTLE), _hover_out(AP_MOTORS_DEFAULT_MID_THROTTLE), @@ -147,7 +147,7 @@ void AP_MotorsMulticopter::output() update_lift_max_from_batt_voltage(); // move throttle_low_comp towards desired throttle low comp - update_throttle_thr_mix(); + update_throttle_rpy_mix(); if (_flags.armed) { if (!_flags.interlock) { @@ -319,18 +319,18 @@ void AP_MotorsMulticopter::update_battery_resistance() } } -// update_throttle_thr_mix - slew set_throttle_thr_mix to requested value -void AP_MotorsMulticopter::update_throttle_thr_mix() +// update_throttle_rpy_mix - slew set_throttle_rpy_mix to requested value +void AP_MotorsMulticopter::update_throttle_rpy_mix() { - // slew _throttle_thr_mix to _throttle_thr_mix_desired - if (_throttle_thr_mix < _throttle_thr_mix_desired) { + // slew _throttle_rpy_mix to _throttle_rpy_mix_desired + if (_throttle_rpy_mix < _throttle_rpy_mix_desired) { // increase quickly (i.e. from 0.1 to 0.9 in 0.4 seconds) - _throttle_thr_mix += MIN(2.0f/_loop_rate, _throttle_thr_mix_desired-_throttle_thr_mix); - } else if (_throttle_thr_mix > _throttle_thr_mix_desired) { + _throttle_rpy_mix += MIN(2.0f/_loop_rate, _throttle_rpy_mix_desired-_throttle_rpy_mix); + } else if (_throttle_rpy_mix > _throttle_rpy_mix_desired) { // reduce more slowly (from 0.9 to 0.1 in 1.6 seconds) - _throttle_thr_mix -= MIN(0.5f/_loop_rate, _throttle_thr_mix-_throttle_thr_mix_desired); + _throttle_rpy_mix -= MIN(0.5f/_loop_rate, _throttle_rpy_mix-_throttle_rpy_mix_desired); } - _throttle_thr_mix = constrain_float(_throttle_thr_mix, 0.1f, 1.0f); + _throttle_rpy_mix = constrain_float(_throttle_rpy_mix, 0.1f, 1.0f); } // get_hover_throttle_as_pwm - converts hover throttle to pwm (i.e. range 1000 ~ 2000) diff --git a/libraries/AP_Motors/AP_MotorsMulticopter.h b/libraries/AP_Motors/AP_MotorsMulticopter.h index 48b991d039..5ae40dc382 100644 --- a/libraries/AP_Motors/AP_MotorsMulticopter.h +++ b/libraries/AP_Motors/AP_MotorsMulticopter.h @@ -51,15 +51,15 @@ public: // set_yaw_headroom - set yaw headroom (yaw is given at least this amount of pwm) void set_yaw_headroom(int16_t pwm) { _yaw_headroom = pwm; } - // set_throttle_thr_mix - set desired throttle_thr_mix (actual throttle_thr_mix is slewed towards this value over 1~2 seconds) + // set_throttle_rpy_mix - set desired throttle_thr_mix (actual throttle_thr_mix is slewed towards this value over 1~2 seconds) // low values favour pilot/autopilot throttle over attitude control, high values favour attitude control over throttle // has no effect when throttle is above hover throttle - void set_throttle_mix_min() { _throttle_thr_mix_desired = _thr_mix_min; } - void set_throttle_mix_mid() { _throttle_thr_mix_desired = AP_MOTORS_THR_MIX_MID_DEFAULT; } - void set_throttle_mix_max() { _throttle_thr_mix_desired = _thr_mix_max; } + void set_throttle_mix_min() { _throttle_rpy_mix_desired = _thr_mix_min; } + void set_throttle_mix_mid() { _throttle_rpy_mix_desired = AP_MOTORS_THR_MIX_MID_DEFAULT; } + void set_throttle_mix_max() { _throttle_rpy_mix_desired = _thr_mix_max; } - // get_throttle_thr_mix - get low throttle compensation value - bool is_throttle_mix_min() const { return (_throttle_thr_mix < 1.25f*_thr_mix_min); } + // get_throttle_rpy_mix - get low throttle compensation value + bool is_throttle_mix_min() const { return (_throttle_rpy_mix < 1.25f*_thr_mix_min); } // returns warning throttle float get_throttle_warn() const { return rel_pwm_to_thr_range(_spin_when_armed); } @@ -119,8 +119,8 @@ protected: // update_battery_resistance - calculate battery resistance when throttle is above hover_out void update_battery_resistance(); - // update_throttle_thr_mix - updates thr_low_comp value towards the target - void update_throttle_thr_mix(); + // update_throttle_rpy_mix - updates thr_low_comp value towards the target + void update_throttle_rpy_mix(); // return gain scheduling gain based on voltage and air density float get_compensation_gain() const; @@ -164,8 +164,8 @@ protected: // internal variables bool motor_enabled[AP_MOTORS_MAX_NUM_MOTORS]; // true if motor is enabled int16_t _spin_when_armed_ramped; // equal to _spin_when_armed parameter but slowly ramped up from zero - float _throttle_thr_mix_desired; // desired throttle_low_comp value, actual throttle_low_comp is slewed towards this value over 1~2 seconds - float _throttle_thr_mix; // mix between throttle and hover throttle for 0 to 1 and ratio above hover throttle for >1 + float _throttle_rpy_mix_desired; // desired throttle_low_comp value, actual throttle_low_comp is slewed towards this value over 1~2 seconds + float _throttle_rpy_mix; // mix between throttle and hover throttle for 0 to 1 and ratio above hover throttle for >1 int16_t _min_throttle; // the minimum throttle to be sent to the motors when they're on (prevents motors stalling while flying) int16_t _max_throttle; // the maximum throttle to be sent to the motors (sometimes limited by slow start) int16_t _hover_out; // the estimated hover throttle as pct * 10 (i.e. 0 ~ 1000)