From cde94078b75e95003dfbd874e3276ec694682cd0 Mon Sep 17 00:00:00 2001 From: Fredrik Hedberg Date: Wed, 22 Jul 2015 15:46:53 +0200 Subject: [PATCH] AP_Motors: Move traditional helicopter controls into AP_MotorsHeli_Single. Original commit by fhedberg, had to fix merge conflicts and now it appears I did the commit? --- libraries/AP_Motors/AP_Motors.h | 2 +- libraries/AP_Motors/AP_MotorsHeli.cpp | 483 +----------------- libraries/AP_Motors/AP_MotorsHeli.h | 122 +---- libraries/AP_Motors/AP_MotorsHeli_Single.cpp | 498 +++++++++++++++++++ libraries/AP_Motors/AP_MotorsHeli_Single.h | 173 +++++++ 5 files changed, 708 insertions(+), 570 deletions(-) create mode 100644 libraries/AP_Motors/AP_MotorsHeli_Single.cpp create mode 100644 libraries/AP_Motors/AP_MotorsHeli_Single.h diff --git a/libraries/AP_Motors/AP_Motors.h b/libraries/AP_Motors/AP_Motors.h index 584e93457e..a13c351fcf 100644 --- a/libraries/AP_Motors/AP_Motors.h +++ b/libraries/AP_Motors/AP_Motors.h @@ -12,7 +12,7 @@ #include "AP_MotorsY6.h" #include "AP_MotorsOcta.h" #include "AP_MotorsOctaQuad.h" -#include "AP_MotorsHeli.h" +#include "AP_MotorsHeli_Single.h" #include "AP_MotorsSingle.h" #include "AP_MotorsCoax.h" diff --git a/libraries/AP_Motors/AP_MotorsHeli.cpp b/libraries/AP_Motors/AP_MotorsHeli.cpp index a1ed10c872..6bba0950ea 100644 --- a/libraries/AP_Motors/AP_MotorsHeli.cpp +++ b/libraries/AP_Motors/AP_MotorsHeli.cpp @@ -27,33 +27,6 @@ extern const AP_HAL::HAL& hal; const AP_Param::GroupInfo AP_MotorsHeli::var_info[] PROGMEM = { - // @Param: SV1_POS - // @DisplayName: Servo 1 Position - // @Description: Angular location of swash servo #1 - // @Range: -180 180 - // @Units: Degrees - // @User: Standard - // @Increment: 1 - AP_GROUPINFO("SV1_POS", 1, AP_MotorsHeli, _servo1_pos, AP_MOTORS_HELI_SERVO1_POS), - - // @Param: SV2_POS - // @DisplayName: Servo 2 Position - // @Description: Angular location of swash servo #2 - // @Range: -180 180 - // @Units: Degrees - // @User: Standard - // @Increment: 1 - AP_GROUPINFO("SV2_POS", 2, AP_MotorsHeli, _servo2_pos, AP_MOTORS_HELI_SERVO2_POS), - - // @Param: SV3_POS - // @DisplayName: Servo 3 Position - // @Description: Angular location of swash servo #3 - // @Range: -180 180 - // @Units: Degrees - // @User: Standard - // @Increment: 1 - AP_GROUPINFO("SV3_POS", 3, AP_MotorsHeli, _servo3_pos, AP_MOTORS_HELI_SERVO3_POS), - // @Param: ROL_MAX // @DisplayName: Swash Roll Angle Max // @Description: Maximum roll angle of the swash plate @@ -61,7 +34,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", 1, AP_MotorsHeli, _roll_max, AP_MOTORS_HELI_SWASH_ROLL_MAX), // @Param: PIT_MAX // @DisplayName: Swash Pitch Angle Max @@ -70,7 +43,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", 2, AP_MotorsHeli, _pitch_max, AP_MOTORS_HELI_SWASH_PITCH_MAX), // @Param: COL_MIN // @DisplayName: Collective Pitch Minimum @@ -79,7 +52,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", 3, AP_MotorsHeli, _collective_min, AP_MOTORS_HELI_COLLECTIVE_MIN), // @Param: COL_MAX // @DisplayName: Collective Pitch Maximum @@ -88,7 +61,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", 4, AP_MotorsHeli, _collective_max, AP_MOTORS_HELI_COLLECTIVE_MAX), // @Param: COL_MID // @DisplayName: Collective Pitch Mid-Point @@ -97,53 +70,14 @@ 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), - - // @Param: TAIL_TYPE - // @DisplayName: Tail Type - // @Description: Tail type selection. Simpler yaw controller used if external gyro is selected - // @Values: 0:Servo only,1:Servo with ExtGyro,2:DirectDrive VarPitch,3:DirectDrive FixedPitch - // @User: Standard - AP_GROUPINFO("TAIL_TYPE",9, AP_MotorsHeli, _tail_type, AP_MOTORS_HELI_TAILTYPE_SERVO), - - // @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), - - // @Param: GYR_GAIN - // @DisplayName: External Gyro Gain - // @Description: PWM sent to external gyro on ch7 when tail type is Servo w/ ExtGyro - // @Range: 0 1000 - // @Units: PWM - // @Increment: 1 - // @User: Standard - AP_GROUPINFO("GYR_GAIN", 11, AP_MotorsHeli, _ext_gyro_gain, AP_MOTORS_HELI_EXT_GYRO_GAIN), + AP_GROUPINFO("COL_MID", 5, AP_MotorsHeli, _collective_mid, AP_MOTORS_HELI_COLLECTIVE_MID), // @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), - - // @Param: PHANG - // @DisplayName: Swashplate Phase Angle Compensation - // @Description: Phase angle correction for rotor head. If pitching the swash forward induces a roll, this can be correct the problem - // @Range: -90 90 - // @Units: Degrees - // @User: Advanced - // @Increment: 1 - 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 - // @Increment: 0.1 - AP_GROUPINFO("COLYAW", 14, AP_MotorsHeli, _collective_yaw_effect, 0), + AP_GROUPINFO("SV_MAN", 7, AP_MotorsHeli, _servo_manual, 0), // @Param: GOV_SETPOINT // @DisplayName: External Motor Governor Setpoint @@ -152,21 +86,14 @@ const AP_Param::GroupInfo AP_MotorsHeli::var_info[] PROGMEM = { // @Units: PWM // @Increment: 10 // @User: Standard - AP_GROUPINFO("RSC_SETPOINT", 15, AP_MotorsHeli, _rsc_setpoint, AP_MOTORS_HELI_RSC_SETPOINT), + AP_GROUPINFO("RSC_SETPOINT", 8, AP_MotorsHeli, _rsc_setpoint, AP_MOTORS_HELI_RSC_SETPOINT), // @Param: RSC_MODE // @DisplayName: Rotor Speed Control Mode // @Description: Controls the source of the desired rotor speed, either ch8 or RSC_SETPOINT // @Values: 0:None, 1:Ch8 Input, 2:SetPoint // @User: Standard - AP_GROUPINFO("RSC_MODE", 16, AP_MotorsHeli, _rsc_mode, AP_MOTORS_HELI_RSC_MODE_CH8_PASSTHROUGH), - - // @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", 17, AP_MotorsHeli, _flybar_mode, AP_MOTORS_HELI_NOFLYBAR), + AP_GROUPINFO("RSC_MODE", 9, AP_MotorsHeli, _rsc_mode, AP_MOTORS_HELI_RSC_MODE_CH8_PASSTHROUGH), // @Param: LAND_COL_MIN // @DisplayName: Landing Collective Minimum @@ -175,7 +102,7 @@ const AP_Param::GroupInfo AP_MotorsHeli::var_info[] PROGMEM = { // @Units: pwm // @Increment: 1 // @User: Standard - AP_GROUPINFO("LAND_COL_MIN", 18, AP_MotorsHeli, _land_collective_min, AP_MOTORS_HELI_LAND_COLLECTIVE_MIN), + AP_GROUPINFO("LAND_COL_MIN", 10, AP_MotorsHeli, _land_collective_min, AP_MOTORS_HELI_LAND_COLLECTIVE_MIN), // @Param: RSC_RAMP_TIME // @DisplayName: RSC Ramp Time @@ -183,7 +110,7 @@ const AP_Param::GroupInfo AP_MotorsHeli::var_info[] PROGMEM = { // @Range: 0 60 // @Units: Seconds // @User: Standard - AP_GROUPINFO("RSC_RAMP_TIME", 19, AP_MotorsHeli,_rsc_ramp_time, AP_MOTORS_HELI_RSC_RAMP_TIME), + AP_GROUPINFO("RSC_RAMP_TIME", 11, AP_MotorsHeli, _rsc_ramp_time, AP_MOTORS_HELI_RSC_RAMP_TIME), // @Param: RSC_RUNUP_TIME // @DisplayName: RSC Runup Time @@ -191,24 +118,15 @@ const AP_Param::GroupInfo AP_MotorsHeli::var_info[] PROGMEM = { // @Range: 0 60 // @Units: Seconds // @User: Standard - AP_GROUPINFO("RSC_RUNUP_TIME", 20, AP_MotorsHeli,_rsc_runup_time, AP_MOTORS_HELI_RSC_RUNUP_TIME), - - // @Param: TAIL_SPEED - // @DisplayName: Direct Drive VarPitch Tail ESC speed - // @Description: Direct Drive VarPitch Tail ESC speed. Only used when TailType is DirectDrive VarPitch - // @Range: 0 1000 - // @Units: PWM - // @Increment: 1 - // @User: Standard - AP_GROUPINFO("TAIL_SPEED", 21, AP_MotorsHeli, _direct_drive_tailspeed, AP_MOTOR_HELI_DDTAIL_DEFAULT), + AP_GROUPINFO("RSC_RUNUP_TIME", 12, AP_MotorsHeli, _rsc_runup_time, AP_MOTORS_HELI_RSC_RUNUP_TIME), // @Param: RSC_CRITICAL // @DisplayName: Critical Rotor Speed // @Description: Rotor speed below which flight is not possible - // @Range: 0 1000 + // @Range: 0 0-1000 // @Increment: 10 // @User: Standard - AP_GROUPINFO("RSC_CRITICAL", 22, AP_MotorsHeli, _rsc_critical, AP_MOTORS_HELI_RSC_CRITICAL), + AP_GROUPINFO("RSC_CRITICAL", 13, AP_MotorsHeli, _rsc_critical, AP_MOTORS_HELI_RSC_CRITICAL), // parameters 1 ~ 29 reserved for tradheli // parameters 30 ~ 39 reserved for tricopter @@ -235,37 +153,6 @@ void AP_MotorsHeli::Init() // initialise swash plate init_swash(); - - // disable channels 7 and 8 from being used by RC_Channel_aux - RC_Channel_aux::disable_aux_channel(_motor_to_channel_map[AP_MOTORS_HELI_AUX]); - RC_Channel_aux::disable_aux_channel(_motor_to_channel_map[AP_MOTORS_HELI_RSC]); -} - -// set update rate to motors - a value in hertz -void AP_MotorsHeli::set_update_rate( uint16_t speed_hz ) -{ - // record requested speed - _speed_hz = speed_hz; - - // setup fast channels - uint32_t mask = - 1U << pgm_read_byte(&_motor_to_channel_map[AP_MOTORS_MOT_1]) | - 1U << pgm_read_byte(&_motor_to_channel_map[AP_MOTORS_MOT_2]) | - 1U << pgm_read_byte(&_motor_to_channel_map[AP_MOTORS_MOT_3]) | - 1U << pgm_read_byte(&_motor_to_channel_map[AP_MOTORS_MOT_4]); - hal.rcout->set_freq(mask, _speed_hz); -} - -// enable - starts allowing signals to be sent to motors -void AP_MotorsHeli::enable() -{ - // enable output channels - hal.rcout->enable_ch(pgm_read_byte(&_motor_to_channel_map[AP_MOTORS_MOT_1])); // swash servo 1 - hal.rcout->enable_ch(pgm_read_byte(&_motor_to_channel_map[AP_MOTORS_MOT_2])); // swash servo 2 - hal.rcout->enable_ch(pgm_read_byte(&_motor_to_channel_map[AP_MOTORS_MOT_3])); // swash servo 3 - hal.rcout->enable_ch(pgm_read_byte(&_motor_to_channel_map[AP_MOTORS_MOT_4])); // yaw - hal.rcout->enable_ch(AP_MOTORS_HELI_AUX); // output for gyro gain or direct drive variable pitch tail motor - hal.rcout->enable_ch(AP_MOTORS_HELI_RSC); // output for main rotor esc } // output - sends commands to the servos @@ -300,60 +187,6 @@ void AP_MotorsHeli::output_min() limit.throttle_upper = false; } - -// 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 AP_MotorsHeli::output_test(uint8_t motor_seq, int16_t pwm) -{ - // exit immediately if not armed - if (!armed()) { - return; - } - - // output to motors and servos - switch (motor_seq) { - case 1: - // swash servo 1 - hal.rcout->write(pgm_read_byte(&_motor_to_channel_map[AP_MOTORS_MOT_1]), pwm); - break; - case 2: - // swash servo 2 - hal.rcout->write(pgm_read_byte(&_motor_to_channel_map[AP_MOTORS_MOT_2]), pwm); - break; - case 3: - // swash servo 3 - hal.rcout->write(pgm_read_byte(&_motor_to_channel_map[AP_MOTORS_MOT_3]), pwm); - break; - case 4: - // external gyro & tail servo - if (_tail_type == AP_MOTORS_HELI_TAILTYPE_SERVO_EXTGYRO) { - write_aux(_ext_gyro_gain); - } - hal.rcout->write(pgm_read_byte(&_motor_to_channel_map[AP_MOTORS_MOT_4]), pwm); - break; - case 5: - // main rotor - hal.rcout->write(pgm_read_byte(&_motor_to_channel_map[AP_MOTORS_HELI_RSC]), pwm); - break; - default: - // do nothing - break; - } -} - -// allow_arming - check if it's safe to arm -bool AP_MotorsHeli::allow_arming() const -{ - // returns false if main rotor speed is not zero - if (_rsc_mode != AP_MOTORS_HELI_RSC_MODE_NONE && _main_rotor.get_estimated_speed() > 0) { - return false; - } - - // all other cases it is OK to arm - return true; -} - // parameter_check - check if helicopter specific parameters are sensible bool AP_MotorsHeli::parameter_check() const { @@ -366,86 +199,18 @@ bool AP_MotorsHeli::parameter_check() const return true; } -// set_desired_rotor_speed -void AP_MotorsHeli::set_desired_rotor_speed(int16_t desired_speed) -{ - _main_rotor.set_desired_speed(desired_speed); - - if (desired_speed > 0 && _tail_type == AP_MOTORS_HELI_TAILTYPE_DIRECTDRIVE_VARPITCH) { - _tail_rotor.set_desired_speed(_direct_drive_tailspeed); - } else { - _tail_rotor.set_desired_speed(0); - } -} - // return true if the main rotor is up to speed bool AP_MotorsHeli::rotor_runup_complete() const { return _heliflags.rotor_runup_complete; } -// recalc_scalers - recalculates various scalers used. Should be called at about 1hz to allow users to see effect of changing parameters -void AP_MotorsHeli::recalc_scalers() -{ - if (_rsc_mode != AP_MOTORS_HELI_RSC_MODE_SETPOINT) { - _tail_rotor.set_ramp_time(0); - _tail_rotor.set_runup_time(0); - _tail_rotor.set_critical_speed(0); - } else { - _main_rotor.set_ramp_time(_rsc_ramp_time); - _main_rotor.set_runup_time(_rsc_runup_time); - _main_rotor.set_critical_speed(_rsc_critical); - } - - _main_rotor.recalc_scalers(); - - if (_rsc_mode != AP_MOTORS_HELI_TAILTYPE_DIRECTDRIVE_VARPITCH) { - _tail_rotor.set_ramp_time(0); - _tail_rotor.set_runup_time(0); - _tail_rotor.set_critical_speed(0); - } else { - _tail_rotor.set_ramp_time(_rsc_ramp_time); - _tail_rotor.set_runup_time(_rsc_runup_time); - _tail_rotor.set_critical_speed(_rsc_critical); - } - - _tail_rotor.recalc_scalers(); -} - -// 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 AP_MotorsHeli::get_motor_mask() -{ - // heli uses channels 1,2,3,4,7 and 8 - return (1U << 0 | 1U << 1 | 1U << 2 | 1U << 3 | 1U << AP_MOTORS_HELI_AUX | 1U << AP_MOTORS_HELI_RSC); -} - void AP_MotorsHeli::output_armed_not_stabilizing() { // stabilizing servos always operate for helicopters output_armed_stabilizing(); } -// sends commands to the motors -void AP_MotorsHeli::output_armed_stabilizing() -{ - move_swash(_roll_control_input, _pitch_control_input, _throttle_control_input, _yaw_control_input); - - if (_tail_type == AP_MOTORS_HELI_TAILTYPE_DIRECTDRIVE_VARPITCH) { - _tail_rotor.output_armed(); - - if (!_tail_rotor.is_runup_complete()) - { - _heliflags.rotor_runup_complete = false; - return; - } - } - - _main_rotor.output_armed(); - - _heliflags.rotor_runup_complete = _main_rotor.is_runup_complete(); -} - // output_armed_zero_throttle - sends commands to the motors void AP_MotorsHeli::output_armed_zero_throttle() { @@ -454,19 +219,6 @@ void AP_MotorsHeli::output_armed_zero_throttle() output_armed_stabilizing(); } -// output_disarmed - sends commands to the motors -void AP_MotorsHeli::output_disarmed() -{ - move_swash(_roll_control_input, _pitch_control_input, _throttle_control_input, _yaw_control_input); - - if (_tail_type == AP_MOTORS_HELI_TAILTYPE_DIRECTDRIVE_VARPITCH) { - _tail_rotor.output_disarmed(); - } - - _main_rotor.output_disarmed(); - - _heliflags.rotor_runup_complete = false; -} // reset_swash - free up swash for maximum movements. Used for set-up void AP_MotorsHeli::reset_swash() @@ -487,14 +239,6 @@ void AP_MotorsHeli::reset_swash() _heliflags.swash_initialised = false; } -// reset_servos -void AP_MotorsHeli::reset_servos() -{ - reset_swash_servo (_servo_1); - reset_swash_servo (_servo_2); - reset_swash_servo (_servo_3); -} - // reset_swash_servo void AP_MotorsHeli::reset_swash_servo(RC_Channel& servo) { @@ -531,16 +275,6 @@ void AP_MotorsHeli::init_swash() _heliflags.swash_initialised = true; } -// init_servos -void AP_MotorsHeli::init_servos() -{ - init_swash_servo (_servo_1); - init_swash_servo (_servo_2); - init_swash_servo (_servo_3); - - _servo_4.set_angle(4500); -} - // init_swash_servo void AP_MotorsHeli::init_swash_servo(RC_Channel& servo) { @@ -550,197 +284,6 @@ void AP_MotorsHeli::init_swash_servo(RC_Channel& servo) servo.radio_max = 2000; } -// 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 + _delta_phase_angle))); - _rollFactor[CH_2] = cosf(radians(_servo2_pos + 90 - (_phase_angle + _delta_phase_angle))); - _rollFactor[CH_3] = cosf(radians(_servo3_pos + 90 - (_phase_angle + _delta_phase_angle))); - - // pitch factors - _pitchFactor[CH_1] = cosf(radians(_servo1_pos - (_phase_angle + _delta_phase_angle))); - _pitchFactor[CH_2] = cosf(radians(_servo2_pos - (_phase_angle + _delta_phase_angle))); - _pitchFactor[CH_3] = cosf(radians(_servo3_pos - (_phase_angle + _delta_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; - } -} - -// -// heli_move_swash - moves swash plate to attitude of parameters passed in -// - expected ranges: -// roll : -4500 ~ 4500 -// pitch: -4500 ~ 4500 -// collective: 0 ~ 1000 -// yaw: -4500 ~ 4500 -// -void AP_MotorsHeli::move_swash(int16_t roll_out, int16_t pitch_out, int16_t coll_in, int16_t yaw_out) -{ - // if manual override (i.e. when setting up swash), pass pilot commands straight through to swash - if (_servo_manual == 1) { - _roll_control_input = _roll_radio_passthrough; - _pitch_control_input = _pitch_radio_passthrough; - _throttle_control_input = _throttle_radio_passthrough; - _yaw_control_input = _yaw_radio_passthrough; - } - - int16_t yaw_offset = 0; - int16_t coll_out_scaled; - - // initialize limits flag - limit.roll_pitch = false; - limit.yaw = false; - limit.throttle_lower = false; - limit.throttle_upper = false; - - 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 (_heliflags.swash_initialised) { - reset_swash(); - } - // To-Do: This equation seems to be wrong. It probably restricts swash movement so that swash setup doesn't work right. - // _collective_scalar should probably not be used or set to 1? - coll_out_scaled = coll_in * _collective_scalar + _throttle_radio_min - 1000; - }else{ // regular flight mode - - // check if we need to reinitialise the swash - if (!_heliflags.swash_initialised) { - init_swash(); - } - - // rescale roll_out and pitch-out into the min and max ranges to provide linear motion - // across the input range instead of stopping when the input hits the constrain value - // these calculations are based on an assumption of the user specified roll_max and pitch_max - // 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; - if (roll_out < -_roll_max) { - roll_out = -_roll_max; - limit.roll_pitch = true; - } - if (roll_out > _roll_max) { - roll_out = _roll_max; - limit.roll_pitch = true; - } - - // scale pitch and update limits - pitch_out = pitch_out * _pitch_scaler; - if (pitch_out < -_pitch_max) { - pitch_out = -_pitch_max; - limit.roll_pitch = true; - } - if (pitch_out > _pitch_max) { - pitch_out = _pitch_max; - limit.roll_pitch = true; - } - - // constrain collective input - _collective_out = coll_in; - if (_collective_out <= 0) { - _collective_out = 0; - limit.throttle_lower = true; - } - if (_collective_out >= 1000) { - _collective_out = 1000; - limit.throttle_upper = true; - } - - // ensure not below landed/landing collective - if (_heliflags.landing_collective && _collective_out < _land_collective_min) { - _collective_out = _land_collective_min; - limit.throttle_lower = true; - } - - // scale collective pitch - coll_out_scaled = _collective_out * _collective_scalar + _collective_min - 1000; - - // rudder feed forward based on collective - // the feed-forward is not required when the motor is shut down and not creating torque - // also not required if we are using external gyro - if ((_main_rotor.get_desired_speed() > 0) && _tail_type != AP_MOTORS_HELI_TAILTYPE_SERVO_EXTGYRO) { - // sanity check collective_yaw_effect - _collective_yaw_effect = constrain_float(_collective_yaw_effect, -AP_MOTOR_HELI_COLYAW_RANGE, AP_MOTOR_HELI_COLYAW_RANGE); - yaw_offset = _collective_yaw_effect * abs(_collective_out - _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) { - _servo_1.servo_out += 500; - _servo_2.servo_out += 500; - } - _servo_3.servo_out = (_rollFactor[CH_3] * roll_out + _pitchFactor[CH_3] * pitch_out)/10 + _collectiveFactor[CH_3] * coll_out_scaled + (_servo_3.radio_trim-1500); - - // use servo_out to calculate pwm_out and radio_out - _servo_1.calc_pwm(); - _servo_2.calc_pwm(); - _servo_3.calc_pwm(); - - // actually move the servos - hal.rcout->write(pgm_read_byte(&_motor_to_channel_map[AP_MOTORS_MOT_1]), _servo_1.radio_out); - hal.rcout->write(pgm_read_byte(&_motor_to_channel_map[AP_MOTORS_MOT_2]), _servo_2.radio_out); - hal.rcout->write(pgm_read_byte(&_motor_to_channel_map[AP_MOTORS_MOT_3]), _servo_3.radio_out); - - // update the yaw rate using the tail rotor/servo - output_yaw(yaw_out + yaw_offset); -} - -// output_yaw -void AP_MotorsHeli::output_yaw(int16_t yaw_out) -{ - _servo_4.servo_out = constrain_int16(yaw_out, -4500, 4500); - - if (_servo_4.servo_out != yaw_out) { - limit.yaw = true; - } - - _servo_4.calc_pwm(); - - hal.rcout->write(pgm_read_byte(&_motor_to_channel_map[AP_MOTORS_MOT_4]), _servo_4.radio_out); - - if (_tail_type == AP_MOTORS_HELI_TAILTYPE_SERVO_EXTGYRO) { - // output gain to exernal gyro - write_aux(_ext_gyro_gain); - } else if (_tail_type == AP_MOTORS_HELI_TAILTYPE_DIRECTDRIVE_FIXEDPITCH && _main_rotor.get_desired_speed() > 0) { - // output yaw servo to tail rsc - write_aux(_servo_4.servo_out); - } -} - -// write_aux - outputs pwm onto output aux channel (ch7) -// servo_out parameter is of the range 0 ~ 1000 -void AP_MotorsHeli::write_aux(int16_t servo_out) -{ - _servo_aux.servo_out = servo_out; - _servo_aux.calc_pwm(); - hal.rcout->write(AP_MOTORS_HELI_AUX, _servo_aux.radio_out); -} - // set_delta_phase_angle for setting variable phase angle compensation and force // recalculation of collective factors void AP_MotorsHeli::set_delta_phase_angle(int16_t angle) diff --git a/libraries/AP_Motors/AP_MotorsHeli.h b/libraries/AP_Motors/AP_MotorsHeli.h index f343bbaea2..681984a2dc 100644 --- a/libraries/AP_Motors/AP_MotorsHeli.h +++ b/libraries/AP_Motors/AP_MotorsHeli.h @@ -7,6 +7,7 @@ #define __AP_MOTORS_HELI_H__ #include + #include #include // ArduPilot Mega Vector/Matrix math Library #include // RC Channel Library @@ -21,19 +22,6 @@ #define AP_MOTORS_HELI_SPEED_DIGITAL_SERVOS 125 // update rate for digital servos #define AP_MOTORS_HELI_SPEED_ANALOG_SERVOS 125 // update rate for analog servos -// TradHeli Aux Function Output Channels -#define AP_MOTORS_HELI_AUX CH_7 -#define AP_MOTORS_HELI_RSC CH_8 - -// servo position defaults -#define AP_MOTORS_HELI_SERVO1_POS -60 -#define AP_MOTORS_HELI_SERVO2_POS 60 -#define AP_MOTORS_HELI_SERVO3_POS 180 - -// swash type definitions -#define AP_MOTORS_HELI_SWASH_CCPM 0 -#define AP_MOTORS_HELI_SWASH_H1 1 - // default swash min and max angles and positions #define AP_MOTORS_HELI_SWASH_ROLL_MAX 2500 #define AP_MOTORS_HELI_SWASH_PITCH_MAX 2500 @@ -48,21 +36,6 @@ // swash min while landed or landing (as a number from 0 ~ 1000 #define AP_MOTORS_HELI_LAND_COLLECTIVE_MIN 0 -// tail types -#define AP_MOTORS_HELI_TAILTYPE_SERVO 0 -#define AP_MOTORS_HELI_TAILTYPE_SERVO_EXTGYRO 1 -#define AP_MOTORS_HELI_TAILTYPE_DIRECTDRIVE_VARPITCH 2 -#define AP_MOTORS_HELI_TAILTYPE_DIRECTDRIVE_FIXEDPITCH 3 - -// default external gyro gain (ch7 out) -#define AP_MOTORS_HELI_EXT_GYRO_GAIN 350 - -// minimum outputs for direct drive motors -#define AP_MOTOR_HELI_DDTAIL_DEFAULT 500 - -// COLYAW parameter min and max values -#define AP_MOTOR_HELI_COLYAW_RANGE 10.0f - // main rotor speed control types (ch8 out) #define AP_MOTORS_HELI_RSC_MODE_NONE 0 // main rotor ESC is directly connected to receiver, pilot controls ESC speed through transmitter directly #define AP_MOTORS_HELI_RSC_MODE_CH8_PASSTHROUGH 1 // main rotor ESC is connected to RC8 (out), pilot desired rotor speed provided by CH8 input @@ -77,7 +50,6 @@ // default main rotor ramp up time in seconds #define AP_MOTORS_HELI_RSC_RAMP_TIME 1 // 1 second to ramp output to main rotor ESC to full power (most people use exterrnal govenors so we can ramp up quickly) #define AP_MOTORS_HELI_RSC_RUNUP_TIME 10 // 10 seconds for rotor to reach full speed -#define AP_MOTORS_HELI_TAIL_RAMP_INCREMENT 5 // 5 is 2 seconds for direct drive tail rotor to reach to full speed (5 = (2sec*100hz)/1000) // flybar types #define AP_MOTORS_HELI_NOFLYBAR 0 @@ -90,23 +62,9 @@ class AP_MotorsHeli : public AP_Motors { public: /// Constructor - AP_MotorsHeli( RC_Channel& servo_aux, - RC_Channel& servo_rotor, - RC_Channel& swash_servo_1, - RC_Channel& swash_servo_2, - RC_Channel& swash_servo_3, - RC_Channel& yaw_servo, - uint16_t loop_rate, + AP_MotorsHeli( uint16_t loop_rate, uint16_t speed_hz = AP_MOTORS_HELI_SPEED_DEFAULT) : - AP_Motors(loop_rate, speed_hz), - _servo_aux(servo_aux), - _servo_rsc(servo_rotor), - _servo_1(swash_servo_1), - _servo_2(swash_servo_2), - _servo_3(swash_servo_3), - _servo_4(yaw_servo), - _main_rotor(servo_rotor, AP_MOTORS_HELI_RSC, loop_rate), - _tail_rotor(servo_aux, AP_MOTORS_HELI_AUX, loop_rate) + AP_Motors(loop_rate, speed_hz) { AP_Param::setup_object_defaults(this, var_info); @@ -121,10 +79,10 @@ public: // set update rate to motors - a value in hertz // you must have setup_motors before calling this - void set_update_rate( uint16_t speed_hz ); + virtual void set_update_rate( uint16_t speed_hz ) = 0; // enable - starts allowing signals to be sent to motors - void enable(); + virtual void enable() = 0; // output_min - sets servos to neutral point void output_min(); @@ -132,7 +90,7 @@ public: // 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 - virtual void output_test(uint8_t motor_seq, int16_t pwm); + virtual void output_test(uint8_t motor_seq, int16_t pwm) = 0; // slow_start - ignored by helicopters void slow_start(bool true_false) {}; @@ -142,20 +100,13 @@ public: // // allow_arming - returns true if main rotor is spinning and it is ok to arm - bool allow_arming() const; + virtual bool allow_arming() const = 0; // parameter_check - returns true if helicopter specific parameters are sensible, used for pre-arm check bool parameter_check() const; - // _tail_type - returns the tail type (servo, servo with ext gyro, direct drive var pitch, direct drive fixed pitch) - int16_t tail_type() const { return _tail_type; } - - // ext_gyro_gain - gets and sets external gyro gain as a pwm (1000~2000) - int16_t ext_gyro_gain() const { return _ext_gyro_gain; } - void ext_gyro_gain(int16_t pwm) { _ext_gyro_gain = pwm; } - // has_flybar - returns true if we have a mechical flybar - bool has_flybar() const { return _flybar_mode; } + virtual bool has_flybar() const { return AP_MOTORS_HELI_NOFLYBAR; } // get_collective_mid - returns collective mid position as a number from 0 ~ 1000 int16_t get_collective_mid() const { return _collective_mid; } @@ -173,25 +124,22 @@ public: int16_t get_rsc_setpoint() const { return _rsc_setpoint; } // set_desired_rotor_speed - sets target rotor speed as a number from 0 ~ 1000 - void set_desired_rotor_speed(int16_t desired_speed); + virtual void set_desired_rotor_speed(int16_t desired_speed) = 0; // get_desired_rotor_speed - gets target rotor speed as a number from 0 ~ 1000 - int16_t get_desired_rotor_speed() const { return _main_rotor.get_desired_speed(); } + virtual int16_t get_desired_rotor_speed() const = 0; // get_estimated_rotor_speed - gets estimated rotor speed as a number from 0 ~ 1000 - int16_t get_estimated_rotor_speed() { return _main_rotor.get_estimated_speed(); } + virtual int16_t get_estimated_rotor_speed() const = 0; // return true if the main rotor is up to speed bool rotor_runup_complete() const; // 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_estimated_speed() > _main_rotor.get_critical_speed(); } + virtual bool rotor_speed_above_critical() const = 0; // recalc_scalers - recalculates various scalers used. Should be called at about 1hz to allow users to see effect of changing parameters - void recalc_scalers(); - - // get_phase_angle - returns phase angle - int16_t get_phase_angle() const { return _phase_angle; } + virtual void recalc_scalers() = 0; // var_info for holding Parameter information static const struct AP_Param::GroupInfo var_info[]; @@ -202,7 +150,7 @@ public: // 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 - virtual uint16_t get_motor_mask(); + virtual uint16_t get_motor_mask() = 0; // set_radio_passthrough used to pass radio inputs directly to outputs void set_radio_passthrough(int16_t radio_roll_input, int16_t radio_pitch_input, int16_t radio_throttle_input, int16_t radio_yaw_input); @@ -213,28 +161,28 @@ public: // output - sends commands to the motors void output(); + // supports_yaw_passthrough + virtual bool supports_yaw_passthrough() const { return false; } + protected: // output - sends commands to the motors - void output_armed_stabilizing(); + virtual void output_armed_stabilizing() = 0; void output_armed_not_stabilizing(); void output_armed_zero_throttle(); - void output_disarmed(); - void output_yaw(int16_t yaw_out); + virtual void output_disarmed() = 0; // update the throttle input filter void update_throttle_filter(); -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); + virtual void move_swash(int16_t roll_out, int16_t pitch_out, int16_t coll_in, int16_t yaw_out) = 0; // reset_swash - free up swash for maximum movements. Used for set-up void reset_swash(); // reset_servos - free up the swash servos for maximum movement - void reset_servos(); + virtual void reset_servos() = 0; // reset_swash_servo - free up swash servo for maximum movement static void reset_swash_servo(RC_Channel& servo); @@ -243,27 +191,13 @@ private: void init_swash(); // init_servos - initialize the servos - void init_servos(); + virtual void init_servos() = 0; // init_swash_servo - initialize a swash servo static void init_swash_servo(RC_Channel& servo); // calculate_roll_pitch_collective_factors - calculate factors based on swash type and servo position - void calculate_roll_pitch_collective_factors(); - - // write_aux - outputs pwm onto output aux channel (ch7). servo_out parameter is of the range 0 ~ 1000 - void write_aux(int16_t servo_out); - - // external objects we depend upon - RC_Channel& _servo_aux; // output to ext gyro gain and tail direct drive esc (ch7) - RC_Channel& _servo_rsc; // output to main rotor esc (ch8) - RC_Channel& _servo_1; // swash plate servo #1 - RC_Channel& _servo_2; // swash plate servo #2 - RC_Channel& _servo_3; // swash plate servo #3 - RC_Channel& _servo_4; // tail servo - - AP_MotorsHeli_RSC _main_rotor; // main rotor - AP_MotorsHeli_RSC _tail_rotor; // tail rotor + virtual void calculate_roll_pitch_collective_factors() = 0; // flags bitmask struct heliflags_type { @@ -273,27 +207,17 @@ private: } _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 _tail_type; // Tail type used: Servo, Servo with external gyro, direct drive variable pitch or direct drive fixed pitch - AP_Int8 _swash_type; // Swash Type Setting - either 3-servo CCPM or H1 Mechanical Mixing - AP_Int16 _ext_gyro_gain; // PWM sent to external gyro on ch7 when tail type is Servo w/ ExtGyro 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_Float _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 _rsc_setpoint; // rotor speed when RSC mode is set to is enabledv AP_Int8 _rsc_mode; // Which main rotor ESC control mode is active AP_Int8 _rsc_ramp_time; // Time in seconds for the output to the main rotor's ESC to reach full speed AP_Int8 _rsc_runup_time; // Time in seconds for the main rotor to reach full speed. Must be longer than _rsc_ramp_time - AP_Int8 _flybar_mode; // Flybar present or not. Affects attitude controller used during ACRO flight mode AP_Int16 _land_collective_min; // Minimum collective when landed or landing - AP_Int16 _direct_drive_tailspeed; // Direct Drive VarPitch Tail ESC speed (0 ~ 1000) AP_Int16 _rsc_critical; // Rotor speed below which flight is not possible // internal variables diff --git a/libraries/AP_Motors/AP_MotorsHeli_Single.cpp b/libraries/AP_Motors/AP_MotorsHeli_Single.cpp new file mode 100644 index 0000000000..8de6e12248 --- /dev/null +++ b/libraries/AP_Motors/AP_MotorsHeli_Single.cpp @@ -0,0 +1,498 @@ +// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- +/* + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + */ + +#include +#include +#include "AP_MotorsHeli_Single.h" + +extern const AP_HAL::HAL& hal; + +const AP_Param::GroupInfo AP_MotorsHeli_Single::var_info[] PROGMEM = { + AP_NESTEDGROUPINFO(AP_MotorsHeli, 0), + + // @Param: SV1_POS + // @DisplayName: Servo 1 Position + // @Description: Angular location of swash servo #1 + // @Range: -180 180 + // @Units: Degrees + // @User: Standard + // @Increment: 1 + AP_GROUPINFO("SV1_POS", 1, AP_MotorsHeli_Single, _servo1_pos, AP_MOTORS_HELI_SINGLE_SERVO1_POS), + + // @Param: SV2_POS + // @DisplayName: Servo 2 Position + // @Description: Angular location of swash servo #2 + // @Range: -180 180 + // @Units: Degrees + // @User: Standard + // @Increment: 1 + AP_GROUPINFO("SV2_POS", 2, AP_MotorsHeli_Single, _servo2_pos, AP_MOTORS_HELI_SINGLE_SERVO2_POS), + + // @Param: SV3_POS + // @DisplayName: Servo 3 Position + // @Description: Angular location of swash servo #3 + // @Range: -180 180 + // @Units: Degrees + // @User: Standard + // @Increment: 1 + AP_GROUPINFO("SV3_POS", 3, AP_MotorsHeli_Single, _servo3_pos, AP_MOTORS_HELI_SINGLE_SERVO3_POS), + + // @Param: TAIL_TYPE + // @DisplayName: Tail Type + // @Description: Tail type selection. Simpler yaw controller used if external gyro is selected + // @Values: 0:Servo only,1:Servo with ExtGyro,2:DirectDrive VarPitch,3:DirectDrive FixedPitch + // @User: Standard + AP_GROUPINFO("TAIL_TYPE", 4, AP_MotorsHeli_Single, _tail_type, AP_MOTORS_HELI_SINGLE_TAILTYPE_SERVO), + + // @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", 5, AP_MotorsHeli_Single, _swash_type, AP_MOTORS_HELI_SINGLE_SWASH_CCPM), + + // @Param: GYR_GAIN + // @DisplayName: External Gyro Gain + // @Description: PWM sent to external gyro on ch7 when tail type is Servo w/ ExtGyro + // @Range: 0 1000 + // @Units: PWM + // @Increment: 1 + // @User: Standard + AP_GROUPINFO("GYR_GAIN", 6, AP_MotorsHeli_Single, _ext_gyro_gain, AP_MOTORS_HELI_SINGLE_EXT_GYRO_GAIN), + + // @Param: PHANG + // @DisplayName: Swashplate Phase Angle Compensation + // @Description: Phase angle correction for rotor head. If pitching the swash forward induces a roll, this can be correct the problem + // @Range: -90 90 + // @Units: Degrees + // @User: Advanced + // @Increment: 1 + AP_GROUPINFO("PHANG", 7, AP_MotorsHeli_Single, _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 + // @Increment: 0.1 + AP_GROUPINFO("COLYAW", 8, AP_MotorsHeli_Single, _collective_yaw_effect, 0), + + // @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", 9, AP_MotorsHeli_Single, _flybar_mode, AP_MOTORS_HELI_NOFLYBAR), + + // @Param: TAIL_SPEED + // @DisplayName: Direct Drive VarPitch Tail ESC speed + // @Description: Direct Drive VarPitch Tail ESC speed. Only used when TailType is DirectDrive VarPitch + // @Range: 0 1000 + // @Units: PWM + // @Increment: 1 + // @User: Standard + AP_GROUPINFO("TAIL_SPEED", 10, AP_MotorsHeli_Single, _direct_drive_tailspeed, AP_MOTOR_HELI_SINGLE_DDTAIL_DEFAULT), + + AP_GROUPEND +}; + +// +// public methods +// + +// init +void AP_MotorsHeli_Single::Init() +{ + AP_MotorsHeli::Init(); + + // disable channels 7 and 8 from being used by RC_Channel_aux + RC_Channel_aux::disable_aux_channel(_motor_to_channel_map[AP_MOTORS_HELI_SINGLE_AUX]); + RC_Channel_aux::disable_aux_channel(_motor_to_channel_map[AP_MOTORS_HELI_SINGLE_RSC]); +} + + +// set update rate to motors - a value in hertz +void AP_MotorsHeli_Single::set_update_rate( uint16_t speed_hz ) +{ + // record requested speed + _speed_hz = speed_hz; + + // setup fast channels + uint32_t mask = + 1U << pgm_read_byte(&_motor_to_channel_map[AP_MOTORS_MOT_1]) | + 1U << pgm_read_byte(&_motor_to_channel_map[AP_MOTORS_MOT_2]) | + 1U << pgm_read_byte(&_motor_to_channel_map[AP_MOTORS_MOT_3]) | + 1U << pgm_read_byte(&_motor_to_channel_map[AP_MOTORS_MOT_4]); + hal.rcout->set_freq(mask, _speed_hz); +} + +// enable - starts allowing signals to be sent to motors +void AP_MotorsHeli_Single::enable() +{ + // enable output channels + hal.rcout->enable_ch(pgm_read_byte(&_motor_to_channel_map[AP_MOTORS_MOT_1])); // swash servo 1 + hal.rcout->enable_ch(pgm_read_byte(&_motor_to_channel_map[AP_MOTORS_MOT_2])); // swash servo 2 + hal.rcout->enable_ch(pgm_read_byte(&_motor_to_channel_map[AP_MOTORS_MOT_3])); // swash servo 3 + hal.rcout->enable_ch(pgm_read_byte(&_motor_to_channel_map[AP_MOTORS_MOT_4])); // yaw + hal.rcout->enable_ch(AP_MOTORS_HELI_SINGLE_AUX); // output for gyro gain or direct drive variable pitch tail motor + hal.rcout->enable_ch(AP_MOTORS_HELI_SINGLE_RSC); // output for main rotor esc +} + +// 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 AP_MotorsHeli_Single::output_test(uint8_t motor_seq, int16_t pwm) +{ + // exit immediately if not armed + if (!armed()) { + return; + } + + // output to motors and servos + switch (motor_seq) { + case 1: + // swash servo 1 + hal.rcout->write(pgm_read_byte(&_motor_to_channel_map[AP_MOTORS_MOT_1]), pwm); + break; + case 2: + // swash servo 2 + hal.rcout->write(pgm_read_byte(&_motor_to_channel_map[AP_MOTORS_MOT_2]), pwm); + break; + case 3: + // swash servo 3 + hal.rcout->write(pgm_read_byte(&_motor_to_channel_map[AP_MOTORS_MOT_3]), pwm); + break; + case 4: + // external gyro & tail servo + if (_tail_type == AP_MOTORS_HELI_SINGLE_TAILTYPE_SERVO_EXTGYRO) { + write_aux(_ext_gyro_gain); + } + hal.rcout->write(pgm_read_byte(&_motor_to_channel_map[AP_MOTORS_MOT_4]), pwm); + break; + case 5: + // main rotor + hal.rcout->write(pgm_read_byte(&_motor_to_channel_map[AP_MOTORS_HELI_SINGLE_RSC]), pwm); + break; + default: + // do nothing + break; + } +} + +// allow_arming - check if it's safe to arm +bool AP_MotorsHeli_Single::allow_arming() const +{ + // returns false if main rotor speed is not zero + if (_rsc_mode != AP_MOTORS_HELI_RSC_MODE_NONE && _main_rotor.get_estimated_speed() > 0) { + return false; + } + + // all other cases it is OK to arm + return true; +} + + +// set_desired_rotor_speed +void AP_MotorsHeli_Single::set_desired_rotor_speed(int16_t desired_speed) +{ + _main_rotor.set_desired_speed(desired_speed); + + if (desired_speed > 0 && _tail_type == AP_MOTORS_HELI_SINGLE_TAILTYPE_DIRECTDRIVE_VARPITCH) { + _tail_rotor.set_desired_speed(_direct_drive_tailspeed); + } else { + _tail_rotor.set_desired_speed(0); + } +} + + +// recalc_scalers - recalculates various scalers used. Should be called at about 1hz to allow users to see effect of changing parameters +void AP_MotorsHeli_Single::recalc_scalers() +{ + if (_rsc_mode != AP_MOTORS_HELI_RSC_MODE_SETPOINT) { + _tail_rotor.set_ramp_time(0); + _tail_rotor.set_runup_time(0); + _tail_rotor.set_critical_speed(0); + } else { + _main_rotor.set_ramp_time(_rsc_ramp_time); + _main_rotor.set_runup_time(_rsc_runup_time); + _main_rotor.set_critical_speed(_rsc_critical); + } + + _main_rotor.recalc_scalers(); + + if (_rsc_mode != AP_MOTORS_HELI_SINGLE_TAILTYPE_DIRECTDRIVE_VARPITCH) { + _tail_rotor.set_ramp_time(0); + _tail_rotor.set_runup_time(0); + _tail_rotor.set_critical_speed(0); + } else { + _tail_rotor.set_ramp_time(_rsc_ramp_time); + _tail_rotor.set_runup_time(_rsc_runup_time); + _tail_rotor.set_critical_speed(_rsc_critical); + } + + _tail_rotor.recalc_scalers(); +} + + +// 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 AP_MotorsHeli_Single::get_motor_mask() +{ + // heli uses channels 1,2,3,4,7 and 8 + return (1U << 0 | 1U << 1 | 1U << 2 | 1U << 3 | 1U << AP_MOTORS_HELI_SINGLE_AUX | 1U << AP_MOTORS_HELI_SINGLE_RSC); +} + +// sends commands to the motors +void AP_MotorsHeli_Single::output_armed_stabilizing() +{ + move_swash(_roll_control_input, _pitch_control_input, _throttle_control_input, _yaw_control_input); + + if (_tail_type == AP_MOTORS_HELI_SINGLE_TAILTYPE_DIRECTDRIVE_VARPITCH) { + _tail_rotor.output_armed(); + + if (!_tail_rotor.is_runup_complete()) + { + _heliflags.rotor_runup_complete = false; + return; + } + } + + _main_rotor.output_armed(); + + _heliflags.rotor_runup_complete = _main_rotor.is_runup_complete(); +} + +// output_disarmed - sends commands to the motors +void AP_MotorsHeli_Single::output_disarmed() +{ + move_swash(_roll_control_input, _pitch_control_input, _throttle_control_input, _yaw_control_input); + + if (_tail_type == AP_MOTORS_HELI_SINGLE_TAILTYPE_DIRECTDRIVE_VARPITCH) { + _tail_rotor.output_disarmed(); + } + + _main_rotor.output_disarmed(); + + _heliflags.rotor_runup_complete = false; +} + +// reset_servos +void AP_MotorsHeli_Single::reset_servos() +{ + reset_swash_servo (_servo_1); + reset_swash_servo (_servo_2); + reset_swash_servo (_servo_3); +} + +// init_servos +void AP_MotorsHeli_Single::init_servos() +{ + init_swash_servo (_servo_1); + init_swash_servo (_servo_2); + init_swash_servo (_servo_3); + + _servo_4.set_angle(4500); +} + +// calculate_roll_pitch_collective_factors - calculate factors based on swash type and servo position +void AP_MotorsHeli_Single::calculate_roll_pitch_collective_factors() +{ + if (_swash_type == AP_MOTORS_HELI_SINGLE_SWASH_CCPM) { //CCPM Swashplate, perform control mixing + + // roll factors + _rollFactor[CH_1] = cosf(radians(_servo1_pos + 90 - (_phase_angle + _delta_phase_angle))); + _rollFactor[CH_2] = cosf(radians(_servo2_pos + 90 - (_phase_angle + _delta_phase_angle))); + _rollFactor[CH_3] = cosf(radians(_servo3_pos + 90 - (_phase_angle + _delta_phase_angle))); + + // pitch factors + _pitchFactor[CH_1] = cosf(radians(_servo1_pos - (_phase_angle + _delta_phase_angle))); + _pitchFactor[CH_2] = cosf(radians(_servo2_pos - (_phase_angle + _delta_phase_angle))); + _pitchFactor[CH_3] = cosf(radians(_servo3_pos - (_phase_angle + _delta_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; + } +} + +// +// heli_move_swash - moves swash plate to attitude of parameters passed in +// - expected ranges: +// roll : -4500 ~ 4500 +// pitch: -4500 ~ 4500 +// collective: 0 ~ 1000 +// yaw: -4500 ~ 4500 +// +void AP_MotorsHeli_Single::move_swash(int16_t roll_out, int16_t pitch_out, int16_t coll_in, int16_t yaw_out) +{ + // if manual override (i.e. when setting up swash), pass pilot commands straight through to swash + if (_servo_manual == 1) { + _roll_control_input = _roll_radio_passthrough; + _pitch_control_input = _pitch_radio_passthrough; + _throttle_control_input = _throttle_radio_passthrough; + _yaw_control_input = _yaw_radio_passthrough; + } + + int16_t yaw_offset = 0; + int16_t coll_out_scaled; + + // initialize limits flag + limit.roll_pitch = false; + limit.yaw = false; + limit.throttle_lower = false; + limit.throttle_upper = false; + + 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 (_heliflags.swash_initialised) { + reset_swash(); + } + // To-Do: This equation seems to be wrong. It probably restricts swash movement so that swash setup doesn't work right. + // _collective_scalar should probably not be used or set to 1? + coll_out_scaled = coll_in * _collective_scalar + _throttle_radio_min - 1000; + }else{ // regular flight mode + + // check if we need to reinitialise the swash + if (!_heliflags.swash_initialised) { + init_swash(); + } + + // rescale roll_out and pitch-out into the min and max ranges to provide linear motion + // across the input range instead of stopping when the input hits the constrain value + // these calculations are based on an assumption of the user specified roll_max and pitch_max + // 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; + if (roll_out < -_roll_max) { + roll_out = -_roll_max; + limit.roll_pitch = true; + } + if (roll_out > _roll_max) { + roll_out = _roll_max; + limit.roll_pitch = true; + } + + // scale pitch and update limits + pitch_out = pitch_out * _pitch_scaler; + if (pitch_out < -_pitch_max) { + pitch_out = -_pitch_max; + limit.roll_pitch = true; + } + if (pitch_out > _pitch_max) { + pitch_out = _pitch_max; + limit.roll_pitch = true; + } + + // constrain collective input + _collective_out = coll_in; + if (_collective_out <= 0) { + _collective_out = 0; + limit.throttle_lower = true; + } + if (_collective_out >= 1000) { + _collective_out = 1000; + limit.throttle_upper = true; + } + + // ensure not below landed/landing collective + if (_heliflags.landing_collective && _collective_out < _land_collective_min) { + _collective_out = _land_collective_min; + limit.throttle_lower = true; + } + + // scale collective pitch + coll_out_scaled = _collective_out * _collective_scalar + _collective_min - 1000; + + // rudder feed forward based on collective + // the feed-forward is not required when the motor is shut down and not creating torque + // also not required if we are using external gyro + if ((_main_rotor.get_desired_speed() > 0) && _tail_type != AP_MOTORS_HELI_SINGLE_TAILTYPE_SERVO_EXTGYRO) { + // sanity check collective_yaw_effect + _collective_yaw_effect = constrain_float(_collective_yaw_effect, -AP_MOTORS_HELI_SINGLE_COLYAW_RANGE, AP_MOTORS_HELI_SINGLE_COLYAW_RANGE); + yaw_offset = _collective_yaw_effect * abs(_collective_out - _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_SINGLE_SWASH_H1) { + _servo_1.servo_out += 500; + _servo_2.servo_out += 500; + } + _servo_3.servo_out = (_rollFactor[CH_3] * roll_out + _pitchFactor[CH_3] * pitch_out)/10 + _collectiveFactor[CH_3] * coll_out_scaled + (_servo_3.radio_trim-1500); + + // use servo_out to calculate pwm_out and radio_out + _servo_1.calc_pwm(); + _servo_2.calc_pwm(); + _servo_3.calc_pwm(); + + // actually move the servos + hal.rcout->write(pgm_read_byte(&_motor_to_channel_map[AP_MOTORS_MOT_1]), _servo_1.radio_out); + hal.rcout->write(pgm_read_byte(&_motor_to_channel_map[AP_MOTORS_MOT_2]), _servo_2.radio_out); + hal.rcout->write(pgm_read_byte(&_motor_to_channel_map[AP_MOTORS_MOT_3]), _servo_3.radio_out); + + // update the yaw rate using the tail rotor/servo + output_yaw(yaw_out + yaw_offset); +} + +// output_yaw +void AP_MotorsHeli_Single::output_yaw(int16_t yaw_out) +{ + _servo_4.servo_out = constrain_int16(yaw_out, -4500, 4500); + + if (_servo_4.servo_out != yaw_out) { + limit.yaw = true; + } + + _servo_4.calc_pwm(); + + hal.rcout->write(pgm_read_byte(&_motor_to_channel_map[AP_MOTORS_MOT_4]), _servo_4.radio_out); + + if (_tail_type == AP_MOTORS_HELI_SINGLE_TAILTYPE_SERVO_EXTGYRO) { + // output gain to exernal gyro + write_aux(_ext_gyro_gain); + } else if (_tail_type == AP_MOTORS_HELI_SINGLE_TAILTYPE_DIRECTDRIVE_FIXEDPITCH && _main_rotor.get_desired_speed() > 0) { + // output yaw servo to tail rsc + write_aux(_servo_4.servo_out); + } +} + +// write_aux - outputs pwm onto output aux channel (ch7) +// servo_out parameter is of the range 0 ~ 1000 +void AP_MotorsHeli_Single::write_aux(int16_t servo_out) +{ + _servo_aux.servo_out = servo_out; + _servo_aux.calc_pwm(); + hal.rcout->write(AP_MOTORS_HELI_SINGLE_AUX, _servo_aux.radio_out); +} \ No newline at end of file diff --git a/libraries/AP_Motors/AP_MotorsHeli_Single.h b/libraries/AP_Motors/AP_MotorsHeli_Single.h new file mode 100644 index 0000000000..05db269cca --- /dev/null +++ b/libraries/AP_Motors/AP_MotorsHeli_Single.h @@ -0,0 +1,173 @@ +// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- + +/// @file AP_MotorsHeli_Single.h +/// @brief Motor control class for traditional heli + +#ifndef __AP_MOTORS_HELI_SINGLE_H__ +#define __AP_MOTORS_HELI_SINGLE_H__ + +#include +#include +#include +#include + +#include "AP_MotorsHeli.h" +#include "AP_MotorsHeli_RSC.h" + +// rsc and aux function output channels +#define AP_MOTORS_HELI_SINGLE_RSC CH_8 +#define AP_MOTORS_HELI_SINGLE_AUX CH_7 + +// servo position defaults +#define AP_MOTORS_HELI_SINGLE_SERVO1_POS -60 +#define AP_MOTORS_HELI_SINGLE_SERVO2_POS 60 +#define AP_MOTORS_HELI_SINGLE_SERVO3_POS 180 + +// swash type definitions +#define AP_MOTORS_HELI_SINGLE_SWASH_CCPM 0 +#define AP_MOTORS_HELI_SINGLE_SWASH_H1 1 + +// tail types +#define AP_MOTORS_HELI_SINGLE_TAILTYPE_SERVO 0 +#define AP_MOTORS_HELI_SINGLE_TAILTYPE_SERVO_EXTGYRO 1 +#define AP_MOTORS_HELI_SINGLE_TAILTYPE_DIRECTDRIVE_VARPITCH 2 +#define AP_MOTORS_HELI_SINGLE_TAILTYPE_DIRECTDRIVE_FIXEDPITCH 3 + +// default direct-drive variable pitch speed +#define AP_MOTOR_HELI_SINGLE_DDTAIL_DEFAULT 500 + +// default external gyro gain +#define AP_MOTORS_HELI_SINGLE_EXT_GYRO_GAIN 350 + +// COLYAW parameter min and max values +#define AP_MOTORS_HELI_SINGLE_COLYAW_RANGE 10.0f + +#define AP_MOTORS_HELI_SINGLE_TAIL_RAMP_INCREMENT 5 // 5 is 2 seconds for direct drive tail rotor to reach to full speed (5 = (2sec*100hz)/1000) + +/// @class AP_MotorsHeli_Single +class AP_MotorsHeli_Single : public AP_MotorsHeli { +public: + // constructor + AP_MotorsHeli_Single(RC_Channel& servo_aux, + RC_Channel& servo_rsc, + RC_Channel& servo_1, + RC_Channel& servo_2, + RC_Channel& servo_3, + RC_Channel& servo_4, + uint16_t loop_rate, + uint16_t speed_hz = AP_MOTORS_HELI_SPEED_DEFAULT) : + AP_MotorsHeli(loop_rate, speed_hz), + _servo_aux(servo_aux), + _servo_1(servo_1), + _servo_2(servo_2), + _servo_3(servo_3), + _servo_4(servo_4), + _main_rotor(servo_rsc, AP_MOTORS_HELI_SINGLE_RSC, loop_rate), + _tail_rotor(servo_aux, AP_MOTORS_HELI_SINGLE_AUX, loop_rate) + { + AP_Param::setup_object_defaults(this, var_info); + }; + + // init + void Init(); + + // set update rate to motors - a value in hertz + // you must have setup_motors before calling this + void set_update_rate(uint16_t speed_hz); + + // enable - starts allowing signals to be sent to motors + void enable(); + + // 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); + + // allow_arming - returns true if main rotor is spinning and it is ok to arm + bool allow_arming() const; + + // set_desired_rotor_speed - sets target rotor speed as a number from 0 ~ 1000 + void set_desired_rotor_speed(int16_t desired_speed); + + // get_estimated_rotor_speed - gets estimated rotor speed as a number from 0 ~ 1000 + int16_t get_estimated_rotor_speed() const { return _main_rotor.get_estimated_speed(); } + + // get_desired_rotor_speed - gets target rotor speed as a number from 0 ~ 1000 + int16_t get_desired_rotor_speed() const { 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_estimated_speed() > _main_rotor.get_critical_speed(); } + + // recalc_scalers - recalculates various scalers used. Should be called at about 1hz to allow users to see effect of changing parameters + void recalc_scalers(); + + // 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(); + + // _tail_type - returns the tail type (servo, servo with ext gyro, direct drive var pitch, direct drive fixed pitch) + int16_t tail_type() const { return _tail_type; } + + // ext_gyro_gain - gets and sets external gyro gain as a pwm (1000~2000) + int16_t ext_gyro_gain() const { return _ext_gyro_gain; } + void ext_gyro_gain(int16_t pwm) { _ext_gyro_gain = pwm; } + + // has_flybar - returns true if we have a mechical flybar + bool has_flybar() const { return _flybar_mode; } + + // get_phase_angle - returns phase angle + int16_t get_phase_angle() const { return _phase_angle; } + + // 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; } + + // var_info + static const struct AP_Param::GroupInfo var_info[]; + +protected: + + // output - sends commands to the motors + void output_armed_stabilizing(); + void output_disarmed(); + void output_yaw(int16_t yaw_out); + + // reset_servos - free up the swash servos for maximum movement + void reset_servos(); + + // init_servos - initialize the servos + void init_servos(); + + // calculate_roll_pitch_collective_factors - calculate factors based on swash type and servo position + void calculate_roll_pitch_collective_factors(); + + // 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); + + // write_aux - outputs pwm onto output aux channel (ch7). servo_out parameter is of the range 0 ~ 1000 + void write_aux(int16_t servo_out); + + // external objects we depend upon + RC_Channel& _servo_aux; // output to ext gyro gain and tail direct drive esc (ch7) + RC_Channel& _servo_1; // swash plate servo #1 + RC_Channel& _servo_2; // swash plate servo #2 + RC_Channel& _servo_3; // swash plate servo #3 + RC_Channel& _servo_4; // tail servo + + AP_MotorsHeli_RSC _main_rotor; // main rotor + AP_MotorsHeli_RSC _tail_rotor; // tail rotor + + // 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 _tail_type; // Tail type used: Servo, Servo with external gyro, direct drive variable pitch or direct drive fixed pitch + AP_Int8 _swash_type; // Swash Type Setting - either 3-servo CCPM or H1 Mechanical Mixing + AP_Int16 _ext_gyro_gain; // PWM sent to external gyro on ch7 when tail type is Servo w/ ExtGyro + 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_Float _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_Int8 _flybar_mode; // Flybar present or not. Affects attitude controller used during ACRO flight mode + AP_Int16 _direct_drive_tailspeed; // Direct Drive VarPitch Tail ESC speed (0 ~ 1000) + +}; + +#endif // __AP_MOTORS_HELI_SINGLE_H__