Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

AP_MotorsHeli: add thrust linearization for ddfp tail rotors #22661

Merged
merged 4 commits into from
Jun 15, 2023
Merged

AP_MotorsHeli: add thrust linearization for ddfp tail rotors #22661

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
4 commits
Select commit Hold shift + click to select a range
2dc8785
AP_MotorsHeli: add thrust linearization for DDFP tails
bnsgeyer May 25, 2023
af98dd2
AP_Motors: Tradheli-fix servo4 param calls and ensure no divide by zero
bnsgeyer Jun 4, 2023
2a3b6c9
AP_Motors: Move DDFP thrust linearization params into sub group
bnsgeyer Jun 3, 2023
62e39d9
AP_Motors: make defaults dependent on vehicle type
bnsgeyer Jun 12, 2023
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
87 changes: 79 additions & 8 deletions libraries/AP_Motors/AP_MotorsHeli_Single.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -137,6 +137,45 @@ const AP_Param::GroupInfo AP_MotorsHeli_Single::var_info[] = {
// @User: Standard
AP_GROUPINFO("COL2YAW", 21, AP_MotorsHeli_Single, _collective_yaw_scale, 0),

// @Param: DDFP_THST_EXPO
// @DisplayName: DDFP Tail Rotor Thrust Curve Expo
// @Description: Tail rotor DDFP motor thrust curve exponent (0.0 for linear to 1.0 for second order curve)
// @Range: -1 1
// @User: Standard

// @Param: DDFP_SPIN_MIN
// @DisplayName: DDFP Tail Rotor Motor Spin minimum
// @Description: Point at which the thrust starts expressed as a number from 0 to 1 in the entire output range. Should be higher than MOT_SPIN_ARM.
// @Values: 0.0:Low, 0.15:Default, 0.3:High
// @User: Standard

// @Param: DDFP_SPIN_MAX
// @DisplayName: DDFP Tail Rotor Motor Spin maximum
// @Description: Point at which the thrust saturates expressed as a number from 0 to 1 in the entire output range
// @Values: 0.9:Low, 0.95:Default, 1.0:High
// @User: Standard

// @Param: DDFP_BAT_IDX
// @DisplayName: DDFP Tail Rotor Battery compensation index
// @Description: Which battery monitor should be used for doing compensation
// @Values: 0:First battery, 1:Second battery
// @User: Standard

// @Param: DDFP_BAT_V_MAX
// @DisplayName: Battery voltage compensation maximum voltage
// @Description: Battery voltage compensation maximum voltage (voltage above this will have no additional scaling effect on thrust). Recommend 4.2 * cell count, 0 = Disabled
// @Range: 6 53
// @Units: V
// @User: Standard

// @Param: DDFP_BAT_V_MIN
// @DisplayName: Battery voltage compensation minimum voltage
// @Description: Battery voltage compensation minimum voltage (voltage below this will have no additional scaling effect on thrust). Recommend 3.3 * cell count, 0 = Disabled
// @Range: 6 42
// @Units: V
// @User: Standard
AP_SUBGROUPINFO(thr_lin, "DDFP_", 22, AP_MotorsHeli_Single, Thrust_Linearization),

AP_GROUPEND
};

Expand Down Expand Up @@ -199,8 +238,10 @@ bool AP_MotorsHeli_Single::init_outputs()
reset_swash_servo(SRV_Channels::get_motor_function(4));
}

// yaw servo is an angle from -4500 to 4500
SRV_Channels::set_angle(SRV_Channel::k_motor4, YAW_SERVO_MAX_ANGLE);
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);
}

set_initialised_ok(_frame_class == MOTOR_FRAME_HELI);

Expand Down Expand Up @@ -520,43 +561,73 @@ void AP_MotorsHeli_Single::output_to_motors()
_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();
}

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_angle(AP_MOTORS_MOT_4, -YAW_SERVO_MAX_ANGLE);
rc_write(AP_MOTORS_MOT_4, calculate_ddfp_output(0.0f));
}
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_angle(AP_MOTORS_MOT_4, -YAW_SERVO_MAX_ANGLE);
rc_write(AP_MOTORS_MOT_4, calculate_ddfp_output(0.0f));
}
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){
// constrain output so that motor never fully stops
_servo4_out = constrain_float(_servo4_out, -0.9f, 1.0f);
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_angle(AP_MOTORS_MOT_4, _servo4_out * YAW_SERVO_MAX_ANGLE);
rc_write(AP_MOTORS_MOT_4, calculate_ddfp_output(thr_lin.thrust_to_actuator(constrain_float(servo_out, 0.0f, 1.0f))));
}
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_angle(AP_MOTORS_MOT_4, -YAW_SERVO_MAX_ANGLE);
rc_write(AP_MOTORS_MOT_4, calculate_ddfp_output(0.0f));
}
break;

}
}

// calculate the motor output for DDFP tails from yaw_out
uint16_t AP_MotorsHeli_Single::calculate_ddfp_output(float yaw_out)
{
uint16_t ret = _ddfp_pwm_min + (_ddfp_pwm_max - _ddfp_pwm_min) * yaw_out;
return ret;
}

// servo_test - move servos through full range of movement
void AP_MotorsHeli_Single::servo_test()
{
Expand Down
10 changes: 10 additions & 0 deletions libraries/AP_Motors/AP_MotorsHeli_Single.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -8,6 +8,7 @@
#include "AP_MotorsHeli.h"
#include "AP_MotorsHeli_RSC.h"
#include "AP_MotorsHeli_Swash.h"
#include "AP_Motors_Thrust_Linearization.h"

// rsc and extgyro function output channels.
#define AP_MOTORS_HELI_SINGLE_EXTGYRO CH_7
Expand Down Expand Up @@ -79,6 +80,9 @@ class AP_MotorsHeli_Single : public AP_MotorsHeli {
// parameter_check - returns true if helicopter specific parameters are sensible, used for pre-arm check
bool parameter_check(bool display_msg) const override;

// Thrust Linearization handling
Thrust_Linearization thr_lin {*this};

// var_info
static const struct AP_Param::GroupInfo var_info[];

Expand All @@ -96,6 +100,9 @@ class AP_MotorsHeli_Single : public AP_MotorsHeli {
// 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);

// servo_test - move servos through full range of movement
void servo_test() override;

Expand All @@ -120,6 +127,9 @@ class AP_MotorsHeli_Single : public AP_MotorsHeli {
float _servo3_out = 0.0f; // output value sent to motor
float _servo4_out = 0.0f; // output value sent to motor
float _servo5_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
Expand Down
69 changes: 69 additions & 0 deletions libraries/AP_Motors/AP_Motors_Thrust_Linearization.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -16,6 +16,71 @@
#endif
#endif

#if APM_BUILD_TYPE(APM_BUILD_Heli)
#define THRST_LIN_THST_EXPO_DEFAULT 0.55f // set to 0 for linear and 1 for second order approximation
#define THRST_LIN_SPIN_MIN_DEFAULT 0.15f // throttle out ratio which produces the minimum thrust. (i.e. 0 ~ 1 ) of the full throttle range
#define THRST_LIN_SPIN_MAX_DEFAULT 0.95f // throttle out ratio which produces the maximum thrust. (i.e. 0 ~ 1 ) of the full throttle range
#define THRST_LIN_BAT_VOLT_MAX_DEFAULT 0.0f // voltage limiting max default
#define THRST_LIN_BAT_VOLT_MIN_DEFAULT 0.0f // voltage limiting min default (voltage dropping below this level will have no effect)
#else
#define THRST_LIN_THST_EXPO_DEFAULT 0.65f // set to 0 for linear and 1 for second order approximation
#define THRST_LIN_SPIN_MIN_DEFAULT 0.15f // throttle out ratio which produces the minimum thrust. (i.e. 0 ~ 1 ) of the full throttle range
#define THRST_LIN_SPIN_MAX_DEFAULT 0.95f // throttle out ratio which produces the maximum thrust. (i.e. 0 ~ 1 ) of the full throttle range
#define THRST_LIN_BAT_VOLT_MAX_DEFAULT 0.0f // voltage limiting max default
#define THRST_LIN_BAT_VOLT_MIN_DEFAULT 0.0f // voltage limiting min default (voltage dropping below this level will have no effect)
#endif


extern const AP_HAL::HAL& hal;

const AP_Param::GroupInfo Thrust_Linearization::var_info[] = {

// @Param: THST_EXPO
// @DisplayName: Thrust Curve Expo
// @Description: motor thrust curve exponent (0.0 for linear to 1.0 for second order curve)
// @Range: -1 1
// @User: Standard
AP_GROUPINFO("THST_EXPO", 1, Thrust_Linearization, curve_expo, THRST_LIN_THST_EXPO_DEFAULT),

// @Param: SPIN_MIN
// @DisplayName: Motor Spin minimum
// @Description: Point at which the thrust starts expressed as a number from 0 to 1 in the entire output range. Should be higher than MOT_SPIN_ARM.
// @Values: 0.0:Low, 0.15:Default, 0.3:High
// @User: Standard
AP_GROUPINFO("SPIN_MIN", 2, Thrust_Linearization, spin_min, THRST_LIN_SPIN_MIN_DEFAULT),

// @Param: SPIN_MAX
// @DisplayName: Motor Spin maximum
// @Description: Point at which the thrust saturates expressed as a number from 0 to 1 in the entire output range
// @Values: 0.9:Low, 0.95:Default, 1.0:High
// @User: Standard
AP_GROUPINFO("SPIN_MAX", 3, Thrust_Linearization, spin_max, THRST_LIN_SPIN_MAX_DEFAULT),

// @Param: BAT_IDX
// @DisplayName: Battery compensation index
// @Description: Which battery monitor should be used for doing compensation
// @Values: 0:First battery, 1:Second battery
// @User: Standard
AP_GROUPINFO("BAT_IDX", 4, Thrust_Linearization, batt_idx, 0),

// @Param: BAT_V_MAX
// @DisplayName: Battery voltage compensation maximum voltage
// @Description: Battery voltage compensation maximum voltage (voltage above this will have no additional scaling effect on thrust). Recommend 4.2 * cell count, 0 = Disabled
// @Range: 6 53
// @Units: V
// @User: Standard
AP_GROUPINFO("BAT_V_MAX", 5, Thrust_Linearization, batt_voltage_max, THRST_LIN_BAT_VOLT_MAX_DEFAULT),

// @Param: BAT_V_MIN
// @DisplayName: Battery voltage compensation minimum voltage
// @Description: Battery voltage compensation minimum voltage (voltage below this will have no additional scaling effect on thrust). Recommend 3.3 * cell count, 0 = Disabled
// @Range: 6 42
// @Units: V
// @User: Standard
AP_GROUPINFO("BAT_V_MIN", 6, Thrust_Linearization, batt_voltage_min, THRST_LIN_BAT_VOLT_MIN_DEFAULT),

AP_GROUPEND
};

bnsgeyer marked this conversation as resolved.
Show resolved Hide resolved
Thrust_Linearization::Thrust_Linearization(AP_Motors& _motors) :
motors(_motors),
Expand All @@ -24,6 +89,10 @@ Thrust_Linearization::Thrust_Linearization(AP_Motors& _motors) :
// setup battery voltage filtering
batt_voltage_filt.set_cutoff_frequency(AP_MOTORS_BATT_VOLT_FILT_HZ);
batt_voltage_filt.reset(1.0);

#if APM_BUILD_TYPE(APM_BUILD_Heli)
AP_Param::setup_object_defaults(this, var_info);
bnsgeyer marked this conversation as resolved.
Show resolved Hide resolved
#endif
}

// converts desired thrust to linearized actuator output in a range of 0~1
Expand Down
4 changes: 4 additions & 0 deletions libraries/AP_Motors/AP_Motors_Thrust_Linearization.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -7,6 +7,7 @@ class AP_Motors;
class Thrust_Linearization {
friend class AP_MotorsMulticopter;
friend class AP_MotorsMulticopter_test;
friend class AP_MotorsHeli_Single;
public:
Thrust_Linearization(AP_Motors& _motors);

Expand Down Expand Up @@ -43,6 +44,9 @@ friend class AP_MotorsMulticopter_test;
// Get lift max
float get_lift_max() const { return lift_max; }

// var_info for holding Parameter information
static const struct AP_Param::GroupInfo var_info[];

protected:
AP_Float curve_expo; // curve used to linearize pwm to thrust conversion. set to 0 for linear and 1 for second order approximation
AP_Float spin_min; // throttle out ratio which produces the minimum thrust. (i.e. 0 ~ 1 ) of the full throttle range
Expand Down