dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

fixed elevator wobbling at low airspeed. 

Removed not applicable if conditions.
This commit is contained in:
fludwig 2015-03-02 15:13:06 +01:00 committed by Friedemann Ludwig
parent 9574c6dd2a
commit 32012e8aac
4 changed files with 20 additions and 25 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
src/lib/ecl/attitude_fw/ecl_controller.cpp
View File

@ -49,6 +49,7 @@
#include "ecl_controller.h"
#include <stdio.h>
#include <mathlib/mathlib.h>
ECL_Controller::ECL_Controller(const char *name) :
_last_run(0),
@ -126,3 +127,14 @@ float ECL_Controller::get_desired_bodyrate()
{
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;
}

2
src/lib/ecl/attitude_fw/ecl_controller.h
View File

@ -51,6 +51,7 @@
#include <stdbool.h>
#include <stdint.h>
#include <systemlib/perf_counter.h>
#include <mathlib/mathlib.h>
struct ECL_ControlData {
float roll;
@ -119,4 +120,5 @@ protected:
perf_counter_t _nonfinite_input_perf;
static const uint8_t _perf_name_max = 40;
char _perf_name[_perf_name_max];
float constrain_airspeed(float airspeed, float minspeed, float maxspeed);
};

17
src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp
View File

@ -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 */
//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;
if (inverted) {
@ -154,17 +156,6 @@ float ECL_PitchController::control_bodyrate(const struct ECL_ControlData &ctl_da
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) */
_bodyrate_setpoint = cosf(ctl_data.roll) * _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;
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;

14
src/lib/ecl/attitude_fw/ecl_roll_controller.cpp
View File

@ -106,17 +106,6 @@ float ECL_RollController::control_bodyrate(const struct ECL_ControlData &ctl_dat
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) */
_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 */
_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;
@ -157,3 +146,4 @@ float ECL_RollController::control_bodyrate(const struct ECL_ControlData &ctl_dat
return math::constrain(_last_output, -1.0f, 1.0f);
}