diff --git a/libraries/AP_Motors/AP_MotorsHeli.cpp b/libraries/AP_Motors/AP_MotorsHeli.cpp index 31ebb87c31..711e33fb03 100644 --- a/libraries/AP_Motors/AP_MotorsHeli.cpp +++ b/libraries/AP_Motors/AP_MotorsHeli.cpp @@ -90,8 +90,8 @@ const AP_Param::GroupInfo AP_MotorsHeli::var_info[] PROGMEM = { // @Param: RSC_MODE // @DisplayName: Rotor Speed Control Mode - // @Description: Controls the source of the desired rotor speed, either ch8 or RSC_SETPOINT - // @Values: 1:Ch8 Input, 2:SetPoint + // @Description: Determines the method of rotor speed control + // @Values: 1:Ch8 Input, 2:SetPoint, 3:Throttle Curve // @User: Standard AP_GROUPINFO("RSC_MODE", 8, AP_MotorsHeli, _rsc_mode, AP_MOTORS_HELI_RSC_MODE_CH8_PASSTHROUGH), @@ -134,7 +134,23 @@ const AP_Param::GroupInfo AP_MotorsHeli::var_info[] PROGMEM = { // @Range: 0 500 // @Increment: 10 // @User: Standard - AP_GROUPINFO("RSC_IDLE", 13, AP_MotorsHeli, _rsc_idle, AP_MOTORS_HELI_RSC_IDLE_DEFAULT), + AP_GROUPINFO("RSC_IDLE", 13, AP_MotorsHeli, _rsc_idle_output, AP_MOTORS_HELI_RSC_IDLE_DEFAULT), + + // @Param: RSC_POWER_LOW + // @DisplayName: Throttle Servo Low Power Position + // @Description: Throttle output at zero collective pitch. + // @Range: 0 1000 + // @Increment: 10 + // @User: Standard + AP_GROUPINFO("RSC_POWER_LOW", 14, AP_MotorsHeli, _rsc_power_low, AP_MOTORS_HELI_RSC_POWER_LOW_DEFAULT), + + // @Param: RSC_POWER_HIGH + // @DisplayName: Throttle Servo High Power Position + // @Description: Throttle output at max collective pitch. + // @Range: 0 1000 + // @Increment: 10 + // @User: Standard + AP_GROUPINFO("RSC_POWER_HIGH", 15, AP_MotorsHeli, _rsc_power_high, AP_MOTORS_HELI_RSC_POWER_HIGH_DEFAULT), AP_GROUPEND }; @@ -196,8 +212,8 @@ bool AP_MotorsHeli::parameter_check() const return false; } - // returns false if Critical Rotor speed is not higher than Idle speed - if (_rsc_critical <= _rsc_idle){ + // returns false if idle output is higher than critical rotor speed as this could block runup_complete from going false + if ( _rsc_idle_output >= _rsc_critical){ return false; } @@ -248,6 +264,9 @@ void AP_MotorsHeli::init_swash() // 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; diff --git a/libraries/AP_Motors/AP_MotorsHeli.h b/libraries/AP_Motors/AP_MotorsHeli.h index e7ee7bd654..a4500df872 100644 --- a/libraries/AP_Motors/AP_MotorsHeli.h +++ b/libraries/AP_Motors/AP_MotorsHeli.h @@ -40,6 +40,7 @@ #define AP_MOTORS_HELI_RSC_MODE_DISABLED 0 // not a valid RSC Mode #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 #define AP_MOTORS_HELI_RSC_MODE_SETPOINT 2 // main rotor ESC is connected to RC8 (out), desired speed is held in RSC_SETPOINT parameter +#define AP_MOTORS_HELI_RSC_MODE_THROTTLE_CURVE 3 // main rotor speed is controlled open-loop by a throttle servo or ESC connected to RC8(out) // default main rotor speed (ch8 out) as a number from 0 ~ 1000 #define AP_MOTORS_HELI_RSC_SETPOINT 700 @@ -47,8 +48,10 @@ // default main rotor critical speed #define AP_MOTORS_HELI_RSC_CRITICAL 500 -// default main rotor idle speed +// RSC output defaults #define AP_MOTORS_HELI_RSC_IDLE_DEFAULT 0 +#define AP_MOTORS_HELI_RSC_POWER_LOW_DEFAULT 200 +#define AP_MOTORS_HELI_RSC_POWER_HIGH_DEFAULT 700 // 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) @@ -222,7 +225,9 @@ protected: AP_Int8 _rsc_runup_time; // Time in seconds for the main rotor to reach full speed. Must be longer than _rsc_ramp_time AP_Int16 _land_collective_min; // Minimum collective when landed or landing AP_Int16 _rsc_critical; // Rotor speed below which flight is not possible - AP_Int16 _rsc_idle; // Rotor speed output while at idle + AP_Int16 _rsc_idle_output; // Rotor control output while at idle + AP_Int16 _rsc_power_low; // throttle value sent to throttle servo at zero collective pitch + AP_Int16 _rsc_power_high; // throttle value sent to throttle servo at maximum collective pitch // internal variables float _rollFactor[AP_MOTORS_HELI_NUM_SWASHPLATE_SERVOS]; @@ -239,6 +244,8 @@ protected: int16_t _pitch_radio_passthrough = 0; // pitch control PWM direct from radio, used for manual control int16_t _throttle_radio_passthrough = 0; // throttle control PWM direct from radio, used for manual control int16_t _yaw_radio_passthrough = 0; // yaw control PWM direct from radio, used for manual control + int16_t _collective_range = 0; // maximum absolute collective pitch range (500 - 1000) + int16_t _main_rotor_power = 0; // estimated main rotor power load, range 0-1.0f, used for RSC feedforward }; #endif // AP_MOTORSHELI diff --git a/libraries/AP_Motors/AP_MotorsHeli_RSC.cpp b/libraries/AP_Motors/AP_MotorsHeli_RSC.cpp index 552a6acb4e..f6668281b4 100644 --- a/libraries/AP_Motors/AP_MotorsHeli_RSC.cpp +++ b/libraries/AP_Motors/AP_MotorsHeli_RSC.cpp @@ -50,6 +50,14 @@ void AP_MotorsHeli_RSC::recalc_scalers() _runup_increment = 1.0f / (_runup_time * _loop_rate); } +// set_power_output_range +void AP_MotorsHeli_RSC::set_power_output_range(uint16_t power_low, uint16_t power_high) +{ + _power_output_low = power_low; + _power_output_high = power_high; + _power_output_range = _power_output_high - _power_output_low; +} + // output - update value to send to ESC/Servo void AP_MotorsHeli_RSC::output(uint8_t state) { @@ -59,7 +67,7 @@ void AP_MotorsHeli_RSC::output(uint8_t state) update_rotor_ramp(0.0f); // control output forced to zero - _control_speed = 0; + _control_output = 0; break; case ROTOR_CONTROL_IDLE: @@ -67,16 +75,19 @@ void AP_MotorsHeli_RSC::output(uint8_t state) update_rotor_ramp(0.0f); // set rotor control speed to idle speed parameter, this happens instantly and ignore ramping - _control_speed = _idle_speed; + _control_output = _idle_output; break; case ROTOR_CONTROL_ACTIVE: // set main rotor ramp to increase to full speed update_rotor_ramp(1.0f); - if ((_control_mode == ROTOR_CONTROL_MODE_PASSTHROUGH) || (_control_mode == ROTOR_CONTROL_MODE_SETPOINT)) { + if ((_control_mode == ROTOR_CONTROL_MODE_SPEED_PASSTHROUGH) || (_control_mode == ROTOR_CONTROL_MODE_SPEED_SETPOINT)) { // set control rotor speed to ramp slewed value between idle and desired speed - _control_speed = _idle_speed + (_rotor_ramp_output * (_desired_speed - _idle_speed)); + _control_output = _idle_output + (_rotor_ramp_output * (_desired_speed - _idle_output)); + } else if (_control_mode == ROTOR_CONTROL_MODE_OPEN_LOOP_POWER_OUTPUT) { + // throttle output depending on estimated power demand. Output is ramped up from idle speed during rotor runup. + _control_output = _idle_output + (_rotor_ramp_output * ((_power_output_low - _idle_output) + (_power_output_range * _load_feedforward))); } break; } @@ -85,7 +96,7 @@ void AP_MotorsHeli_RSC::output(uint8_t state) update_rotor_runup(); // output to rsc servo - write_rsc(_control_speed); + write_rsc(_control_output); } // update_rotor_ramp - slews rotor output scalar between 0 and 1, outputs float scalar to _rotor_ramp_output diff --git a/libraries/AP_Motors/AP_MotorsHeli_RSC.h b/libraries/AP_Motors/AP_MotorsHeli_RSC.h index fab3e6fc23..087e229f68 100644 --- a/libraries/AP_Motors/AP_MotorsHeli_RSC.h +++ b/libraries/AP_Motors/AP_MotorsHeli_RSC.h @@ -13,9 +13,11 @@ #define ROTOR_CONTROL_ACTIVE 2 // rotor control modes -#define ROTOR_CONTROL_MODE_DISABLED 0 -#define ROTOR_CONTROL_MODE_PASSTHROUGH 1 -#define ROTOR_CONTROL_MODE_SETPOINT 2 +#define ROTOR_CONTROL_MODE_DISABLED 0 +#define ROTOR_CONTROL_MODE_SPEED_PASSTHROUGH 1 +#define ROTOR_CONTROL_MODE_SPEED_SETPOINT 2 +#define ROTOR_CONTROL_MODE_OPEN_LOOP_POWER_OUTPUT 3 +#define ROTOR_CONTROL_MODE_CLOSED_LOOP_POWER_OUTPUT 4 class AP_MotorsHeli_RSC { public: @@ -39,8 +41,8 @@ public: // get_critical_speed int16_t get_critical_speed() const { return _critical_speed; } - // set_idle_speed - void set_idle_speed(int16_t idle_speed) { _idle_speed = idle_speed; } + // set_idle_output + void set_idle_output(int16_t idle_output) { _idle_output = idle_output; } // get_desired_speed int16_t get_desired_speed() const { return _desired_speed; } @@ -49,7 +51,7 @@ public: void set_desired_speed(int16_t desired_speed) { _desired_speed = desired_speed; } // get_control_speed - int16_t get_control_speed() const { return _control_speed; } + int16_t get_control_output() const { return _control_output; } // get_rotor_speed - return estimated or measured rotor speed int16_t get_rotor_speed() const; @@ -63,6 +65,12 @@ public: // set_runup_time void set_runup_time(int8_t runup_time) { _runup_time = runup_time; } + // set_power_output_range + void set_power_output_range(uint16_t power_low, uint16_t power_high); + + // set_motor_load + void set_motor_load(float load) { _load_feedforward = load; } + // recalc_scalers void recalc_scalers(); @@ -79,17 +87,21 @@ private: // internal variables int8_t _control_mode = 0; // motor control mode, Passthrough or Setpoint int16_t _critical_speed = 0; // rotor speed below which flight is not possible - int16_t _idle_speed = 0; // motor output idle speed + int16_t _idle_output = 0; // motor output idle speed int16_t _max_speed = 1000; // rotor maximum speed. Placeholder value until we have measured speed input (ToDo) int16_t _desired_speed = 0; // latest desired rotor speed from pilot - int16_t _control_speed = 0; // latest logic controlled rotor speed - float _rotor_ramp_output = 0; // scalar used to ramp rotor speed between _rsc_idle and full speed (0.0-1.0f) + int16_t _control_output = 0; // latest logic controlled output + float _rotor_ramp_output = 0; // scalar used to ramp rotor speed between _rsc_idle_output and full speed (0.0-1.0f) float _rotor_runup_output = 0; // scalar used to store status of rotor run-up time (0.0-1.0f) float _ramp_increment = 0; // the amount to increase/decrease the rotor ramp scalar during each iteration int8_t _ramp_time = 0; // time in seconds for the output to the main rotor's ESC to reach full speed int8_t _runup_time = 0; // time in seconds for the main rotor to reach full speed. Must be longer than _rsc_ramp_time float _runup_increment = 0; // the amount to increase/decrease the rotor run-up scalar during each iteration bool _runup_complete = false; // flag for determining if runup is complete + uint16_t _power_output_low = 0; // setpoint for power output at minimum rotor power + uint16_t _power_output_high = 0; // setpoint for power output at maximum rotor power + uint16_t _power_output_range = 0; // maximum range of output power + float _load_feedforward = 0; // estimate of motor load, range 0-1.0f // update_rotor_ramp - slews rotor output scalar between 0 and 1, outputs float scalar to _rotor_ramp_output void update_rotor_ramp(float rotor_ramp_input); diff --git a/libraries/AP_Motors/AP_MotorsHeli_Single.cpp b/libraries/AP_Motors/AP_MotorsHeli_Single.cpp index 5fdff974bd..16c31c9fcc 100644 --- a/libraries/AP_Motors/AP_MotorsHeli_Single.cpp +++ b/libraries/AP_Motors/AP_MotorsHeli_Single.cpp @@ -223,7 +223,8 @@ void AP_MotorsHeli_Single::recalc_scalers() _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.set_idle_speed(_rsc_idle); + _main_rotor.set_idle_output(_rsc_idle_output); + _main_rotor.set_power_output_range(_rsc_power_low, _rsc_power_high); _main_rotor.recalc_scalers(); if (_rsc_mode == AP_MOTORS_HELI_SINGLE_TAILTYPE_DIRECTDRIVE_VARPITCH) { @@ -231,13 +232,13 @@ void AP_MotorsHeli_Single::recalc_scalers() _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.set_idle_speed(_rsc_idle); + _tail_rotor.set_idle_output(_rsc_idle_output); } else { _tail_rotor.set_control_mode(AP_MOTORS_HELI_RSC_MODE_DISABLED); _tail_rotor.set_ramp_time(0); _tail_rotor.set_runup_time(0); _tail_rotor.set_critical_speed(0); - _tail_rotor.set_idle_speed(0); + _tail_rotor.set_idle_output(0); } _tail_rotor.recalc_scalers(); } @@ -510,13 +511,19 @@ void AP_MotorsHeli_Single::move_swash(int16_t roll_out, int16_t pitch_out, int16 // 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 - if ((_main_rotor.get_control_speed() > _rsc_idle) && _tail_type != AP_MOTORS_HELI_SINGLE_TAILTYPE_SERVO_EXTGYRO) { + if ((_main_rotor.get_control_output() > _rsc_idle_output) && _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); } } + // feed power estimate into main rotor controller + // ToDo: include tail rotor power? + // ToDo: add main rotor cyclic power? + _main_rotor_power = ((abs(_collective_out - _collective_mid_pwm)) / _collective_range); + _main_rotor.set_motor_load(_main_rotor_power); + // swashplate servos _swash_servo_1.servo_out = (_rollFactor[CH_1] * roll_out + _pitchFactor[CH_1] * pitch_out)/10 + _collectiveFactor[CH_1] * coll_out_scaled + (_swash_servo_1.radio_trim-1500); _swash_servo_2.servo_out = (_rollFactor[CH_2] * roll_out + _pitchFactor[CH_2] * pitch_out)/10 + _collectiveFactor[CH_2] * coll_out_scaled + (_swash_servo_2.radio_trim-1500);