Skip to content

Commit

Permalink
AP_Motors: add support for offsetting collective when landed
Browse files Browse the repository at this point in the history
  • Loading branch information
@bnsgeyer
bnsgeyer committed Dec 20, 2024
1 parent c2036a0 commit a85917c
Show file tree
Hide file tree
Showing 4 changed files with 60 additions and 12 deletions.
19 changes: 15 additions & 4 deletions libraries/AP_Motors/AP_MotorsHeli.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -146,6 +146,17 @@ const AP_Param::GroupInfo AP_MotorsHeli::var_info[] = {
// @User: Standard
AP_GROUPINFO("COL_LAND_MIN", 32, AP_MotorsHeli, _collective_land_min_deg, AP_MOTORS_HELI_COLLECTIVE_LAND_MIN),

// index 33 reserved for AROT coming in 4.6

// @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
};

Expand Down Expand Up @@ -358,7 +369,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);
Expand All @@ -380,7 +391,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);
Expand All @@ -402,7 +413,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);
Expand All @@ -422,7 +433,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);
Expand Down
10 changes: 9 additions & 1 deletion libraries/AP_Motors/AP_MotorsHeli.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down Expand Up @@ -141,7 +142,10 @@ class AP_MotorsHeli : public AP_Motors {

// 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; }

Expand Down Expand Up @@ -263,6 +267,7 @@ class AP_MotorsHeli : public AP_Motors {
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
Expand All @@ -278,11 +283,14 @@ class AP_MotorsHeli : public AP_Motors {
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;
Expand Down
21 changes: 18 additions & 3 deletions libraries/AP_Motors/AP_MotorsHeli_Dual.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -277,13 +277,15 @@ 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
_collective_zero_thrust_pct = (_collective_zero_thrust_deg-_collective_min_deg)/(_collective_max_deg-_collective_min_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);
_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;
Expand Down Expand Up @@ -430,14 +432,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 && !_heliflags.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;
Expand Down
22 changes: 18 additions & 4 deletions libraries/AP_Motors/AP_MotorsHeli_Single.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -300,13 +300,15 @@ 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
_collective_zero_thrust_pct = (_collective_zero_thrust_deg-_collective_min_deg)/(_collective_max_deg-_collective_min_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);
_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;
Expand Down Expand Up @@ -413,15 +415,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 && !_heliflags.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 && !_heliflags.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;
Expand Down

0 comments on commit a85917c

Please sign in to comment.