diff --git a/libraries/AP_Motors/AP_MotorsHeli.cpp b/libraries/AP_Motors/AP_MotorsHeli.cpp index 55f5b7080f..be138cdeec 100644 --- a/libraries/AP_Motors/AP_MotorsHeli.cpp +++ b/libraries/AP_Motors/AP_MotorsHeli.cpp @@ -35,7 +35,7 @@ const AP_Param::GroupInfo AP_MotorsHeli::var_info[] PROGMEM = { // @Units: Degrees // @User: Standard // @Increment: 1 - AP_GROUPINFO("SV1_POS", 1, AP_MotorsHeli, servo1_pos, AP_MOTORS_HELI_SERVO1_POS), + AP_GROUPINFO("SV1_POS", 1, AP_MotorsHeli, _servo1_pos, AP_MOTORS_HELI_SERVO1_POS), // @Param: SV2_POS // @DisplayName: Servo 2 Position @@ -44,7 +44,7 @@ const AP_Param::GroupInfo AP_MotorsHeli::var_info[] PROGMEM = { // @Units: Degrees // @User: Standard // @Increment: 1 - AP_GROUPINFO("SV2_POS", 2, AP_MotorsHeli, servo2_pos, AP_MOTORS_HELI_SERVO2_POS), + AP_GROUPINFO("SV2_POS", 2, AP_MotorsHeli, _servo2_pos, AP_MOTORS_HELI_SERVO2_POS), // @Param: SV3_POS // @DisplayName: Servo 3 Position @@ -53,7 +53,7 @@ const AP_Param::GroupInfo AP_MotorsHeli::var_info[] PROGMEM = { // @Units: Degrees // @User: Standard // @Increment: 1 - AP_GROUPINFO("SV3_POS", 3, AP_MotorsHeli, servo3_pos, AP_MOTORS_HELI_SERVO3_POS), + AP_GROUPINFO("SV3_POS", 3, AP_MotorsHeli, _servo3_pos, AP_MOTORS_HELI_SERVO3_POS), // @Param: ROL_MAX // @DisplayName: Swash Roll Angle Max @@ -62,7 +62,7 @@ const AP_Param::GroupInfo AP_MotorsHeli::var_info[] PROGMEM = { // @Units: Centi-Degrees // @Increment: 100 // @User: Advanced - AP_GROUPINFO("ROL_MAX", 4, AP_MotorsHeli, roll_max, AP_MOTORS_HELI_SWASH_ROLL_MAX), + AP_GROUPINFO("ROL_MAX", 4, AP_MotorsHeli, _roll_max, AP_MOTORS_HELI_SWASH_ROLL_MAX), // @Param: PIT_MAX // @DisplayName: Swash Pitch Angle Max @@ -71,7 +71,7 @@ const AP_Param::GroupInfo AP_MotorsHeli::var_info[] PROGMEM = { // @Units: Centi-Degrees // @Increment: 100 // @User: Advanced - AP_GROUPINFO("PIT_MAX", 5, AP_MotorsHeli, pitch_max, AP_MOTORS_HELI_SWASH_PITCH_MAX), + AP_GROUPINFO("PIT_MAX", 5, AP_MotorsHeli, _pitch_max, AP_MOTORS_HELI_SWASH_PITCH_MAX), // @Param: COL_MIN // @DisplayName: Collective Pitch Minimum @@ -80,7 +80,7 @@ const AP_Param::GroupInfo AP_MotorsHeli::var_info[] PROGMEM = { // @Units: PWM // @Increment: 1 // @User: Standard - AP_GROUPINFO("COL_MIN", 6, AP_MotorsHeli, collective_min, AP_MOTORS_HELI_COLLECTIVE_MIN), + AP_GROUPINFO("COL_MIN", 6, AP_MotorsHeli, _collective_min, AP_MOTORS_HELI_COLLECTIVE_MIN), // @Param: COL_MAX // @DisplayName: Collective Pitch Maximum @@ -89,7 +89,7 @@ const AP_Param::GroupInfo AP_MotorsHeli::var_info[] PROGMEM = { // @Units: PWM // @Increment: 1 // @User: Standard - AP_GROUPINFO("COL_MAX", 7, AP_MotorsHeli, collective_max, AP_MOTORS_HELI_COLLECTIVE_MAX), + AP_GROUPINFO("COL_MAX", 7, AP_MotorsHeli, _collective_max, AP_MOTORS_HELI_COLLECTIVE_MAX), // @Param: COL_MID // @DisplayName: Collective Pitch Mid-Point @@ -98,21 +98,21 @@ const AP_Param::GroupInfo AP_MotorsHeli::var_info[] PROGMEM = { // @Units: PWM // @Increment: 1 // @User: Standard - AP_GROUPINFO("COL_MID", 8, AP_MotorsHeli, collective_mid, AP_MOTORS_HELI_COLLECTIVE_MID), + AP_GROUPINFO("COL_MID", 8, AP_MotorsHeli, _collective_mid, AP_MOTORS_HELI_COLLECTIVE_MID), // @Param: GYR_ENABLE // @DisplayName: External Gyro Enabled // @Description: Enabled/Disable an external rudder gyro connected to channel 7. With no external gyro a more complex yaw controller is used // @Values: 0:Disabled,1:Enabled // @User: Standard - AP_GROUPINFO("GYR_ENABLE", 9, AP_MotorsHeli, ext_gyro_enabled, 0), + AP_GROUPINFO("GYR_ENABLE",9, AP_MotorsHeli, _ext_gyro_enabled, 0), // @Param: SWASH_TYPE // @DisplayName: Swash Type // @Description: Swash Type Setting - either 3-servo CCPM or H1 Mechanical Mixing // @Values: 0:3-Servo CCPM, 1:H1 Mechanical Mixing // @User: Standard - AP_GROUPINFO("SWASH_TYPE", 10, AP_MotorsHeli, swash_type, AP_MOTORS_HELI_SWASH_CCPM), + AP_GROUPINFO("SWASH_TYPE",10, AP_MotorsHeli, _swash_type, AP_MOTORS_HELI_SWASH_CCPM), // @Param: GYR_GAIN // @DisplayName: External Gyro Gain @@ -121,14 +121,14 @@ const AP_Param::GroupInfo AP_MotorsHeli::var_info[] PROGMEM = { // @Units: PWM // @Increment: 10 // @User: Standard - AP_GROUPINFO("GYR_GAIN", 11, AP_MotorsHeli, ext_gyro_gain, AP_MOTORS_HELI_EXT_GYRO_GAIN), + AP_GROUPINFO("GYR_GAIN",11, AP_MotorsHeli, _ext_gyro_gain, AP_MOTORS_HELI_EXT_GYRO_GAIN), // @Param: SV_MAN // @DisplayName: Manual Servo Mode // @Description: Pass radio inputs directly to servos for set-up. Do not set this manually! // @Values: 0:Disabled,1:Enabled // @User: Standard - AP_GROUPINFO("SV_MAN", 12, AP_MotorsHeli, servo_manual, 0), + AP_GROUPINFO("SV_MAN", 12, AP_MotorsHeli, _servo_manual, 0), // @Param: PHANG // @DisplayName: Swashplate Phase Angle Compensation @@ -137,13 +137,13 @@ const AP_Param::GroupInfo AP_MotorsHeli::var_info[] PROGMEM = { // @Units: Degrees // @User: Advanced // @Increment: 1 - AP_GROUPINFO("PHANG", 13, AP_MotorsHeli, phase_angle, 0), + AP_GROUPINFO("PHANG", 13, AP_MotorsHeli, _phase_angle, 0), // @Param: COLYAW // @DisplayName: Collective-Yaw Mixing // @Description: Feed-forward compensation to automatically add rudder input when collective pitch is increased. Can be positive or negative depending on mechanics. // @Range: -10 10 - AP_GROUPINFO("COLYAW", 14, AP_MotorsHeli, collective_yaw_effect, 0), + AP_GROUPINFO("COLYAW", 14, AP_MotorsHeli, _collective_yaw_effect, 0), // @Param: GOV_SETPOINT // @DisplayName: External Motor Governor Setpoint @@ -152,14 +152,14 @@ const AP_Param::GroupInfo AP_MotorsHeli::var_info[] PROGMEM = { // @Units: PWM // @Increment: 10 // @User: Standard - AP_GROUPINFO("GOV_SETPOINT", 15, AP_MotorsHeli, ext_gov_setpoint, AP_MOTORS_HELI_EXT_GOVERNOR_SETPOINT), + AP_GROUPINFO("GOV_SETPOINT", 15, AP_MotorsHeli, _ext_gov_setpoint, AP_MOTORS_HELI_EXT_GOVERNOR_SETPOINT), // @Param: RSC_MODE // @DisplayName: Rotor Speed Control Mode // @Description: Which main rotor ESC control mode is active - // @Values: 1:Ch8 passthrough, 2:External Governor + // @Values: 0:None, 1:Ch8 passthrough, 2:External Governor // @User: Standard - AP_GROUPINFO("RSC_MODE", 16, AP_MotorsHeli, rsc_mode, AP_MOTORS_HELI_RSC_MODE_CH8_PASSTHROUGH), + AP_GROUPINFO("RSC_MODE", 16, AP_MotorsHeli, _rsc_mode, AP_MOTORS_HELI_RSC_MODE_CH8_PASSTHROUGH), // @Param: RSC_RATE // @DisplayName: RSC Ramp Rate @@ -167,41 +167,51 @@ const AP_Param::GroupInfo AP_MotorsHeli::var_info[] PROGMEM = { // @Range: 0 6000 // @Units: 100ths of Seconds // @User: Standard - AP_GROUPINFO("RSC_RATE", 17, AP_MotorsHeli, rsc_ramp_up_rate, AP_MOTORS_HELI_RSC_RATE), + AP_GROUPINFO("RSC_RATE", 17, AP_MotorsHeli, _rsc_ramp_up_rate, AP_MOTORS_HELI_RSC_RATE), // @Param: FLYBAR_MODE // @DisplayName: Flybar Mode Selector // @Description: Flybar present or not. Affects attitude controller used during ACRO flight mode // @Range: 0:NoFlybar 1:Flybar // @User: Standard - AP_GROUPINFO("FLYBAR_MODE", 18, AP_MotorsHeli, flybar_mode, AP_MOTORS_HELI_NOFLYBAR), + AP_GROUPINFO("FLYBAR_MODE", 18, AP_MotorsHeli, _flybar_mode, AP_MOTORS_HELI_NOFLYBAR), // @Param: STAB_COL_MIN // @DisplayName: Stabilize Throttle Minimum - // @Description: Minimum collective position while flying in Stabilize Mode + // @Description: Minimum collective position while pilot directly controls collective // @Range: 0 50 // @Units: Percent // @Increment: 1 // @User: Standard - AP_GROUPINFO("STAB_COL_MIN", 19, AP_MotorsHeli, stab_col_min, AP_MOTORS_HELI_STAB_COL_MIN), + AP_GROUPINFO("STAB_COL_MIN", 19, AP_MotorsHeli, _manual_collective_min, AP_MOTORS_HELI_MANUAL_COLLECTIVE_MIN), // @Param: STAB_COL_MAX // @DisplayName: Stabilize Throttle Maximum - // @Description: Maximum collective position while flying in Stabilize Mode + // @Description: Maximum collective position while pilot directly controls collective // @Range: 50 100 // @Units: Percent // @Increment: 1 // @User: Standard - AP_GROUPINFO("STAB_COL_MAX", 20, AP_MotorsHeli, stab_col_max, AP_MOTORS_HELI_STAB_COL_MAX), + AP_GROUPINFO("STAB_COL_MAX", 20, AP_MotorsHeli, _manual_collective_max, AP_MOTORS_HELI_MANUAL_COLLECTIVE_MAX), AP_GROUPEND }; +// +// public methods +// + // init void AP_MotorsHeli::Init() { // set update rate set_update_rate(_speed_hz); + + // ensure inputs are not passed through to servos + _servo_manual = 0; + + // initialise swash plate + init_swash(); } // set update rate to motors - a value in hertz @@ -238,36 +248,8 @@ void AP_MotorsHeli::output_min() move_swash(0,0,500,0); } -// output_armed - sends commands to the motors -void AP_MotorsHeli::output_armed() -{ - // if manual override (i.e. when setting up swash), pass pilot commands straight through to swash - if( servo_manual == 1 ) { - _rc_roll->servo_out = _rc_roll->control_in; - _rc_pitch->servo_out = _rc_pitch->control_in; - _rc_throttle->servo_out = _rc_throttle->control_in; - _rc_yaw->servo_out = _rc_yaw->control_in; - } - //static int counter = 0; - _rc_roll->calc_pwm(); - _rc_pitch->calc_pwm(); - _rc_throttle->calc_pwm(); - _rc_yaw->calc_pwm(); - - move_swash( _rc_roll->servo_out, _rc_pitch->servo_out, _rc_throttle->servo_out, _rc_yaw->servo_out ); - - rsc_control(); -} - -// output_disarmed - sends commands to the motors -void AP_MotorsHeli::output_disarmed() -{ - // for helis - armed or disarmed we allow servos to move - output_armed(); -} - -// output_disarmed - sends commands to the motors +// output_test - wiggle servos in order to show connections are correct void AP_MotorsHeli::output_test() { int16_t i; @@ -305,8 +287,8 @@ void AP_MotorsHeli::output_test() } // external gyro - if( ext_gyro_enabled ) { - hal.rcout->write(AP_MOTORS_HELI_EXT_GYRO, ext_gyro_gain); + if (_ext_gyro_enabled) { + hal.rcout->write(AP_MOTORS_HELI_EXT_GYRO, _ext_gyro_gain); } // servo 4 @@ -323,6 +305,55 @@ void AP_MotorsHeli::output_test() output_min(); } +// allow_arming - returns true if main rotor is spinning and it is ok to arm +bool AP_MotorsHeli::allow_arming() +{ + // ensure main rotor has started + if (_rsc_mode != AP_MOTORS_HELI_RSC_MODE_NONE && _rc_8->control_in >= 10) { + return false; + } + + // all other cases it is ok to arm + return true; +} + +// +// protected methods +// + +// output_armed - sends commands to the motors +void AP_MotorsHeli::output_armed() +{ + // if manual override (i.e. when setting up swash), pass pilot commands straight through to swash + if (_servo_manual == 1) { + _rc_roll->servo_out = _rc_roll->control_in; + _rc_pitch->servo_out = _rc_pitch->control_in; + _rc_throttle->servo_out = _rc_throttle->control_in; + _rc_yaw->servo_out = _rc_yaw->control_in; + } + + //static int counter = 0; + _rc_roll->calc_pwm(); + _rc_pitch->calc_pwm(); + _rc_throttle->calc_pwm(); + _rc_yaw->calc_pwm(); + + move_swash( _rc_roll->servo_out, _rc_pitch->servo_out, _rc_throttle->servo_out, _rc_yaw->servo_out ); + + rsc_control(); +} + +// output_disarmed - sends commands to the motors +void AP_MotorsHeli::output_disarmed() +{ + // for helis - armed or disarmed we allow servos to move + output_armed(); +} + +// +// private methods +// + // reset_swash - free up swash for maximum movements. Used for set-up void AP_MotorsHeli::reset_swash() { @@ -334,49 +365,17 @@ void AP_MotorsHeli::reset_swash() _servo_3->radio_min = 1000; _servo_3->radio_max = 2000; - if( swash_type == AP_MOTORS_HELI_SWASH_CCPM ) { //CCPM Swashplate, perform servo control mixing - - // roll factors - _rollFactor[CH_1] = cosf(radians(servo1_pos + 90 - phase_angle)); - _rollFactor[CH_2] = cosf(radians(servo2_pos + 90 - phase_angle)); - _rollFactor[CH_3] = cosf(radians(servo3_pos + 90 - phase_angle)); - - // pitch factors - _pitchFactor[CH_1] = cosf(radians(servo1_pos - phase_angle)); - _pitchFactor[CH_2] = cosf(radians(servo2_pos - phase_angle)); - _pitchFactor[CH_3] = cosf(radians(servo3_pos - phase_angle)); - - // collective factors - _collectiveFactor[CH_1] = 1; - _collectiveFactor[CH_2] = 1; - _collectiveFactor[CH_3] = 1; - - }else{ //H1 Swashplate, keep servo outputs seperated - - // roll factors - _rollFactor[CH_1] = 1; - _rollFactor[CH_2] = 0; - _rollFactor[CH_3] = 0; - - // pitch factors - _pitchFactor[CH_1] = 0; - _pitchFactor[CH_2] = 1; - _pitchFactor[CH_3] = 0; - - // collective factors - _collectiveFactor[CH_1] = 0; - _collectiveFactor[CH_2] = 0; - _collectiveFactor[CH_3] = 1; - } + // calculate factors based on swash type and servo position + calculate_roll_pitch_collective_factors(); // set roll, pitch and throttle scaling _roll_scaler = 1.0f; _pitch_scaler = 1.0f; _collective_scalar = ((float)(_rc_throttle->radio_max - _rc_throttle->radio_min))/1000.0f; - _stab_throttle_scalar = 1.0f; + _collective_scalar_manual = 1.0f; // we must be in set-up mode so mark swash as uninitialised - _swash_initialised = false; + _heliflags.swash_initialised = false; } // init_swash - initialise the swash plate @@ -389,34 +388,51 @@ void AP_MotorsHeli::init_swash() _servo_3->set_range(0,1000); _servo_4->set_angle(4500); - // ensure _coll values are reasonable - if( collective_min >= collective_max ) { - collective_min = 1000; - collective_max = 2000; + // range check collective min, max and mid + if( _collective_min >= _collective_max ) { + _collective_min = 1000; + _collective_max = 2000; } + _collective_mid = constrain_int16(_collective_mid, _collective_min, _collective_max); - collective_mid = constrain_int16(collective_mid, collective_min, collective_max); + // calculate collective mid point as a number from 0 to 1000 + _collective_mid_pwm = ((float)(_collective_mid-_collective_min))/((float)(_collective_max-_collective_min))*1000.0f; - // calculate throttle mid point - throttle_mid = ((float)(collective_mid-collective_min))/((float)(collective_max-collective_min))*1000.0f; + // determine roll, pitch and collective input scaling + _roll_scaler = (float)_roll_max/4500.0f; + _pitch_scaler = (float)_pitch_max/4500.0f; + _collective_scalar = ((float)(_collective_max-_collective_min))/1000.0f; + _collective_scalar_manual = ((float)(_manual_collective_max - _manual_collective_min))/100.0f; - // determine roll, pitch and throttle scaling - _roll_scaler = (float)roll_max/4500.0f; - _pitch_scaler = (float)pitch_max/4500.0f; - _collective_scalar = ((float)(collective_max-collective_min))/1000.0f; - _stab_throttle_scalar = ((float)(stab_col_max - stab_col_min))/100.0f; + // calculate factors based on swash type and servo position + calculate_roll_pitch_collective_factors(); - if( swash_type == AP_MOTORS_HELI_SWASH_CCPM ) { //CCPM Swashplate, perform control mixing + // servo min/max values + _servo_1->radio_min = 1000; + _servo_1->radio_max = 2000; + _servo_2->radio_min = 1000; + _servo_2->radio_max = 2000; + _servo_3->radio_min = 1000; + _servo_3->radio_max = 2000; + + // mark swash as initialised + _heliflags.swash_initialised = true; +} + +// calculate_roll_pitch_collective_factors - calculate factors based on swash type and servo position +void AP_MotorsHeli::calculate_roll_pitch_collective_factors() +{ + if (_swash_type == AP_MOTORS_HELI_SWASH_CCPM) { //CCPM Swashplate, perform control mixing // roll factors - _rollFactor[CH_1] = cosf(radians(servo1_pos + 90 - phase_angle)); - _rollFactor[CH_2] = cosf(radians(servo2_pos + 90 - phase_angle)); - _rollFactor[CH_3] = cosf(radians(servo3_pos + 90 - phase_angle)); + _rollFactor[CH_1] = cosf(radians(_servo1_pos + 90 - _phase_angle)); + _rollFactor[CH_2] = cosf(radians(_servo2_pos + 90 - _phase_angle)); + _rollFactor[CH_3] = cosf(radians(_servo3_pos + 90 - _phase_angle)); // pitch factors - _pitchFactor[CH_1] = cosf(radians(servo1_pos - phase_angle)); - _pitchFactor[CH_2] = cosf(radians(servo2_pos - phase_angle)); - _pitchFactor[CH_3] = cosf(radians(servo3_pos - phase_angle)); + _pitchFactor[CH_1] = cosf(radians(_servo1_pos - _phase_angle)); + _pitchFactor[CH_2] = cosf(radians(_servo2_pos - _phase_angle)); + _pitchFactor[CH_3] = cosf(radians(_servo3_pos - _phase_angle)); // collective factors _collectiveFactor[CH_1] = 1; @@ -440,17 +456,6 @@ void AP_MotorsHeli::init_swash() _collectiveFactor[CH_2] = 0; _collectiveFactor[CH_3] = 1; } - - // servo min/max values - _servo_1->radio_min = 1000; - _servo_1->radio_max = 2000; - _servo_2->radio_min = 1000; - _servo_2->radio_max = 2000; - _servo_3->radio_min = 1000; - _servo_3->radio_max = 2000; - - // mark swash as initialised - _swash_initialised = true; } // @@ -466,16 +471,16 @@ void AP_MotorsHeli::move_swash(int16_t roll_out, int16_t pitch_out, int16_t coll int16_t yaw_offset = 0; int16_t coll_out_scaled; - if( servo_manual == 1 ) { // are we in manual servo mode? (i.e. swash set-up mode)? + if (_servo_manual == 1) { // are we in manual servo mode? (i.e. swash set-up mode)? // check if we need to free up the swash - if( _swash_initialised ) { + if (_heliflags.swash_initialised) { reset_swash(); } coll_out_scaled = coll_in * _collective_scalar + _rc_throttle->radio_min - 1000; }else{ // regular flight mode // check if we need to reinitialise the swash - if( !_swash_initialised ) { + if (!_heliflags.swash_initialised) { init_swash(); } @@ -485,28 +490,28 @@ void AP_MotorsHeli::move_swash(int16_t roll_out, int16_t pitch_out, int16_t coll // coming into this equation at 4500 or less, and based on the original assumption of the // total _servo_x.servo_out range being -4500 to 4500. roll_out = roll_out * _roll_scaler; - roll_out = constrain_int16(roll_out, (int16_t)-roll_max, (int16_t)roll_max); + roll_out = constrain_int16(roll_out, (int16_t)-_roll_max, (int16_t)_roll_max); pitch_out = pitch_out * _pitch_scaler; - pitch_out = constrain_int16(pitch_out, (int16_t)-pitch_max, (int16_t)pitch_max); + pitch_out = constrain_int16(pitch_out, (int16_t)-_pitch_max, (int16_t)_pitch_max); // scale collective pitch - coll_out = constrain_int16(coll_in, 0, 1000); - if (stab_throttle){ - coll_out = coll_out * _stab_throttle_scalar + stab_col_min*10; - } - coll_out_scaled = coll_out * _collective_scalar + collective_min - 1000; - + _collective_out = constrain_int16(coll_in, 0, 1000); + if (_heliflags.manual_collective){ + _collective_out = _collective_out * _collective_scalar_manual + _manual_collective_min*10; + } + coll_out_scaled = _collective_out * _collective_scalar + _collective_min - 1000; + // rudder feed forward based on collective - if( !ext_gyro_enabled ) { - yaw_offset = collective_yaw_effect * abs(coll_out_scaled - throttle_mid); + if (!_ext_gyro_enabled) { + yaw_offset = _collective_yaw_effect * abs(coll_out_scaled - _collective_mid_pwm); } } // swashplate servos _servo_1->servo_out = (_rollFactor[CH_1] * roll_out + _pitchFactor[CH_1] * pitch_out)/10 + _collectiveFactor[CH_1] * coll_out_scaled + (_servo_1->radio_trim-1500); _servo_2->servo_out = (_rollFactor[CH_2] * roll_out + _pitchFactor[CH_2] * pitch_out)/10 + _collectiveFactor[CH_2] * coll_out_scaled + (_servo_2->radio_trim-1500); - if( swash_type == AP_MOTORS_HELI_SWASH_H1 ) { + if (_swash_type == AP_MOTORS_HELI_SWASH_H1) { _servo_1->servo_out += 500; _servo_2->servo_out += 500; } @@ -532,8 +537,8 @@ void AP_MotorsHeli::move_swash(int16_t roll_out, int16_t pitch_out, int16_t coll motor_out[AP_MOTORS_MOT_4] = _servo_4->radio_out; // output gyro value - if( ext_gyro_enabled ) { - hal.rcout->write(AP_MOTORS_HELI_EXT_GYRO, ext_gyro_gain); + if (_ext_gyro_enabled) { + hal.rcout->write(AP_MOTORS_HELI_EXT_GYRO, _ext_gyro_gain); } } @@ -542,61 +547,60 @@ static long map(long x, long in_min, long in_max, long out_min, long out_max) return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min; } - -void AP_MotorsHeli::rsc_control() { - - if (armed() && (rsc_ramp >= rsc_ramp_up_rate)){ // rsc_ramp will never increase if rsc_mode = 0 - if (motor_runup_timer < AP_MOTORS_HELI_MOTOR_RUNUP_TIME){ // therefore motor_runup_complete can never be true - motor_runup_timer++; +// rsc_control - update value to send to main rotor's ESC +void AP_MotorsHeli::rsc_control() +{ + if (armed() && (_rsc_ramp >= _rsc_ramp_up_rate)){ // rsc_ramp will never increase if rsc_mode = 0 + if (_motor_runup_timer < AP_MOTORS_HELI_MOTOR_RUNUP_TIME){ // therefore motor_runup_complete can never be true + _motor_runup_timer++; } else { - motor_runup_complete = true; + _heliflags.motor_runup_complete = true; } } else { - motor_runup_complete = false; // motor_runup_complete will go to false if we - motor_runup_timer = 0; // disarm or wind down the motor + _heliflags.motor_runup_complete = false; // motor_runup_complete will go to false if we + _motor_runup_timer = 0; // disarm or wind down the motor } - - switch ( rsc_mode ) { + switch (_rsc_mode) { case AP_MOTORS_HELI_RSC_MODE_CH8_PASSTHROUGH: if( armed() && (_rc_8->radio_in > (_rc_8->radio_min + 10))) { - if (rsc_ramp < rsc_ramp_up_rate) { - rsc_ramp++; - rsc_output = map(rsc_ramp, 0, rsc_ramp_up_rate, _rc_8->radio_min, _rc_8->radio_in); + if (_rsc_ramp < _rsc_ramp_up_rate) { + _rsc_ramp++; + _rsc_output = map(_rsc_ramp, 0, _rsc_ramp_up_rate, _rc_8->radio_min, _rc_8->radio_in); } else { - rsc_output = _rc_8->radio_in; + _rsc_output = _rc_8->radio_in; } } else { - rsc_ramp--; //Return RSC Ramp to 0 slowly, allowing for "warm restart" - if (rsc_ramp < 0) { - rsc_ramp = 0; - } - rsc_output = _rc_8->radio_min; + _rsc_ramp--; //Return RSC Ramp to 0 slowly, allowing for "warm restart" + if (_rsc_ramp < 0) { + _rsc_ramp = 0; + } + _rsc_output = _rc_8->radio_min; } - hal.rcout->write(AP_MOTORS_HELI_EXT_RSC, rsc_output); + hal.rcout->write(AP_MOTORS_HELI_EXT_RSC, _rsc_output); break; case AP_MOTORS_HELI_RSC_MODE_EXT_GOVERNOR: - if( armed() && _rc_8->control_in > 100) { - if (rsc_ramp < rsc_ramp_up_rate) { - rsc_ramp++; - rsc_output = map(rsc_ramp, 0, rsc_ramp_up_rate, 1000, ext_gov_setpoint); + if (armed() && _rc_8->control_in > 100) { + if (_rsc_ramp < _rsc_ramp_up_rate) { + _rsc_ramp++; + _rsc_output = map(_rsc_ramp, 0, _rsc_ramp_up_rate, 1000, _ext_gov_setpoint); } else { - rsc_output = ext_gov_setpoint; + _rsc_output = _ext_gov_setpoint; } } else { - rsc_ramp--; //Return RSC Ramp to 0 slowly, allowing for "warm restart" - if (rsc_ramp < 0) { - rsc_ramp = 0; + _rsc_ramp--; //Return RSC Ramp to 0 slowly, allowing for "warm restart" + if (_rsc_ramp < 0) { + _rsc_ramp = 0; } - rsc_output = 1000; //Just to be sure RSC output is 0 + _rsc_output = 1000; //Just to be sure RSC output is 0 } - hal.rcout->write(AP_MOTORS_HELI_EXT_RSC, rsc_output); + hal.rcout->write(AP_MOTORS_HELI_EXT_RSC, _rsc_output); break; default: break; } -}; +} diff --git a/libraries/AP_Motors/AP_MotorsHeli.h b/libraries/AP_Motors/AP_MotorsHeli.h index bd59fad583..bf302d9cee 100644 --- a/libraries/AP_Motors/AP_MotorsHeli.h +++ b/libraries/AP_Motors/AP_MotorsHeli.h @@ -41,15 +41,16 @@ #define AP_MOTORS_HELI_COLLECTIVE_MID 1500 // swash min and max position (expressed as percentage) while in stabilize mode -#define AP_MOTORS_HELI_STAB_COL_MIN 0 -#define AP_MOTORS_HELI_STAB_COL_MAX 100 +#define AP_MOTORS_HELI_MANUAL_COLLECTIVE_MIN 0 +#define AP_MOTORS_HELI_MANUAL_COLLECTIVE_MAX 100 // default external gyro gain (ch7 out) #define AP_MOTORS_HELI_EXT_GYRO_GAIN 1350 -// main rotor control types (ch8 out) -#define AP_MOTORS_HELI_RSC_MODE_CH8_PASSTHROUGH 1 -#define AP_MOTORS_HELI_RSC_MODE_EXT_GOVERNOR 2 +// main rotor speed control types (ch8 out) +#define AP_MOTORS_HELI_RSC_MODE_NONE 0 // main rotor ESC is directly connected to receiver +#define AP_MOTORS_HELI_RSC_MODE_CH8_PASSTHROUGH 1 // main rotor ESC is connected to RC8 (out) but pilot still directly controls speed with a passthrough from CH8 (in) +#define AP_MOTORS_HELI_RSC_MODE_EXT_GOVERNOR 2 // main rotor ESC is connected to RC8 and controlled by arducopter // default main rotor governor set-point (ch8 out) #define AP_MOTORS_HELI_EXT_GOVERNOR_SETPOINT 1500 @@ -86,54 +87,25 @@ public: _servo_3(swash_servo_3), _servo_4(yaw_servo), _rc_8(rc_8), - throttle_mid(0), _roll_scaler(1), _pitch_scaler(1), _collective_scalar(1), - _stab_throttle_scalar(1), - _swash_initialised(false), - stab_throttle(false), - motor_runup_complete(false) + _collective_scalar_manual(1), + _collective_out(0), + _collective_mid_pwm(0), + _rsc_output(0), + _rsc_ramp(0), + _motor_runup_timer(0) { AP_Param::setup_object_defaults(this, var_info); + + // initialise flags + _heliflags.swash_initialised = 0; + _heliflags.manual_collective = 0; + _heliflags.landing_collective = 0; + _heliflags.motor_runup_complete = 0; }; - // external objects we depend upon - RC_Channel *_servo_1; - RC_Channel *_servo_2; - RC_Channel *_servo_3; - RC_Channel *_servo_4; - RC_Channel *_rc_8; - - // parameters - AP_Int16 servo1_pos; // Angular location of swash servo #1 - AP_Int16 servo2_pos; // Angular location of swash servo #2 - AP_Int16 servo3_pos; // Angular location of swash servo #3 - AP_Int16 roll_max; // Maximum roll angle of the swash plate in centi-degrees - AP_Int16 pitch_max; // Maximum pitch angle of the swash plate in centi-degrees - AP_Int16 collective_min; // Lowest possible servo position for the swashplate - AP_Int16 collective_max; // Highest possible servo position for the swashplate - AP_Int16 collective_mid; // Swash servo position corresponding to zero collective pitch (or zero lift for Assymetrical blades) - AP_Int16 ext_gyro_enabled; // Enabled/Disable an external rudder gyro connected to channel 7. With no external gyro a more complex yaw controller is used - AP_Int8 swash_type; // Swash Type Setting - either 3-servo CCPM or H1 Mechanical Mixing - AP_Int16 ext_gyro_gain; // PWM sent to the external gyro on Ch7 - AP_Int8 servo_manual; // Pass radio inputs directly to servos during set-up through mission planner - AP_Int16 phase_angle; // Phase angle correction for rotor head. If pitching the swash forward induces a roll, this can be correct the problem - AP_Int16 collective_yaw_effect; // Feed-forward compensation to automatically add rudder input when collective pitch is increased. Can be positive or negative depending on mechanics. - AP_Int16 ext_gov_setpoint; // PWM passed to the external motor governor when external governor is enabledv - AP_Int8 rsc_mode; // Sets which main rotor ESC control mode is active - AP_Int16 rsc_ramp_up_rate; // The time in 100th seconds the RSC takes to ramp up to speed - AP_Int8 flybar_mode; // Flybar present or not. Affects attitude controller used during ACRO flight mode - AP_Int8 stab_col_min; // Minimum collective position while flying in Stabilize Mode - AP_Int8 stab_col_max; // Maximum collective position while flying in Stabilize Mode - - // internal variables - int16_t throttle_mid; // throttle mid point in pwm form (i.e. 0 ~ 1000) - - bool stab_throttle; // true if we are in Stabilize Mode for reduced Swash Range - bool motor_runup_complete; // true if the rotors have had enough time to wind up - int16_t coll_out; // returns the actual collective in use to the main code - // init void Init(); @@ -144,46 +116,124 @@ public: // enable - starts allowing signals to be sent to motors void enable(); - // motor test - void output_test(); - // output_min - sends minimum values out to the motors void output_min(); - // init_swash - initialise the swash plate - void init_swash(); + // output_test - wiggle servos in order to show connections are correct + void output_test(); - // output - sends commands to the motors - void output_armed(); + // + // heli specific methods + // + + // allow_arming - returns true if main rotor is spinning and it is ok to arm + bool allow_arming(); + + // ext_gyro_enabled - returns true if we have an external gyro for yaw control + bool ext_gyro_enabled() { return _ext_gyro_enabled; } + + // ext_gyro_gain - gets and sets external gyro gain output on ch7 + int16_t ext_gyro_gain() { return _ext_gyro_gain; } + void ext_gyro_gain(int16_t gain) { _ext_gyro_gain = gain; } + + // has_flybar - returns true if we have a mechical flybar + bool has_flybar() { return _flybar_mode; } + + // get_collective_mid - returns collective mid position as a number from 0 ~ 1000 + int16_t get_collective_mid() { return _collective_mid; } + + // get_collective_out - returns collective position from last output as a number from 0 ~ 1000 + int16_t get_collective_out() { return _collective_out; } + + // set_collective_for_manual_control - limits collective to reduced range for stabilize (i.e. manual) flying + void set_collective_for_manual_control(bool true_false) { _heliflags.manual_collective = true_false; } + + // get min/max collective when controlled manually as a number from 0 ~ 1000 (note that parameter is stored as percentage) + int16_t get_manual_collective_min() { return _manual_collective_min*10; } + int16_t get_manual_collective_max() { return _manual_collective_max*10; } + + // set_collective_for_landing - limits collective from going too low if we know we are landed + void set_collective_for_landing(bool landing) { _heliflags.landing_collective = landing; } + + // return true if the main rotor is up to speed + bool motor_runup_complete() { return _heliflags.motor_runup_complete; } // var_info for holding Parameter information static const struct AP_Param::GroupInfo var_info[]; protected: + // output - sends commands to the motors + void output_armed(); + void output_disarmed(); + +private: + // 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_in, int16_t yaw_out); // reset_swash - free up swash for maximum movements. Used for set-up void reset_swash(); - void output_disarmed(); + // init_swash - initialise the swash plate + void init_swash(); + // calculate_roll_pitch_collective_factors - calculate factors based on swash type and servo position + void calculate_roll_pitch_collective_factors(); + + // rsc_control - update value to send to main rotor's ESC void rsc_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]; + // external objects we depend upon + RC_Channel *_servo_1; + RC_Channel *_servo_2; + RC_Channel *_servo_3; + RC_Channel *_servo_4; + RC_Channel *_rc_8; - // internally used variables - 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) - float _stab_throttle_scalar; // throttle scalar to reduce the range of the collective movement in stabilize mode - bool _swash_initialised; // true if swash has been initialised - int16_t rsc_output; // final output to the external motor governor 1000-2000 - int16_t rsc_ramp; // current state of ramping - int16_t motor_runup_timer; // timer to determine if motor has run up fully + // flags bitmask + struct heliflags_type { + uint8_t swash_initialised : 1; // true if swash has been initialised + uint8_t manual_collective : 1; // true if pilot is manually controlling the collective. If true then we reduce the swash range + uint8_t landing_collective : 1; // true if collective is setup for landing which has much higher minimum + uint8_t motor_runup_complete : 1; // true if the rotors have had enough time to wind up + } _heliflags; + + // parameters + AP_Int16 _servo1_pos; // Angular location of swash servo #1 + AP_Int16 _servo2_pos; // Angular location of swash servo #2 + AP_Int16 _servo3_pos; // Angular location of swash servo #3 + AP_Int16 _roll_max; // Maximum roll angle of the swash plate in centi-degrees + AP_Int16 _pitch_max; // Maximum pitch angle of the swash plate in centi-degrees + AP_Int16 _collective_min; // Lowest possible servo position for the swashplate + AP_Int16 _collective_max; // Highest possible servo position for the swashplate + AP_Int16 _collective_mid; // Swash servo position corresponding to zero collective pitch (or zero lift for Assymetrical blades) + AP_Int16 _ext_gyro_enabled; // Enabled/Disable an external rudder gyro connected to channel 7. With no external gyro a more complex yaw controller is used + AP_Int8 _swash_type; // Swash Type Setting - either 3-servo CCPM or H1 Mechanical Mixing + AP_Int16 _ext_gyro_gain; // PWM sent to the external gyro on Ch7 + AP_Int8 _servo_manual; // Pass radio inputs directly to servos during set-up through mission planner + AP_Int16 _phase_angle; // Phase angle correction for rotor head. If pitching the swash forward induces a roll, this can be correct the problem + AP_Int16 _collective_yaw_effect; // Feed-forward compensation to automatically add rudder input when collective pitch is increased. Can be positive or negative depending on mechanics. + AP_Int16 _ext_gov_setpoint; // PWM passed to the external motor governor when external governor is enabledv + AP_Int8 _rsc_mode; // Sets which main rotor ESC control mode is active + AP_Int16 _rsc_ramp_up_rate; // The time in 100th seconds the RSC takes to ramp up to speed + AP_Int8 _flybar_mode; // Flybar present or not. Affects attitude controller used during ACRO flight mode + AP_Int8 _manual_collective_min; // Minimum collective position while pilot directly controls the collective + AP_Int8 _manual_collective_max; // Maximum collective position while pilot directly controls the collective + + // 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 _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; // collective scalar to convert pwm form (i.e. 0 ~ 1000) passed in to actual servo range (i.e 1250~1750 would be 500) + float _collective_scalar_manual; // collective scalar to reduce the range of the collective movement while collective is being controlled manually (i.e. directly by the pilot) + int16_t _collective_out; // actual collective pitch value. Required by the main code for calculating cruise throttle + int16_t _collective_mid_pwm; // collective mid parameter value converted to pwm form (i.e. 0 ~ 1000) + int16_t _rsc_output; // final output to the external motor governor 1000-2000 + int16_t _rsc_ramp; // current state of ramping + int16_t _motor_runup_timer; // timer to determine if motor has run up fully }; #endif // AP_MOTORSHELI