Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_Motors: add support for offsetting collective when landed

This commit is contained in:
bnsgeyer 2024-11-28 23:30:04 -05:00
parent 9d381f17a6
commit 8e3d2a3dcf
4 changed files with 58 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

17
libraries/AP_Motors/AP_MotorsHeli.cpp
View File

@ -152,6 +152,15 @@ const AP_Param::GroupInfo AP_MotorsHeli::var_info[] = {
// @Path: ../AC_Autorotation/RSC_Autorotation.cpp
AP_SUBGROUPINFO(_main_rotor.autorotation, "RSC_AROT_", 33, AP_MotorsHeli, RSC_Autorotation),
// @Param: COL_LAND_OFSET
// @DisplayName: Offset of Collective Blade Pitch when Landed
// @Description: Offset of the collective blade pitch angle when landed in degrees for non-manual collective modes (i.e. modes that use altitude hold).
// @Range: -2 2
// @Units: deg
// @Increment: 0.1
// @User: Standard
AP_GROUPINFO("COL_LAND_OFSET", 34, AP_MotorsHeli, _collective_land_offset_deg, 0.0),
AP_GROUPEND
};
@ -349,7 +358,7 @@ void AP_MotorsHeli::output_logic()
case SpoolState::GROUND_IDLE: {
// Motors should be stationary or at ground idle.
// set limits flags
if (_heliflags.land_complete && !using_leaky_integrator()) {
if ((_heliflags.land_complete || _heliflags.land_complete_maybe) && !using_leaky_integrator()) {
set_limit_flag_pitch_roll_yaw(true);
} else {
set_limit_flag_pitch_roll_yaw(false);
@ -371,7 +380,7 @@ void AP_MotorsHeli::output_logic()
// Servos should exhibit normal flight behavior.
// set limits flags
if (_heliflags.land_complete && !using_leaky_integrator()) {
if ((_heliflags.land_complete || _heliflags.land_complete_maybe) && !using_leaky_integrator()) {
set_limit_flag_pitch_roll_yaw(true);
} else {
set_limit_flag_pitch_roll_yaw(false);
@ -393,7 +402,7 @@ void AP_MotorsHeli::output_logic()
// Servos should exhibit normal flight behavior.
// set limits flags
if (_heliflags.land_complete && !using_leaky_integrator()) {
if ((_heliflags.land_complete || _heliflags.land_complete_maybe) && !using_leaky_integrator()) {
set_limit_flag_pitch_roll_yaw(true);
} else {
set_limit_flag_pitch_roll_yaw(false);
@ -413,7 +422,7 @@ void AP_MotorsHeli::output_logic()
// Servos should exhibit normal flight behavior.
// set limits flags
if (_heliflags.land_complete && !using_leaky_integrator()) {
if ((_heliflags.land_complete || _heliflags.land_complete_maybe) && !using_leaky_integrator()) {
set_limit_flag_pitch_roll_yaw(true);
} else {
set_limit_flag_pitch_roll_yaw(false);

10
libraries/AP_Motors/AP_MotorsHeli.h
View File

@ -25,6 +25,7 @@
#define AP_MOTORS_HELI_COLLECTIVE_MIN_DEG -90.0f // minimum collective blade pitch angle in deg
#define AP_MOTORS_HELI_COLLECTIVE_MAX_DEG 90.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
#define AP_MOTORS_HELI_COLLECTIVE_FILTER_FREQ 2.0f // time constant used to update estimated hover throttle, 0 ~ 1
// flybar types
@ -132,7 +133,10 @@ public:
// set land complete flag
void set_land_complete(bool landed) { _heliflags.land_complete = landed; }
// set land complete maybe flag
void set_land_complete_maybe(bool landed) { _heliflags.land_complete_maybe = landed; }
//return zero lift collective position
float get_coll_mid() const { return _collective_zero_thrust_pct; }
@ -254,6 +258,7 @@ protected:
uint8_t below_land_min_coll : 1; // true if collective is below H_COL_LAND_MIN
uint8_t rotor_spooldown_complete : 1; // true if the rotors have spooled down completely
uint8_t start_engine : 1; // true if turbine start RC option is initiated
uint8_t land_complete_maybe : 1; // true if aircraft is landed_maybe
} _heliflags;
// parameters
@ -269,11 +274,14 @@ protected:
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)
AP_Float _collective_land_offset_deg;// Offset of Landed collective blade pitch in degrees for non-manual collective modes (i.e. modes that use altitude hold)
// internal variables
float _collective_zero_thrust_pct; // collective zero thrutst parameter value converted to 0 ~ 1 range
float _collective_land_min_pct; // collective land min parameter value converted to 0 ~ 1 range
float _collective_land_offset_pct; // collective land offset parameter value converted to fraction of collective range
uint8_t _servo_test_cycle_counter = 0; // number of test cycles left to run after bootup
float _collective_offset_landed; // current value of collective offset
motor_frame_type _frame_type;
motor_frame_class _frame_class;

21
libraries/AP_Motors/AP_MotorsHeli_Dual.cpp
View File

@ -269,6 +269,7 @@ void AP_MotorsHeli_Dual::calculate_scalars()
_collective_zero_thrust_deg.set(constrain_float(_collective_zero_thrust_deg, _collective_min_deg, _collective_max_deg));
_collective_land_min_deg.set(constrain_float(_collective_land_min_deg, _collective_min_deg, _collective_max_deg));
_collective_land_offset_deg.set(constrain_float(_collective_land_offset_deg, 0.0, _collective_zero_thrust_deg-_collective_land_min_deg));
if (!is_equal((float)_collective_max_deg, (float)_collective_min_deg)) {
// calculate collective zero thrust point as a number from 0 to 1
@ -276,6 +277,7 @@ void AP_MotorsHeli_Dual::calculate_scalars()
// 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);
_collective_land_offset_pct = _collective_land_offset_deg/(_collective_max_deg-_collective_min_deg);
} else {
_collective_zero_thrust_pct = 0.0f;
_collective_land_min_pct = 0.0f;
@ -420,14 +422,27 @@ void AP_MotorsHeli_Dual::move_actuators(float roll_out, float pitch_out, float c
limit.throttle_upper = true;
}
// ensure not below landed/landing collective
if (_heliflags.landing_collective && collective_out < _collective_land_min_pct) {
// In non-manual collective modes, ensure not below landed/landing collective. If collective offset is used, don't allow negative
// collective offsets. Only allow positive collective offsets to keep collective above landed collective.
if (_heliflags.land_complete && _heliflags.landing_collective && collective_out <= _collective_land_min_pct + _collective_offset_landed) {
float alpha = _dt / (_dt + 1.0/(2.0 * M_PI * AP_MOTORS_HELI_COLLECTIVE_FILTER_FREQ)); //calculate LP filter alpha for 2 hz cutoff freq
_collective_offset_landed += alpha * (_collective_land_offset_pct - _collective_offset_landed);
// constrain collective_out so it never goes above collective zero thrust
collective_out = constrain_float((_collective_land_min_pct + _collective_offset_landed), _collective_land_min_pct, _collective_zero_thrust_pct);
limit.throttle_lower = true;
} else if (_heliflags.landing_collective && collective_out < _collective_land_min_pct && !_main_rotor.in_autorotation()) {
collective_out = _collective_land_min_pct;
limit.throttle_lower = true;
} else {
_collective_offset_landed = 0.0;
}
// updates below land min collective flag
if (collective_out <= _collective_land_min_pct) {
float collective_land_min = _collective_land_min_pct;
if (_heliflags.land_complete && _heliflags.landing_collective) {
collective_land_min += _collective_offset_landed;
}
if (collective_out <= collective_land_min) {
_heliflags.below_land_min_coll = true;
} else {
_heliflags.below_land_min_coll = false;

21
libraries/AP_Motors/AP_MotorsHeli_Single.cpp
View File

@ -284,6 +284,7 @@ void AP_MotorsHeli_Single::calculate_scalars()
_collective_zero_thrust_deg.set(constrain_float(_collective_zero_thrust_deg, _collective_min_deg, _collective_max_deg));
_collective_land_min_deg.set(constrain_float(_collective_land_min_deg, _collective_min_deg, _collective_max_deg));
_collective_land_offset_deg.set(constrain_float(_collective_land_offset_deg, 0.0, _collective_zero_thrust_deg-_collective_land_min_deg));
if (!is_equal((float)_collective_max_deg, (float)_collective_min_deg)) {
// calculate collective zero thrust point as a number from 0 to 1
@ -291,6 +292,7 @@ void AP_MotorsHeli_Single::calculate_scalars()
// 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);
_collective_land_offset_pct = _collective_land_offset_deg/(_collective_max_deg-_collective_min_deg);
} else {
_collective_zero_thrust_pct = 0.0f;
_collective_land_min_pct = 0.0f;
@ -387,14 +389,27 @@ void AP_MotorsHeli_Single::move_actuators(float roll_out, float pitch_out, float
limit.throttle_upper = true;
}
// ensure not below landed/landing collective
if (_heliflags.landing_collective && collective_out < _collective_land_min_pct && !_main_rotor.in_autorotation()) {
// In non-manual collective modes, ensure not below landed/landing collective. If collective offset is used, don't allow negative
// collective offsets. Only allow positive collective offsets to keep collective above landed collective.
if (_heliflags.land_complete && _heliflags.landing_collective && collective_out <= _collective_land_min_pct + _collective_offset_landed) {
float alpha = _dt / (_dt + 1.0/(2.0 * M_PI * AP_MOTORS_HELI_COLLECTIVE_FILTER_FREQ)); //calculate LP filter alpha for 2 hz cutoff freq
_collective_offset_landed += alpha * (_collective_land_offset_pct - _collective_offset_landed);
// constrain collective_out so it never goes above collective zero thrust
collective_out = constrain_float((_collective_land_min_pct + _collective_offset_landed), _collective_land_min_pct, _collective_zero_thrust_pct);
limit.throttle_lower = true;
} else if (_heliflags.landing_collective && collective_out < _collective_land_min_pct && !_main_rotor.in_autorotation()) {
collective_out = _collective_land_min_pct;
limit.throttle_lower = true;
} else {
_collective_offset_landed = 0.0;
}
// updates below land min collective flag
if (collective_out <= _collective_land_min_pct) {
float collective_land_min = _collective_land_min_pct;
if (_heliflags.land_complete && _heliflags.landing_collective) {
collective_land_min += _collective_offset_landed;
}
if (collective_out <= collective_land_min) {
_heliflags.below_land_min_coll = true;
} else {
_heliflags.below_land_min_coll = false;