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_Motors: allow setting of loop rate

Browse Source 
this allows for SCHED_LOOP_RATE in copter
This commit is contained in:
Andrew Tridgell 2016-06-05 11:20:58 +10:00
parent fb4cdafef8
commit 255bda9f9c
4 changed files with 29 additions and 19 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

27
libraries/AP_Motors/AP_MotorsHeli_RSC.cpp
View File

@ -39,10 +39,21 @@ void AP_MotorsHeli_RSC::set_power_output_range(float power_low, float power_high
// output - update value to send to ESC/Servo
void AP_MotorsHeli_RSC::output(RotorControlState state)
{
float dt;
uint64_t now = AP_HAL::micros64();
if (_last_update_us == 0) {
_last_update_us = now;
dt = 0.001f;
} else {
dt = 1.0e-6f * (now - _last_update_us);
_last_update_us = now;
}
switch (state){
case ROTOR_CONTROL_STOP:
// set rotor ramp to decrease speed to zero, this happens instantly inside update_rotor_ramp()
update_rotor_ramp(0.0f);
update_rotor_ramp(0.0f, dt);
// control output forced to zero
_control_output = 0.0f;
@ -50,7 +61,7 @@ void AP_MotorsHeli_RSC::output(RotorControlState state)
case ROTOR_CONTROL_IDLE:
// set rotor ramp to decrease speed to zero
update_rotor_ramp(0.0f);
update_rotor_ramp(0.0f, dt);
// set rotor control speed to idle speed parameter, this happens instantly and ignore ramping
_control_output = _idle_output;
@ -58,7 +69,7 @@ void AP_MotorsHeli_RSC::output(RotorControlState state)
case ROTOR_CONTROL_ACTIVE:
// set main rotor ramp to increase to full speed
update_rotor_ramp(1.0f);
update_rotor_ramp(1.0f, dt);
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
@ -71,14 +82,14 @@ void AP_MotorsHeli_RSC::output(RotorControlState state)
}
// update rotor speed run-up estimate
update_rotor_runup();
update_rotor_runup(dt);
// output to rsc servo
write_rsc(_control_output);
}
// 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)
void AP_MotorsHeli_RSC::update_rotor_ramp(float rotor_ramp_input, float dt)
{
// sanity check ramp time
if (_ramp_time <= 0) {
@ -92,7 +103,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 += (1.0f / (_ramp_time * _loop_rate));
_rotor_ramp_output += (dt / _ramp_time);
if (_rotor_ramp_output > rotor_ramp_input) {
_rotor_ramp_output = rotor_ramp_input;
}
@ -103,7 +114,7 @@ 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()
void AP_MotorsHeli_RSC::update_rotor_runup(float dt)
{
// sanity check runup time
if (_runup_time < _ramp_time) {
@ -114,7 +125,7 @@ void AP_MotorsHeli_RSC::update_rotor_runup()
}
// ramp speed estimate towards control out
float runup_increment = 1.0f / (_runup_time * _loop_rate);
float runup_increment = dt / _runup_time;
if (_rotor_runup_output < _rotor_ramp_output) {
_rotor_runup_output += runup_increment;
if (_rotor_runup_output > _rotor_ramp_output) {

14
libraries/AP_Motors/AP_MotorsHeli_RSC.h
View File

@ -26,9 +26,7 @@ public:
friend class AP_MotorsHeli_Single;
AP_MotorsHeli_RSC(RC_Channel_aux::Aux_servo_function_t aux_fn,
uint8_t default_channel,
uint16_t loop_rate) :
_loop_rate(loop_rate),
uint8_t default_channel) :
_aux_fn(aux_fn),
_default_channel(default_channel)
{};
@ -80,10 +78,8 @@ public:
void output(RotorControlState state);
private:
// external variables
float _loop_rate; // main loop rate
uint64_t _last_update_us;
// channel setup for aux function
RC_Channel_aux::Aux_servo_function_t _aux_fn;
uint8_t _default_channel;
@ -109,10 +105,10 @@ private:
AP_Int8 _pwm_rev;
// 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);
void update_rotor_ramp(float rotor_ramp_input, float dt);
// update_rotor_runup - function to slew rotor runup scalar, outputs float scalar to _rotor_runup_ouptut
void update_rotor_runup();
void update_rotor_runup(float dt);
// write_rsc - outputs pwm onto output rsc channel. servo_out parameter is of the range 0 ~ 1
void write_rsc(float servo_out);

4
libraries/AP_Motors/AP_MotorsHeli_Single.h
View File

@ -49,8 +49,8 @@ public:
uint16_t speed_hz = AP_MOTORS_HELI_SPEED_DEFAULT) :
AP_MotorsHeli(loop_rate, speed_hz),
_servo_aux(servo_aux),
_main_rotor(RC_Channel_aux::k_heli_rsc, AP_MOTORS_HELI_SINGLE_RSC, loop_rate),
_tail_rotor(RC_Channel_aux::k_heli_tail_rsc, AP_MOTORS_HELI_SINGLE_AUX, loop_rate),
_main_rotor(RC_Channel_aux::k_heli_rsc, AP_MOTORS_HELI_SINGLE_RSC),
_tail_rotor(RC_Channel_aux::k_heli_tail_rsc, AP_MOTORS_HELI_SINGLE_AUX),
_swash_servo_1(CH_NONE), _swash_servo_2(CH_NONE), _swash_servo_3(CH_NONE), _yaw_servo(CH_NONE)
{
AP_Param::setup_object_defaults(this, var_info);

3
libraries/AP_Motors/AP_Motors_Class.h
View File

@ -128,6 +128,9 @@ public:
// pilot input in the -1 ~ +1 range for roll, pitch and yaw. 0~1 range for throttle
void set_radio_passthrough(float roll_input, float pitch_input, float throttle_input, float yaw_input);
// set loop rate. Used to support loop rate as a parameter
void set_loop_rate(uint16_t loop_rate) { _loop_rate = loop_rate; }
protected:
// output functions that should be overloaded by child classes
virtual void output_armed_stabilizing()=0;