From b61b7611411d750f05bcbc153f3dbdcaeafc1313 Mon Sep 17 00:00:00 2001 From: Iampete1 Date: Fri, 5 May 2023 00:41:08 +0100 Subject: [PATCH] AP_Motors: Heli: move swashplate output state and code into swash lib --- libraries/AP_Motors/AP_MotorsHeli.cpp | 11 ---- libraries/AP_Motors/AP_MotorsHeli.h | 3 - libraries/AP_Motors/AP_MotorsHeli_Dual.cpp | 33 ++-------- libraries/AP_Motors/AP_MotorsHeli_Dual.h | 4 +- libraries/AP_Motors/AP_MotorsHeli_Single.cpp | 20 ++---- libraries/AP_Motors/AP_MotorsHeli_Single.h | 6 +- libraries/AP_Motors/AP_MotorsHeli_Swash.cpp | 67 ++++++++++++++++---- libraries/AP_Motors/AP_MotorsHeli_Swash.h | 18 ++++-- 8 files changed, 79 insertions(+), 83 deletions(-) diff --git a/libraries/AP_Motors/AP_MotorsHeli.cpp b/libraries/AP_Motors/AP_MotorsHeli.cpp index 41eff40153..a2a17f448d 100644 --- a/libraries/AP_Motors/AP_MotorsHeli.cpp +++ b/libraries/AP_Motors/AP_MotorsHeli.cpp @@ -535,17 +535,6 @@ void AP_MotorsHeli::reset_flight_controls() calculate_scalars(); } -// convert input in -1 to +1 range to pwm output for swashplate servo. -// The value 0 corresponds to the trim value of the servo. Swashplate -// servo travel range is fixed to 1000 pwm and therefore the input is -// multiplied by 500 to get PWM output. -void AP_MotorsHeli::rc_write_swash(uint8_t chan, float swash_in) -{ - uint16_t pwm = (uint16_t)(1500 + 500 * swash_in); - SRV_Channel::Aux_servo_function_t function = SRV_Channels::get_motor_function(chan); - SRV_Channels::set_output_pwm_trimmed(function, pwm); -} - // update the collective input filter. should be called at 100hz void AP_MotorsHeli::update_throttle_hover(float dt) { diff --git a/libraries/AP_Motors/AP_MotorsHeli.h b/libraries/AP_Motors/AP_MotorsHeli.h index 2ae7fff1a4..7425151732 100644 --- a/libraries/AP_Motors/AP_MotorsHeli.h +++ b/libraries/AP_Motors/AP_MotorsHeli.h @@ -223,9 +223,6 @@ protected: // to be overloaded by child classes, different vehicle types would have different movement patterns virtual void servo_test() = 0; - // write to a swash servo. output value is pwm - void rc_write_swash(uint8_t chan, float swash_in); - // save parameters as part of disarming void save_params_on_disarm() override; diff --git a/libraries/AP_Motors/AP_MotorsHeli_Dual.cpp b/libraries/AP_Motors/AP_MotorsHeli_Dual.cpp index 63199e51e6..35cc1d0460 100644 --- a/libraries/AP_Motors/AP_MotorsHeli_Dual.cpp +++ b/libraries/AP_Motors/AP_MotorsHeli_Dual.cpp @@ -557,21 +557,9 @@ void AP_MotorsHeli_Dual::move_actuators(float roll_out, float pitch_out, float c float swash2_roll = get_swashplate(2, AP_MOTORS_HELI_DUAL_SWASH_AXIS_ROLL, pitch_out, roll_out, yaw_out, collective2_out_scaled); float swash2_coll = get_swashplate(2, AP_MOTORS_HELI_DUAL_SWASH_AXIS_COLL, pitch_out, roll_out, yaw_out, collective2_out_scaled); - // get servo positions from swashplate library - _servo_out[CH_1] = _swashplate1.get_servo_out(CH_1,swash1_pitch,swash1_roll,swash1_coll); - _servo_out[CH_2] = _swashplate1.get_servo_out(CH_2,swash1_pitch,swash1_roll,swash1_coll); - _servo_out[CH_3] = _swashplate1.get_servo_out(CH_3,swash1_pitch,swash1_roll,swash1_coll); - if (_swashplate1.get_swash_type() == SWASHPLATE_TYPE_H4_90 || _swashplate1.get_swash_type() == SWASHPLATE_TYPE_H4_45) { - _servo_out[CH_7] = _swashplate1.get_servo_out(CH_4,swash1_pitch,swash1_roll,swash1_coll); - } - - // get servo positions from swashplate library - _servo_out[CH_4] = _swashplate2.get_servo_out(CH_1,swash2_pitch,swash2_roll,swash2_coll); - _servo_out[CH_5] = _swashplate2.get_servo_out(CH_2,swash2_pitch,swash2_roll,swash2_coll); - _servo_out[CH_6] = _swashplate2.get_servo_out(CH_3,swash2_pitch,swash2_roll,swash2_coll); - if (_swashplate2.get_swash_type() == SWASHPLATE_TYPE_H4_90 || _swashplate2.get_swash_type() == SWASHPLATE_TYPE_H4_45) { - _servo_out[CH_8] = _swashplate2.get_servo_out(CH_4,swash2_pitch,swash2_roll,swash2_coll); - } + // Calculate servo positions in swashplate library + _swashplate1.calculate(swash1_roll, swash1_pitch, swash1_coll); + _swashplate2.calculate(swash2_roll, swash2_pitch, swash2_coll); } @@ -580,19 +568,10 @@ void AP_MotorsHeli_Dual::output_to_motors() if (!initialised_ok()) { return; } - // actually move the servos. PWM is sent based on nominal 1500 center. servo output shifts center based on trim value. - for (uint8_t i=0; i #include +#include #include "AP_MotorsHeli_Swash.h" @@ -86,6 +87,16 @@ const AP_Param::GroupInfo AP_MotorsHeli_Swash::var_info[] = { AP_GROUPEND }; +AP_MotorsHeli_Swash::AP_MotorsHeli_Swash(uint8_t mot_0, uint8_t mot_1, uint8_t mot_2, uint8_t mot_3) +{ + _motor_num[0] = mot_0; + _motor_num[1] = mot_1; + _motor_num[2] = mot_2; + _motor_num[3] = mot_3; + + AP_Param::setup_object_defaults(this, var_info); +} + // configure - configure the swashplate settings for any updated parameters void AP_MotorsHeli_Swash::configure() { @@ -103,6 +114,7 @@ void AP_MotorsHeli_Swash::calculate_roll_pitch_collective_factors() { // Clear existing setup for (uint8_t i = 0; i < _max_num_servos; i++) { + _enabled[i] = false; _rollFactor[i] = 0.0; _pitchFactor[i] = 0.0; _collectiveFactor[i] = 0.0; @@ -182,33 +194,40 @@ void AP_MotorsHeli_Swash::add_servo_raw(uint8_t num, float roll, float pitch, fl return; } + _enabled[num] = true; _rollFactor[num] = roll * 0.45; _pitchFactor[num] = pitch * 0.45; _collectiveFactor[num] = collective; } -// get_servo_out - calculates servo output -float AP_MotorsHeli_Swash::get_servo_out(int8_t ch_num, float pitch, float roll, float collective) const +// calculates servo output +void AP_MotorsHeli_Swash::calculate(float roll, float pitch, float collective) { // Collective control direction. Swash moves up for negative collective pitch, down for positive collective pitch if (_collective_direction == COLLECTIVE_DIRECTION_REVERSED){ collective = 1 - collective; } - float servo = (_rollFactor[ch_num] * roll) + (_pitchFactor[ch_num] * pitch) + _collectiveFactor[ch_num] * collective; - if (_swash_type == SWASHPLATE_TYPE_H1 && (ch_num == CH_1 || ch_num == CH_2)) { - servo += 0.5f; + for (uint8_t i = 0; i < _max_num_servos; i++) { + if (!_enabled[i]) { + // This servo is not enabled + continue; + } + + _output[i] = (_rollFactor[i] * roll) + (_pitchFactor[i] * pitch) + _collectiveFactor[i] * collective; + if (_swash_type == SWASHPLATE_TYPE_H1 && (i == CH_1 || i == CH_2)) { + _output[i] += 0.5f; + } + + // rescale from -1..1, so we can use the pwm calc that includes trim + _output[i] = 2.0f * _output[i] - 1.0f; + + if (_make_servo_linear) { + _output[i] = get_linear_servo_output(_output[i]); + } + } - - // rescale from -1..1, so we can use the pwm calc that includes trim - servo = 2.0f * servo - 1.0f; - - if (_make_servo_linear) { - servo = get_linear_servo_output(servo); - } - - return servo; } // set_linear_servo_out - sets swashplate servo output to be linear @@ -222,3 +241,23 @@ float AP_MotorsHeli_Swash::get_linear_servo_output(float input) const } +// Output calculated values to servos +void AP_MotorsHeli_Swash::output() +{ + for (uint8_t i = 0; i < _max_num_servos; i++) { + if (_enabled[i]) { + rc_write(_motor_num[i], _output[i]); + } + } +} + +// convert input in -1 to +1 range to pwm output for swashplate servo. +// The value 0 corresponds to the trim value of the servo. Swashplate +// servo travel range is fixed to 1000 pwm and therefore the input is +// multiplied by 500 to get PWM output. +void AP_MotorsHeli_Swash::rc_write(uint8_t chan, float swash_in) +{ + uint16_t pwm = (uint16_t)(1500 + 500 * swash_in); + SRV_Channel::Aux_servo_function_t function = SRV_Channels::get_motor_function(chan); + SRV_Channels::set_output_pwm_trimmed(function, pwm); +} diff --git a/libraries/AP_Motors/AP_MotorsHeli_Swash.h b/libraries/AP_Motors/AP_MotorsHeli_Swash.h index 62e2ed4951..4dff50122e 100644 --- a/libraries/AP_Motors/AP_MotorsHeli_Swash.h +++ b/libraries/AP_Motors/AP_MotorsHeli_Swash.h @@ -19,10 +19,7 @@ enum SwashPlateType { class AP_MotorsHeli_Swash { public: - AP_MotorsHeli_Swash() - { - AP_Param::setup_object_defaults(this, var_info); - }; + AP_MotorsHeli_Swash(uint8_t mot_0, uint8_t mot_1, uint8_t mot_2, uint8_t mot_3); // configure - configure the swashplate settings for any updated parameters void configure(); @@ -30,8 +27,11 @@ public: // get_swash_type - gets swashplate type SwashPlateType get_swash_type() const { return _swash_type; } - // get_servo_out - calculates servo output - float get_servo_out(int8_t servo_num, float pitch, float roll, float collective) const; + // calculates servo output + void calculate(float roll, float pitch, float collective); + + // Output calculated values to servos + void output(); // get_phase_angle - returns the rotor phase angle int16_t get_phase_angle() const { return _phase_angle; } @@ -51,6 +51,9 @@ private: void add_servo_angle(uint8_t num, float angle, float collective); void add_servo_raw(uint8_t num, float roll, float pitch, float collective); + // write to a swash servo. output value is pwm + void rc_write(uint8_t chan, float swash_in); + enum CollectiveDirection { COLLECTIVE_DIRECTION_NORMAL = 0, COLLECTIVE_DIRECTION_REVERSED @@ -64,9 +67,12 @@ private: bool _make_servo_linear; // Sets servo output to be linearized // Internal variables + bool _enabled[_max_num_servos]; // True if this output servo is enabled float _rollFactor[_max_num_servos]; // Roll axis scaling of servo output based on servo position float _pitchFactor[_max_num_servos]; // Pitch axis scaling of servo output based on servo position float _collectiveFactor[_max_num_servos]; // Collective axis scaling of servo output based on servo position + float _output[_max_num_servos]; // Servo output value + uint8_t _motor_num[_max_num_servos]; // Motor function to use for output // parameters AP_Int8 _swashplate_type; // Swash Type Setting