From 1e521d1a5d43a55acd753a10de1b25a30edb4a68 Mon Sep 17 00:00:00 2001 From: bnsgeyer Date: Fri, 2 Jun 2023 23:30:17 -0400 Subject: [PATCH] AP_Motors: Move DDFP thrust linearization params into sub group --- libraries/AP_Motors/AP_MotorsHeli_Single.cpp | 9 ++-- libraries/AP_Motors/AP_MotorsHeli_Single.h | 3 -- .../AP_Motors_Thrust_Linearization.cpp | 52 +++++++++++++++++++ .../AP_Motors_Thrust_Linearization.h | 9 ++++ 4 files changed, 64 insertions(+), 9 deletions(-) diff --git a/libraries/AP_Motors/AP_MotorsHeli_Single.cpp b/libraries/AP_Motors/AP_MotorsHeli_Single.cpp index 97d34c9614..18992b52b5 100644 --- a/libraries/AP_Motors/AP_MotorsHeli_Single.cpp +++ b/libraries/AP_Motors/AP_MotorsHeli_Single.cpp @@ -142,28 +142,24 @@ const AP_Param::GroupInfo AP_MotorsHeli_Single::var_info[] = { // @Description: Tail rotor DDFP motor thrust curve exponent (0.0 for linear to 1.0 for second order curve) // @Range: -1 1 // @User: Standard - AP_GROUPINFO("DDFP_THST_EXPO", 22, AP_MotorsHeli_Single, thr_lin.curve_expo, 0.55), // @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 - AP_GROUPINFO("DDFP_SPIN_MIN", 23, AP_MotorsHeli_Single, thr_lin.spin_min, 0.15), // @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 - AP_GROUPINFO("DDFP_SPIN_MAX", 24, AP_MotorsHeli_Single, thr_lin.spin_max, 0.95), // @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 - AP_GROUPINFO("DDFP_BAT_IDX", 25, AP_MotorsHeli_Single, thr_lin.batt_idx, 0), // @Param: DDFP_BAT_V_MAX // @DisplayName: Battery voltage compensation maximum voltage @@ -171,7 +167,6 @@ const AP_Param::GroupInfo AP_MotorsHeli_Single::var_info[] = { // @Range: 6 53 // @Units: V // @User: Standard - AP_GROUPINFO("DDFP_BAT_V_MAX", 26, AP_MotorsHeli_Single, thr_lin.batt_voltage_max, AP_MOTORS_HELI_SINGLE_BAT_VOLT_MAX_DEFAULT), // @Param: DDFP_BAT_V_MIN // @DisplayName: Battery voltage compensation minimum voltage @@ -179,7 +174,7 @@ const AP_Param::GroupInfo AP_MotorsHeli_Single::var_info[] = { // @Range: 6 42 // @Units: V // @User: Standard - AP_GROUPINFO("DDFP_BAT_V_MIN", 27, AP_MotorsHeli_Single, thr_lin.batt_voltage_min, AP_MOTORS_HELI_SINGLE_BAT_VOLT_MIN_DEFAULT), + AP_SUBGROUPINFO(thr_lin, "DDFP_", 22, AP_MotorsHeli_Single, Thrust_Linearization), AP_GROUPEND }; @@ -608,6 +603,8 @@ void AP_MotorsHeli_Single::output_to_motors() } 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)))); diff --git a/libraries/AP_Motors/AP_MotorsHeli_Single.h b/libraries/AP_Motors/AP_MotorsHeli_Single.h index d609f000df..a2af6b2653 100644 --- a/libraries/AP_Motors/AP_MotorsHeli_Single.h +++ b/libraries/AP_Motors/AP_MotorsHeli_Single.h @@ -35,9 +35,6 @@ // maximum number of swashplate servos #define AP_MOTORS_HELI_SINGLE_NUM_SWASHPLATE_SERVOS 3 -#define AP_MOTORS_HELI_SINGLE_BAT_VOLT_MAX_DEFAULT 0.0f // voltage limiting max default -#define AP_MOTORS_HELI_SINGLE_BAT_VOLT_MIN_DEFAULT 0.0f // voltage limiting min default (voltage dropping below this level will have no effect) - /// @class AP_MotorsHeli_Single class AP_MotorsHeli_Single : public AP_MotorsHeli { public: diff --git a/libraries/AP_Motors/AP_Motors_Thrust_Linearization.cpp b/libraries/AP_Motors/AP_Motors_Thrust_Linearization.cpp index 759e10b749..a548b12491 100644 --- a/libraries/AP_Motors/AP_Motors_Thrust_Linearization.cpp +++ b/libraries/AP_Motors/AP_Motors_Thrust_Linearization.cpp @@ -16,6 +16,56 @@ #endif #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 +}; Thrust_Linearization::Thrust_Linearization(AP_Motors& _motors) : motors(_motors), @@ -24,6 +74,8 @@ 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); + + AP_Param::setup_object_defaults(this, var_info); } // converts desired thrust to linearized actuator output in a range of 0~1 diff --git a/libraries/AP_Motors/AP_Motors_Thrust_Linearization.h b/libraries/AP_Motors/AP_Motors_Thrust_Linearization.h index d3b097c38d..d34cd3fba8 100644 --- a/libraries/AP_Motors/AP_Motors_Thrust_Linearization.h +++ b/libraries/AP_Motors/AP_Motors_Thrust_Linearization.h @@ -3,6 +3,12 @@ #include #include +#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) + class AP_Motors; class Thrust_Linearization { friend class AP_MotorsMulticopter; @@ -44,6 +50,9 @@ public: // 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