Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_Motors: Support changing update period

This commit is contained in:
Leonard Hall 2022-12-07 11:33:36 +10:30 committed by Peter Barker
parent 5d690f0360
commit 67205f8114
23 changed files with 75 additions and 79 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
libraries/AP_Motors/AP_Motors6DOF.cpp
View File

@ -367,11 +367,9 @@ 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 * loop_interval * 5);
float predicted_current = _batt_current + (_current_change_rate * _dt * 5);
float batt_current_ratio = _batt_current / _batt_current_max;
@ -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);

4
libraries/AP_Motors/AP_Motors6DOF.h
View File

@ -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);
};

4
libraries/AP_Motors/AP_MotorsCoax.h
View File

@ -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)
{
};

2
libraries/AP_Motors/AP_MotorsHeli.cpp
View File

@ -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) {

5
libraries/AP_Motors/AP_MotorsHeli.h
View File

@ -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);

20
libraries/AP_Motors/AP_MotorsHeli_Dual.cpp
View File

@ -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;

5
libraries/AP_Motors/AP_MotorsHeli_Dual.h
View File

@ -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);
};

5
libraries/AP_Motors/AP_MotorsHeli_Quad.h
View File

@ -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);
};

20
libraries/AP_Motors/AP_MotorsHeli_Single.cpp
View File

@ -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;

5
libraries/AP_Motors/AP_MotorsHeli_Single.h
View File

@ -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()
{

2
libraries/AP_Motors/AP_MotorsMatrix.cpp
View File

@ -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]);

4
libraries/AP_Motors/AP_MotorsMatrix.h
View File

@ -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");

4
libraries/AP_Motors/AP_MotorsMatrix_6DoF_Scripting.h
View File

@ -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");

4
libraries/AP_Motors/AP_MotorsMatrix_Scripting_Dynamic.h
View File

@ -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");

19
libraries/AP_Motors/AP_MotorsMulticopter.cpp
View File

@ -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.

2
libraries/AP_Motors/AP_MotorsMulticopter.h
View File

@ -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;

4
libraries/AP_Motors/AP_MotorsSingle.h
View File

@ -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)
{
};

4
libraries/AP_Motors/AP_MotorsTailsitter.cpp
View File

@ -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);
}

2
libraries/AP_Motors/AP_MotorsTailsitter.h
View File

@ -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;

4
libraries/AP_Motors/AP_MotorsTri.h
View File

@ -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)
{
};

3
libraries/AP_Motors/AP_Motors_Class.cpp
View File

@ -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),

13
libraries/AP_Motors/AP_Motors_Class.h
View File

@ -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

7
libraries/AP_Motors/examples/expo_inverse_test/expo_inverse_test.cpp
View File

@ -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);