Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-02 14:08:45 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_MotorsHeli: RSC controller to use speed ramp as simple float scalar

Browse Source
This commit is contained in:
Robert Lefebvre 2015-08-11 14:31:20 -04:00 committed by Randy Mackay
parent 904fa7f8b9
commit 19536c1c11
6 changed files with 109 additions and 76 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
libraries/AP_Motors/AP_MotorsHeli.cpp
View File

@ -200,7 +200,7 @@ bool AP_MotorsHeli::parameter_check() const
}
// returns false if RSC Mode is not set to a valid control mode
if (_rsc_mode <= AP_MOTORS_HELI_RSC_MODE_NONE || _rsc_mode > AP_MOTORS_HELI_RSC_MODE_SETPOINT) {
if (_rsc_mode <= AP_MOTORS_HELI_RSC_MODE_DISABLED || _rsc_mode > AP_MOTORS_HELI_RSC_MODE_SETPOINT) {
return false;
}

8
libraries/AP_Motors/AP_MotorsHeli.h
View File

@ -37,7 +37,7 @@
#define AP_MOTORS_HELI_LAND_COLLECTIVE_MIN 0
// main rotor speed control types (ch8 out)
#define AP_MOTORS_HELI_RSC_MODE_NONE 0 // not a valid RSC Mode
#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
@ -120,7 +120,7 @@ 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)
// get_rsc_mode - gets the rotor speed control method (AP_MOTORS_HELI_RSC_MODE_CH8_PASSTHROUGH or AP_MOTORS_HELI_RSC_MODE_SETPOINT)
uint8_t get_rsc_mode() const { return _rsc_mode; }
// get_rsc_setpoint - gets contents of _rsc_setpoint parameter (0~1000)
@ -132,8 +132,8 @@ public:
// get_desired_rotor_speed - gets target rotor speed as a number from 0 ~ 1000
virtual int16_t get_desired_rotor_speed() const = 0;
// get_estimated_rotor_speed - gets estimated rotor speed as a number from 0 ~ 1000
virtual int16_t get_estimated_rotor_speed() const = 0;
// get_main_rotor_speed - gets estimated or measured main rotor speed
virtual int16_t get_main_rotor_speed() const = 0;
// return true if the main rotor is up to speed
bool rotor_runup_complete() const { return _heliflags.rotor_runup_complete; }

112
libraries/AP_Motors/AP_MotorsHeli_RSC.cpp
View File

@ -36,7 +36,7 @@ void AP_MotorsHeli_RSC::recalc_scalers()
_ramp_time = 1;
}
_ramp_increment = 1000.0f / (_ramp_time * _loop_rate);
_ramp_increment = 1.0f / (_ramp_time * _loop_rate);
// recalculate rotor runup increment
if (_runup_time <= 0 ) {
@ -47,7 +47,7 @@ void AP_MotorsHeli_RSC::recalc_scalers()
_runup_time = _ramp_time;
}
_runup_increment = 1000.0f / (_runup_time * _loop_rate);
_runup_increment = 1.0f / (_runup_time * _loop_rate);
}
// output - update value to send to ESC/Servo
@ -55,89 +55,113 @@ 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
// set rotor ramp to decrease speed to zero, this happens instantly inside update_rotor_ramp()
update_rotor_ramp(0.0f);
// control output forced to zero
_control_speed = 0;
break;
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
}
// set rotor ramp to decrease speed to zero
update_rotor_ramp(0.0f);
// set rotor control speed to idle speed parameter, this happens instantly and ignore ramping
_control_speed = _idle_speed;
break;
case ROTOR_CONTROL_ACTIVE:
_control_speed = _desired_speed; // set control speed to desired speed
// 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)) {
// set control rotor speed to ramp slewed value between idle and desired speed
_control_speed = _idle_speed + (_rotor_ramp_output * (_desired_speed - _idle_speed));
}
break;
}
// run speed ramp function to slew output smoothly
speed_ramp(_control_speed);
// update rotor speed estimate
update_speed_estimate();
// update rotor speed run-up estimate
update_rotor_runup();
// output to rsc servo
write_rsc(_control_out);
write_rsc(_control_speed);
}
// speed_ramp - ramps speed towards target, result put in _control_out
void AP_MotorsHeli_RSC::speed_ramp(int16_t speed_target)
// update_rotor_ramp - slews rotor output scalar between 0 and 1, outputs float scalar to _rotor_ramp_output
void AP_MotorsHeli_RSC::update_rotor_ramp(float rotor_ramp_input)
{
// range check speed_target
speed_target = constrain_int16(speed_target,0,1000);
// ramp output upwards towards target
if (_control_out < speed_target) {
if (_rotor_ramp_output < rotor_ramp_input) {
// allow control output to jump to estimated speed
if (_control_out < _estimated_speed) {
_control_out = _estimated_speed;
if (_rotor_ramp_output < _rotor_runup_output) {
_rotor_ramp_output = _rotor_runup_output;
}
// ramp up slowly to target
_control_out += _ramp_increment;
if (_control_out > speed_target) {
_control_out = speed_target;
_rotor_ramp_output += _ramp_increment;
if (_rotor_ramp_output > rotor_ramp_input) {
_rotor_ramp_output = rotor_ramp_input;
}
}else{
// ramping down happens instantly
_control_out = speed_target;
_rotor_ramp_output = rotor_ramp_input;
}
}
// update_speed_estimate - function to estimate speed
void AP_MotorsHeli_RSC::update_speed_estimate()
// update_rotor_runup - function to slew rotor runup scalar, outputs float scalar to _rotor_runup_ouptut
void AP_MotorsHeli_RSC::update_rotor_runup()
{
// 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 (_rotor_runup_output < _rotor_ramp_output) {
_rotor_runup_output += _runup_increment;
if (_rotor_runup_output > _rotor_ramp_output) {
_rotor_runup_output = _rotor_ramp_output;
}
}else{
_estimated_speed -= _runup_increment;
if (_estimated_speed < _control_out) {
_estimated_speed = _control_out;
_rotor_runup_output -= _runup_increment;
if (_rotor_runup_output < _rotor_ramp_output) {
_rotor_runup_output = _rotor_ramp_output;
}
}
// update run-up complete flag
if (!_runup_complete && _control_out > _idle_speed && _estimated_speed >= _control_out) {
// if control mode is disabled, then run-up complete always returns true
if ( _control_mode == ROTOR_CONTROL_MODE_DISABLED ){
_runup_complete = true;
return;
}
// if rotor ramp and runup are both at full speed, then run-up has been completed
if (!_runup_complete && (_rotor_ramp_output >= 1.0f) && (_rotor_runup_output >= 1.0f)) {
_runup_complete = true;
}
if (_runup_complete && _estimated_speed <= _critical_speed) {
// if rotor speed is less than critical speed, then run-up is not complete
// this will prevent the case where the target rotor speed is less than critical speed
if (_runup_complete && (get_rotor_speed() <= _critical_speed)) {
_runup_complete = false;
}
}
// get_rotor_speed - gets rotor speed either as an estimate, or (ToDO) a measured value
int16_t AP_MotorsHeli_RSC::get_rotor_speed() const
{
// if no actual measured rotor speed is available, estimate speed based on rotor runup scalar.
return (_rotor_runup_output * _max_speed);
}
// write_rsc - outputs pwm onto output rsc channel
// servo_out parameter is of the range 0 ~ 1000
void AP_MotorsHeli_RSC::write_rsc(int16_t servo_out)
{
_servo_output.servo_out = servo_out;
_servo_output.calc_pwm();
if (_control_mode == ROTOR_CONTROL_MODE_DISABLED){
// do not do servo output to avoid conflicting with other output on the channel
// ToDo: We should probably use RC_Channel_Aux to avoid this problem
return;
} else {
_servo_output.servo_out = servo_out;
_servo_output.calc_pwm();
hal.rcout->write(_servo_output_channel, _servo_output.radio_out);
hal.rcout->write(_servo_output_channel, _servo_output.radio_out);
}
}

30
libraries/AP_Motors/AP_MotorsHeli_RSC.h
View File

@ -12,6 +12,11 @@
#define ROTOR_CONTROL_IDLE 1
#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
class AP_MotorsHeli_RSC {
public:
AP_MotorsHeli_RSC(RC_Channel& servo_output,
@ -25,6 +30,9 @@ public:
// init_servo - servo initialization on start-up
void init_servo();
// set_control_mode - sets control mode
void set_control_mode(int8_t mode) { _control_mode = mode; }
// set_critical_speed
void set_critical_speed(int16_t critical_speed) { _critical_speed = critical_speed; }
@ -43,8 +51,8 @@ public:
// get_control_speed
int16_t get_control_speed() const { return _control_speed; }
// get_estimated_speed
int16_t get_estimated_speed() const { return _estimated_speed; }
// get_rotor_speed - return estimated or measured rotor speed
int16_t get_rotor_speed() const;
// is_runup_complete
bool is_runup_complete() const { return _runup_complete; }
@ -69,23 +77,25 @@ private:
float _loop_rate; // main loop rate
// 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 _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 _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
float _rotor_ramp_output = 0; // scalar used to ramp rotor speed between _rsc_idle 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 we can increase the rotor's estimated speed during each 100hz iteration
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
// speed_ramp - ramps speed towards target, result put in _control_out
void speed_ramp(int16_t rotor_target);
// 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);
// update_speed_estimate - function to estimate speed
void update_speed_estimate();
// update_rotor_runup - function to slew rotor runup scalar, outputs float scalar to _rotor_runup_ouptut
void update_rotor_runup();
// write_rsc - outputs pwm onto output rsc channel. servo_out parameter is of the range 0 ~ 1000
void write_rsc(int16_t servo_out);

25
libraries/AP_Motors/AP_MotorsHeli_Single.cpp
View File

@ -195,7 +195,7 @@ void AP_MotorsHeli_Single::output_test(uint8_t motor_seq, int16_t pwm)
bool AP_MotorsHeli_Single::allow_arming() const
{
// returns false if main rotor speed is not zero
if (_main_rotor.get_estimated_speed() > 0) {
if (_main_rotor.get_rotor_speed() > 0) {
return false;
}
@ -209,40 +209,39 @@ void AP_MotorsHeli_Single::set_desired_rotor_speed(int16_t desired_speed)
{
_main_rotor.set_desired_speed(desired_speed);
if (desired_speed > 0 && _tail_type == AP_MOTORS_HELI_SINGLE_TAILTYPE_DIRECTDRIVE_VARPITCH) {
if (_tail_type == AP_MOTORS_HELI_SINGLE_TAILTYPE_DIRECTDRIVE_VARPITCH) {
_tail_rotor.set_desired_speed(_direct_drive_tailspeed);
} else {
_tail_rotor.set_desired_speed(0);
}
}
// recalc_scalers - recalculates various scalers used. Should be called at about 1hz to allow users to see effect of changing parameters
// recalc_scalers - recalculates various scalers used.
void AP_MotorsHeli_Single::recalc_scalers()
{
_main_rotor.set_control_mode(_rsc_mode);
_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();
if (_rsc_mode != AP_MOTORS_HELI_SINGLE_TAILTYPE_DIRECTDRIVE_VARPITCH) {
_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 {
if (_rsc_mode == AP_MOTORS_HELI_SINGLE_TAILTYPE_DIRECTDRIVE_VARPITCH) {
_tail_rotor.set_control_mode(AP_MOTORS_HELI_RSC_MODE_SETPOINT);
_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);
} 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.recalc_scalers();
}
// get_motor_mask - returns a bitmask of which outputs are being used for motors or servos (1 means being used)
// this can be used to ensure other pwm outputs (i.e. for servos) do not conflict
uint16_t AP_MotorsHeli_Single::get_motor_mask()

8
libraries/AP_Motors/AP_MotorsHeli_Single.h
View File

@ -87,16 +87,16 @@ public:
// set_desired_rotor_speed - sets target rotor speed as a number from 0 ~ 1000
void set_desired_rotor_speed(int16_t desired_speed);
// get_estimated_rotor_speed - gets estimated rotor speed as a number from 0 ~ 1000
int16_t get_estimated_rotor_speed() const { return _main_rotor.get_estimated_speed(); }
// get_main_rotor_speed - gets estimated or measured main rotor speed
int16_t get_main_rotor_speed() const { return _main_rotor.get_rotor_speed(); }
// get_desired_rotor_speed - gets target rotor speed as a number from 0 ~ 1000
int16_t get_desired_rotor_speed() const { return _main_rotor.get_desired_speed(); }
// rotor_speed_above_critical - return true if rotor speed is above that critical for flight
bool rotor_speed_above_critical() const { return _main_rotor.get_estimated_speed() > _main_rotor.get_critical_speed(); }
bool rotor_speed_above_critical() const { return _main_rotor.get_rotor_speed() > _main_rotor.get_critical_speed(); }
// recalc_scalers - recalculates various scalers used. Should be called at about 1hz to allow users to see effect of changing parameters
// recalc_scalers - recalculates various scalers used.
void recalc_scalers();
// get_motor_mask - returns a bitmask of which outputs are being used for motors or servos (1 means being used)