Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-05 07:28:29 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_Motors: coll setup uses actual blade pitch angle

Browse Source
This commit is contained in:
Bill Geyer 2021-10-21 18:03:36 -04:00 committed by Bill Geyer
parent c3442d0143
commit 1a50dce206
6 changed files with 94 additions and 53 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

55
libraries/AP_Motors/AP_MotorsHeli.cpp
View File

@ -44,14 +44,7 @@ const AP_Param::GroupInfo AP_MotorsHeli::var_info[] = {
// @User: Standard
AP_GROUPINFO("COL_MAX", 4, AP_MotorsHeli, _collective_max, AP_MOTORS_HELI_COLLECTIVE_MAX),
// @Param: COL_MID
// @DisplayName: Zero-Thrust Collective Pitch
// @Description: Swash servo position in PWM microseconds corresponding to zero collective pitch (or zero lift for Asymmetrical blades)
// @Range: 1000 2000
// @Units: PWM
// @Increment: 1
// @User: Standard
AP_GROUPINFO("COL_MID", 5, AP_MotorsHeli, _collective_mid, AP_MOTORS_HELI_COLLECTIVE_MID),
// index 5 was COL_MID. Do not use this index in the future.
// @Param: SV_MAN
// @DisplayName: Manual Servo Mode
@ -117,6 +110,42 @@ const AP_Param::GroupInfo AP_MotorsHeli::var_info[] = {
// @User: Standard
AP_GROUPINFO("OPTIONS", 28, AP_MotorsHeli, _heli_options, (uint8_t)HeliOption::USE_LEAKY_I),
// @Param: COL_MIN_DEG
// @DisplayName: Minimum Collective Blade Pitch Angle
// @Description: Minimum collective blade pitch angle in deg that corresponds to the PWM set for minimum collective pitch (H_COL_MIN).
// @Range: -20 0
// @Units: deg
// @Increment: 0.1
// @User: Standard
AP_GROUPINFO("COL_MIN_DEG", 29, AP_MotorsHeli, _collective_min_deg, AP_MOTORS_HELI_COLLECTIVE_MIN_DEG),
// @Param: COL_MAX_DEG
// @DisplayName: Maximum Collective Blade Pitch Angle
// @Description: Maximum collective blade pitch angle in deg that corresponds to the PWM set for maximum collective pitch (H_COL_MAX).
// @Range: 5 20
// @Units: deg
// @Increment: 0.1
// @User: Standard
AP_GROUPINFO("COL_MAX_DEG", 30, AP_MotorsHeli, _collective_max_deg, AP_MOTORS_HELI_COLLECTIVE_MAX_DEG),
// @Param: COL_ZERO_THRST
// @DisplayName: Zero-Thrust Collective Pitch
// @Description: Zero thrust blade collective pitch in degrees.
// @Range: -5 0
// @Units: deg
// @Increment: 0.1
// @User: Standard
AP_GROUPINFO("COL_ZERO_THRST", 31, AP_MotorsHeli, _collective_zero_thrst_deg, 0.0f),
// @Param: COL_LAND_MIN
// @DisplayName: Minimum Landed Collective Blade Pitch
// @Description: Minimum Landed collective blade pitch in degrees for non-manual collective modes (i.e. modes that use altitude hold).
// @Range: -5 0
// @Units: deg
// @Increment: 0.1
// @User: Standard
AP_GROUPINFO("COL_LAND_MIN", 32, AP_MotorsHeli, _collective_land_min_deg, AP_MOTORS_HELI_COLLECTIVE_LAND_MIN),
AP_GROUPEND
};
@ -253,7 +282,7 @@ void AP_MotorsHeli::output_disarmed()
// fixate mid collective
_roll_in = 0.0f;
_pitch_in = 0.0f;
_throttle_filter.reset(_collective_mid_pct);
_throttle_filter.reset(_collective_zero_pct);
_yaw_in = 0.0f;
break;
case SERVO_CONTROL_MODE_MANUAL_MAX:
@ -516,10 +545,10 @@ void AP_MotorsHeli::update_throttle_hover(float dt)
{
if (_collective_hover_learn != HOVER_LEARN_DISABLED) {
// Don't let _collective_hover go below H_COLL_MID
// Don't let _collective_hover go below H_COLL_ZERO_THRST
float curr_collective = get_throttle();
if (curr_collective < _collective_mid_pct) {
curr_collective = _collective_mid_pct;
if (curr_collective < _collective_zero_pct) {
curr_collective = _collective_zero_pct;
}
// we have chosen to constrain the hover collective to be within the range reachable by the third order expo polynomial.
@ -539,7 +568,7 @@ void AP_MotorsHeli::save_params_on_disarm()
// updates the takeoff collective flag
void AP_MotorsHeli::update_takeoff_collective_flag(float coll_out)
{
if (coll_out > _collective_mid_pct + 0.5f * (_collective_hover - _collective_mid_pct)) {
if (coll_out > _collective_zero_pct + 0.5f * (_collective_hover - _collective_zero_pct)) {
_heliflags.takeoff_collective = true;
} else {
_heliflags.takeoff_collective = false;

13
libraries/AP_Motors/AP_MotorsHeli.h
View File

@ -18,11 +18,14 @@
#define AP_MOTORS_HELI_SWASH_CYCLIC_MAX 2500
#define AP_MOTORS_HELI_COLLECTIVE_MIN 1250
#define AP_MOTORS_HELI_COLLECTIVE_MAX 1750
#define AP_MOTORS_HELI_COLLECTIVE_MID 1500
#define AP_MOTORS_HELI_COLLECTIVE_HOVER_DEFAULT 0.5f // the estimated hover throttle, 0 ~ 1
#define AP_MOTORS_HELI_COLLECTIVE_HOVER_TC 10.0f // time constant used to update estimated hover throttle, 0 ~ 1
#define AP_MOTORS_HELI_COLLECTIVE_HOVER_MIN 0.3f // minimum possible hover throttle
#define AP_MOTORS_HELI_COLLECTIVE_HOVER_MAX 0.8f // maximum possible hover throttle
#define AP_MOTORS_HELI_COLLECTIVE_MIN_DEG -10.0f // minimum collective blade pitch angle in deg
#define AP_MOTORS_HELI_COLLECTIVE_MAX_DEG 10.0f // maximum collective blade pitch angle in deg
#define AP_MOTORS_HELI_COLLECTIVE_LAND_MIN -2.0f // minimum landed collective blade pitch angle in deg for modes using althold
// flybar types
#define AP_MOTORS_HELI_NOFLYBAR 0
@ -253,15 +256,19 @@ protected:
AP_Int16 _cyclic_max; // Maximum cyclic angle of the swash plate in centi-degrees
AP_Int16 _collective_min; // Lowest possible servo position for the swashplate
AP_Int16 _collective_max; // Highest possible servo position for the swashplate
AP_Int16 _collective_mid; // Swash servo position corresponding to zero collective pitch (or zero lift for Asymmetrical blades)
AP_Int8 _servo_mode; // Pass radio inputs directly to servos during set-up through mission planner
AP_Int8 _servo_test; // sets number of cycles to test servo movement on bootup
AP_Float _collective_hover; // estimated collective required to hover throttle in the range 0 ~ 1
AP_Int8 _collective_hover_learn; // enable/disabled hover collective learning
AP_Int8 _heli_options; // bitmask for optional features
AP_Float _collective_zero_thrst_deg; // Zero thrust blade collective pitch in degrees
AP_Float _collective_land_min_deg; // Minimum Landed collective blade pitch in degrees for non-manual collective modes (i.e. modes that use altitude hold)
AP_Float _collective_max_deg; // Maximum collective blade pitch angle in deg that corresponds to the PWM set for maximum collective pitch (H_COL_MAX)
AP_Float _collective_min_deg; // Minimum collective blade pitch angle in deg that corresponds to the PWM set for minimum collective pitch (H_COL_MIN)
// internal variables
float _collective_mid_pct = 0.0f; // collective mid parameter value converted to 0 ~ 1 range
float _collective_zero_pct = 0.0f; // collective mid parameter value converted to 0 ~ 1 range
float _collective_land_min_pct = 0.0f; // collective mid parameter value converted to 0 ~ 1 range
uint8_t _servo_test_cycle_counter = 0; // number of test cycles left to run after bootup
motor_frame_type _frame_type;

33
libraries/AP_Motors/AP_MotorsHeli_Dual.cpp
View File

@ -77,14 +77,7 @@ const AP_Param::GroupInfo AP_MotorsHeli_Dual::var_info[] = {
// @User: Standard
AP_GROUPINFO("COL2_MAX", 17, AP_MotorsHeli_Dual, _collective2_max, AP_MOTORS_HELI_DUAL_COLLECTIVE2_MAX),
// @Param: COL2_MID
// @DisplayName: Swash 2 Zero-Thrust Collective Pitch
// @Description: Swash servo position in PWM microseconds corresponding to zero collective pitch for the rear swashplate (or zero lift for Asymmetrical blades)
// @Range: 1000 2000
// @Units: PWM
// @Increment: 1
// @User: Standard
AP_GROUPINFO("COL2_MID", 18, AP_MotorsHeli_Dual, _collective2_mid, AP_MOTORS_HELI_DUAL_COLLECTIVE2_MID),
// Indice 18 was used by COL2_MID and should not be used
// Indice 19 was used by COL_CTRL_DIR and should not be used
@ -382,12 +375,16 @@ void AP_MotorsHeli_Dual::calculate_scalars()
_collective2_max = AP_MOTORS_HELI_DUAL_COLLECTIVE2_MAX;
}
_collective_mid = constrain_int16(_collective_mid, _collective_min, _collective_max);
_collective2_mid = constrain_int16(_collective2_mid, _collective2_min, _collective2_max);
_collective_zero_thrst_deg = constrain_float(_collective_zero_thrst_deg, _collective_min_deg, _collective_max_deg);
// calculate collective mid point as a number from 0 to 1000
_collective_mid_pct = ((float)(_collective_mid-_collective_min))/((float)(_collective_max-_collective_min));
_collective2_mid_pct = ((float)(_collective2_mid-_collective2_min))/((float)(_collective2_max-_collective2_min));
// calculate collective zero thrust point as a number from 0 to 1
_collective_zero_pct = (_collective_zero_thrst_deg-_collective_min_deg)/(_collective_max_deg-_collective_min_deg);
_collective2_zero_pct = _collective_zero_pct;
_collective_land_min_deg = constrain_float(_collective_land_min_deg, _collective_min_deg, _collective_max_deg);
// calculate collective land min point as a number from 0 to 1
_collective_land_min_pct = (_collective_land_min_deg-_collective_min_deg)/(_collective_max_deg-_collective_min_deg);
// configure swashplate 1 and update scalars
_swashplate1.configure();
@ -568,13 +565,13 @@ void AP_MotorsHeli_Dual::move_actuators(float roll_out, float pitch_out, float c
}
// ensure not below landed/landing collective
if (_heliflags.landing_collective && collective_out < _collective_mid_pct) {
collective_out = _collective_mid_pct;
if (_heliflags.landing_collective && collective_out < _collective_land_min_pct) {
collective_out = _collective_land_min_pct;
limit.throttle_lower = true;
}
// updates below mid collective flag
if (collective_out <= _collective_mid_pct) {
if (collective_out <= _collective_land_min_pct) {
_heliflags.below_mid_collective = true;
} else {
_heliflags.below_mid_collective = false;
@ -586,7 +583,7 @@ void AP_MotorsHeli_Dual::move_actuators(float roll_out, float pitch_out, float c
// Set rear collective to midpoint if required
float collective2_out = collective_out;
if (_servo_mode == SERVO_CONTROL_MODE_MANUAL_CENTER) {
collective2_out = _collective2_mid_pct;
collective2_out = _collective2_zero_pct;
}
// if servo output not in manual mode, process pre-compensation factors
@ -598,7 +595,7 @@ void AP_MotorsHeli_Dual::move_actuators(float roll_out, float pitch_out, float c
// for intermeshing, reverse yaw in negative collective region and smoothen transition near zero collective
if (_yaw_rev_expo > 0.01f) {
// yaw_compensation range: (-1,1) S-shaped curve (Logistic Model) 1/(1 + e^kt)
yaw_compensation = 1.0f - (2.0f / (1.0f + powf(2.7182818f , _yaw_rev_expo * (collective_out-_collective_mid_pct))));
yaw_compensation = 1.0f - (2.0f / (1.0f + powf(2.7182818f , _yaw_rev_expo * (collective_out-_collective_zero_pct))));
yaw_out = yaw_out * yaw_compensation;
}
} else {

4
libraries/AP_Motors/AP_MotorsHeli_Dual.h
View File

@ -31,7 +31,6 @@
// default collective min, max and midpoints for the rear swashplate
#define AP_MOTORS_HELI_DUAL_COLLECTIVE2_MIN 1250
#define AP_MOTORS_HELI_DUAL_COLLECTIVE2_MAX 1750
#define AP_MOTORS_HELI_DUAL_COLLECTIVE2_MID 1500
/// @class AP_MotorsHeli_Dual
class AP_MotorsHeli_Dual : public AP_MotorsHeli {
@ -130,7 +129,6 @@ protected:
// parameters
AP_Int16 _collective2_min; // Lowest possible servo position for the rear swashplate
AP_Int16 _collective2_max; // Highest possible servo position for the rear swashplate
AP_Int16 _collective2_mid; // Swash servo position corresponding to zero collective pitch for the rear swashplate (or zero lift for Asymmetrical blades)
AP_Int8 _dual_mode; // which dual mode the heli is
AP_Float _dcp_scaler; // scaling factor applied to the differential-collective-pitch
AP_Float _dcp_yaw_effect; // feed-forward compensation to automatically add yaw input when differential collective pitch is applied.
@ -139,5 +137,5 @@ protected:
AP_Float _yaw_rev_expo; // yaw reverser smoothing exponent, for intermeshing mode only.
// internal variables
float _collective2_mid_pct = 0.0f; // collective mid parameter value for rear swashplate converted to 0 ~ 1 range
float _collective2_zero_pct;
};

21
libraries/AP_Motors/AP_MotorsHeli_Quad.cpp
View File

@ -147,10 +147,15 @@ void AP_MotorsHeli_Quad::calculate_scalars()
_collective_max = AP_MOTORS_HELI_COLLECTIVE_MAX;
}
_collective_mid = constrain_int16(_collective_mid, _collective_min, _collective_max);
_collective_zero_thrst_deg = constrain_float(_collective_zero_thrst_deg, _collective_min_deg, _collective_max_deg);
// calculate collective mid point as a number from 0 to 1000
_collective_mid_pct = ((float)(_collective_mid-_collective_min))/((float)(_collective_max-_collective_min));
// calculate collective zero thrust point as a number from 0 to 1
_collective_zero_pct = (_collective_zero_thrst_deg-_collective_min_deg)/(_collective_max_deg-_collective_min_deg);
_collective_land_min_deg = constrain_float(_collective_land_min_deg, _collective_min_deg, _collective_max_deg);
// calculate collective land min point as a number from 0 to 1
_collective_land_min_pct = (_collective_land_min_deg-_collective_min_deg)/(_collective_max_deg-_collective_min_deg);
// calculate factors based on swash type and servo position
calculate_roll_pitch_collective_factors();
@ -238,13 +243,13 @@ void AP_MotorsHeli_Quad::move_actuators(float roll_out, float pitch_out, float c
}
// ensure not below landed/landing collective
if (_heliflags.landing_collective && collective_out < _collective_mid_pct) {
collective_out = _collective_mid_pct;
if (_heliflags.landing_collective && collective_out < _collective_land_min_pct) {
collective_out = _collective_land_min_pct;
limit.throttle_lower = true;
}
// updates below mid collective flag
if (collective_out <= _collective_mid_pct) {
if (collective_out <= _collective_land_min_pct) {
_heliflags.below_mid_collective = true;
} else {
_heliflags.below_mid_collective = false;
@ -263,7 +268,7 @@ void AP_MotorsHeli_Quad::move_actuators(float roll_out, float pitch_out, float c
_main_rotor.set_collective(fabsf(collective_out));
// rescale collective for overhead calc
collective_out -= _collective_mid_pct;
collective_out -= _collective_zero_pct;
// reserve some collective for attitude control
collective_out *= collective_range;
@ -304,7 +309,7 @@ void AP_MotorsHeli_Quad::move_actuators(float roll_out, float pitch_out, float c
for (uint8_t i=0; i<AP_MOTORS_HELI_QUAD_NUM_MOTORS; i++) {
// scale output to 0 to 1
_out[i] += _collective_mid_pct;
_out[i] += _collective_zero_pct;
// scale output to -1 to 1 for servo output
_out[i] = _out[i] * 2.0f - 1.0f;
}

21
libraries/AP_Motors/AP_MotorsHeli_Single.cpp
View File

@ -319,15 +319,20 @@ void AP_MotorsHeli_Single::calculate_armed_scalars()
// calculate_scalars - recalculates various scalers used.
void AP_MotorsHeli_Single::calculate_scalars()
{
// range check collective min, max and mid
// range check collective min, max and zero
if( _collective_min >= _collective_max ) {
_collective_min = AP_MOTORS_HELI_COLLECTIVE_MIN;
_collective_max = AP_MOTORS_HELI_COLLECTIVE_MAX;
}
_collective_mid = constrain_int16(_collective_mid, _collective_min, _collective_max);
_collective_zero_thrst_deg = constrain_float(_collective_zero_thrst_deg, _collective_min_deg, _collective_max_deg);
// calculate collective mid point as a number from 0 to 1
_collective_mid_pct = ((float)(_collective_mid-_collective_min))/((float)(_collective_max-_collective_min));
// calculate collective zero thrust point as a number from 0 to 1
_collective_zero_pct = (_collective_zero_thrst_deg-_collective_min_deg)/(_collective_max_deg-_collective_min_deg);
_collective_land_min_deg = constrain_float(_collective_land_min_deg, _collective_min_deg, _collective_max_deg);
// calculate collective land min point as a number from 0 to 1
_collective_land_min_pct = (_collective_land_min_deg-_collective_min_deg)/(_collective_max_deg-_collective_min_deg);
// configure swashplate and update scalars
_swashplate.configure();
@ -441,14 +446,14 @@ void AP_MotorsHeli_Single::move_actuators(float roll_out, float pitch_out, float
}
// ensure not below landed/landing collective
if (_heliflags.landing_collective && collective_out < _collective_mid_pct && !_heliflags.in_autorotation) {
collective_out = _collective_mid_pct;
if (_heliflags.landing_collective && collective_out < _collective_land_min_pct && !_heliflags.in_autorotation) {
collective_out = _collective_land_min_pct;
limit.throttle_lower = true;
}
// updates below mid collective flag
if (collective_out <= _collective_mid_pct) {
if (collective_out <= _collective_land_min_pct) {
_heliflags.below_mid_collective = true;
} else {
_heliflags.below_mid_collective = false;
@ -466,7 +471,7 @@ void AP_MotorsHeli_Single::move_actuators(float roll_out, float pitch_out, float
// sanity check collective_yaw_effect
_collective_yaw_effect = constrain_float(_collective_yaw_effect, -AP_MOTORS_HELI_SINGLE_COLYAW_RANGE, AP_MOTORS_HELI_SINGLE_COLYAW_RANGE);
// the 4.5 scaling factor is to bring the values in line with previous releases
yaw_offset = _collective_yaw_effect * fabsf(collective_out - _collective_mid_pct) / 4.5f;
yaw_offset = _collective_yaw_effect * fabsf(collective_out - _collective_zero_pct) / 4.5f;
}
} else {
yaw_offset = 0.0f;