fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-11-03 02:31:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

slew rate limiting: implemented for fmu

Browse Source 
Signed-off-by: Roman <bapstroman@gmail.com>
This commit is contained in:
Roman
2016-09-30 13:55:53 +02:00
committed by Julian Oes
parent 2607769470
commit 8b889caa33
1 changed files with 30 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/px4fmu/fmu.cpp
+30 -4
View File
@@ -170,6 +170,7 @@ private:
hrt_abstime _cycle_timestamp = 0; hrt_abstime _cycle_timestamp = 0;
hrt_abstime _last_safety_check = 0; hrt_abstime _last_safety_check = 0;
hrt_abstime _time_last_mix = 0;
static const unsigned _max_actuators = DIRECT_PWM_OUTPUT_CHANNELS; static const unsigned _max_actuators = DIRECT_PWM_OUTPUT_CHANNELS;
@@ -221,6 +222,8 @@ private:
bool _safety_disabled; bool _safety_disabled;
orb_advert_t _to_safety; orb_advert_t _to_safety;
float _mot_t_max; // maximum rise time for motor (slew rate limiting)
static bool arm_nothrottle() static bool arm_nothrottle()
{ {
return ((_armed.prearmed && !_armed.armed) || _armed.in_esc_calibration_mode); return ((_armed.prearmed && !_armed.armed) || _armed.in_esc_calibration_mode);
@@ -328,7 +331,8 @@ PX4FMU::PX4FMU() :
_num_disarmed_set(0), _num_disarmed_set(0),
_safety_off(false), _safety_off(false),
_safety_disabled(false), _safety_disabled(false),
_to_safety(nullptr) _to_safety(nullptr),
_mot_t_max(0.0f)
{ {
for (unsigned i = 0; i < _max_actuators; i++) { for (unsigned i = 0; i < _max_actuators; i++) {
_min_pwm[i] = PWM_DEFAULT_MIN; _min_pwm[i] = PWM_DEFAULT_MIN;
@@ -924,6 +928,7 @@ PX4FMU::cycle()
px4_arch_unconfiggpio(GPIO_PPM_IN); px4_arch_unconfiggpio(GPIO_PPM_IN);
#endif #endif
#endif #endif
param_find("MOT_SLEW_MAX");
_initialized = true; _initialized = true;
} }
@@ -1059,6 +1064,20 @@ PX4FMU::cycle()
break; break;
} }
hrt_abstime now = hrt_absolute_time();
float dt = (now - _time_last_mix) / 1e6f;
_time_last_mix = now;
if (dt < 0.0001f) {
dt = 0.0001f;
} else if (dt > 0.02f) {
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);
/* do mixing */ /* do mixing */
float outputs[_max_actuators]; float outputs[_max_actuators];
num_outputs = _mixers->mix(outputs, num_outputs, NULL); num_outputs = _mixers->mix(outputs, num_outputs, NULL);
@@ -1169,15 +1188,22 @@ PX4FMU::cycle()
update_pwm_rev_mask(); update_pwm_rev_mask();
int32_t dsm_bind_val; int32_t dsm_bind_val;
param_t dsm_bind_param; param_t param_handle;
/* see if bind parameter has been set, and reset it to -1 */ /* see if bind parameter has been set, and reset it to -1 */
param_get(dsm_bind_param = param_find("RC_DSM_BIND"), &dsm_bind_val); param_get(param_handle = param_find("RC_DSM_BIND"), &dsm_bind_val);
if (dsm_bind_val > -1) { if (dsm_bind_val > -1) {
dsm_bind_ioctl(dsm_bind_val); dsm_bind_ioctl(dsm_bind_val);
dsm_bind_val = -1; dsm_bind_val = -1;
param_set(dsm_bind_param, &dsm_bind_val); param_set(param_handle, &dsm_bind_val);
}
// maximum motor slew rate parameter
param_handle = param_find("MOT_SLEW_MAX");
if (param_handle != PARAM_INVALID) {
param_get(param_handle, &_mot_t_max);
} }
} }