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

AP_Motors_Heli: Fix DDFP thrust linearisation by using min max pwm in servo library 

AP_Motors_Heli: Add H_YAW_TRIM param conversion for DDFP tails
This commit is contained in:
Gone4Dirt 2023-10-01 18:48:45 +01:00 committed by Andrew Tridgell
parent 5bd67d8e04
commit 927418b87c
3 changed files with 81 additions and 39 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
libraries/AP_Motors/AP_MotorsHeli.h
View File

@ -162,6 +162,9 @@ public:
// output_test_seq - disabled on heli, do nothing
void _output_test_seq(uint8_t motor_seq, int16_t pwm) override {};
// Helper function for param conversions to be done in motors class
virtual void heli_motors_param_conversions(void) { return; }
// var_info for holding Parameter information
static const struct AP_Param::GroupInfo var_info[];

106
libraries/AP_Motors/AP_MotorsHeli_Single.cpp
View File

@ -176,6 +176,13 @@ const AP_Param::GroupInfo AP_MotorsHeli_Single::var_info[] = {
// @User: Standard
AP_SUBGROUPINFO(thr_lin, "DDFP_", 22, AP_MotorsHeli_Single, Thrust_Linearization),
// @Param: YAW_TRIM
// @DisplayName: Tail Rotor Trim
// @Description: Fixed offset applied to yaw output to minimize yaw I-term contribution needed to counter rotor drag. Currently only works of DDFP tails (H_TAIL_TYPE = 3 or H_TAIL_TYPE = 4). If using the H_COL2YAW compensation this trim is used to compensate for the main rotor profile drag. If H_COL2YAW is not used, this value can be set to reduce the yaw I contribution to zero when in a steady hover.
// @Range: 0 1
// @User: Standard
AP_GROUPINFO("YAW_TRIM", 23, AP_MotorsHeli_Single, _yaw_trim, 0.0f),
AP_GROUPEND
};
@ -216,7 +223,10 @@ void AP_MotorsHeli_Single::init_outputs()
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_VARPITCH && _tail_type != AP_MOTORS_HELI_SINGLE_TAILTYPE_DIRECTDRIVE_VARPIT_EXT_GOV) {
if (_tail_type == AP_MOTORS_HELI_SINGLE_TAILTYPE_DIRECTDRIVE_FIXEDPITCH_CW || _tail_type == AP_MOTORS_HELI_SINGLE_TAILTYPE_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) {
// yaw servo is an angle from -4500 to 4500
SRV_Channels::set_angle(SRV_Channel::k_motor4, YAW_SERVO_MAX_ANGLE);
}
@ -423,6 +433,11 @@ void AP_MotorsHeli_Single::move_actuators(float roll_out, float pitch_out, float
// 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;
@ -490,66 +505,61 @@ void AP_MotorsHeli_Single::output_to_motors()
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
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){
rc_write(AP_MOTORS_MOT_4, calculate_ddfp_output(0.0f));
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){
rc_write(AP_MOTORS_MOT_4, calculate_ddfp_output(0.0f));
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){
SRV_Channel *c = SRV_Channels::srv_channel(AP_MOTORS_MOT_4);
if (c != nullptr) {
_ddfp_pwm_min = c->get_output_min();
_ddfp_pwm_max = c->get_output_max();
_ddfp_pwm_trim = c->get_trim();
}
float servo_out = 0.0f;
if (is_positive((float) (_ddfp_pwm_max - _ddfp_pwm_min))) {
float servo4_trim = constrain_float((_ddfp_pwm_trim - 1000) / (_ddfp_pwm_max - _ddfp_pwm_min), 0.0f, 1.0f);
if (is_positive(_servo4_out)) {
servo_out = (1.0f - servo4_trim) * _servo4_out + servo4_trim;
} else {
servo_out = servo4_trim * _servo4_out + servo4_trim;
}
} else {
// if servo pwm min and max are bad, convert servo4_out from -1 to 1 to 0 to 1
servo_out = 0.5f * (_servo4_out + 1.0f);
// this should never happen
INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
}
// output yaw servo to tail rsc
rc_write(AP_MOTORS_MOT_4, calculate_ddfp_output(thr_lin.thrust_to_actuator(constrain_float(servo_out, 0.0f, 1.0f))));
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){
rc_write(AP_MOTORS_MOT_4, calculate_ddfp_output(0.0f));
// Set DDFP to servo min
output_to_ddfp_tail(0.0);
}
break;
}
}
// calculate the motor output for DDFP tails from yaw_out
uint16_t AP_MotorsHeli_Single::calculate_ddfp_output(float yaw_out)
// handle output limit flags and send throttle to servos lib
void AP_MotorsHeli_Single::output_to_ddfp_tail(float throttle)
{
uint16_t ret = _ddfp_pwm_min + (_ddfp_pwm_max - _ddfp_pwm_min) * yaw_out;
return ret;
// Note: yaw trim thrust has already been applied. the output should only be from 0 to 1.
// Upper limit
if (throttle >= 1.0){
throttle = 1.0;
limit.yaw = true;
}
// Lower limit
if (throttle <= 0.0){
throttle = 0.0;
limit.yaw = true;
}
SRV_Channels::set_output_scaled(SRV_Channel::k_motor4, throttle);
}
// servo_test - move servos through full range of movement
@ -635,3 +645,31 @@ bool AP_MotorsHeli_Single::arming_checks(size_t buflen, char *buffer) const
return true;
}
// Helper function for param conversions which are easier to be done in the motors class
// Called from system.cpp
void AP_MotorsHeli_Single::heli_motors_param_conversions(void)
{
// PARAMETER_CONVERSION - Added: Nov-2023
// Convert trim for DDFP tails
// 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()) {
SRV_Channel *c = SRV_Channels::get_channel_for(SRV_Channel::k_motor4);
if (c != nullptr) {
uint16_t pwm_min = c->get_output_min();
uint16_t pwm_max = c->get_output_max();
uint16_t pwm_trim = c->get_trim();
float trim = (float)(pwm_trim - pwm_min) / constrain_uint16(pwm_max - pwm_min, 1, 2000);
_yaw_trim.set(trim);
}
// Motor 4 may not have been assigned to an output yet in which case this is unlikely to be a conversion from old setup.
// Prevent future attempts to convert the param so we don't put a non-sense value in.
_yaw_trim.save();
}
}

11
libraries/AP_Motors/AP_MotorsHeli_Single.h
View File

@ -80,6 +80,9 @@ public:
// Run arming checks
bool arming_checks(size_t buflen, char *buffer) const override;
// Helper function for param conversions to be done in motors class
void heli_motors_param_conversions(void) override;
// Thrust Linearization handling
Thrust_Linearization thr_lin {*this};
@ -100,8 +103,8 @@ protected:
// move_yaw - moves the yaw servo
void move_yaw(float yaw_out);
// calculate the motor output for DDFP tails from yaw_out
uint16_t calculate_ddfp_output(float yaw_out);
// 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;
@ -118,9 +121,6 @@ protected:
float _pitch_test = 0.0f; // over-ride for pitch output, used by servo_test function
float _yaw_test = 0.0f; // over-ride for yaw output, used by servo_test function
float _servo4_out = 0.0f; // output value sent to motor
uint16_t _ddfp_pwm_min = 0; // minimum ddfp servo min
uint16_t _ddfp_pwm_max = 0; // minimum ddfp servo max
uint16_t _ddfp_pwm_trim = 0; // minimum ddfp servo trim
// parameters
AP_Int16 _tail_type; // Tail type used: Servo, Servo with external gyro, direct drive variable pitch or direct drive fixed pitch
@ -129,6 +129,7 @@ protected:
AP_Int8 _flybar_mode; // Flybar present or not. Affects attitude controller used during ACRO flight mode
AP_Int16 _direct_drive_tailspeed; // Direct Drive VarPitch Tail ESC speed (0 ~ 1000)
AP_Float _collective_yaw_scale; // Feed-forward compensation to automatically add rudder input when collective pitch is increased. Can be positive or negative depending on mechanics.
AP_Float _yaw_trim; // Fixed offset applied to yaw output to reduce yaw I.
bool _acro_tail = false;
};