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

AP_Motors: Heli: Single: tail type tidyup

This commit is contained in:
Iampete1 2023-11-22 14:07:37 +00:00 committed by Andrew Tridgell
parent 3dd27b7ac5
commit 1c8ab3853c
2 changed files with 148 additions and 96 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

198
libraries/AP_Motors/AP_MotorsHeli_Single.cpp
View File

@ -31,7 +31,7 @@ const AP_Param::GroupInfo AP_MotorsHeli_Single::var_info[] = {
// @Description: Tail type selection. Simpler yaw controller used if external gyro is selected. Direct Drive Variable Pitch is used for tails that have a motor that is governed at constant speed by an ESC. Tail pitch is still accomplished with a servo. Direct Drive Fixed Pitch (DDFP) CW is used for helicopters with a rotor that spins clockwise when viewed from above. Direct Drive Fixed Pitch (DDFP) CCW is used for helicopters with a rotor that spins counter clockwise when viewed from above. In both DDFP cases, no servo is used for the tail and the tail motor esc is controlled by the yaw axis.
// @Values: 0:Servo only,1:Servo with ExtGyro,2:DirectDrive VarPitch,3:DirectDrive FixedPitch CW,4:DirectDrive FixedPitch CCW,5:DDVP with external governor
// @User: Standard
AP_GROUPINFO("TAIL_TYPE", 4, AP_MotorsHeli_Single, _tail_type, AP_MOTORS_HELI_SINGLE_TAILTYPE_SERVO),
AP_GROUPINFO("TAIL_TYPE", 4, AP_MotorsHeli_Single, _tail_type, float(TAIL_TYPE::SERVO)),
// Indice 5 was used by SWASH_TYPE and should not be used
@ -210,25 +210,33 @@ void AP_MotorsHeli_Single::init_outputs()
// initialize main rotor servo
_main_rotor.init_servo();
if (_tail_type == AP_MOTORS_HELI_SINGLE_TAILTYPE_DIRECTDRIVE_VARPITCH || _tail_type == AP_MOTORS_HELI_SINGLE_TAILTYPE_DIRECTDRIVE_VARPIT_EXT_GOV) {
_tail_rotor.init_servo();
} else if (_tail_type == AP_MOTORS_HELI_SINGLE_TAILTYPE_SERVO_EXTGYRO) {
// external gyro output
add_motor_num(AP_MOTORS_HELI_SINGLE_EXTGYRO);
}
}
if (_tail_type == AP_MOTORS_HELI_SINGLE_TAILTYPE_SERVO_EXTGYRO) {
// External Gyro uses PWM output thus servo endpoints are forced
SRV_Channels::set_output_min_max(SRV_Channels::get_motor_function(AP_MOTORS_HELI_SINGLE_EXTGYRO), 1000, 2000);
}
if (_tail_type == AP_MOTORS_HELI_SINGLE_TAILTYPE_DIRECTDRIVE_FIXEDPITCH_CW || _tail_type == AP_MOTORS_HELI_SINGLE_TAILTYPE_DIRECTDRIVE_FIXEDPITCH_CCW) {
switch (get_tail_type()) {
case TAIL_TYPE::DIRECTDRIVE_FIXEDPITCH_CW:
case TAIL_TYPE::DIRECTDRIVE_FIXEDPITCH_CCW:
// DDFP tails use range as it is easier to ignore servo trim in making for simple implementation of thrust linearisation.
SRV_Channels::set_range(SRV_Channel::k_motor4, 1.0f);
} else if (_tail_type != AP_MOTORS_HELI_SINGLE_TAILTYPE_DIRECTDRIVE_VARPITCH && _tail_type != AP_MOTORS_HELI_SINGLE_TAILTYPE_DIRECTDRIVE_VARPIT_EXT_GOV) {
break;
case TAIL_TYPE::DIRECTDRIVE_VARPITCH:
case TAIL_TYPE::DIRECTDRIVE_VARPIT_EXT_GOV:
_tail_rotor.init_servo();
break;
case TAIL_TYPE::SERVO_EXTGYRO:
// external gyro output
add_motor_num(AP_MOTORS_HELI_SINGLE_EXTGYRO);
// External Gyro uses PWM output thus servo endpoints are forced
SRV_Channels::set_output_min_max(SRV_Channels::get_motor_function(AP_MOTORS_HELI_SINGLE_EXTGYRO), 1000, 2000);
FALLTHROUGH;
case TAIL_TYPE::SERVO:
default:
// yaw servo is an angle from -4500 to 4500
SRV_Channels::set_angle(SRV_Channel::k_motor4, YAW_SERVO_MAX_ANGLE);
break;
}
}
set_initialised_ok(_frame_class == MOTOR_FRAME_HELI);
@ -266,13 +274,13 @@ void AP_MotorsHeli_Single::calculate_armed_scalars()
_main_rotor.set_autorotation_flag(_heliflags.in_autorotation);
// set bailout ramp time
_main_rotor.use_bailout_ramp_time(_heliflags.enable_bailout);
if (_tail_type == AP_MOTORS_HELI_SINGLE_TAILTYPE_DIRECTDRIVE_VARPITCH || _tail_type == AP_MOTORS_HELI_SINGLE_TAILTYPE_DIRECTDRIVE_VARPIT_EXT_GOV) {
if (use_tail_RSC()) {
_tail_rotor.set_autorotation_flag(_heliflags.in_autorotation);
_tail_rotor.use_bailout_ramp_time(_heliflags.enable_bailout);
}
} else {
_main_rotor.set_autorotation_flag(false);
if (_tail_type == AP_MOTORS_HELI_SINGLE_TAILTYPE_DIRECTDRIVE_VARPITCH || _tail_type == AP_MOTORS_HELI_SINGLE_TAILTYPE_DIRECTDRIVE_VARPIT_EXT_GOV) {
if (use_tail_RSC()) {
_tail_rotor.set_autorotation_flag(false);
}
}
@ -310,7 +318,7 @@ void AP_MotorsHeli_Single::calculate_scalars()
calculate_armed_scalars();
// send setpoints to DDVP rotor controller and trigger recalculation of scalars
if (_tail_type == AP_MOTORS_HELI_SINGLE_TAILTYPE_DIRECTDRIVE_VARPITCH || _tail_type == AP_MOTORS_HELI_SINGLE_TAILTYPE_DIRECTDRIVE_VARPIT_EXT_GOV) {
if (use_tail_RSC()) {
_tail_rotor.set_control_mode(ROTOR_CONTROL_MODE_SETPOINT);
_tail_rotor.set_ramp_time(_main_rotor._ramp_time.get());
_tail_rotor.set_runup_time(_main_rotor._runup_time.get());
@ -370,8 +378,6 @@ void AP_MotorsHeli_Single::update_motor_control(RotorControlState state)
//
void AP_MotorsHeli_Single::move_actuators(float roll_out, float pitch_out, float coll_in, float yaw_out)
{
float yaw_offset = 0.0f;
// initialize limits flag
limit.throttle_lower = false;
limit.throttle_upper = false;
@ -422,26 +428,8 @@ void AP_MotorsHeli_Single::move_actuators(float roll_out, float pitch_out, float
// updates takeoff collective flag based on 50% hover collective
update_takeoff_collective_flag(collective_out);
// if servo output not in manual mode and heli is not in autorotation, process pre-compensation factors
if (_servo_mode == SERVO_CONTROL_MODE_AUTOMATED && !_heliflags.in_autorotation) {
// rudder feed forward based on collective
// the feed-forward is not required when the motor is stopped or at idle, and thus not creating torque
// also not required if we are using external gyro
if ((get_control_output() > _main_rotor.get_idle_output()) && _tail_type != AP_MOTORS_HELI_SINGLE_TAILTYPE_SERVO_EXTGYRO) {
// sanity check collective_yaw_scale
_collective_yaw_scale.set(constrain_float(_collective_yaw_scale, -AP_MOTORS_HELI_SINGLE_COLYAW_RANGE, AP_MOTORS_HELI_SINGLE_COLYAW_RANGE));
// This feedforward compensation follows the hover performance theory that hover power required
// is a function of gross weight to the 3/2 power
yaw_offset = _collective_yaw_scale * powf(fabsf(collective_out - _collective_zero_thrust_pct),1.5f);
// Add yaw trim for DDFP tails
if (_tail_type == AP_MOTORS_HELI_SINGLE_TAILTYPE_DIRECTDRIVE_FIXEDPITCH_CW || _tail_type == AP_MOTORS_HELI_SINGLE_TAILTYPE_DIRECTDRIVE_FIXEDPITCH_CCW) {
yaw_offset += _yaw_trim.get();
}
}
} else {
yaw_offset = 0.0f;
}
// Get yaw offset required to cancel out steady state main rotor torque
const float yaw_offset = get_yaw_offset(collective_out);
// feed power estimate into main rotor controller
// ToDo: include tail rotor power?
@ -475,6 +463,34 @@ void AP_MotorsHeli_Single::move_yaw(float yaw_out)
_servo4_out = yaw_out;
}
// Get yaw offset required to cancel out steady state main rotor torque
float AP_MotorsHeli_Single::get_yaw_offset(float collective)
{
if ((get_tail_type() == TAIL_TYPE::SERVO_EXTGYRO) || (_servo_mode != SERVO_CONTROL_MODE_AUTOMATED)) {
// Not in direct control of tail with external gyro or manual servo mode
return 0.0;
}
if (_heliflags.in_autorotation || (get_control_output() <= _main_rotor.get_idle_output())) {
// Motor is stopped or at idle, and thus not creating torque
return 0.0;
}
// sanity check collective_yaw_scale
_collective_yaw_scale.set(constrain_float(_collective_yaw_scale, -AP_MOTORS_HELI_SINGLE_COLYAW_RANGE, AP_MOTORS_HELI_SINGLE_COLYAW_RANGE));
// This feedforward compensation follows the hover performance theory that hover power required
// is a function of gross weight to the 3/2 power
float yaw_offset = _collective_yaw_scale * powf(fabsf(collective - _collective_zero_thrust_pct),1.5f);
// Add yaw trim for DDFP tails
if (have_DDFP_tail()) {
yaw_offset += _yaw_trim.get();
}
return yaw_offset;
}
void AP_MotorsHeli_Single::output_to_motors()
{
if (!initialised_ok()) {
@ -484,63 +500,73 @@ void AP_MotorsHeli_Single::output_to_motors()
// Write swashplate outputs
_swashplate.output();
if (_tail_type != AP_MOTORS_HELI_SINGLE_TAILTYPE_DIRECTDRIVE_FIXEDPITCH_CW && _tail_type != AP_MOTORS_HELI_SINGLE_TAILTYPE_DIRECTDRIVE_FIXEDPITCH_CCW) {
rc_write_angle(AP_MOTORS_MOT_4, _servo4_out * YAW_SERVO_MAX_ANGLE);
}
if (_tail_type == AP_MOTORS_HELI_SINGLE_TAILTYPE_SERVO_EXTGYRO) {
// output gain to exernal gyro
if (_acro_tail && _ext_gyro_gain_acro > 0) {
rc_write(AP_MOTORS_HELI_SINGLE_EXTGYRO, 1000 + _ext_gyro_gain_acro);
} else {
rc_write(AP_MOTORS_HELI_SINGLE_EXTGYRO, 1000 + _ext_gyro_gain_std);
}
}
if (_tail_type == AP_MOTORS_HELI_SINGLE_TAILTYPE_DIRECTDRIVE_FIXEDPITCH_CCW) {
_servo4_out = -_servo4_out;
}
if (_tail_type == AP_MOTORS_HELI_SINGLE_TAILTYPE_DIRECTDRIVE_FIXEDPITCH_CW || _tail_type == AP_MOTORS_HELI_SINGLE_TAILTYPE_DIRECTDRIVE_FIXEDPITCH_CCW) {
// calc filtered battery voltage and lift_max
thr_lin.update_lift_max_from_batt_voltage();
}
// Warning: This spool state logic is what prevents DDFP tails from actuating when doing H_SV_MAN and H_SV_TEST tests
// Output main rotor
switch (_spool_state) {
case SpoolState::SHUT_DOWN:
// sends minimum values out to the motors
update_motor_control(ROTOR_CONTROL_STOP);
if (_tail_type == AP_MOTORS_HELI_SINGLE_TAILTYPE_DIRECTDRIVE_FIXEDPITCH_CW || _tail_type == AP_MOTORS_HELI_SINGLE_TAILTYPE_DIRECTDRIVE_FIXEDPITCH_CCW) {
// Set DDFP to servo min
output_to_ddfp_tail(0.0);
}
break;
case SpoolState::GROUND_IDLE:
// sends idle output to motors when armed. rotor could be static or turning (autorotation)
update_motor_control(ROTOR_CONTROL_IDLE);
if (_tail_type == AP_MOTORS_HELI_SINGLE_TAILTYPE_DIRECTDRIVE_FIXEDPITCH_CW || _tail_type == AP_MOTORS_HELI_SINGLE_TAILTYPE_DIRECTDRIVE_FIXEDPITCH_CCW) {
// Set DDFP to servo min
output_to_ddfp_tail(0.0);
}
break;
case SpoolState::SPOOLING_UP:
case SpoolState::THROTTLE_UNLIMITED:
// set motor output based on thrust requests
update_motor_control(ROTOR_CONTROL_ACTIVE);
if (_tail_type == AP_MOTORS_HELI_SINGLE_TAILTYPE_DIRECTDRIVE_FIXEDPITCH_CW || _tail_type == AP_MOTORS_HELI_SINGLE_TAILTYPE_DIRECTDRIVE_FIXEDPITCH_CCW) {
// Operate DDFP to between DDFP_SPIN_MIN and DDFP_SPIN_MAX using thrust linearisation
output_to_ddfp_tail(thr_lin.thrust_to_actuator(_servo4_out));
}
break;
case SpoolState::SPOOLING_DOWN:
// sends idle output to motors and wait for rotor to stop
update_motor_control(ROTOR_CONTROL_IDLE);
if (_tail_type == AP_MOTORS_HELI_SINGLE_TAILTYPE_DIRECTDRIVE_FIXEDPITCH_CW || _tail_type == AP_MOTORS_HELI_SINGLE_TAILTYPE_DIRECTDRIVE_FIXEDPITCH_CCW){
break;
}
// Output tail rotor
switch (get_tail_type()) {
case TAIL_TYPE::DIRECTDRIVE_FIXEDPITCH_CCW:
// Invert output for CCW tail
_servo4_out *= -1.0;
FALLTHROUGH;
case TAIL_TYPE::DIRECTDRIVE_FIXEDPITCH_CW: {
// calc filtered battery voltage and lift_max
thr_lin.update_lift_max_from_batt_voltage();
// Only throttle up if in active spool state
switch (_spool_state) {
case AP_Motors::SpoolState::SHUT_DOWN:
case AP_Motors::SpoolState::GROUND_IDLE:
case AP_Motors::SpoolState::SPOOLING_DOWN:
// Set DDFP to servo min
output_to_ddfp_tail(0.0);
break;
case AP_Motors::SpoolState::SPOOLING_UP:
case AP_Motors::SpoolState::THROTTLE_UNLIMITED:
// Operate DDFP to between DDFP_SPIN_MIN and DDFP_SPIN_MAX using thrust linearisation
output_to_ddfp_tail(thr_lin.thrust_to_actuator(_servo4_out));
break;
}
break;
}
case TAIL_TYPE::SERVO_EXTGYRO:
// output gain to external gyro
if (_acro_tail && _ext_gyro_gain_acro > 0) {
rc_write(AP_MOTORS_HELI_SINGLE_EXTGYRO, 1000 + _ext_gyro_gain_acro);
} else {
rc_write(AP_MOTORS_HELI_SINGLE_EXTGYRO, 1000 + _ext_gyro_gain_std);
}
FALLTHROUGH;
case TAIL_TYPE::SERVO:
case TAIL_TYPE::DIRECTDRIVE_VARPITCH:
case TAIL_TYPE::DIRECTDRIVE_VARPIT_EXT_GOV:
default:
rc_write_angle(AP_MOTORS_MOT_4, _servo4_out * YAW_SERVO_MAX_ANGLE);
break;
}
}
// handle output limit flags and send throttle to servos lib
@ -655,9 +681,7 @@ void AP_MotorsHeli_Single::heli_motors_param_conversions(void)
// Previous DDFP configs used servo trim for setting the yaw trim, which no longer works with thrust linearisation. Convert servo trim
// to H_YAW_TRIM. Default thrust linearisation gives linear thrust to throttle relationship to preserve previous setup behaviours so
// we can assume linear relationship in the conversion.
if ((_tail_type == AP_MOTORS_HELI_SINGLE_TAILTYPE_DIRECTDRIVE_FIXEDPITCH_CW ||
_tail_type == AP_MOTORS_HELI_SINGLE_TAILTYPE_DIRECTDRIVE_FIXEDPITCH_CCW) &&
!_yaw_trim.configured()) {
if (have_DDFP_tail() && !_yaw_trim.configured()) {
SRV_Channel *c = SRV_Channels::get_channel_for(SRV_Channel::k_motor4);
if (c != nullptr) {
@ -673,3 +697,19 @@ void AP_MotorsHeli_Single::heli_motors_param_conversions(void)
_yaw_trim.save();
}
}
// Helper to return true for direct drive fixed pitch tail, either CW or CCW
bool AP_MotorsHeli_Single::have_DDFP_tail() const
{
const TAIL_TYPE type = get_tail_type();
return (type == TAIL_TYPE::DIRECTDRIVE_FIXEDPITCH_CW) ||
(type == TAIL_TYPE::DIRECTDRIVE_FIXEDPITCH_CCW);
}
// Helper to return true if the tail RSC should be used
bool AP_MotorsHeli_Single::use_tail_RSC() const
{
const TAIL_TYPE type = get_tail_type();
return (type == TAIL_TYPE::DIRECTDRIVE_VARPITCH) ||
(type == TAIL_TYPE::DIRECTDRIVE_VARPIT_EXT_GOV);
}

34
libraries/AP_Motors/AP_MotorsHeli_Single.h
View File

@ -14,15 +14,6 @@
#define AP_MOTORS_HELI_SINGLE_EXTGYRO CH_7
#define AP_MOTORS_HELI_SINGLE_TAILRSC CH_7
// tail types
#define AP_MOTORS_HELI_SINGLE_TAILTYPE_SERVO 0
#define AP_MOTORS_HELI_SINGLE_TAILTYPE_SERVO_EXTGYRO 1
#define AP_MOTORS_HELI_SINGLE_TAILTYPE_DIRECTDRIVE_VARPITCH 2
#define AP_MOTORS_HELI_SINGLE_TAILTYPE_DIRECTDRIVE_FIXEDPITCH_CW 3
#define AP_MOTORS_HELI_SINGLE_TAILTYPE_DIRECTDRIVE_FIXEDPITCH_CCW 4
#define AP_MOTORS_HELI_SINGLE_TAILTYPE_DIRECTDRIVE_VARPIT_EXT_GOV 5
// direct-drive variable pitch defaults
#define AP_MOTORS_HELI_SINGLE_DDVP_SPEED_DEFAULT 50
@ -72,8 +63,8 @@ public:
// has_flybar - returns true if we have a mechical flybar
bool has_flybar() const override { return _flybar_mode; }
// supports_yaw_passthrought - returns true if we support yaw passthrough
bool supports_yaw_passthrough() const override { return _tail_type == AP_MOTORS_HELI_SINGLE_TAILTYPE_SERVO_EXTGYRO; }
// supports_yaw_passthrough - returns true if we support yaw passthrough
bool supports_yaw_passthrough() const override { return get_tail_type() == TAIL_TYPE::SERVO_EXTGYRO; }
void set_acro_tail(bool set) override { _acro_tail = set; }
@ -103,12 +94,33 @@ protected:
// move_yaw - moves the yaw servo
void move_yaw(float yaw_out);
// Get yaw offset required to cancel out steady state main rotor torque
float get_yaw_offset(float collective);
// handle output limit flags and send throttle to servos lib
void output_to_ddfp_tail(float throttle);
// servo_test - move servos through full range of movement
void servo_test() override;
// Tail types
enum class TAIL_TYPE {
SERVO = 0,
SERVO_EXTGYRO = 1,
DIRECTDRIVE_VARPITCH = 2,
DIRECTDRIVE_FIXEDPITCH_CW = 3,
DIRECTDRIVE_FIXEDPITCH_CCW = 4,
DIRECTDRIVE_VARPIT_EXT_GOV = 5
};
TAIL_TYPE get_tail_type() const { return TAIL_TYPE(_tail_type.get()); }
// Helper to return true for direct drive fixed pitch tail, either CW or CCW
bool have_DDFP_tail() const;
// Helper to return true if the tail RSC should be used
bool use_tail_RSC() const;
// external objects we depend upon
AP_MotorsHeli_RSC _tail_rotor; // tail rotor
AP_MotorsHeli_Swash _swashplate; // swashplate