mirror of https://github.com/ArduPilot/ardupilot
AP_MotorsHeli: rework RSC output() function to implement idle speed function
Also, split out speed_ramp function
This commit is contained in:
parent
b879b312e9
commit
d788f0307d
|
@ -43,63 +43,76 @@ void AP_MotorsHeli_RSC::recalc_scalers()
|
|||
_runup_increment = 1000.0f / (_runup_time * _loop_rate);
|
||||
}
|
||||
|
||||
// output - update value to send to ESC/Servo
|
||||
void AP_MotorsHeli_RSC::output(uint8_t state)
|
||||
{
|
||||
switch (state){
|
||||
case ROTOR_CONTROL_STOP:
|
||||
_control_speed = 0; // ramp input to zero
|
||||
_control_out = 0; // force ramp output to zero
|
||||
_estimated_speed = 0; // force speed estimate to zero
|
||||
break;
|
||||
|
||||
case ROTOR_CONTROL_STOP:
|
||||
|
||||
_estimated_speed = 0;
|
||||
_speed_out = 0;
|
||||
write_rsc(0);
|
||||
break;
|
||||
|
||||
case ROTOR_CONTROL_IDLE:
|
||||
case ROTOR_CONTROL_ACTIVE:
|
||||
|
||||
// ramp rotor esc output towards target
|
||||
if (_speed_out < _desired_speed) {
|
||||
// allow rotor out to jump to rotor's current speed
|
||||
if (_speed_out < _estimated_speed) {
|
||||
_speed_out = _estimated_speed;
|
||||
case ROTOR_CONTROL_IDLE:
|
||||
_control_speed = _idle_speed; // set control speed to idle speed
|
||||
if (_control_out < _idle_speed){
|
||||
_control_out = _idle_speed; // if control output is less than idle speed, force ramp function to jump to idle speed
|
||||
}
|
||||
break;
|
||||
|
||||
// ramp up slowly to target
|
||||
_speed_out += _ramp_increment;
|
||||
if (_speed_out > _desired_speed) {
|
||||
_speed_out = _desired_speed;
|
||||
}
|
||||
} else {
|
||||
// ramping down happens instantly
|
||||
_speed_out = _desired_speed;
|
||||
case ROTOR_CONTROL_ACTIVE:
|
||||
_control_speed = _desired_speed; // set control speed to desired speed
|
||||
break;
|
||||
}
|
||||
|
||||
// run speed ramp function to slew output smoothly, also updates estimated speed
|
||||
speed_ramp(_control_speed);
|
||||
|
||||
// output to rsc servo
|
||||
write_rsc(_control_out);
|
||||
}
|
||||
|
||||
// speed_ramp - ramps speed towards target, result put in _control_out
|
||||
void AP_MotorsHeli_RSC::speed_ramp(int16_t speed_target)
|
||||
{
|
||||
// range check speed_target
|
||||
speed_target = constrain_int16(speed_target,0,1000);
|
||||
|
||||
// ramp output upwards towards target
|
||||
if (_control_out < speed_target) {
|
||||
// allow control output to jump to estimated speed
|
||||
if (_control_out < _estimated_speed) {
|
||||
_control_out = _estimated_speed;
|
||||
}
|
||||
|
||||
// ramp rotor speed estimate towards speed out
|
||||
if (_estimated_speed < _speed_out) {
|
||||
_estimated_speed += _runup_increment;
|
||||
if (_estimated_speed > _speed_out) {
|
||||
_estimated_speed = _speed_out;
|
||||
}
|
||||
} else {
|
||||
_estimated_speed -= _runup_increment;
|
||||
if (_estimated_speed < _speed_out) {
|
||||
_estimated_speed = _speed_out;
|
||||
}
|
||||
// ramp up slowly to target
|
||||
_control_out += _ramp_increment;
|
||||
if (_control_out > speed_target) {
|
||||
_control_out = speed_target;
|
||||
}
|
||||
}else{
|
||||
// ramping down happens instantly
|
||||
_control_out = speed_target;
|
||||
}
|
||||
|
||||
// set runup complete flag
|
||||
if (!_runup_complete && _desired_speed > 0 && _estimated_speed >= _desired_speed) {
|
||||
_runup_complete = true;
|
||||
// ramp speed estimate towards control out
|
||||
if (_estimated_speed < _control_out) {
|
||||
_estimated_speed += _runup_increment;
|
||||
if (_estimated_speed > _control_out) {
|
||||
_estimated_speed = _control_out;
|
||||
}
|
||||
|
||||
if (_runup_complete && _estimated_speed <= _critical_speed) {
|
||||
_runup_complete = false;
|
||||
}else{
|
||||
_estimated_speed -= _runup_increment;
|
||||
if (_estimated_speed < _control_out) {
|
||||
_estimated_speed = _control_out;
|
||||
}
|
||||
}
|
||||
|
||||
// output to rsc servo
|
||||
write_rsc(_speed_out);
|
||||
|
||||
break;
|
||||
// set runup complete flag
|
||||
if (!_runup_complete && speed_target > 0 && _estimated_speed >= speed_target) {
|
||||
_runup_complete = true;
|
||||
}
|
||||
if (_runup_complete && _estimated_speed <= _critical_speed) {
|
||||
_runup_complete = false;
|
||||
}
|
||||
}
|
||||
|
||||
|
|
|
@ -28,12 +28,18 @@ 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; }
|
||||
|
||||
// get_desired_speed
|
||||
int16_t get_desired_speed() const { return _desired_speed; }
|
||||
|
||||
// set_desired_speed
|
||||
void set_desired_speed(int16_t desired_speed) { _desired_speed = desired_speed; }
|
||||
|
||||
// get_control_speed
|
||||
int16_t get_control_speed() const { return _control_speed; }
|
||||
|
||||
// get_estimated_speed
|
||||
int16_t get_estimated_speed() const { return _estimated_speed; }
|
||||
|
||||
|
@ -49,7 +55,7 @@ public:
|
|||
// recalc_scalers
|
||||
void recalc_scalers();
|
||||
|
||||
// output_armed
|
||||
// output - update value to send to ESC/Servo
|
||||
void output(uint8_t state);
|
||||
|
||||
private:
|
||||
|
@ -61,8 +67,10 @@ private:
|
|||
|
||||
// internal variables
|
||||
int16_t _critical_speed = 0; // rotor speed below which flight is not possible
|
||||
int16_t _idle_speed = 0; // motor output idle speed
|
||||
int16_t _desired_speed = 0; // latest desired rotor speed from pilot
|
||||
float _speed_out = 0; // latest output sent to the main rotor or an estimate of the rotors actual speed (whichever is higher) (0 ~ 1000)
|
||||
int16_t _control_speed = 0; // latest logic controlled rotor speed
|
||||
float _control_out = 0; // latest output sent to the main rotor or an estimate of the rotors actual speed (whichever is higher) (0 ~ 1000)
|
||||
float _estimated_speed = 0; // estimated speed of the main rotor (0~1000)
|
||||
float _ramp_increment = 0; // the amount we can increase the rotor output during each 100hz iteration
|
||||
int8_t _ramp_time = 0; // time in seconds for the output to the main rotor's ESC to reach full speed
|
||||
|
@ -70,6 +78,9 @@ private:
|
|||
float _runup_increment = 0; // the amount we can increase the rotor's estimated speed during each 100hz iteration
|
||||
bool _runup_complete = false; // flag for determining if runup is complete
|
||||
|
||||
// speed_ramp - ramps speed towards target, result put in _control_out
|
||||
void speed_ramp(int16_t rotor_target);
|
||||
|
||||
// write_rsc - outputs pwm onto output rsc channel. servo_out parameter is of the range 0 ~ 1000
|
||||
void write_rsc(int16_t servo_out);
|
||||
};
|
||||
|
|
|
@ -224,10 +224,12 @@ void AP_MotorsHeli_Single::recalc_scalers()
|
|||
_main_rotor.set_ramp_time(0);
|
||||
_main_rotor.set_runup_time(0);
|
||||
_main_rotor.set_critical_speed(0);
|
||||
_main_rotor.set_idle_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.set_idle_speed(_rsc_idle);
|
||||
}
|
||||
|
||||
_main_rotor.recalc_scalers();
|
||||
|
@ -236,10 +238,12 @@ void AP_MotorsHeli_Single::recalc_scalers()
|
|||
_tail_rotor.set_ramp_time(0);
|
||||
_tail_rotor.set_runup_time(0);
|
||||
_tail_rotor.set_critical_speed(0);
|
||||
_tail_rotor.set_idle_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.set_idle_speed(_rsc_idle);
|
||||
}
|
||||
|
||||
_tail_rotor.recalc_scalers();
|
||||
|
@ -494,7 +498,7 @@ 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 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) {
|
||||
if ((_main_rotor.get_control_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);
|
||||
|
|
Loading…
Reference in New Issue