Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_MotorsHeli_RSC: rotor speeds in 0 to 1 range

Browse Source 
Also move recalc_scalers functionality into update_rotor_ramp and update_rotor_runup
This commit is contained in:
Randy Mackay 2016-02-03 17:53:34 +09:00
parent 4775843e3c
commit 832a226f13
2 changed files with 42 additions and 56 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

54
libraries/AP_Motors/AP_MotorsHeli_RSC.cpp
View File

@ -28,28 +28,6 @@ void AP_MotorsHeli_RSC::init_servo()
RC_Channel_aux::set_aux_channel_default(_aux_fn, _default_channel);
}
// recalc_scalers - recalculates various scalers used. Should be called at about 1hz to allow users to see effect of changing parameters
void AP_MotorsHeli_RSC::recalc_scalers()
{
// recalculate rotor ramp up increment
if (_ramp_time <= 0) {
_ramp_time = 1;
}
_ramp_increment = 1.0f / (_ramp_time * _loop_rate);
// recalculate rotor runup increment
if (_runup_time <= 0 ) {
_runup_time = 1;
}
if (_runup_time < _ramp_time) {
_runup_time = _ramp_time;
}
_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)
{
@ -67,7 +45,7 @@ void AP_MotorsHeli_RSC::output(RotorControlState state)
update_rotor_ramp(0.0f);
// control output forced to zero
_control_output = 0;
_control_output = 0.0f;
break;
case ROTOR_CONTROL_IDLE:
@ -102,6 +80,11 @@ void AP_MotorsHeli_RSC::output(RotorControlState state)
// 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)
{
// sanity check ramp time
if (_ramp_time <= 0) {
_ramp_time = 1;
}
// ramp output upwards towards target
if (_rotor_ramp_output < rotor_ramp_input) {
// allow control output to jump to estimated speed
@ -109,7 +92,7 @@ void AP_MotorsHeli_RSC::update_rotor_ramp(float rotor_ramp_input)
_rotor_ramp_output = _rotor_runup_output;
}
// ramp up slowly to target
_rotor_ramp_output += _ramp_increment;
_rotor_ramp_output += (1.0f / (_ramp_time * _loop_rate));
if (_rotor_ramp_output > rotor_ramp_input) {
_rotor_ramp_output = rotor_ramp_input;
}
@ -122,14 +105,23 @@ void AP_MotorsHeli_RSC::update_rotor_ramp(float rotor_ramp_input)
// update_rotor_runup - function to slew rotor runup scalar, outputs float scalar to _rotor_runup_ouptut
void AP_MotorsHeli_RSC::update_rotor_runup()
{
// sanity check runup time
if (_runup_time < _ramp_time) {
_runup_time = _ramp_time;
}
if (_runup_time <= 0 ) {
_runup_time = 1;
}
// ramp speed estimate towards control out
float runup_increment = 1.0f / (_runup_time * _loop_rate);
if (_rotor_runup_output < _rotor_ramp_output) {
_rotor_runup_output += _runup_increment;
_rotor_runup_output += runup_increment;
if (_rotor_runup_output > _rotor_ramp_output) {
_rotor_runup_output = _rotor_ramp_output;
}
}else{
_rotor_runup_output -= _runup_increment;
_rotor_runup_output -= runup_increment;
if (_rotor_runup_output < _rotor_ramp_output) {
_rotor_runup_output = _rotor_ramp_output;
}
@ -155,21 +147,21 @@ void AP_MotorsHeli_RSC::update_rotor_runup()
}
// get_rotor_speed - gets rotor speed either as an estimate, or (ToDO) a measured value
int16_t AP_MotorsHeli_RSC::get_rotor_speed() const
float 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);
return _rotor_runup_output;
}
// 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_out parameter is of the range 0 ~ 1
void AP_MotorsHeli_RSC::write_rsc(float servo_out)
{
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 {
RC_Channel_aux::set_servo_out(RC_Channel_aux::k_heli_rsc, servo_out);
RC_Channel_aux::set_servo_out(RC_Channel_aux::k_heli_rsc, servo_out * 1000.0f);
}
}

44
libraries/AP_Motors/AP_MotorsHeli_RSC.h
View File

@ -38,26 +38,26 @@ public:
void set_control_mode(RotorControlMode mode) { _control_mode = mode; }
// set_critical_speed
void set_critical_speed(int16_t critical_speed) { _critical_speed = critical_speed; }
void set_critical_speed(float critical_speed) { _critical_speed = critical_speed; }
// get_critical_speed
int16_t get_critical_speed() const { return _critical_speed; }
float get_critical_speed() const { return _critical_speed; }
// set_idle_output
void set_idle_output(int16_t idle_output) { _idle_output = idle_output; }
float get_idle_output() { return _idle_output; }
void set_idle_output(float idle_output) { _idle_output = idle_output; }
// get_desired_speed
int16_t get_desired_speed() const { return _desired_speed; }
float get_desired_speed() const { return _desired_speed; }
// set_desired_speed
void set_desired_speed(int16_t desired_speed) { _desired_speed = desired_speed; }
void set_desired_speed(float desired_speed) { _desired_speed = desired_speed; }
// get_control_speed
int16_t get_control_output() const { return _control_output; }
float get_control_output() const { return _control_output; }
// get_rotor_speed - return estimated or measured rotor speed
int16_t get_rotor_speed() const;
float get_rotor_speed() const;
// is_runup_complete
bool is_runup_complete() const { return _runup_complete; }
@ -74,9 +74,6 @@ public:
// set_motor_load
void set_motor_load(float load) { _load_feedforward = load; }
// recalc_scalers
void recalc_scalers();
// output - update value to send to ESC/Servo
void output(RotorControlState state);
@ -91,22 +88,19 @@ private:
// internal variables
RotorControlMode _control_mode = ROTOR_CONTROL_MODE_DISABLED; // motor control mode, Passthrough or Setpoint
int16_t _critical_speed = 0; // rotor speed below which flight is not possible
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_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
float _critical_speed = 0.0f; // rotor speed below which flight is not possible
float _idle_output = 0.0f; // motor output idle speed
float _desired_speed = 0.0f; // latest desired rotor speed from pilot
float _control_output = 0.0f; // latest logic controlled output
float _rotor_ramp_output = 0.0f; // scalar used to ramp rotor speed between _rsc_idle_output and full speed (0.0-1.0f)
float _rotor_runup_output = 0.0f; // scalar used to store status of rotor run-up time (0.0-1.0f)
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
float _power_output_low = 0.0f; // setpoint for power output at minimum rotor power
float _power_output_high = 0.0f; // setpoint for power output at maximum rotor power
float _power_output_range = 0.0f; // maximum range of output power
float _load_feedforward = 0.0f; // 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);
@ -114,6 +108,6 @@ private:
// 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);
// write_rsc - outputs pwm onto output rsc channel. servo_out parameter is of the range 0 ~ 1
void write_rsc(float servo_out);
};