From 197fc67897106c943e1552faf28e1ca16d836a1c Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Fri, 8 Nov 2013 17:29:54 +0900 Subject: [PATCH] TradHeli: move pilot desired rotor speed to heli.pde --- ArduCopter/ArduCopter.pde | 3 ++ ArduCopter/heli.pde | 25 ++++++++++ libraries/AP_Motors/AP_MotorsHeli.cpp | 69 +++++---------------------- libraries/AP_Motors/AP_MotorsHeli.h | 15 +++++- 4 files changed, 53 insertions(+), 59 deletions(-) diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index 59ea17cf99..302017fa31 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -1086,6 +1086,9 @@ static void throttle_loop() update_auto_armed(); #if FRAME_CONFIG == HELI_FRAME + // update rotor speed + heli_update_rotor_speed_targets(); + // update trad heli swash plate movement heli_update_landing_swash(); #endif diff --git a/ArduCopter/heli.pde b/ArduCopter/heli.pde index c9da25f123..a621f8a887 100644 --- a/ArduCopter/heli.pde +++ b/ArduCopter/heli.pde @@ -225,4 +225,29 @@ static void heli_update_landing_swash() } } +// heli_update_rotor_speed_targets - reads pilot input and passes new rotor speed targets to heli motors object +static void heli_update_rotor_speed_targets() +{ + // get rotor control method + uint8_t rsc_control_mode = motors.get_rsc_mode(); + + switch (rsc_control_mode) { + case AP_MOTORS_HELI_RSC_MODE_NONE: + // even though pilot passes rotors speed directly to rotor ESC via receiver, motor lib needs to know if + // rotor is spinning in case we are using direct drive tailrotor which must be spun up at same time + case AP_MOTORS_HELI_RSC_MODE_CH8_PASSTHROUGH: + // pass through pilot desired rotor speed + motors.set_desired_rotor_speed(g.rc_8.control_in); + break; + case AP_MOTORS_HELI_RSC_MODE_SETPOINT: + // pass setpoint through as desired rotor speed + if (g.rc_8.control_in > 0) { + motors.set_desired_rotor_speed(motors.get_rsc_setpoint()); + }else{ + motors.set_desired_rotor_speed(0); + } + break; + } +} + #endif // FRAME_CONFIG == HELI_FRAME \ No newline at end of file diff --git a/libraries/AP_Motors/AP_MotorsHeli.cpp b/libraries/AP_Motors/AP_MotorsHeli.cpp index e615169af0..15561d6f61 100644 --- a/libraries/AP_Motors/AP_MotorsHeli.cpp +++ b/libraries/AP_Motors/AP_MotorsHeli.cpp @@ -361,6 +361,12 @@ int16_t AP_MotorsHeli::get_pilot_desired_collective(int16_t control_in) return collective_out; } +// set_desired_rotor_speed - sets target rotor speed as a number from 0 ~ 1000 +void AP_MotorsHeli::set_desired_rotor_speed(int16_t desired_speed) +{ + _rotor_desired = desired_speed; +} + // return true if the main rotor is up to speed bool AP_MotorsHeli::motor_runup_complete() { @@ -398,7 +404,6 @@ void AP_MotorsHeli::output_armed() _rc_yaw->servo_out = _rc_yaw->control_in; } - //static int counter = 0; _rc_roll->calc_pwm(); _rc_pitch->calc_pwm(); _rc_throttle->calc_pwm(); @@ -406,8 +411,8 @@ void AP_MotorsHeli::output_armed() move_swash( _rc_roll->servo_out, _rc_pitch->servo_out, _rc_throttle->servo_out, _rc_yaw->servo_out ); - // To-Do: clean-up the passing in of the desired rotor speed - rsc_control(_servo_rsc->control_in); + // update rotor and direct drive esc speeds + rsc_control(); } // output_disarmed - sends commands to the motors @@ -657,7 +662,7 @@ void AP_MotorsHeli::move_swash(int16_t roll_out, int16_t pitch_out, int16_t coll // rsc_control - update value to send to tail and main rotor's ESC // desired_rotor_speed is a desired speed from 0 to 1000 -void AP_MotorsHeli::rsc_control(int16_t desired_rotor_speed) +void AP_MotorsHeli::rsc_control() { // if disarmed output minimums if (!armed()) { @@ -674,63 +679,13 @@ void AP_MotorsHeli::rsc_control(int16_t desired_rotor_speed) return; } - // handle simpler case of pilot directly controlling main rotor - if (_rsc_mode == AP_MOTORS_HELI_RSC_MODE_NONE) { - switch (_tail_type) { - // return immediately if no direct drive tail motor - case AP_MOTORS_HELI_TAILTYPE_SERVO: - case AP_MOTORS_HELI_TAILTYPE_SERVO_EXTGYRO: - default: - return; - break; - - // direct drive variable pitch tail - case AP_MOTORS_HELI_TAILTYPE_DIRECTDRIVE_VARPITCH: - if (desired_rotor_speed > 0) { - // ramp up tail if main rotor on - tail_ramp(_direct_drive_setpoint); - }else{ - // ramp down tail rotor if main rotor off - tail_ramp(0); - } - return; - break; - - // direct drive fixed pitch tail - case AP_MOTORS_HELI_TAILTYPE_DIRECTDRIVE_FIXEDPITCH: - // output fixed-pitch speed control if Ch8 is high - if (desired_rotor_speed > 0) { - // copy yaw output to tail esc - write_aux(_servo_4->servo_out); - }else{ - write_aux(0); - } - return; - break; - } - return; - } - - // handle more complex case of channel 8 passthrough and external governor - int16_t temp_rotor_speed; - if (_rsc_mode == AP_MOTORS_HELI_RSC_MODE_CH8_PASSTHROUGH) { - temp_rotor_speed = desired_rotor_speed; - }else{ - // rotor speed is predefined set-point - if (desired_rotor_speed > 0) { - temp_rotor_speed = _rsc_setpoint; - }else{ - temp_rotor_speed = 0; - } - } - // ramp up or down main rotor and tail - if (temp_rotor_speed > 0) { + if (_rotor_desired > 0) { // ramp up tail rotor (this does nothing if not using direct drive variable pitch tail) tail_ramp(_direct_drive_tailspeed); // note: this always returns true if not using direct drive variable pitch tail if (tail_rotor_runup_complete()) { - rotor_ramp(temp_rotor_speed); + rotor_ramp(_rotor_desired); } }else{ // shutting down main rotor @@ -744,7 +699,7 @@ void AP_MotorsHeli::rsc_control(int16_t desired_rotor_speed) // direct drive fixed pitch tail servo gets copy of yaw servo out (ch4) while main rotor is running if (_tail_type == AP_MOTORS_HELI_TAILTYPE_DIRECTDRIVE_FIXEDPITCH) { // output fixed-pitch speed control if Ch8 is high - if (temp_rotor_speed > 0 || _rotor_out > 0) { + if (_rotor_desired > 0 || _rotor_out > 0) { // copy yaw output to tail esc write_aux(_servo_4->servo_out); }else{ diff --git a/libraries/AP_Motors/AP_MotorsHeli.h b/libraries/AP_Motors/AP_MotorsHeli.h index ebacdcf540..74673fe662 100644 --- a/libraries/AP_Motors/AP_MotorsHeli.h +++ b/libraries/AP_Motors/AP_MotorsHeli.h @@ -114,6 +114,7 @@ public: _collective_scalar_manual(1), _collective_out(0), _collective_mid_pwm(0), + _rotor_desired(0), _rotor_out(0), _rsc_ramp_increment(0.0f), _tail_direct_drive_out(0) @@ -175,6 +176,15 @@ public: // 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; } + // get_rsc_mode - gets the rotor speed control method (AP_MOTORS_HELI_RSC_MODE_NONE, AP_MOTORS_HELI_RSC_MODE_CH8_PASSTHROUGH or AP_MOTORS_HELI_RSC_MODE_SETPOINT) + uint8_t get_rsc_mode() { return _rsc_mode; } + + // get_rsc_setpoint - gets contents of _rsc_setpoint parameter (0~1000) + int16_t get_rsc_setpoint() { 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); + // return true if the main rotor is up to speed bool motor_runup_complete(); @@ -205,7 +215,7 @@ private: void calculate_roll_pitch_collective_factors(); // rsc_control - main function to update values to send to main rotor and tail rotor ESCs - void rsc_control(int16_t desired_rotor_speed); + void rsc_control(); // rotor_ramp - ramps rotor towards target. result put rotor_out and sent to ESC void rotor_ramp(int16_t rotor_target); @@ -272,7 +282,8 @@ private: 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) - float _rotor_out; // output to the rotor (0 ~ 1000) + int16_t _rotor_desired; // latest desired rotor speed from pilot + float _rotor_out; // latest output sent to the main rotor or an estimate of the rotors actual speed (whichever is higher) (0 ~ 1000) float _rsc_ramp_increment; // the amount we can increase the rotor output during each 100hz iteration int16_t _tail_direct_drive_out; // current ramped speed of output on ch7 when using direct drive variable pitch tail type };