AP_Motors: integrate AP_MotorsHeli_Dual

fixed issues with combining with Heli_Single
This commit is contained in:
Andrew Tridgell 2017-03-14 20:46:48 +11:00
parent 998231ab0d
commit 1ad5e1db4e
5 changed files with 33 additions and 25 deletions

View File

@ -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"

View File

@ -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
};

View File

@ -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) :

View File

@ -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];
};

View File

@ -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,