From 67205f81145c881d83d458255af04731ba0ff00e Mon Sep 17 00:00:00 2001 From: Leonard Hall Date: Wed, 7 Dec 2022 11:33:36 +1030 Subject: [PATCH] AP_Motors: Support changing update period --- libraries/AP_Motors/AP_Motors6DOF.cpp | 12 +++++------ libraries/AP_Motors/AP_Motors6DOF.h | 4 ++-- libraries/AP_Motors/AP_MotorsCoax.h | 4 ++-- libraries/AP_Motors/AP_MotorsHeli.cpp | 2 +- libraries/AP_Motors/AP_MotorsHeli.h | 5 ++--- libraries/AP_Motors/AP_MotorsHeli_Dual.cpp | 20 +++++++++---------- libraries/AP_Motors/AP_MotorsHeli_Dual.h | 5 ++--- libraries/AP_Motors/AP_MotorsHeli_Quad.h | 5 ++--- libraries/AP_Motors/AP_MotorsHeli_Single.cpp | 20 +++++++++---------- libraries/AP_Motors/AP_MotorsHeli_Single.h | 5 ++--- libraries/AP_Motors/AP_MotorsMatrix.cpp | 2 +- libraries/AP_Motors/AP_MotorsMatrix.h | 4 ++-- .../AP_MotorsMatrix_6DoF_Scripting.h | 4 ++-- .../AP_MotorsMatrix_Scripting_Dynamic.h | 4 ++-- libraries/AP_Motors/AP_MotorsMulticopter.cpp | 19 +++++++++--------- libraries/AP_Motors/AP_MotorsMulticopter.h | 2 +- libraries/AP_Motors/AP_MotorsSingle.h | 4 ++-- libraries/AP_Motors/AP_MotorsTailsitter.cpp | 4 ++-- libraries/AP_Motors/AP_MotorsTailsitter.h | 2 +- libraries/AP_Motors/AP_MotorsTri.h | 4 ++-- libraries/AP_Motors/AP_Motors_Class.cpp | 3 +-- libraries/AP_Motors/AP_Motors_Class.h | 13 +++++++----- .../expo_inverse_test/expo_inverse_test.cpp | 7 ++++--- 23 files changed, 75 insertions(+), 79 deletions(-) diff --git a/libraries/AP_Motors/AP_Motors6DOF.cpp b/libraries/AP_Motors/AP_Motors6DOF.cpp index 003215eb9f..f64e0a5aae 100644 --- a/libraries/AP_Motors/AP_Motors6DOF.cpp +++ b/libraries/AP_Motors/AP_Motors6DOF.cpp @@ -367,15 +367,13 @@ void AP_Motors6DOF::output_armed_stabilizing() float _batt_current_delta = _batt_current - _batt_current_last; - float loop_interval = 1.0f/_loop_rate; + float _current_change_rate = _batt_current_delta / _dt; - float _current_change_rate = _batt_current_delta / loop_interval; + float predicted_current = _batt_current + (_current_change_rate * _dt * 5); - float predicted_current = _batt_current + (_current_change_rate * loop_interval * 5); + float batt_current_ratio = _batt_current / _batt_current_max; - float batt_current_ratio = _batt_current/_batt_current_max; - - float predicted_current_ratio = predicted_current/_batt_current_max; + float predicted_current_ratio = predicted_current / _batt_current_max; _batt_current_last = _batt_current; if (predicted_current > _batt_current_max * 1.5f) { @@ -383,7 +381,7 @@ void AP_Motors6DOF::output_armed_stabilizing() } else if (_batt_current < _batt_current_max && predicted_current > _batt_current_max) { batt_current_ratio = predicted_current_ratio; } - _output_limited += (loop_interval/(loop_interval+_batt_current_time_constant)) * (1 - batt_current_ratio); + _output_limited += (_dt / (_dt + _batt_current_time_constant)) * (1 - batt_current_ratio); _output_limited = constrain_float(_output_limited, 0.0f, 1.0f); diff --git a/libraries/AP_Motors/AP_Motors6DOF.h b/libraries/AP_Motors/AP_Motors6DOF.h index 14ed738fa9..90d7c0e00f 100644 --- a/libraries/AP_Motors/AP_Motors6DOF.h +++ b/libraries/AP_Motors/AP_Motors6DOF.h @@ -12,8 +12,8 @@ class AP_Motors6DOF : public AP_MotorsMatrix { public: - AP_Motors6DOF(uint16_t loop_rate, uint16_t speed_hz = AP_MOTORS_SPEED_DEFAULT) : - AP_MotorsMatrix(loop_rate, speed_hz) { + AP_Motors6DOF(uint16_t speed_hz = AP_MOTORS_SPEED_DEFAULT) : + AP_MotorsMatrix(speed_hz) { AP_Param::setup_object_defaults(this, var_info); }; diff --git a/libraries/AP_Motors/AP_MotorsCoax.h b/libraries/AP_Motors/AP_MotorsCoax.h index 739192145f..cbc07b39cd 100644 --- a/libraries/AP_Motors/AP_MotorsCoax.h +++ b/libraries/AP_Motors/AP_MotorsCoax.h @@ -23,8 +23,8 @@ class AP_MotorsCoax : public AP_MotorsMulticopter { public: /// Constructor - AP_MotorsCoax(uint16_t loop_rate, uint16_t speed_hz = AP_MOTORS_SPEED_DEFAULT) : - AP_MotorsMulticopter(loop_rate, speed_hz) + AP_MotorsCoax(uint16_t speed_hz = AP_MOTORS_SPEED_DEFAULT) : + AP_MotorsMulticopter(speed_hz) { }; diff --git a/libraries/AP_Motors/AP_MotorsHeli.cpp b/libraries/AP_Motors/AP_MotorsHeli.cpp index b70ec15f88..43db815da7 100644 --- a/libraries/AP_Motors/AP_MotorsHeli.cpp +++ b/libraries/AP_Motors/AP_MotorsHeli.cpp @@ -522,7 +522,7 @@ void AP_MotorsHeli::reset_swash_servo(SRV_Channel::Aux_servo_function_t function // update the throttle input filter void AP_MotorsHeli::update_throttle_filter() { - _throttle_filter.apply(_throttle_in, 1.0f/_loop_rate); + _throttle_filter.apply(_throttle_in, _dt); // constrain filtered throttle if (_throttle_filter.get() < 0.0f) { diff --git a/libraries/AP_Motors/AP_MotorsHeli.h b/libraries/AP_Motors/AP_MotorsHeli.h index b36b461f13..53cba9a3a0 100644 --- a/libraries/AP_Motors/AP_MotorsHeli.h +++ b/libraries/AP_Motors/AP_MotorsHeli.h @@ -40,9 +40,8 @@ class AP_MotorsHeli : public AP_Motors { public: /// Constructor - AP_MotorsHeli( uint16_t loop_rate, - uint16_t speed_hz = AP_MOTORS_HELI_SPEED_DEFAULT) : - AP_Motors(loop_rate, speed_hz), + AP_MotorsHeli( uint16_t speed_hz = AP_MOTORS_HELI_SPEED_DEFAULT) : + AP_Motors(speed_hz), _main_rotor(SRV_Channel::k_heli_rsc, AP_MOTORS_HELI_RSC) { AP_Param::setup_object_defaults(this, var_info); diff --git a/libraries/AP_Motors/AP_MotorsHeli_Dual.cpp b/libraries/AP_Motors/AP_MotorsHeli_Dual.cpp index 2ea25afe59..89a5cdd2c6 100644 --- a/libraries/AP_Motors/AP_MotorsHeli_Dual.cpp +++ b/libraries/AP_Motors/AP_MotorsHeli_Dual.cpp @@ -699,27 +699,27 @@ void AP_MotorsHeli_Dual::servo_test() // this test cycle is equivalent to that of AP_MotorsHeli_Single, but excluding // mixing of yaw, as that physical movement is represented by pitch and roll - _servo_test_cycle_time += 1.0f / _loop_rate; + _servo_test_cycle_time += _dt; if ((_servo_test_cycle_time >= 0.0f && _servo_test_cycle_time < 0.5f)|| // Tilt swash back (_servo_test_cycle_time >= 6.0f && _servo_test_cycle_time < 6.5f)){ - _pitch_test += (1.0f / (_loop_rate/2)); - _oscillate_angle += 8 * M_PI / _loop_rate; + _pitch_test += 2.0 * _dt; + _oscillate_angle += 8 * M_PI * _dt; } else if ((_servo_test_cycle_time >= 0.5f && _servo_test_cycle_time < 4.5f)|| // Roll swash around (_servo_test_cycle_time >= 6.5f && _servo_test_cycle_time < 10.5f)){ - _oscillate_angle += M_PI / (2 * _loop_rate); + _oscillate_angle += 0.5 * M_PI * _dt; _roll_test = sinf(_oscillate_angle); _pitch_test = cosf(_oscillate_angle); } else if ((_servo_test_cycle_time >= 4.5f && _servo_test_cycle_time < 5.0f)|| // Return swash to level (_servo_test_cycle_time >= 10.5f && _servo_test_cycle_time < 11.0f)){ - _pitch_test -= (1.0f / (_loop_rate/2)); - _oscillate_angle += 8 * M_PI / _loop_rate; + _pitch_test -= 2.0 * _dt; + _oscillate_angle += 8 * M_PI * _dt; } else if (_servo_test_cycle_time >= 5.0f && _servo_test_cycle_time < 6.0f){ // Raise swash to top - _collective_test += (1.0f / _loop_rate); - _oscillate_angle += 2 * M_PI / _loop_rate; + _collective_test += _dt; + _oscillate_angle += 2 * M_PI * _dt; } else if (_servo_test_cycle_time >= 11.0f && _servo_test_cycle_time < 12.0f){ // Lower swash to bottom - _collective_test -= (1.0f / _loop_rate); - _oscillate_angle += 2 * M_PI / _loop_rate; + _collective_test -= _dt; + _oscillate_angle += 2 * M_PI * _dt; } else { // reset cycle _servo_test_cycle_time = 0.0f; _oscillate_angle = 0.0f; diff --git a/libraries/AP_Motors/AP_MotorsHeli_Dual.h b/libraries/AP_Motors/AP_MotorsHeli_Dual.h index 9441674f21..f18a892672 100644 --- a/libraries/AP_Motors/AP_MotorsHeli_Dual.h +++ b/libraries/AP_Motors/AP_MotorsHeli_Dual.h @@ -36,9 +36,8 @@ class AP_MotorsHeli_Dual : public AP_MotorsHeli { public: // constructor - AP_MotorsHeli_Dual(uint16_t loop_rate, - uint16_t speed_hz = AP_MOTORS_HELI_SPEED_DEFAULT) : - AP_MotorsHeli(loop_rate, speed_hz) + AP_MotorsHeli_Dual(uint16_t speed_hz = AP_MOTORS_HELI_SPEED_DEFAULT) : + AP_MotorsHeli(speed_hz) { AP_Param::setup_object_defaults(this, var_info); }; diff --git a/libraries/AP_Motors/AP_MotorsHeli_Quad.h b/libraries/AP_Motors/AP_MotorsHeli_Quad.h index e0fc33b200..d93182edee 100644 --- a/libraries/AP_Motors/AP_MotorsHeli_Quad.h +++ b/libraries/AP_Motors/AP_MotorsHeli_Quad.h @@ -19,9 +19,8 @@ class AP_MotorsHeli_Quad : public AP_MotorsHeli { public: // constructor - AP_MotorsHeli_Quad(uint16_t loop_rate, - uint16_t speed_hz = AP_MOTORS_HELI_SPEED_DEFAULT) : - AP_MotorsHeli(loop_rate, speed_hz) + AP_MotorsHeli_Quad(uint16_t speed_hz = AP_MOTORS_HELI_SPEED_DEFAULT) : + AP_MotorsHeli(speed_hz) { AP_Param::setup_object_defaults(this, var_info); }; diff --git a/libraries/AP_Motors/AP_MotorsHeli_Single.cpp b/libraries/AP_Motors/AP_MotorsHeli_Single.cpp index 3c89f519cd..ce33484e14 100644 --- a/libraries/AP_Motors/AP_MotorsHeli_Single.cpp +++ b/libraries/AP_Motors/AP_MotorsHeli_Single.cpp @@ -582,31 +582,31 @@ void AP_MotorsHeli_Single::output_to_motors() // servo_test - move servos through full range of movement void AP_MotorsHeli_Single::servo_test() { - _servo_test_cycle_time += 1.0f / _loop_rate; + _servo_test_cycle_time += _dt; if ((_servo_test_cycle_time >= 0.0f && _servo_test_cycle_time < 0.5f)|| // Tilt swash back (_servo_test_cycle_time >= 6.0f && _servo_test_cycle_time < 6.5f)){ - _pitch_test += (1.0f / (_loop_rate / 2.0f)); - _oscillate_angle += 8 * M_PI / _loop_rate; + _pitch_test += 2.0 * _dt; + _oscillate_angle += 8 * M_PI * _dt; _yaw_test = 0.5f * sinf(_oscillate_angle); } else if ((_servo_test_cycle_time >= 0.5f && _servo_test_cycle_time < 4.5f)|| // Roll swash around (_servo_test_cycle_time >= 6.5f && _servo_test_cycle_time < 10.5f)){ - _oscillate_angle += M_PI / (2 * _loop_rate); + _oscillate_angle += 0.5 * M_PI * _dt; _roll_test = sinf(_oscillate_angle); _pitch_test = cosf(_oscillate_angle); _yaw_test = sinf(_oscillate_angle); } else if ((_servo_test_cycle_time >= 4.5f && _servo_test_cycle_time < 5.0f)|| // Return swash to level (_servo_test_cycle_time >= 10.5f && _servo_test_cycle_time < 11.0f)){ - _pitch_test -= (1.0f / (_loop_rate / 2.0f)); - _oscillate_angle += 8 * M_PI / _loop_rate; + _pitch_test -= 2.0 * _dt; + _oscillate_angle += 8 * M_PI * _dt; _yaw_test = 0.5f * sinf(_oscillate_angle); } else if (_servo_test_cycle_time >= 5.0f && _servo_test_cycle_time < 6.0f){ // Raise swash to top - _collective_test += (1.0f / _loop_rate); - _oscillate_angle += 2 * M_PI / _loop_rate; + _collective_test += _dt; + _oscillate_angle += 2 * M_PI * _dt; _yaw_test = sinf(_oscillate_angle); } else if (_servo_test_cycle_time >= 11.0f && _servo_test_cycle_time < 12.0f){ // Lower swash to bottom - _collective_test -= (1.0f / _loop_rate); - _oscillate_angle += 2 * M_PI / _loop_rate; + _collective_test -= _dt; + _oscillate_angle += 2 * M_PI * _dt; _yaw_test = sinf(_oscillate_angle); } else { // reset cycle _servo_test_cycle_time = 0.0f; diff --git a/libraries/AP_Motors/AP_MotorsHeli_Single.h b/libraries/AP_Motors/AP_MotorsHeli_Single.h index c9c0d76739..c2079e7afa 100644 --- a/libraries/AP_Motors/AP_MotorsHeli_Single.h +++ b/libraries/AP_Motors/AP_MotorsHeli_Single.h @@ -38,9 +38,8 @@ class AP_MotorsHeli_Single : public AP_MotorsHeli { public: // constructor - AP_MotorsHeli_Single(uint16_t loop_rate, - uint16_t speed_hz = AP_MOTORS_HELI_SPEED_DEFAULT) : - AP_MotorsHeli(loop_rate, speed_hz), + AP_MotorsHeli_Single(uint16_t speed_hz = AP_MOTORS_HELI_SPEED_DEFAULT) : + AP_MotorsHeli(speed_hz), _tail_rotor(SRV_Channel::k_heli_tail_rsc, AP_MOTORS_HELI_SINGLE_TAILRSC), _swashplate() { diff --git a/libraries/AP_Motors/AP_MotorsMatrix.cpp b/libraries/AP_Motors/AP_MotorsMatrix.cpp index de211e57c6..e915526d75 100644 --- a/libraries/AP_Motors/AP_MotorsMatrix.cpp +++ b/libraries/AP_Motors/AP_MotorsMatrix.cpp @@ -414,7 +414,7 @@ void AP_MotorsMatrix::output_armed_stabilizing() void AP_MotorsMatrix::check_for_failed_motor(float throttle_thrust_best_plus_adj) { // record filtered and scaled thrust output for motor loss monitoring purposes - float alpha = 1.0f / (1.0f + _loop_rate * 0.5f); + float alpha = _dt / (_dt + 0.5f); for (uint8_t i = 0; i < AP_MOTORS_MAX_NUM_MOTORS; i++) { if (motor_enabled[i]) { _thrust_rpyt_out_filt[i] += alpha * (_thrust_rpyt_out[i] - _thrust_rpyt_out_filt[i]); diff --git a/libraries/AP_Motors/AP_MotorsMatrix.h b/libraries/AP_Motors/AP_MotorsMatrix.h index d4bc6e61e9..5b2061546f 100644 --- a/libraries/AP_Motors/AP_MotorsMatrix.h +++ b/libraries/AP_Motors/AP_MotorsMatrix.h @@ -15,8 +15,8 @@ class AP_MotorsMatrix : public AP_MotorsMulticopter { public: /// Constructor - AP_MotorsMatrix(uint16_t loop_rate, uint16_t speed_hz = AP_MOTORS_SPEED_DEFAULT) : - AP_MotorsMulticopter(loop_rate, speed_hz) + AP_MotorsMatrix(uint16_t speed_hz = AP_MOTORS_SPEED_DEFAULT) : + AP_MotorsMulticopter(speed_hz) { if (_singleton != nullptr) { AP_HAL::panic("AP_MotorsMatrix must be singleton"); diff --git a/libraries/AP_Motors/AP_MotorsMatrix_6DoF_Scripting.h b/libraries/AP_Motors/AP_MotorsMatrix_6DoF_Scripting.h index 86b973b093..3b083b7e68 100644 --- a/libraries/AP_Motors/AP_MotorsMatrix_6DoF_Scripting.h +++ b/libraries/AP_Motors/AP_MotorsMatrix_6DoF_Scripting.h @@ -10,8 +10,8 @@ class AP_MotorsMatrix_6DoF_Scripting : public AP_MotorsMatrix { public: /// Constructor - AP_MotorsMatrix_6DoF_Scripting(uint16_t loop_rate, uint16_t speed_hz = AP_MOTORS_SPEED_DEFAULT) : - AP_MotorsMatrix(loop_rate, speed_hz) + AP_MotorsMatrix_6DoF_Scripting(uint16_t speed_hz = AP_MOTORS_SPEED_DEFAULT) : + AP_MotorsMatrix(speed_hz) { if (_singleton != nullptr) { AP_HAL::panic("AP_MotorsMatrix 6DoF must be singleton"); diff --git a/libraries/AP_Motors/AP_MotorsMatrix_Scripting_Dynamic.h b/libraries/AP_Motors/AP_MotorsMatrix_Scripting_Dynamic.h index 70879d0040..12b86281c5 100644 --- a/libraries/AP_Motors/AP_MotorsMatrix_Scripting_Dynamic.h +++ b/libraries/AP_Motors/AP_MotorsMatrix_Scripting_Dynamic.h @@ -7,8 +7,8 @@ class AP_MotorsMatrix_Scripting_Dynamic : public AP_MotorsMatrix { public: // Constructor - AP_MotorsMatrix_Scripting_Dynamic(uint16_t loop_rate, uint16_t speed_hz = AP_MOTORS_SPEED_DEFAULT) : - AP_MotorsMatrix(loop_rate, speed_hz) + AP_MotorsMatrix_Scripting_Dynamic(uint16_t speed_hz = AP_MOTORS_SPEED_DEFAULT) : + AP_MotorsMatrix(speed_hz) { if (_singleton != nullptr) { AP_HAL::panic("AP_MotorsMatrix_Scripting_Dynamic must be singleton"); diff --git a/libraries/AP_Motors/AP_MotorsMulticopter.cpp b/libraries/AP_Motors/AP_MotorsMulticopter.cpp index 2f039a83b0..418ee57715 100644 --- a/libraries/AP_Motors/AP_MotorsMulticopter.cpp +++ b/libraries/AP_Motors/AP_MotorsMulticopter.cpp @@ -219,8 +219,8 @@ const AP_Param::GroupInfo AP_MotorsMulticopter::var_info[] = { }; // Constructor -AP_MotorsMulticopter::AP_MotorsMulticopter(uint16_t loop_rate, uint16_t speed_hz) : - AP_Motors(loop_rate, speed_hz), +AP_MotorsMulticopter::AP_MotorsMulticopter(uint16_t speed_hz) : + AP_Motors(speed_hz), _lift_max(1.0f), _throttle_limit(1.0f) { @@ -292,7 +292,7 @@ void AP_MotorsMulticopter::output_min() void AP_MotorsMulticopter::update_throttle_filter() { if (armed()) { - _throttle_filter.apply(_throttle_in, 1.0f / _loop_rate); + _throttle_filter.apply(_throttle_in, _dt); // constrain filtered throttle if (_throttle_filter.get() < 0.0f) { _throttle_filter.reset(0.0f); @@ -331,8 +331,7 @@ float AP_MotorsMulticopter::get_current_limit_max_throttle() float batt_current_ratio = _batt_current / batt_current_max; - float loop_interval = 1.0f / _loop_rate; - _throttle_limit += (loop_interval / (loop_interval + _batt_current_time_constant)) * (1.0f - batt_current_ratio); + _throttle_limit += (_dt / (_dt + _batt_current_time_constant)) * (1.0f - batt_current_ratio); // throttle limit drops to 20% between hover and full throttle _throttle_limit = constrain_float(_throttle_limit, 0.2f, 1.0f); @@ -396,7 +395,7 @@ void AP_MotorsMulticopter::update_lift_max_from_batt_voltage() _batt_voltage_resting_estimate = constrain_float(_batt_voltage_resting_estimate, _batt_voltage_min, _batt_voltage_max); // filter at 0.5 Hz - float batt_voltage_filt = _batt_voltage_filt.apply(_batt_voltage_resting_estimate / _batt_voltage_max, 1.0f / _loop_rate); + float batt_voltage_filt = _batt_voltage_filt.apply(_batt_voltage_resting_estimate / _batt_voltage_max, _dt); // calculate lift max float thrust_curve_expo = constrain_float(_thrust_curve_expo, -1.0f, 1.0f); @@ -486,13 +485,13 @@ void AP_MotorsMulticopter::set_actuator_with_slew(float& actuator_output, float // If MOT_SLEW_UP_TIME is set, calculate the highest allowed new output value, constrained 0.0~1.0 if (is_positive(_slew_up_time)) { - float output_delta_up_max = 1.0f / (constrain_float(_slew_up_time, 0.0f, 0.5f) * _loop_rate); + float output_delta_up_max = _dt / (constrain_float(_slew_up_time, 0.0f, 0.5f)); output_slew_limit_up = constrain_float(actuator_output + output_delta_up_max, 0.0f, 1.0f); } // If MOT_SLEW_DN_TIME is set, calculate the lowest allowed new output value, constrained 0.0~1.0 if (is_positive(_slew_dn_time)) { - float output_delta_dn_max = 1.0f / (constrain_float(_slew_dn_time, 0.0f, 0.5f) * _loop_rate); + float output_delta_dn_max = _dt / (constrain_float(_slew_dn_time, 0.0f, 0.5f)); output_slew_limit_dn = constrain_float(actuator_output - output_delta_dn_max, 0.0f, 1.0f); } @@ -545,7 +544,7 @@ void AP_MotorsMulticopter::output_logic() { if (armed()) { if (_disarm_disable_pwm && (_disarm_safe_timer < _safe_time)) { - _disarm_safe_timer += 1.0f/_loop_rate; + _disarm_safe_timer += _dt; } else { _disarm_safe_timer = _safe_time; } @@ -564,7 +563,7 @@ void AP_MotorsMulticopter::output_logic() _spool_up_time.set(0.05); } - const float spool_step = 1.0f / (_spool_up_time * _loop_rate); + const float spool_step = _dt / _spool_up_time; switch (_spool_state) { case SpoolState::SHUT_DOWN: // Motors should be stationary. diff --git a/libraries/AP_Motors/AP_MotorsMulticopter.h b/libraries/AP_Motors/AP_MotorsMulticopter.h index aa462db447..9a41f2276f 100644 --- a/libraries/AP_Motors/AP_MotorsMulticopter.h +++ b/libraries/AP_Motors/AP_MotorsMulticopter.h @@ -33,7 +33,7 @@ class AP_MotorsMulticopter : public AP_Motors { public: // Constructor - AP_MotorsMulticopter(uint16_t loop_rate, uint16_t speed_hz = AP_MOTORS_SPEED_DEFAULT); + AP_MotorsMulticopter(uint16_t speed_hz = AP_MOTORS_SPEED_DEFAULT); // output - sends commands to the motors virtual void output() override; diff --git a/libraries/AP_Motors/AP_MotorsSingle.h b/libraries/AP_Motors/AP_MotorsSingle.h index fb5ea19445..5f9264dcf5 100644 --- a/libraries/AP_Motors/AP_MotorsSingle.h +++ b/libraries/AP_Motors/AP_MotorsSingle.h @@ -23,8 +23,8 @@ class AP_MotorsSingle : public AP_MotorsMulticopter { public: /// Constructor - AP_MotorsSingle(uint16_t loop_rate, uint16_t speed_hz = AP_MOTORS_SPEED_DEFAULT) : - AP_MotorsMulticopter(loop_rate, speed_hz) + AP_MotorsSingle(uint16_t speed_hz = AP_MOTORS_SPEED_DEFAULT) : + AP_MotorsMulticopter(speed_hz) { }; diff --git a/libraries/AP_Motors/AP_MotorsTailsitter.cpp b/libraries/AP_Motors/AP_MotorsTailsitter.cpp index c19092d957..ae1a361d3c 100644 --- a/libraries/AP_Motors/AP_MotorsTailsitter.cpp +++ b/libraries/AP_Motors/AP_MotorsTailsitter.cpp @@ -55,8 +55,8 @@ void AP_MotorsTailsitter::init(motor_frame_class frame_class, motor_frame_type f /// Constructor -AP_MotorsTailsitter::AP_MotorsTailsitter(uint16_t loop_rate, uint16_t speed_hz) : - AP_MotorsMulticopter(loop_rate, speed_hz) +AP_MotorsTailsitter::AP_MotorsTailsitter(uint16_t speed_hz) : + AP_MotorsMulticopter(speed_hz) { set_update_rate(speed_hz); } diff --git a/libraries/AP_Motors/AP_MotorsTailsitter.h b/libraries/AP_Motors/AP_MotorsTailsitter.h index a456be765b..e1d6e9fa3e 100644 --- a/libraries/AP_Motors/AP_MotorsTailsitter.h +++ b/libraries/AP_Motors/AP_MotorsTailsitter.h @@ -12,7 +12,7 @@ class AP_MotorsTailsitter : public AP_MotorsMulticopter { public: /// Constructor - AP_MotorsTailsitter(uint16_t loop_rate, uint16_t speed_hz = AP_MOTORS_SPEED_DEFAULT); + AP_MotorsTailsitter(uint16_t speed_hz = AP_MOTORS_SPEED_DEFAULT); // init void init(motor_frame_class frame_class, motor_frame_type frame_type) override; diff --git a/libraries/AP_Motors/AP_MotorsTri.h b/libraries/AP_Motors/AP_MotorsTri.h index 1f6317ac6e..d4c9f051b2 100644 --- a/libraries/AP_Motors/AP_MotorsTri.h +++ b/libraries/AP_Motors/AP_MotorsTri.h @@ -18,8 +18,8 @@ class AP_MotorsTri : public AP_MotorsMulticopter { public: /// Constructor - AP_MotorsTri(uint16_t loop_rate, uint16_t speed_hz = AP_MOTORS_SPEED_DEFAULT) : - AP_MotorsMulticopter(loop_rate, speed_hz) + AP_MotorsTri(uint16_t speed_hz = AP_MOTORS_SPEED_DEFAULT) : + AP_MotorsMulticopter(speed_hz) { }; diff --git a/libraries/AP_Motors/AP_Motors_Class.cpp b/libraries/AP_Motors/AP_Motors_Class.cpp index 78932e8b7c..3951e17d29 100644 --- a/libraries/AP_Motors/AP_Motors_Class.cpp +++ b/libraries/AP_Motors/AP_Motors_Class.cpp @@ -25,8 +25,7 @@ extern const AP_HAL::HAL& hal; AP_Motors *AP_Motors::_singleton; // Constructor -AP_Motors::AP_Motors(uint16_t loop_rate, uint16_t speed_hz) : - _loop_rate(loop_rate), +AP_Motors::AP_Motors(uint16_t speed_hz) : _speed_hz(speed_hz), _throttle_filter(), _spool_desired(DesiredSpoolState::SHUT_DOWN), diff --git a/libraries/AP_Motors/AP_Motors_Class.h b/libraries/AP_Motors/AP_Motors_Class.h index 5573f4cf79..83e23b0b15 100644 --- a/libraries/AP_Motors/AP_Motors_Class.h +++ b/libraries/AP_Motors/AP_Motors_Class.h @@ -103,7 +103,7 @@ public: void get_frame_and_type_string(char *buffer, uint8_t buflen) const; // Constructor - AP_Motors(uint16_t loop_rate, uint16_t speed_hz = AP_MOTORS_SPEED_DEFAULT); + AP_Motors(uint16_t speed_hz = AP_MOTORS_SPEED_DEFAULT); // singleton support static AP_Motors *get_singleton(void) { return _singleton; } @@ -188,6 +188,12 @@ public: // set_density_ratio - sets air density as a proportion of sea level density void set_air_density_ratio(float ratio) { _air_density_ratio = ratio; } + // set_dt / get_dt - dt is the time since the last time the motor mixers were updated + // _dt should be set based on the time of the last IMU read used by these controllers + // the motor mixers should run on each loop to ensure normal operation + void set_dt(float dt) { _dt = dt; } + float get_dt() const { return _dt; } + // structure for holding motor limit flags struct AP_Motors_limit { uint8_t roll : 1; // we have reached roll or pitch limit @@ -231,9 +237,6 @@ 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; } - // return the roll factor of any motor, this is used for tilt rotors and tail sitters // using copter motors for forward flight virtual float get_roll_factor(uint8_t i) { return 0.0f; } @@ -293,7 +296,7 @@ protected: virtual void save_params_on_disarm() {} // internal variables - uint16_t _loop_rate; // rate in Hz at which output() function is called (normally 400hz) + float _dt; // time difference (in seconds) since the last loop time uint16_t _speed_hz; // speed in hz to send updates to motors float _roll_in; // desired roll control from attitude controllers, -1 ~ +1 float _roll_in_ff; // desired roll feed forward control from attitude controllers, -1 ~ +1 diff --git a/libraries/AP_Motors/examples/expo_inverse_test/expo_inverse_test.cpp b/libraries/AP_Motors/examples/expo_inverse_test/expo_inverse_test.cpp index 5a3ad80d62..0398193401 100644 --- a/libraries/AP_Motors/examples/expo_inverse_test/expo_inverse_test.cpp +++ b/libraries/AP_Motors/examples/expo_inverse_test/expo_inverse_test.cpp @@ -30,8 +30,8 @@ const AP_HAL::HAL& hal = AP_HAL::get_HAL(); class AP_MotorsMulticopter_test : public AP_MotorsMulticopter { public: - AP_MotorsMulticopter_test(uint16_t loop_rate, uint16_t speed_hz = AP_MOTORS_SPEED_DEFAULT) : - AP_MotorsMulticopter(loop_rate, speed_hz) + AP_MotorsMulticopter_test(uint16_t speed_hz = AP_MOTORS_SPEED_DEFAULT) : + AP_MotorsMulticopter(speed_hz) { }; @@ -48,7 +48,7 @@ public: }; -AP_MotorsMulticopter_test motors{1}; +AP_MotorsMulticopter_test motors; /* * rotation tests @@ -66,6 +66,7 @@ void setup(void) float max_diff_expo = 0; float expo = -1.0; + motors.set_dt(1); while (expo < 1.0+expo_step*0.5) { hal.console->printf("expo: %0.4f\n",expo); motors.set_expo(expo);