From 84102c3e3f567edbf09e35298cbf4af49a9c18f0 Mon Sep 17 00:00:00 2001 From: Robert Lefebvre Date: Wed, 12 Aug 2015 11:41:40 -0400 Subject: [PATCH] AP_MotorsHeli: Rework how servo setup is done. --- libraries/AP_Motors/AP_MotorsHeli.cpp | 63 +-- libraries/AP_Motors/AP_MotorsHeli.h | 24 +- libraries/AP_Motors/AP_MotorsHeli_Single.cpp | 452 ++++++++++--------- libraries/AP_Motors/AP_MotorsHeli_Single.h | 20 +- 4 files changed, 249 insertions(+), 310 deletions(-) diff --git a/libraries/AP_Motors/AP_MotorsHeli.cpp b/libraries/AP_Motors/AP_MotorsHeli.cpp index a4aa3a49ff..a658871700 100644 --- a/libraries/AP_Motors/AP_MotorsHeli.cpp +++ b/libraries/AP_Motors/AP_MotorsHeli.cpp @@ -165,14 +165,14 @@ void AP_MotorsHeli::Init() // set update rate set_update_rate(_speed_hz); - // ensure inputs are not passed through to servos + // ensure inputs are not passed through to servos on start-up _servo_manual = 0; - // initialise some scalers - recalc_scalers(); - // initialise swash plate - init_swash(); + init_outputs(); + + // calculate all scalars + calculate_scalars(); } // output - sends commands to the servos @@ -221,65 +221,16 @@ bool AP_MotorsHeli::parameter_check() const return true; } -// reset_swash - free up swash for maximum movements. Used for set-up -void AP_MotorsHeli::reset_swash() -{ - // free up servo ranges - reset_servos(); - - // 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)(_throttle_radio_max - _throttle_radio_min))/1000.0f; - _collective_scalar_manual = 1.0f; - - // we must be in set-up mode so mark swash as uninitialised - _heliflags.swash_initialised = false; -} - // reset_swash_servo void AP_MotorsHeli::reset_swash_servo(RC_Channel& servo) { servo.set_range(0, 1000); + + // swash servos always use full endpoints as restricting them would lead to scaling errors servo.radio_min = 1000; servo.radio_max = 2000; } -// init_swash - initialise the swash plate -void AP_MotorsHeli::init_swash() -{ - - // swash servo initialisation - init_servos(); - - // 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); - - // 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 maximum collective pitch range from full positive pitch to zero pitch - _collective_range = 1000 - _collective_mid_pwm; - - // 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; - - // calculate factors based on swash type and servo position - calculate_roll_pitch_collective_factors(); - - // mark swash as initialised - _heliflags.swash_initialised = true; -} - // update the throttle input filter void AP_MotorsHeli::update_throttle_filter() { diff --git a/libraries/AP_Motors/AP_MotorsHeli.h b/libraries/AP_Motors/AP_MotorsHeli.h index 583146d965..4e99ff2d1a 100644 --- a/libraries/AP_Motors/AP_MotorsHeli.h +++ b/libraries/AP_Motors/AP_MotorsHeli.h @@ -75,7 +75,6 @@ public: AP_Param::setup_object_defaults(this, var_info); // initialise flags - _heliflags.swash_initialised = 0; _heliflags.landing_collective = 0; _heliflags.rotor_runup_complete = 0; }; @@ -144,8 +143,8 @@ public: // rotor_speed_above_critical - return true if rotor speed is above that critical for flight 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 - virtual void recalc_scalers() = 0; + // calculate_scalars - must be implemented by child classes + virtual void calculate_scalars() = 0; // var_info for holding Parameter information static const struct AP_Param::GroupInfo var_info[]; @@ -181,30 +180,20 @@ protected: // update the throttle input filter void update_throttle_filter(); - // heli_move_swash - moves swash plate to attitude of parameters passed in - virtual void move_swash(int16_t roll_out, int16_t pitch_out, int16_t coll_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 - virtual void reset_servos() = 0; + // move_actuators - moves swash plate and tail rotor + virtual void move_actuators(int16_t roll_out, int16_t pitch_out, int16_t coll_in, int16_t yaw_out) = 0; // reset_swash_servo - free up swash servo for maximum movement static void reset_swash_servo(RC_Channel& servo); - // init_swash - initialise the swash plate - void init_swash(); - - // init_servos - initialize the servos - virtual void init_servos() = 0; + // init_outputs - initialise Servo/PWM ranges and endpoints + virtual void init_outputs() = 0; // calculate_roll_pitch_collective_factors - calculate factors based on swash type and servo position virtual void calculate_roll_pitch_collective_factors() = 0; // flags bitmask struct heliflags_type { - uint8_t swash_initialised : 1; // true if swash has been initialised uint8_t landing_collective : 1; // true if collective is setup for landing which has much higher minimum uint8_t rotor_runup_complete : 1; // true if the rotors have had enough time to wind up } _heliflags; @@ -233,7 +222,6 @@ protected: float _roll_scaler = 1; // scaler to convert roll input from radio (i.e. -4500 ~ 4500) to max roll range float _pitch_scaler = 1; // scaler to convert pitch input from radio (i.e. -4500 ~ 4500) to max pitch range float _collective_scalar = 1; // 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 = 1; // 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 = 0; // actual collective pitch value. Required by the main code for calculating cruise throttle int16_t _collective_mid_pwm = 0; // collective mid parameter value converted to pwm form (i.e. 0 ~ 1000) int16_t _delta_phase_angle = 0; // phase angle dynamic compensation diff --git a/libraries/AP_Motors/AP_MotorsHeli_Single.cpp b/libraries/AP_Motors/AP_MotorsHeli_Single.cpp index 24529c042b..b463a00791 100644 --- a/libraries/AP_Motors/AP_MotorsHeli_Single.cpp +++ b/libraries/AP_Motors/AP_MotorsHeli_Single.cpp @@ -108,21 +108,6 @@ const AP_Param::GroupInfo AP_MotorsHeli_Single::var_info[] PROGMEM = { 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 ) { @@ -138,7 +123,7 @@ void AP_MotorsHeli_Single::set_update_rate( uint16_t speed_hz ) hal.rcout->set_freq(mask, _speed_hz); } -// enable - starts allowing signals to be sent to motors +// enable - starts allowing signals to be sent to motors and servos void AP_MotorsHeli_Single::enable() { // enable output channels @@ -146,8 +131,27 @@ void AP_MotorsHeli_Single::enable() 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 + 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 + + // 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]); +} + +// init_outputs - initialise Servo/PWM ranges and endpoints +void AP_MotorsHeli_Single::init_outputs() +{ + // reset swash servo range and endpoints + reset_swash_servo (_swash_servo_1); + reset_swash_servo (_swash_servo_2); + reset_swash_servo (_swash_servo_3); + + _yaw_servo.set_angle(4500); + + // set main rotor servo range + // tail rotor servo use range as set in vehicle code for rc7 + _main_rotor.init_servo(); } // output_test - spin a motor at the pwm value specified @@ -216,9 +220,31 @@ void AP_MotorsHeli_Single::set_desired_rotor_speed(int16_t desired_speed) } } -// recalc_scalers - recalculates various scalers used. -void AP_MotorsHeli_Single::recalc_scalers() +// calculate_scalars - recalculates various scalers used. +void AP_MotorsHeli_Single::calculate_scalars() { + // range check collective min, max and mid + if( _collective_min >= _collective_max ) { + _collective_min = AP_MOTORS_HELI_COLLECTIVE_MIN; + _collective_max = AP_MOTORS_HELI_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 maximum collective pitch range from full positive pitch to zero pitch + _collective_range = 1000 - _collective_mid_pwm; + + // 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; + + // calculate factors based on swash type and servo position + calculate_roll_pitch_collective_factors(); + + // send setpoints to main rotor controller and trigger recalculation of scalars _main_rotor.set_control_mode(_rsc_mode); _main_rotor.set_ramp_time(_rsc_ramp_time); _main_rotor.set_runup_time(_rsc_runup_time); @@ -227,6 +253,7 @@ void AP_MotorsHeli_Single::recalc_scalers() _main_rotor.set_power_output_range(_rsc_power_low, _rsc_power_high); _main_rotor.recalc_scalers(); + // send setpoints to tail rotor controller and trigger recalculation of scalars if (_rsc_mode == AP_MOTORS_HELI_SINGLE_TAILTYPE_DIRECTDRIVE_VARPITCH) { _tail_rotor.set_control_mode(AP_MOTORS_HELI_RSC_MODE_SETPOINT); _tail_rotor.set_ramp_time(_rsc_ramp_time); @@ -243,151 +270,6 @@ void AP_MotorsHeli_Single::recalc_scalers() _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); -} - -// output_min - sets servos to neutral point -void AP_MotorsHeli_Single::output_min() -{ - // move swash to mid - move_swash(0,0,500,0); - - _main_rotor.output(ROTOR_CONTROL_STOP); - _tail_rotor.output(ROTOR_CONTROL_STOP); - - // override limits flags - limit.roll_pitch = true; - limit.yaw = true; - limit.throttle_lower = true; - limit.throttle_upper = false; -} - -// sends commands to the motors -void AP_MotorsHeli_Single::output_armed_stabilizing() -{ - // if manual override active after arming, deactivate it. - if (_servo_manual == 1) { - reset_radio_passthrough(); - _servo_manual = 0; - } - - 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(ROTOR_CONTROL_ACTIVE); - - if (!_tail_rotor.is_runup_complete()) - { - _heliflags.rotor_runup_complete = false; - return; - } - } - - _main_rotor.output(ROTOR_CONTROL_ACTIVE); - - _heliflags.rotor_runup_complete = _main_rotor.is_runup_complete(); -} - -void AP_MotorsHeli_Single::output_armed_not_stabilizing() -{ - // if manual override active after arming, deactivate it. - if (_servo_manual == 1) { - reset_radio_passthrough(); - _servo_manual = 0; - } - - 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(ROTOR_CONTROL_ACTIVE); - - if (!_tail_rotor.is_runup_complete()) - { - _heliflags.rotor_runup_complete = false; - return; - } - } - - _main_rotor.output(ROTOR_CONTROL_ACTIVE); - - _heliflags.rotor_runup_complete = _main_rotor.is_runup_complete(); -} - -// output_armed_zero_throttle - sends commands to the motors -void AP_MotorsHeli_Single::output_armed_zero_throttle() -{ - // if manual override active after arming, deactivate it. - if (_servo_manual == 1) { - reset_radio_passthrough(); - _servo_manual = 0; - } - - 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(ROTOR_CONTROL_IDLE); - - if (!_tail_rotor.is_runup_complete()) - { - _heliflags.rotor_runup_complete = false; - return; - } - } - - _main_rotor.output(ROTOR_CONTROL_IDLE); - - _heliflags.rotor_runup_complete = _main_rotor.is_runup_complete(); -} - - -// output_disarmed - sends commands to the motors -void AP_MotorsHeli_Single::output_disarmed() -{ - // 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; - } - - 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(ROTOR_CONTROL_STOP); - } - - _main_rotor.output(ROTOR_CONTROL_STOP); - - _heliflags.rotor_runup_complete = false; -} - -// reset_servos -void AP_MotorsHeli_Single::reset_servos() -{ - reset_swash_servo (_swash_servo_1); - reset_swash_servo (_swash_servo_2); - reset_swash_servo (_swash_servo_3); -} - -// init_servos -void AP_MotorsHeli_Single::init_servos() -{ - // reset swash servo range and endpoints - reset_servos(); - - _yaw_servo.set_angle(4500); - - // set main rotor servo range - // tail rotor servo use range as set in vehicle code for rc7 - _main_rotor.init_servo(); -} - // calculate_roll_pitch_collective_factors - calculate factors based on swash type and servo position void AP_MotorsHeli_Single::calculate_roll_pitch_collective_factors() { @@ -427,6 +309,141 @@ void AP_MotorsHeli_Single::calculate_roll_pitch_collective_factors() } } +// 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); +} + +// output_min - sets servos to neutral point +void AP_MotorsHeli_Single::output_min() +{ + // move swash to mid + move_actuators(0,0,500,0); + + _main_rotor.output(ROTOR_CONTROL_STOP); + _tail_rotor.output(ROTOR_CONTROL_STOP); + + // override limits flags + limit.roll_pitch = true; + limit.yaw = true; + limit.throttle_lower = true; + limit.throttle_upper = false; +} + +// sends commands to the motors +void AP_MotorsHeli_Single::output_armed_stabilizing() +{ + // if manual override active after arming, deactivate it and reinitialize servos + if (_servo_manual == 1) { + reset_radio_passthrough(); + _servo_manual = 0; + init_outputs(); + calculate_scalars(); + } + + move_actuators(_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(ROTOR_CONTROL_ACTIVE); + + if (!_tail_rotor.is_runup_complete()) + { + _heliflags.rotor_runup_complete = false; + return; + } + } + + _main_rotor.output(ROTOR_CONTROL_ACTIVE); + + _heliflags.rotor_runup_complete = _main_rotor.is_runup_complete(); +} + +void AP_MotorsHeli_Single::output_armed_not_stabilizing() +{ + // if manual override active after arming, deactivate it and reinitialize servos + if (_servo_manual == 1) { + reset_radio_passthrough(); + _servo_manual = 0; + init_outputs(); + calculate_scalars(); + } + + move_actuators(_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(ROTOR_CONTROL_ACTIVE); + + if (!_tail_rotor.is_runup_complete()) + { + _heliflags.rotor_runup_complete = false; + return; + } + } + + _main_rotor.output(ROTOR_CONTROL_ACTIVE); + + _heliflags.rotor_runup_complete = _main_rotor.is_runup_complete(); +} + +// output_armed_zero_throttle - sends commands to the motors +void AP_MotorsHeli_Single::output_armed_zero_throttle() +{ + // if manual override active after arming, deactivate it and reinitialize servos + if (_servo_manual == 1) { + reset_radio_passthrough(); + _servo_manual = 0; + init_outputs(); + calculate_scalars(); + } + + move_actuators(_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(ROTOR_CONTROL_IDLE); + + if (!_tail_rotor.is_runup_complete()) + { + _heliflags.rotor_runup_complete = false; + return; + } + } + + _main_rotor.output(ROTOR_CONTROL_IDLE); + + _heliflags.rotor_runup_complete = _main_rotor.is_runup_complete(); +} + +// output_disarmed - sends commands to the motors +void AP_MotorsHeli_Single::output_disarmed() +{ + // 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; + } + + // ensure swash servo endpoints haven't been moved + init_outputs(); + + // continuously recalculate scalars to allow setup + calculate_scalars(); + + move_actuators(_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(ROTOR_CONTROL_STOP); + } + + _main_rotor.output(ROTOR_CONTROL_STOP); + + _heliflags.rotor_runup_complete = false; +} + // set_delta_phase_angle for setting variable phase angle compensation and force // recalculation of collective factors void AP_MotorsHeli_Single::set_delta_phase_angle(int16_t angle) @@ -437,14 +454,14 @@ void AP_MotorsHeli_Single::set_delta_phase_angle(int16_t angle) } // -// heli_move_swash - moves swash plate to attitude of parameters passed in +// move_actuators - moves swash plate and tail rotor // - 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) +void AP_MotorsHeli_Single::move_actuators(int16_t roll_out, int16_t pitch_out, int16_t coll_in, int16_t yaw_out) { int16_t yaw_offset = 0; int16_t coll_out_scaled; @@ -455,67 +472,54 @@ void AP_MotorsHeli_Single::move_swash(int16_t roll_out, int16_t pitch_out, int16 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 + // 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; + } - // check if we need to reinitialise the swash - if (!_heliflags.swash_initialised) { - init_swash(); - } + // 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; + } - // 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; - } + // 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; + } - // 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; - } + // ensure not below landed/landing collective + if (_heliflags.landing_collective && _collective_out < _land_collective_min) { + _collective_out = _land_collective_min; + limit.throttle_lower = 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; - } + // scale collective pitch + coll_out_scaled = _collective_out * _collective_scalar + _collective_min - 1000; - // 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; - + // if servo output not in manual mode, process pre-compensation factors + if (_servo_manual == 0) { // rudder feed forward based on collective // the feed-forward is not required when the motor is stopped or at idle, and thus not creating torque // also not required if we are using external gyro @@ -524,6 +528,8 @@ void AP_MotorsHeli_Single::move_swash(int16_t roll_out, int16_t pitch_out, int16 _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); } + } else { + yaw_offset = 0; } // feed power estimate into main rotor controller diff --git a/libraries/AP_Motors/AP_MotorsHeli_Single.h b/libraries/AP_Motors/AP_MotorsHeli_Single.h index 75671eae38..45e8145044 100644 --- a/libraries/AP_Motors/AP_MotorsHeli_Single.h +++ b/libraries/AP_Motors/AP_MotorsHeli_Single.h @@ -66,14 +66,11 @@ public: 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 + // enable - starts allowing signals to be sent to motors and servos void enable(); // output_test - spin a motor at the pwm value specified @@ -99,8 +96,8 @@ public: // rotor_speed_above_critical - return true if rotor speed is above that critical for flight bool rotor_speed_above_critical() const { return _main_rotor.get_rotor_speed() > _main_rotor.get_critical_speed(); } - // recalc_scalers - recalculates various scalers used. - void recalc_scalers(); + // calculate_scalars - recalculates various scalars used + void calculate_scalars(); // 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 @@ -137,17 +134,14 @@ protected: void output_armed_zero_throttle(); void output_disarmed(); - // reset_servos - free up the swash servos for maximum movement - void reset_servos(); - - // init_servos - initialize the servos - void init_servos(); + // init_outputs - initialise Servo/PWM ranges and endpoints + void init_outputs(); // 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); + // heli_move_actuators - moves swash plate and tail rotor + void move_actuators(int16_t roll_out, int16_t pitch_out, int16_t coll_in, int16_t yaw_out); // move_yaw - moves the yaw servo void move_yaw(int16_t yaw_out);