fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

multirotor mixer slew rate limiting: naming and fixes

Browse Source 
- avoid dividing by zero when calculating max delta output
- better comments when calculating max delta output
- better naming of functions and variables

Signed-off-by: Roman <bapstroman@gmail.com>
This commit is contained in:
Roman 2016-09-27 14:42:15 +02:00 committed by Julian Oes
parent cced6fc8b2
commit c2a511d81d
5 changed files with 48 additions and 26 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
src/drivers/px4fmu/fmu.cpp
View File

@ -1075,8 +1075,12 @@ PX4FMU::cycle()
dt = 0.02f;
}
float slew_max = 2.0f * 1000.0f * dt / (_max_pwm[0] - _min_pwm[0]) / _mot_t_max;
_mixers->update_slew_rate(slew_max);
if (_mot_t_max > FLT_EPSILON) {
// maximum value the ouputs of the multirotor mixer are allowed to change in this cycle
// factor 2 is needed because actuator ouputs are in the range [-1,1]
float delta_out_max = 2.0f * 1000.0f * dt / (_max_pwm[0] - _min_pwm[0]) / _mot_t_max;
_mixers->set_max_delta_out_once(delta_out_max);
}
/* do mixing */
float outputs[_max_actuators];

22
src/modules/px4iofirmware/mixer.cpp
View File

@ -232,10 +232,13 @@ mixer_tick(void)
float outputs[PX4IO_SERVO_COUNT];
unsigned mixed;
// update slew rate value
float slew_max = 2.0f * 1000.0f * dt / (r_page_servo_control_max[0] - r_page_servo_control_min[0]) / REG_TO_FLOAT(
r_setup_slew_max);
mixer_group.update_slew_rate(slew_max);
if (REG_TO_FLOAT(r_setup_slew_max) > FLT_EPSILON) {
// maximum value the ouputs of the multirotor mixer are allowed to change in this cycle
// factor 2 is needed because actuator ouputs are in the range [-1,1]
float delta_out_max = 2.0f * 1000.0f * dt / (r_page_servo_control_max[0] - r_page_servo_control_min[0]) / REG_TO_FLOAT(
r_setup_slew_max);
mixer_group.set_max_delta_out_once(delta_out_max);
}
/* mix */
@ -518,10 +521,13 @@ mixer_set_failsafe()
float outputs[PX4IO_SERVO_COUNT];
unsigned mixed;
// update slew rate value
float slew_max = 2.0f * 1000.0f * dt / (r_page_servo_control_max[0] - r_page_servo_control_min[0]) / REG_TO_FLOAT(
r_setup_slew_max);
mixer_group.update_slew_rate(slew_max);
if (REG_TO_FLOAT(r_setup_slew_max) > FLT_EPSILON) {
// maximum value the ouputs of the multirotor mixer are allowed to change in this cycle
// factor 2 is needed because actuator ouputs are in the range [-1,1]
float delta_out_max = 2.0f * 1000.0f * dt / (r_page_servo_control_max[0] - r_page_servo_control_min[0]) / REG_TO_FLOAT(
r_setup_slew_max);
mixer_group.set_max_delta_out_once(delta_out_max);
}
/* mix */
mixed = mixer_group.mix(&outputs[0], PX4IO_SERVO_COUNT, &r_mixer_limits);

26
src/modules/systemlib/mixer/mixer.h
View File

@ -187,10 +187,14 @@ public:
/**
* @brief Update slew rate parameter. This tells the multicopter mixer
* the maximum allowed change of the output values per cycle.
* The value is only valid for one cycle, in order to have continuous
* slew rate limiting this function needs to be called before every call
* to mix().
*
* @param[in] delta_out_max Maximum delta output.
*
* @param[in] slew_rate_max The maximum slew rate.
*/
virtual void update_slew_rate(float slew_rate_max) {};
virtual void set_max_delta_out_once(float delta_out_max) {};
protected:
@ -324,10 +328,14 @@ public:
/**
* @brief Update slew rate parameter. This tells the multicopter mixer
* the maximum allowed change of the output values per cycle.
* The value is only valid for one cycle, in order to have continuous
* slew rate limiting this function needs to be called before every call
* to mix().
*
* @param[in] delta_out_max Maximum delta output.
*
* @param[in] slew_rate_max The maximum slew rate.
*/
virtual void update_slew_rate(float slew_rate_max);
virtual void set_max_delta_out_once(float delta_out_max);
private:
Mixer *_first; /**< linked list of mixers */
@ -538,10 +546,14 @@ public:
/**
* @brief Update slew rate parameter. This tells the multicopter mixer
* the maximum allowed change of the output values per cycle.
* The value is only valid for one cycle, in order to have continuous
* slew rate limiting this function needs to be called before every call
* to mix().
*
* @param[in] delta_out_max Maximum delta output.
*
* @param[in] slew_rate_max The maximum slew rate.
*/
virtual void update_slew_rate(float slew_rate_max) {_slew_rate_max = slew_rate_max;}
virtual void set_max_delta_out_once(float delta_out_max) {_delta_out_max = delta_out_max;}
private:
float _roll_scale;
@ -549,7 +561,7 @@ private:
float _yaw_scale;
float _idle_speed;
float _slew_rate_max;
float _delta_out_max;
orb_advert_t _limits_pub;
multirotor_motor_limits_s _limits;

4
src/modules/systemlib/mixer/mixer_group.cpp
View File

@ -201,12 +201,12 @@ MixerGroup::load_from_buf(const char *buf, unsigned &buflen)
return ret;
}
void MixerGroup::update_slew_rate(float slew_rate_max)
void MixerGroup::set_max_delta_out_once(float delta_out_max)
{
Mixer *mixer = _first;
while (mixer != nullptr) {
mixer->update_slew_rate(slew_rate_max);
mixer->set_max_delta_out_once(delta_out_max);
mixer = mixer->_next;
}
}

14
src/modules/systemlib/mixer/mixer_multirotor.cpp
View File

@ -90,7 +90,7 @@ MultirotorMixer::MultirotorMixer(ControlCallback control_cb,
_pitch_scale(pitch_scale),
_yaw_scale(yaw_scale),
_idle_speed(-1.0f + idle_speed * 2.0f), /* shift to output range here to avoid runtime calculation */
_slew_rate_max(0.0f),
_delta_out_max(0.0f),
_limits_pub(),
_rotor_count(_config_rotor_count[(MultirotorGeometryUnderlyingType)geometry]),
_rotors(_config_index[(MultirotorGeometryUnderlyingType)geometry]),
@ -375,14 +375,14 @@ MultirotorMixer::mix(float *outputs, unsigned space, uint16_t *status_reg)
outputs[i] = constrain(_idle_speed + (outputs[i] * (1.0f - _idle_speed)), _idle_speed, 1.0f);
// slew rate limiting
if (_slew_rate_max > 0.0f) {
if (_delta_out_max > 0.0f) {
float delta_out = outputs[i] - _outputs_prev[i];
if (delta_out > _slew_rate_max) {
outputs[i] = _outputs_prev[i] + _slew_rate_max;
if (delta_out > _delta_out_max) {
outputs[i] = _outputs_prev[i] + _delta_out_max;
} else if (delta_out < -_slew_rate_max) {
outputs[i] = _outputs_prev[i] - _slew_rate_max;
} else if (delta_out < -_delta_out_max) {
outputs[i] = _outputs_prev[i] - _delta_out_max;
}
}
@ -391,7 +391,7 @@ MultirotorMixer::mix(float *outputs, unsigned space, uint16_t *status_reg)
}
// this will force the caller of the mixer to always supply new slew rate values, otherwise no slew rate limiting will happen
_slew_rate_max = 0.0f;
_delta_out_max = 0.0f;
return _rotor_count;
}