diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index 4196a25953..709486044a 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -403,9 +403,9 @@ static byte oldSwitchPosition; #if FRAME_CONFIG == HELI_FRAME // helicopter constructor requires more arguments #if INSTANT_PWM == 1 - MOTOR_CLASS motors(CONFIG_APM_HARDWARE, &APM_RC, &g.rc_1, &g.rc_2, &g.rc_3, &g.rc_4, &g.heli_servo_1, &g.heli_servo_2, &g.heli_servo_3, &g.heli_servo_4, AP_MOTORS_SPEED_INSTANT_PWM); // this hardware definition is slightly bad because it assumes APM_HARDWARE_APM2 == AP_MOTORS_APM2 + MOTOR_CLASS motors(CONFIG_APM_HARDWARE, &APM_RC, &g.rc_1, &g.rc_2, &g.rc_3, &g.rc_4, &g.rc_8, &g.heli_servo_1, &g.heli_servo_2, &g.heli_servo_3, &g.heli_servo_4, AP_MOTORS_SPEED_INSTANT_PWM); // this hardware definition is slightly bad because it assumes APM_HARDWARE_APM2 == AP_MOTORS_APM2 #else - MOTOR_CLASS motors(CONFIG_APM_HARDWARE, &APM_RC, &g.rc_1, &g.rc_2, &g.rc_3, &g.rc_4, &g.heli_servo_1, &g.heli_servo_2, &g.heli_servo_3, &g.heli_servo_4); + MOTOR_CLASS motors(CONFIG_APM_HARDWARE, &APM_RC, &g.rc_1, &g.rc_2, &g.rc_3, &g.rc_4, &g.rc_8, &g.heli_servo_1, &g.heli_servo_2, &g.heli_servo_3, &g.heli_servo_4); #endif #elif FRAME_CONFIG == TRI_FRAME // tri constructor requires additional rc_7 argument to allow tail servo reversing #if INSTANT_PWM == 1 diff --git a/libraries/AP_Motors/AP_MotorsHeli.cpp b/libraries/AP_Motors/AP_MotorsHeli.cpp index 24a9eb9cb7..71b97c89d6 100644 --- a/libraries/AP_Motors/AP_MotorsHeli.cpp +++ b/libraries/AP_Motors/AP_MotorsHeli.cpp @@ -11,7 +11,7 @@ #include "AP_MotorsHeli.h" const AP_Param::GroupInfo AP_MotorsHeli::var_info[] PROGMEM = { - AP_NESTEDGROUPINFO(AP_Motors, 0), + // @Param: SV1_POS // @DisplayName: Servo 1 Position @@ -96,7 +96,7 @@ const AP_Param::GroupInfo AP_MotorsHeli::var_info[] PROGMEM = { // @DisplayName: Swash Plate Type // @Description: Setting this to 0 will configure for a 3-servo CCPM. Setting this to 1 will configure for mechanically mixed "H1". // @User: Standard - AP_GROUPINFO("SWASH_TYPE", 10, AP_MotorsHeli, swash_type), // changed from trunk + AP_GROUPINFO("SWASH_TYPE", 10, AP_MotorsHeli, swash_type), // @Param: GYR_GAIM // @DisplayName: External Gyro Gain @@ -121,13 +121,23 @@ const AP_Param::GroupInfo AP_MotorsHeli::var_info[] PROGMEM = { // @Units: Degrees // @User: Advanced // @Increment: 1 - AP_GROUPINFO("PHANG", 13, AP_MotorsHeli, phase_angle), // changed from trunk + AP_GROUPINFO("PHANG", 13, AP_MotorsHeli, phase_angle), // @Param: COLYAW // @DisplayName: Collective-Yaw Mixing // @Description: This is a feed-forward compensation to automatically add rudder input when collective pitch is increased. // @Range: 0 5 - AP_GROUPINFO("COLYAW", 14, AP_MotorsHeli, collective_yaw_effect), // changed from trunk + AP_GROUPINFO("COLYAW", 14, AP_MotorsHeli, collective_yaw_effect), + + // @Param: GOV_SETPOINT + // @DisplayName: External Motor Governor Setpoint + // @Description: This is the PWM which is passed to the external motor governor when external governor is enabled. + // @Range: 1000 2000 + // @Units: PWM + // @Increment: 10 + // @User: Standard + AP_GROUPINFO("GOV_SETPOINT", 15, AP_MotorsHeli, ext_gov_setpoint), + AP_GROUPEND }; @@ -159,6 +169,7 @@ void AP_MotorsHeli::enable() _rc->enable_out(_motor_to_channel_map[AP_MOTORS_MOT_3]); // swash servo 3 _rc->enable_out(_motor_to_channel_map[AP_MOTORS_MOT_4]); // yaw _rc->enable_out(AP_MOTORS_HELI_EXT_GYRO); // for external gyro + _rc->enable_out(AP_MOTORS_HELI_EXT_ESC); // for external ESC } // output_min - sends minimum values out to the motors @@ -186,6 +197,8 @@ void AP_MotorsHeli::output_armed() _rc_yaw->calc_pwm(); move_swash( _rc_roll->servo_out, _rc_pitch->servo_out, _rc_throttle->servo_out, _rc_yaw->servo_out ); + + ext_esc_control(); } // output_disarmed - sends commands to the motors @@ -469,4 +482,62 @@ void AP_MotorsHeli::move_swash(int16_t roll_out, int16_t pitch_out, int16_t coll _rc->Force_Out0_Out1(); _rc->Force_Out2_Out3(); } -} \ No newline at end of file +} + +void AP_MotorsHeli::ext_esc_control() + +{ + switch ( AP_MOTORS_ESC_MODE_PASSTHROUGH ) { + + case AP_MOTORS_ESC_MODE_PASSTHROUGH: + if( armed() ){ + _rc->OutputCh(AP_MOTORS_HELI_EXT_ESC, _rc_8->radio_in); + } else { + _rc->OutputCh(AP_MOTORS_HELI_EXT_ESC, _rc_8->radio_min); + } + break; + + case AP_MOTORS_ESC_MODE_EXT_GOV: + + if( armed() && _rc_throttle->control_in > 10){ + if (ext_esc_ramp < AP_MOTORS_EXT_ESC_RAMP_UP){ + ext_esc_ramp++; + ext_esc_output = map(ext_esc_ramp, 0, AP_MOTORS_EXT_ESC_RAMP_UP, 1000, ext_gov_setpoint); + } else { + ext_esc_output = ext_gov_setpoint; + } + } else { + ext_esc_ramp = 0; //Return ESC Ramp to 0 + ext_esc_output = 1000; //Just to be sure ESC output is 0 + } + _rc->OutputCh(AP_MOTORS_HELI_EXT_ESC, ext_esc_output); + break; + + // case 3: // Open Loop ESC Control + // + // coll_scaled = _motors->coll_out_scaled + 1000; + // if(coll_scaled <= _motors->collective_mid){ + // esc_ol_output = map(coll_scaled, _motors->collective_min, _motors->collective_mid, esc_out_low, esc_out_mid); // Bottom half of V-curve + // } else if (coll_scaled > _motors->collective_mid){ + // esc_ol_output = map(coll_scaled, _motors->collective_mid, _motors->collective_max, esc_out_mid, esc_out_high); // Top half of V-curve + // } else { esc_ol_output = 1000; } // Just in case. + // + // if(_motors->armed() && _rc_throttle->control_in > 10){ + // if (ext_esc_ramp < ext_esc_ramp_up){ + // ext_esc_ramp++; + // ext_esc_output = map(ext_esc_ramp, 0, ext_esc_ramp_up, 1000, esc_ol_output); + // } else { + // ext_esc_output = esc_ol_output; + // } + // } else { + // ext_esc_ramp = 0; //Return ESC Ramp to 0 + // ext_esc_output = 1000; //Just to be sure ESC output is 0 + //} + // _rc->OutputCh(AP_MOTORS_HELI_EXT_ESC, ext_esc_output); + // break; + + + default: + break; + } +}; \ No newline at end of file diff --git a/libraries/AP_Motors/AP_MotorsHeli.h b/libraries/AP_Motors/AP_MotorsHeli.h index 3d3b162a73..b880fde656 100644 --- a/libraries/AP_Motors/AP_MotorsHeli.h +++ b/libraries/AP_Motors/AP_MotorsHeli.h @@ -27,6 +27,17 @@ #define AP_MOTORS_HELI_SWASH_CCPM 0 #define AP_MOTORS_HELI_SWASH_H1 1 +// ext ESC definitions +#define AP_MOTORS_HELI_EXT_ESC CH_8 +#define AP_MOTORS_ESC_MODE_PASSTHROUGH 1 +#define AP_MOTORS_ESC_MODE_EXT_GOV 2 +#define AP_MOTORS_EXT_ESC_RAMP_UP 1000 //Temporary until we can make this a parameter +#define AP_MOTORS_EXT_ESC ACTIVE AP_MOTORS_ESC_MODE_PASSTHROUGH //Temporary until we can make this a parameter + + + +class AP_HeliControls; + /// @class AP_MotorsHeli class AP_MotorsHeli : public AP_Motors { public: @@ -38,6 +49,7 @@ public: RC_Channel* rc_pitch, RC_Channel* rc_throttle, RC_Channel* rc_yaw, + RC_Channel* rc_8, RC_Channel* swash_servo_1, RC_Channel* swash_servo_2, RC_Channel* swash_servo_3, @@ -48,6 +60,7 @@ public: _servo_2(swash_servo_2), _servo_3(swash_servo_3), _servo_4(yaw_servo), + _rc_8(rc_8), swash_type(AP_MOTORS_HELI_SWASH_CCPM), servo1_pos (-60), servo2_pos (60), @@ -57,83 +70,98 @@ public: collective_min (1250), collective_max (1750), collective_mid (1500), - ext_gyro_enabled (0), - ext_gyro_gain (1350), + ext_gyro_enabled (0), + ext_gyro_gain (1350), phase_angle (0), collective_yaw_effect (0), servo_manual (0), throttle_mid(0), + ext_gov_setpoint(1500), _roll_scaler(1), _pitch_scaler(1), _collective_scalar(1), _swash_initialised(false) {}; + RC_Channel *_servo_1; + RC_Channel *_servo_2; + RC_Channel *_servo_3; + RC_Channel *_servo_4; + RC_Channel *_rc_8; + AP_Int8 swash_type; + AP_Int16 servo1_pos; + AP_Int16 servo2_pos; + AP_Int16 servo3_pos; + AP_Int16 collective_min; + AP_Int16 collective_max; + AP_Int16 collective_mid; + AP_Int16 ext_gyro_enabled; + AP_Int16 ext_gyro_gain; + AP_Int16 roll_max; + AP_Int16 pitch_max; + AP_Int16 phase_angle; + AP_Int16 collective_yaw_effect; + AP_Int8 servo_manual; // used to trigger swash reset from mission planner + AP_Int16 ext_gov_setpoint; // maximum output to the motor governor + int16_t throttle_mid; // throttle mid point in pwm form (i.e. 0 ~ 1000) + int16_t coll_out_scaled; + + // init - virtual void Init(); + void Init(); // set update rate to motors - a value in hertz or AP_MOTORS_SPEED_INSTANT_PWM for instant pwm // you must have setup_motors before calling this - virtual void set_update_rate( uint16_t speed_hz ); + void set_update_rate( uint16_t speed_hz ); // enable - starts allowing signals to be sent to motors - virtual void enable(); + void enable(); // get basic information about the platform - virtual uint8_t get_num_motors() { return 4; }; + uint8_t get_num_motors() { return 5; }; // motor test - virtual void output_test(); + void output_test(); // output_min - sends minimum values out to the motors - virtual void output_min(); - - // reset_swash - free up swash for maximum movements. Used for set-up - virtual void reset_swash(); + void output_min(); // init_swash - initialise the swash plate - virtual void init_swash(); - - // heli_move_swash - moves swash plate to attitude of parameters passed in - virtual void move_swash(int16_t roll_out, int16_t pitch_out, int16_t coll_out, int16_t yaw_out); + void init_swash(); + // output - sends commands to the motors + void output_armed(); + // var_info for holding Parameter information static const struct AP_Param::GroupInfo var_info[]; -//protected: - // output - sends commands to the motors - virtual void output_armed(); - virtual void output_disarmed(); +protected: + + // heli_move_swash - moves swash plate to attitude of parameters passed in + void move_swash(int16_t roll_out, int16_t pitch_out, int16_t coll_out, int16_t yaw_out); + + // reset_swash - free up swash for maximum movements. Used for set-up + void reset_swash(); + + void output_disarmed(); + + void ext_esc_control(); float _rollFactor[AP_MOTORS_HELI_NUM_SWASHPLATE_SERVOS]; float _pitchFactor[AP_MOTORS_HELI_NUM_SWASHPLATE_SERVOS]; float _collectiveFactor[AP_MOTORS_HELI_NUM_SWASHPLATE_SERVOS]; - RC_Channel *_servo_1; - RC_Channel *_servo_2; - RC_Channel *_servo_3; - RC_Channel *_servo_4; - AP_Int8 swash_type; - AP_Int16 servo1_pos; - AP_Int16 servo2_pos; - AP_Int16 servo3_pos; - AP_Int16 roll_max; - AP_Int16 pitch_max; - AP_Int16 collective_min; - AP_Int16 collective_max; - AP_Int16 collective_mid; - AP_Int16 ext_gyro_enabled; - AP_Int16 ext_gyro_gain; - AP_Int16 phase_angle; - AP_Int16 collective_yaw_effect; - AP_Int8 servo_manual; // used to trigger swash reset from mission planner + // internally used variables - int16_t throttle_mid; // throttle mid point in pwm form (i.e. 0 ~ 1000) + float _roll_scaler; // scaler to convert roll input from radio (i.e. -4500 ~ 4500) to max roll range float _pitch_scaler; // scaler to convert pitch input from radio (i.e. -4500 ~ 4500) to max pitch range float _collective_scalar; // throttle scalar to convert pwm form (i.e. 0 ~ 1000) passed in to actual servo range (i.e 1250~1750 would be 500) bool _swash_initialised; // true if swash has been initialised + int16_t ext_esc_output; // final output to the external motor governor 1000-2000 + int16_t ext_esc_ramp; // current state of ramping + };