diff --git a/libraries/AP_Motors/AP_MotorsHeli.cpp b/libraries/AP_Motors/AP_MotorsHeli.cpp index b7f42e19e4..f45a6f7ef3 100644 --- a/libraries/AP_Motors/AP_MotorsHeli.cpp +++ b/libraries/AP_Motors/AP_MotorsHeli.cpp @@ -138,6 +138,28 @@ const AP_Param::GroupInfo AP_MotorsHeli::var_info[] PROGMEM = { // @User: Standard AP_GROUPINFO("GOV_SETPOINT", 15, AP_MotorsHeli, ext_gov_setpoint, 1500), + // @Param: RSC_MODE + // @DisplayName: Rotor Speed Control Mode + // @Description: This sets which ESC control mode is active. + // @Range: 1 3 + // @User: Standard + AP_GROUPINFO("RSC_MODE", 16, AP_MotorsHeli, rsc_mode, 1), + + // @Param: RSC_RATE + // @DisplayName: RSC Ramp Rate + // @Description: This sets the time the RSC takes to ramp up to full speed (Soft Start). + // @Range: 0 6000 + // @Units: Seconds + // @User: Standard + AP_GROUPINFO("RSC_RATE", 17, AP_MotorsHeli, rsc_ramp_up_rate, 1000), + + // @Param: ACRO_MODE + // @DisplayName: Acro Mode Selector + // @Description: This sets which acro mode is active. (1) is Flybarless (2) is Flybarred Pass-Through + // @Range: 1 2 + // @User: Standard + AP_GROUPINFO("ACRO_MODE", 18, AP_MotorsHeli, acro_mode, 1), + AP_GROUPEND }; @@ -169,7 +191,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 + _rc->enable_out(AP_MOTORS_HELI_EXT_RSC); // for external RSC } // output_min - sends minimum values out to the motors @@ -198,7 +220,7 @@ void AP_MotorsHeli::output_armed() move_swash( _rc_roll->servo_out, _rc_pitch->servo_out, _rc_throttle->servo_out, _rc_yaw->servo_out ); - ext_esc_control(); + rsc_control(); } // output_disarmed - sends commands to the motors @@ -484,42 +506,42 @@ void AP_MotorsHeli::move_swash(int16_t roll_out, int16_t pitch_out, int16_t coll } } -void AP_MotorsHeli::ext_esc_control() +void AP_MotorsHeli::rsc_control() { - switch ( AP_MOTORS_EXT_ESC_ACTIVE ) { + switch ( rsc_mode ) { - case AP_MOTORS_ESC_MODE_PASSTHROUGH: + case AP_MOTORSHELI_RSC_MODE_CH8_PASSTHROUGH: if( armed() && _rc_8->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, _rc_8->control_in); + if (rsc_ramp < rsc_ramp_up_rate){ + rsc_ramp++; + rsc_output = map(rsc_ramp, 0, rsc_ramp_up_rate, 1000, _rc_8->control_in); } else { - ext_esc_output = _rc_8->control_in; + rsc_output = _rc_8->control_in; } } else if( !armed() ) { - _rc->OutputCh(AP_MOTORS_HELI_EXT_ESC, _rc_8->radio_min); - ext_esc_ramp = 0; //Return ESC Ramp to 0 + _rc->OutputCh(AP_MOTORS_HELI_EXT_RSC, _rc_8->radio_min); + rsc_ramp = 0; //Return RSC Ramp to 0 } break; - case AP_MOTORS_ESC_MODE_EXT_GOV: + case AP_MOTORSHELI_RSC_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); + if (rsc_ramp < rsc_ramp_up_rate){ + rsc_ramp++; + rsc_output = map(rsc_ramp, 0, rsc_ramp_up_rate, 1000, ext_gov_setpoint); } else { - ext_esc_output = ext_gov_setpoint; + rsc_output = ext_gov_setpoint; } } else { - ext_esc_ramp--; //Return ESC Ramp to 0 - if (ext_esc_ramp < 0){ - ext_esc_ramp = 0; + rsc_ramp--; //Return RSC Ramp to 0 slowly, allowing for "warm restart" + if (rsc_ramp < 0){ + rsc_ramp = 0; } - ext_esc_output = 1000; //Just to be sure ESC output is 0 + rsc_output = 1000; //Just to be sure RSC output is 0 } - _rc->OutputCh(AP_MOTORS_HELI_EXT_ESC, ext_esc_output); + _rc->OutputCh(AP_MOTORS_HELI_EXT_RSC, rsc_output); break; // case 3: // Open Loop ESC Control diff --git a/libraries/AP_Motors/AP_MotorsHeli.h b/libraries/AP_Motors/AP_MotorsHeli.h index 996065c9ae..f6fd2e4d57 100644 --- a/libraries/AP_Motors/AP_MotorsHeli.h +++ b/libraries/AP_Motors/AP_MotorsHeli.h @@ -34,13 +34,10 @@ #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 - +// ext RSC definitions +#define AP_MOTORS_HELI_EXT_RSC CH_8 +#define AP_MOTORSHELI_RSC_MODE_CH8_PASSTHROUGH 1 +#define AP_MOTORSHELI_RSC_MODE_EXT_GOV 2 class AP_HeliControls; @@ -96,6 +93,9 @@ public: 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 + AP_Int8 rsc_mode; // sets the mode for rotor speed controller + AP_Int16 rsc_ramp_up_rate; // sets the time in 100th seconds the RSC takes to ramp up to speed + AP_Int8 acro_mode; // selects FBL Acro Mode, or Flybarred Acro Mode int16_t throttle_mid; // throttle mid point in pwm form (i.e. 0 ~ 1000) @@ -137,7 +137,7 @@ protected: void output_disarmed(); - void ext_esc_control(); + void rsc_control(); float _rollFactor[AP_MOTORS_HELI_NUM_SWASHPLATE_SERVOS]; float _pitchFactor[AP_MOTORS_HELI_NUM_SWASHPLATE_SERVOS]; @@ -151,8 +151,8 @@ protected: 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 + int16_t rsc_output; // final output to the external motor governor 1000-2000 + int16_t rsc_ramp; // current state of ramping };