From 255bda9f9c06b1f633e50792719985e43f5fa923 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sun, 5 Jun 2016 11:20:58 +1000 Subject: [PATCH] AP_Motors: allow setting of loop rate this allows for SCHED_LOOP_RATE in copter --- libraries/AP_Motors/AP_MotorsHeli_RSC.cpp | 27 +++++++++++++++------- libraries/AP_Motors/AP_MotorsHeli_RSC.h | 14 ++++------- libraries/AP_Motors/AP_MotorsHeli_Single.h | 4 ++-- libraries/AP_Motors/AP_Motors_Class.h | 3 +++ 4 files changed, 29 insertions(+), 19 deletions(-) diff --git a/libraries/AP_Motors/AP_MotorsHeli_RSC.cpp b/libraries/AP_Motors/AP_MotorsHeli_RSC.cpp index 391a7414ac..33aabc7fd2 100644 --- a/libraries/AP_Motors/AP_MotorsHeli_RSC.cpp +++ b/libraries/AP_Motors/AP_MotorsHeli_RSC.cpp @@ -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) { diff --git a/libraries/AP_Motors/AP_MotorsHeli_RSC.h b/libraries/AP_Motors/AP_MotorsHeli_RSC.h index dcb59eb2d8..8bb7c18920 100644 --- a/libraries/AP_Motors/AP_MotorsHeli_RSC.h +++ b/libraries/AP_Motors/AP_MotorsHeli_RSC.h @@ -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); diff --git a/libraries/AP_Motors/AP_MotorsHeli_Single.h b/libraries/AP_Motors/AP_MotorsHeli_Single.h index 88757a7180..7a38a47048 100644 --- a/libraries/AP_Motors/AP_MotorsHeli_Single.h +++ b/libraries/AP_Motors/AP_MotorsHeli_Single.h @@ -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); diff --git a/libraries/AP_Motors/AP_Motors_Class.h b/libraries/AP_Motors/AP_Motors_Class.h index eec8e981a8..6c9ce76111 100644 --- a/libraries/AP_Motors/AP_Motors_Class.h +++ b/libraries/AP_Motors/AP_Motors_Class.h @@ -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;