AP_Motors: Heli: move swashplate output state and code into swash lib

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)
 {

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;
 

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
+    // Calculate servo positions in swashplate library
-    _servo_out[CH_1] = _swashplate1.get_servo_out(CH_1,swash1_pitch,swash1_roll,swash1_coll);
+    _swashplate1.calculate(swash1_roll, swash1_pitch, swash1_coll);
-    _servo_out[CH_2] = _swashplate1.get_servo_out(CH_2,swash1_pitch,swash1_roll,swash1_coll);
+    _swashplate2.calculate(swash2_roll, swash2_pitch, swash2_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);
-    }
 
 }
 
@@ -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<AP_MOTORS_HELI_DUAL_NUM_SWASHPLATE_SERVOS; i++) {
-        rc_write_swash(i, _servo_out[CH_1+i]);
-    }
 
-    // write to servo for 4 servo of 4 servo swashplate
+    // Write swashplate outputs
-    if (_swashplate1.get_swash_type() == SWASHPLATE_TYPE_H4_90 || _swashplate1.get_swash_type() == SWASHPLATE_TYPE_H4_45) {
+    _swashplate1.output();
-        rc_write_swash(AP_MOTORS_MOT_7, _servo_out[CH_7]);
+    _swashplate2.output();
-    }
-    // write to servo for 4 servo of 4 servo swashplate
-    if (_swashplate2.get_swash_type() == SWASHPLATE_TYPE_H4_90 || _swashplate2.get_swash_type() == SWASHPLATE_TYPE_H4_45) {
-        rc_write_swash(AP_MOTORS_MOT_8, _servo_out[CH_8]);
-    }
 
     switch (_spool_state) {
         case SpoolState::SHUT_DOWN:

libraries/AP_Motors/AP_MotorsHeli_Dual.h

@@ -87,8 +87,8 @@ protected:
     const char* _get_frame_string() const override { return "HELI_DUAL"; }
 
     //  objects we depend upon
-    AP_MotorsHeli_Swash        _swashplate1;        // swashplate1
+    AP_MotorsHeli_Swash _swashplate1 { CH_1, CH_2, CH_3, CH_7 }; // swashplate1
-    AP_MotorsHeli_Swash        _swashplate2;        // swashplate2
+    AP_MotorsHeli_Swash _swashplate2 { CH_4, CH_5, CH_6, CH_8 }; // swashplate2
 
     // internal variables
     float _oscillate_angle = 0.0f;                  // cyclic oscillation angle, used by servo_test function

libraries/AP_Motors/AP_MotorsHeli_Single.cpp

@@ -460,13 +460,8 @@ void AP_MotorsHeli_Single::move_actuators(float roll_out, float pitch_out, float
     float collective_scalar = ((float)(_collective_max-_collective_min))*0.001f;
     float collective_out_scaled = collective_out * collective_scalar + (_collective_min - 1000)*0.001f;
 
-    // get servo positions from swashplate library
+    // Caculate servo positions from swashplate library
-    _servo1_out = _swashplate.get_servo_out(CH_1,pitch_out,roll_out,collective_out_scaled);
+    _swashplate.calculate(roll_out, pitch_out, collective_out_scaled);
-    _servo2_out = _swashplate.get_servo_out(CH_2,pitch_out,roll_out,collective_out_scaled);
-    _servo3_out = _swashplate.get_servo_out(CH_3,pitch_out,roll_out,collective_out_scaled);
-    if (_swashplate.get_swash_type() == SWASHPLATE_TYPE_H4_90 || _swashplate.get_swash_type() == SWASHPLATE_TYPE_H4_45) {
-        _servo5_out = _swashplate.get_servo_out(CH_4,pitch_out,roll_out,collective_out_scaled);
-    }
 
     // update the yaw rate using the tail rotor/servo
     move_yaw(yaw_out + yaw_offset);
@@ -494,14 +489,9 @@ void AP_MotorsHeli_Single::output_to_motors()
         return;
     }
 
-    // actually move the servos.  PWM is sent based on nominal 1500 center.  servo output shifts center based on trim value.
+    // Write swashplate outputs
-    rc_write_swash(AP_MOTORS_MOT_1, _servo1_out);
+    _swashplate.output();
-    rc_write_swash(AP_MOTORS_MOT_2, _servo2_out);
+
-    rc_write_swash(AP_MOTORS_MOT_3, _servo3_out);
-    // get servo positions from swashplate library and write to servo for 4 servo of 4 servo swashplate
-    if (_swashplate.get_swash_type() == SWASHPLATE_TYPE_H4_90 || _swashplate.get_swash_type() == SWASHPLATE_TYPE_H4_45) {
-        rc_write_swash(AP_MOTORS_MOT_5, _servo5_out);
-    }
     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);
     }

libraries/AP_Motors/AP_MotorsHeli_Single.h

@@ -42,7 +42,7 @@ public:
     AP_MotorsHeli_Single(uint16_t speed_hz = AP_MOTORS_HELI_SPEED_DEFAULT) :
         AP_MotorsHeli(speed_hz),
         _tail_rotor(SRV_Channel::k_heli_tail_rsc, AP_MOTORS_HELI_SINGLE_TAILRSC),
-        _swashplate()
+        _swashplate(AP_MOTORS_MOT_1, AP_MOTORS_MOT_2, AP_MOTORS_MOT_3, AP_MOTORS_MOT_5)
     {
         AP_Param::setup_object_defaults(this, var_info);
     };
@@ -117,11 +117,7 @@ protected:
     float _roll_test = 0.0f;                    // over-ride for roll output, used by servo_test function
     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 _servo1_out = 0.0f;                   // output value sent to motor
-    float _servo2_out = 0.0f;                   // output value sent to motor
-    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

libraries/AP_Motors/AP_MotorsHeli_Swash.cpp

@@ -15,6 +15,7 @@
 
 #include <stdlib.h>
 #include <AP_HAL/AP_HAL.h>
+#include <SRV_Channel/SRV_Channel.h>
 
 #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
+// calculates servo output
-float AP_MotorsHeli_Swash::get_servo_out(int8_t ch_num, float pitch, float roll, float collective) const
+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;
+    for (uint8_t i = 0; i < _max_num_servos; i++) {
-    if (_swash_type == SWASHPLATE_TYPE_H1 && (ch_num == CH_1 || ch_num == CH_2)) {
+        if (!_enabled[i]) {
-        servo += 0.5f;
+            // 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);
+}

libraries/AP_Motors/AP_MotorsHeli_Swash.h

@@ -19,10 +19,7 @@ enum SwashPlateType {
 class AP_MotorsHeli_Swash {
 public:
 
-    AP_MotorsHeli_Swash() 
+    AP_MotorsHeli_Swash(uint8_t mot_0, uint8_t mot_1, uint8_t mot_2, uint8_t mot_3);
-    {
-        AP_Param::setup_object_defaults(this, var_info);
-    };
 
     // 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
+    // calculates servo output
-    float get_servo_out(int8_t servo_num, float pitch, float roll, float collective) const;
+    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