aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_MotorsHeli: Create RSC Throttle Curve mode for controlling gas engines.

Browse Source
This commit is contained in:
Robert Lefebvre 2015-08-11 20:20:28 -04:00 committed by Randy Mackay
parent b1c7ec9aac
commit 88be4425ac
5 changed files with 81 additions and 25 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

29
libraries/AP_Motors/AP_MotorsHeli.cpp
View File

@ -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;

11
libraries/AP_Motors/AP_MotorsHeli.h
View File

@ -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

21
libraries/AP_Motors/AP_MotorsHeli_RSC.cpp
View File

@ -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

30
libraries/AP_Motors/AP_MotorsHeli_RSC.h
View File

@ -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);

15
libraries/AP_Motors/AP_MotorsHeli_Single.cpp
View File

@ -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);