From 1ad5e1db4edef23091768d2c43eb3b6dd7e118f6 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 14 Mar 2017 20:46:48 +1100 Subject: [PATCH] AP_Motors: integrate AP_MotorsHeli_Dual fixed issues with combining with Heli_Single --- libraries/AP_Motors/AP_Motors.h | 1 + libraries/AP_Motors/AP_MotorsHeli.h | 11 +++--- libraries/AP_Motors/AP_MotorsHeli_RSC.h | 1 + libraries/AP_Motors/AP_MotorsHeli_Single.h | 44 ++++++++++++---------- libraries/AP_Motors/AP_Motors_Class.h | 1 + 5 files changed, 33 insertions(+), 25 deletions(-) diff --git a/libraries/AP_Motors/AP_Motors.h b/libraries/AP_Motors/AP_Motors.h index aa7826cd51..65489fc28a 100644 --- a/libraries/AP_Motors/AP_Motors.h +++ b/libraries/AP_Motors/AP_Motors.h @@ -5,6 +5,7 @@ #include "AP_MotorsMatrix.h" #include "AP_MotorsTri.h" #include "AP_MotorsHeli_Single.h" +#include "AP_MotorsHeli_Dual.h" #include "AP_MotorsSingle.h" #include "AP_MotorsCoax.h" #include "AP_MotorsTailsitter.h" diff --git a/libraries/AP_Motors/AP_MotorsHeli.h b/libraries/AP_Motors/AP_MotorsHeli.h index f4b381fa6a..439973ad01 100644 --- a/libraries/AP_Motors/AP_MotorsHeli.h +++ b/libraries/AP_Motors/AP_MotorsHeli.h @@ -11,9 +11,6 @@ #include "AP_Motors_Class.h" #include "AP_MotorsHeli_RSC.h" -// maximum number of swashplate servos -#define AP_MOTORS_HELI_NUM_SWASHPLATE_SERVOS 3 - // servo output rates #define AP_MOTORS_HELI_SPEED_DEFAULT 125 // default servo update rate for helicopters @@ -120,6 +117,11 @@ public: // this can be used to ensure other pwm outputs (i.e. for servos) do not conflict virtual uint16_t get_motor_mask() = 0; + virtual void set_acro_tail(bool set) {} + + // ext_gyro_gain - set external gyro gain in range 0 ~ 1 + virtual void ext_gyro_gain(float gain) {} + // output - sends commands to the motors void output(); @@ -205,9 +207,6 @@ protected: AP_Int8 _servo_test; // sets number of cycles to test servo movement on bootup // internal variables - float _rollFactor[AP_MOTORS_HELI_NUM_SWASHPLATE_SERVOS]; - float _pitchFactor[AP_MOTORS_HELI_NUM_SWASHPLATE_SERVOS]; - float _collectiveFactor[AP_MOTORS_HELI_NUM_SWASHPLATE_SERVOS]; float _collective_mid_pct = 0.0f; // collective mid parameter value converted to 0 ~ 1 range uint8_t _servo_test_cycle_counter = 0; // number of test cycles left to run after bootup }; diff --git a/libraries/AP_Motors/AP_MotorsHeli_RSC.h b/libraries/AP_Motors/AP_MotorsHeli_RSC.h index 938445e9ba..a9eaba1ae5 100644 --- a/libraries/AP_Motors/AP_MotorsHeli_RSC.h +++ b/libraries/AP_Motors/AP_MotorsHeli_RSC.h @@ -24,6 +24,7 @@ enum RotorControlMode { class AP_MotorsHeli_RSC { public: friend class AP_MotorsHeli_Single; + friend class AP_MotorsHeli_Dual; AP_MotorsHeli_RSC(SRV_Channel::Aux_servo_function_t aux_fn, uint8_t default_channel) : diff --git a/libraries/AP_Motors/AP_MotorsHeli_Single.h b/libraries/AP_Motors/AP_MotorsHeli_Single.h index f51b0a26e3..04befe2b4c 100644 --- a/libraries/AP_Motors/AP_MotorsHeli_Single.h +++ b/libraries/AP_Motors/AP_MotorsHeli_Single.h @@ -38,6 +38,9 @@ // COLYAW parameter min and max values #define AP_MOTORS_HELI_SINGLE_COLYAW_RANGE 10.0f +// maximum number of swashplate servos +#define AP_MOTORS_HELI_SINGLE_NUM_SWASHPLATE_SERVOS 3 + /// @class AP_MotorsHeli_Single class AP_MotorsHeli_Single : public AP_MotorsHeli { public: @@ -52,51 +55,51 @@ public: }; // set update rate to motors - a value in hertz - void set_update_rate(uint16_t speed_hz); + void set_update_rate(uint16_t speed_hz) override; // enable - starts allowing signals to be sent to motors and servos - void enable(); + void enable() override; // output_test - spin a motor at the pwm value specified // motor_seq is the motor's sequence number from 1 to the number of motors on the frame // pwm value is an actual pwm value that will be output, normally in the range of 1000 ~ 2000 - void output_test(uint8_t motor_seq, int16_t pwm); + void output_test(uint8_t motor_seq, int16_t pwm) override; // set_desired_rotor_speed - sets target rotor speed as a number from 0 ~ 1 - void set_desired_rotor_speed(float desired_speed); + void set_desired_rotor_speed(float desired_speed) override; // get_main_rotor_speed - gets estimated or measured main rotor speed - float get_main_rotor_speed() const { return _main_rotor.get_rotor_speed(); } + float get_main_rotor_speed() const override { return _main_rotor.get_rotor_speed(); } // get_desired_rotor_speed - gets target rotor speed as a number from 0 ~ 1 - float get_desired_rotor_speed() const { return _main_rotor.get_desired_speed(); } + float get_desired_rotor_speed() const override { return _main_rotor.get_desired_speed(); } // rotor_speed_above_critical - return true if rotor speed is above that critical for flight - bool rotor_speed_above_critical() const { return _main_rotor.get_rotor_speed() > _main_rotor.get_critical_speed(); } + bool rotor_speed_above_critical() const override { return _main_rotor.get_rotor_speed() > _main_rotor.get_critical_speed(); } // calculate_scalars - recalculates various scalars used - void calculate_scalars(); + void calculate_scalars() override; // calculate_armed_scalars - recalculates scalars that can change while armed - void calculate_armed_scalars(); + void calculate_armed_scalars() override; // get_motor_mask - returns a bitmask of which outputs are being used for motors or servos (1 means being used) // this can be used to ensure other pwm outputs (i.e. for servos) do not conflict - uint16_t get_motor_mask(); + uint16_t get_motor_mask() override; // ext_gyro_gain - set external gyro gain in range 0 ~ 1 - void ext_gyro_gain(float gain) { _ext_gyro_gain_std = gain * 1000.0f; } + void ext_gyro_gain(float gain) override { _ext_gyro_gain_std = gain * 1000.0f; } // has_flybar - returns true if we have a mechical flybar - bool has_flybar() const { return _flybar_mode; } + bool has_flybar() const override { return _flybar_mode; } // supports_yaw_passthrought - returns true if we support yaw passthrough - bool supports_yaw_passthrough() const { return _tail_type == AP_MOTORS_HELI_SINGLE_TAILTYPE_SERVO_EXTGYRO; } + bool supports_yaw_passthrough() const override { return _tail_type == AP_MOTORS_HELI_SINGLE_TAILTYPE_SERVO_EXTGYRO; } - void set_acro_tail(bool set) { _acro_tail = set; } + void set_acro_tail(bool set) override { _acro_tail = set; } // parameter_check - returns true if helicopter specific parameters are sensible, used for pre-arm check - bool parameter_check(bool display_msg) const; + bool parameter_check(bool display_msg) const override; // var_info static const struct AP_Param::GroupInfo var_info[]; @@ -107,13 +110,13 @@ protected: bool init_outputs() override; // update_motor_controls - sends commands to motor controllers - void update_motor_control(RotorControlState state); + void update_motor_control(RotorControlState state) override; // calculate_roll_pitch_collective_factors - calculate factors based on swash type and servo position - void calculate_roll_pitch_collective_factors(); + void calculate_roll_pitch_collective_factors() override; // heli_move_actuators - moves swash plate and tail rotor - void move_actuators(float roll_out, float pitch_out, float coll_in, float yaw_out); + void move_actuators(float roll_out, float pitch_out, float coll_in, float yaw_out) override; // move_yaw - moves the yaw servo void move_yaw(float yaw_out); @@ -122,7 +125,7 @@ protected: void write_aux(float servo_out); // servo_test - move servos through full range of movement - void servo_test(); + void servo_test() override; // external objects we depend upon AP_MotorsHeli_RSC _main_rotor; // main rotor @@ -156,4 +159,7 @@ protected: SRV_Channel *_servo_aux; bool _acro_tail = false; + float _rollFactor[AP_MOTORS_HELI_SINGLE_NUM_SWASHPLATE_SERVOS]; + float _pitchFactor[AP_MOTORS_HELI_SINGLE_NUM_SWASHPLATE_SERVOS]; + float _collectiveFactor[AP_MOTORS_HELI_SINGLE_NUM_SWASHPLATE_SERVOS]; }; diff --git a/libraries/AP_Motors/AP_Motors_Class.h b/libraries/AP_Motors/AP_Motors_Class.h index 73b2d84c9b..ec3cd6d381 100644 --- a/libraries/AP_Motors/AP_Motors_Class.h +++ b/libraries/AP_Motors/AP_Motors_Class.h @@ -37,6 +37,7 @@ public: MOTOR_FRAME_SINGLE = 8, MOTOR_FRAME_COAX = 9, MOTOR_FRAME_TAILSITTER = 10, + MOTOR_FRAME_HELI_DUAL = 11, }; enum motor_frame_type { MOTOR_FRAME_TYPE_PLUS = 0,