Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_MotorsHeli: rework RSC output() function to implement idle speed function 

Also, split out speed_ramp function
This commit is contained in:
Robert Lefebvre 2015-08-07 21:14:45 -04:00 committed by Randy Mackay
parent c3e7eed299
commit 77e4bf39aa
3 changed files with 76 additions and 48 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

103
libraries/AP_Motors/AP_MotorsHeli_RSC.cpp
View File

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

15
libraries/AP_Motors/AP_MotorsHeli_RSC.h
View File

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

6
libraries/AP_Motors/AP_MotorsHeli_Single.cpp
View File

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