forked from Archive/PX4-Autopilot
fixed elevator wobbling at low airspeed.
Removed not applicable if conditions.
This commit is contained in:
parent
9574c6dd2a
commit
32012e8aac
|
@ -49,6 +49,7 @@
|
||||||
#include "ecl_controller.h"
|
#include "ecl_controller.h"
|
||||||
|
|
||||||
#include <stdio.h>
|
#include <stdio.h>
|
||||||
|
#include <mathlib/mathlib.h>
|
||||||
|
|
||||||
ECL_Controller::ECL_Controller(const char *name) :
|
ECL_Controller::ECL_Controller(const char *name) :
|
||||||
_last_run(0),
|
_last_run(0),
|
||||||
|
@ -126,3 +127,14 @@ float ECL_Controller::get_desired_bodyrate()
|
||||||
{
|
{
|
||||||
return _bodyrate_setpoint;
|
return _bodyrate_setpoint;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
float ECL_Controller::constrain_airspeed(float airspeed, float minspeed, float maxspeed) {
|
||||||
|
float airspeed_result = airspeed;
|
||||||
|
if (!isfinite(airspeed)) {
|
||||||
|
/* airspeed is NaN, +- INF or not available, pick center of band */
|
||||||
|
airspeed = 0.5f * (minspeed + maxspeed);
|
||||||
|
} else if (airspeed < minspeed) {
|
||||||
|
airspeed = minspeed;
|
||||||
|
}
|
||||||
|
return airspeed_result;
|
||||||
|
}
|
||||||
|
|
|
@ -51,6 +51,7 @@
|
||||||
#include <stdbool.h>
|
#include <stdbool.h>
|
||||||
#include <stdint.h>
|
#include <stdint.h>
|
||||||
#include <systemlib/perf_counter.h>
|
#include <systemlib/perf_counter.h>
|
||||||
|
#include <mathlib/mathlib.h>
|
||||||
|
|
||||||
struct ECL_ControlData {
|
struct ECL_ControlData {
|
||||||
float roll;
|
float roll;
|
||||||
|
@ -119,4 +120,5 @@ protected:
|
||||||
perf_counter_t _nonfinite_input_perf;
|
perf_counter_t _nonfinite_input_perf;
|
||||||
static const uint8_t _perf_name_max = 40;
|
static const uint8_t _perf_name_max = 40;
|
||||||
char _perf_name[_perf_name_max];
|
char _perf_name[_perf_name_max];
|
||||||
|
float constrain_airspeed(float airspeed, float minspeed, float maxspeed);
|
||||||
};
|
};
|
||||||
|
|
|
@ -94,9 +94,11 @@ float ECL_PitchController::control_attitude(const struct ECL_ControlData &ctl_da
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
/* input conditioning */
|
||||||
|
float airspeed = constrain_airspeed(ctl_data.airspeed, ctl_data.airspeed_min, ctl_data.airspeed_max);
|
||||||
/* calculate the offset in the rate resulting from rolling */
|
/* calculate the offset in the rate resulting from rolling */
|
||||||
//xxx needs explanation and conversion to body angular rates or should be removed
|
//xxx needs explanation and conversion to body angular rates or should be removed
|
||||||
float turn_offset = fabsf((CONSTANTS_ONE_G / ctl_data.airspeed) *
|
float turn_offset = fabsf((CONSTANTS_ONE_G / airspeed) *
|
||||||
tanf(roll) * sinf(roll)) * _roll_ff;
|
tanf(roll) * sinf(roll)) * _roll_ff;
|
||||||
|
|
||||||
if (inverted) {
|
if (inverted) {
|
||||||
|
@ -154,17 +156,6 @@ float ECL_PitchController::control_bodyrate(const struct ECL_ControlData &ctl_da
|
||||||
lock_integrator = true;
|
lock_integrator = true;
|
||||||
}
|
}
|
||||||
|
|
||||||
/* input conditioning */
|
|
||||||
float airspeed = ctl_data.airspeed;
|
|
||||||
|
|
||||||
if (!isfinite(airspeed)) {
|
|
||||||
/* airspeed is NaN, +- INF or not available, pick center of band */
|
|
||||||
airspeed = 0.5f * (ctl_data.airspeed_min + ctl_data.airspeed_max);
|
|
||||||
|
|
||||||
} else if (airspeed < ctl_data.airspeed_min) {
|
|
||||||
airspeed = ctl_data.airspeed_min;
|
|
||||||
}
|
|
||||||
|
|
||||||
/* Transform setpoint to body angular rates (jacobian) */
|
/* Transform setpoint to body angular rates (jacobian) */
|
||||||
_bodyrate_setpoint = cosf(ctl_data.roll) * _rate_setpoint +
|
_bodyrate_setpoint = cosf(ctl_data.roll) * _rate_setpoint +
|
||||||
cosf(ctl_data.pitch) * sinf(ctl_data.roll) * ctl_data.yaw_rate_setpoint;
|
cosf(ctl_data.pitch) * sinf(ctl_data.roll) * ctl_data.yaw_rate_setpoint;
|
||||||
|
@ -175,7 +166,7 @@ float ECL_PitchController::control_bodyrate(const struct ECL_ControlData &ctl_da
|
||||||
|
|
||||||
_rate_error = _bodyrate_setpoint - pitch_bodyrate;
|
_rate_error = _bodyrate_setpoint - pitch_bodyrate;
|
||||||
|
|
||||||
if (!lock_integrator && _k_i > 0.0f && airspeed > 0.5f * ctl_data.airspeed_min) {
|
if (!lock_integrator && _k_i > 0.0f) {
|
||||||
|
|
||||||
float id = _rate_error * dt * ctl_data.scaler;
|
float id = _rate_error * dt * ctl_data.scaler;
|
||||||
|
|
||||||
|
|
|
@ -106,17 +106,6 @@ float ECL_RollController::control_bodyrate(const struct ECL_ControlData &ctl_dat
|
||||||
lock_integrator = true;
|
lock_integrator = true;
|
||||||
}
|
}
|
||||||
|
|
||||||
/* input conditioning */
|
|
||||||
float airspeed = ctl_data.airspeed;
|
|
||||||
|
|
||||||
if (!isfinite(airspeed)) {
|
|
||||||
/* airspeed is NaN, +- INF or not available, pick center of band */
|
|
||||||
airspeed = 0.5f * (ctl_data.airspeed_min + ctl_data.airspeed_max);
|
|
||||||
|
|
||||||
} else if (airspeed < ctl_data.airspeed_min) {
|
|
||||||
airspeed = ctl_data.airspeed_min;
|
|
||||||
}
|
|
||||||
|
|
||||||
/* Transform setpoint to body angular rates (jacobian) */
|
/* Transform setpoint to body angular rates (jacobian) */
|
||||||
_bodyrate_setpoint = _rate_setpoint - sinf(ctl_data.pitch) * ctl_data.yaw_rate_setpoint;
|
_bodyrate_setpoint = _rate_setpoint - sinf(ctl_data.pitch) * ctl_data.yaw_rate_setpoint;
|
||||||
|
|
||||||
|
@ -126,7 +115,7 @@ float ECL_RollController::control_bodyrate(const struct ECL_ControlData &ctl_dat
|
||||||
/* Calculate body angular rate error */
|
/* Calculate body angular rate error */
|
||||||
_rate_error = _bodyrate_setpoint - roll_bodyrate; //body angular rate error
|
_rate_error = _bodyrate_setpoint - roll_bodyrate; //body angular rate error
|
||||||
|
|
||||||
if (!lock_integrator && _k_i > 0.0f && airspeed > 0.5f * ctl_data.airspeed_min) {
|
if (!lock_integrator && _k_i > 0.0f) {
|
||||||
|
|
||||||
float id = _rate_error * dt * ctl_data.scaler;
|
float id = _rate_error * dt * ctl_data.scaler;
|
||||||
|
|
||||||
|
@ -157,3 +146,4 @@ float ECL_RollController::control_bodyrate(const struct ECL_ControlData &ctl_dat
|
||||||
|
|
||||||
return math::constrain(_last_output, -1.0f, 1.0f);
|
return math::constrain(_last_output, -1.0f, 1.0f);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
Loading…
Reference in New Issue